Simulator class
Master simulator class that will create artificial measurements for our visual-inertial algorithms.
Contents
Given a trajectory this will generate a SE(3) BsplineSE3 for that trajectory. This allows us to get the inertial measurement information at each timestep during this trajectory. After creating the bspline we will generate an environmental feature map which will be used as our feature measurements. This map will be projected into the frame at each timestep to get our "raw" uv measurements. We inject bias and white noises into our inertial readings while adding our white noise to the uv measurements also. The user should specify the sensor rates that they desire along with the seeds of the random number generators.
Constructors, destructors, conversion operators
- Simulator(ros::NodeHandle& nh)
- Default constructor, will load all configuration variables.
Public functions
- auto ok() -> bool
- Returns if we are actively simulating.
- auto current_timestamp() -> double
- Gets the timestamp we have simulated up too.
- auto get_state(double desired_time, Eigen::Matrix<double, 17, 1>& imustate) -> bool
- Get the simulation state at a specified timestep.
- auto get_next_imu(double& time_imu, Eigen::Vector3d& wm, Eigen::Vector3d& am) -> bool
- Gets the next inertial reading if we have one.
- auto get_next_cam(double& time_cam, std::vector<int>& camids, std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>>& feats) -> bool
- Gets the next inertial reading if we have one.
- auto get_map() -> std::unordered_map<size_t, Eigen::Vector3d>
- Returns the true 3d map of features.
- auto get_true_intrinsics() -> std::unordered_map<size_t, Eigen::VectorXd>
- Access function for the true camera intrinsics.
- auto get_true_extrinsics() -> std::unordered_map<size_t, Eigen::VectorXd>
- Access function for the true camera extrinsics.
- auto get_true_imucamdt() -> double
- Access function for the true imu+camera time calibration.
- auto get_num_cameras() -> int
- Get number of cameras that we have.
Protected functions
- void load_data(std::string path_traj)
- This will load the trajectory into memory.
- auto project_pointcloud(const Eigen::Matrix3d& R_GtoI, const Eigen::Vector3d& p_IinG, int camid, const std::unordered_map<size_t, Eigen::Vector3d>& feats) -> std::vector<std::pair<size_t, Eigen::VectorXf>>
- Projects the passed map features into the desired camera frame.
- void generate_points(const Eigen::Matrix3d& R_GtoI, const Eigen::Vector3d& p_IinG, int camid, std::unordered_map<size_t, Eigen::Vector3d>& feats, int numpts)
- Will generate points in the fov of the specified camera.
Protected variables
- std::vector<Eigen::VectorXd> traj_data
- Our loaded trajectory data (timestamp(s), q_GtoI, p_IinG)
- BsplineSE3 spline
- Our b-spline trajectory.
- size_t id_map
- Our map of 3d features.
- std::mt19937 gen_meas_imu
- Mersenne twister PRNG for measurements (IMU)
- std::vector<std::mt19937> gen_meas_cams
- Mersenne twister PRNG for measurements (CAMERAS)
- std::mt19937 gen_state_init
- Mersenne twister PRNG for state initialization.
- std::mt19937 gen_state_perturb
- Mersenne twister PRNG for state perturbations.
- bool is_running
- If our simulation is running.
- double timestamp
- Current timestamp of the system.
- double timestamp_last_imu
- Last time we had an IMU reading.
- double timestamp_last_cam
- Last time we had an CAMERA reading.
- int max_cameras
- Number of cameras we should simulate.
- bool use_stereo
- If we should enforce that all cameras see the same map.
- double freq_cam
- Frequency of our camera sensors.
- double freq_imu
- Frequency of our imu sensor.
- Eigen::Vector3d true_bias_accel
- Our running acceleration bias.
- Eigen::Vector3d true_bias_gyro
- Our running gyroscope bias.
- Eigen::Vector3d gravity
- Gravity in the global frame.
- double calib_camimu_dt
- Timeoffset between camera and imu (t_cam = t_imu - t_off)
- int num_pts
- Max number of features to have in a single image.
Function documentation
ov_core::
Default constructor, will load all configuration variables.
| Parameters | |
|---|---|
| nh | ROS node handler which we will load parameters from |
bool ov_core::
Returns if we are actively simulating.
| Returns | True if we still have simulation data |
|---|
double ov_core::
Gets the timestamp we have simulated up too.
| Returns | Timestamp |
|---|
bool ov_core::
Get the simulation state at a specified timestep.
| Parameters | |
|---|---|
| desired_time | Timestamp we want to get the state at |
| imustate | State in the MSCKF ordering: [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel] |
| Returns | True if we have a state |
bool ov_core::
Gets the next inertial reading if we have one.
| Parameters | |
|---|---|
| time_imu | Time that this measurement occured at |
| wm | Angular velocity measurement in the inertial frame |
| am | Linear velocity in the inertial frame |
| Returns | True if we have a measurement |
bool ov_core::
Gets the next inertial reading if we have one.
| Parameters | |
|---|---|
| time_cam | Time that this measurement occured at |
| camids | Camera ids that the corresponding vectors match |
| feats | Noisy uv measurements and ids for the returned time |
| Returns | True if we have a measurement |
void ov_core::
This will load the trajectory into memory.
| Parameters | |
|---|---|
| path_traj | Path to the trajectory file that we want to read in. |
std::vector<std::pair<size_t, Eigen::VectorXf>> ov_core::
Projects the passed map features into the desired camera frame.
| Parameters | |
|---|---|
| R_GtoI | Orientation of the IMU pose |
| p_IinG | Position of the IMU pose |
| camid | Camera id of the camera sensor we want to project into |
| feats | Our set of 3d features |
| Returns | True distorted raw image measurements and their ids for the specified camera |
void ov_core::
Will generate points in the fov of the specified camera.
| Parameters | |
|---|---|
| R_GtoI | Orientation of the IMU pose |
| p_IinG | Position of the IMU pose |
| camid | Camera id of the camera sensor we want to project into |
| feats out | Map we will append new features to |
| numpts | Number of points we should generate |