ov_msckf::Simulator class

Master simulator class that generated visual-inertial measurements.

Given a trajectory this will generate a SE(3) ov_core::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(VioManagerOptions& params_)
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_paramters() -> VioManagerOptions
Access function to get the true parameters (i.e. calibration and settings)

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

VioManagerOptions params
True vio manager params (a copy of the parsed ones)
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.
Eigen::Vector3d true_bias_accel
Our running acceleration bias.
Eigen::Vector3d true_bias_gyro
Our running gyroscope bias.

Function documentation

ov_msckf::Simulator::Simulator(VioManagerOptions& params_)

Default constructor, will load all configuration variables.

Parameters
params_ VioManager parameters. Should have already been loaded from cmd.

bool ov_msckf::Simulator::ok()

Returns if we are actively simulating.

Returns True if we still have simulation data

double ov_msckf::Simulator::current_timestamp()

Gets the timestamp we have simulated up too.

Returns Timestamp

bool ov_msckf::Simulator::get_state(double desired_time, Eigen::Matrix<double, 17, 1>& imustate)

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_msckf::Simulator::get_next_imu(double& time_imu, Eigen::Vector3d& wm, Eigen::Vector3d& am)

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_msckf::Simulator::get_next_cam(double& time_cam, std::vector<int>& camids, std::vector<std::vector<std::pair<size_t, Eigen::VectorXf>>>& feats)

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_msckf::Simulator::load_data(std::string path_traj) protected

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_msckf::Simulator::project_pointcloud(const Eigen::Matrix3d& R_GtoI, const Eigen::Vector3d& p_IinG, int camid, const std::unordered_map<size_t, Eigen::Vector3d>& feats) protected

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_msckf::Simulator::generate_points(const Eigen::Matrix3d& R_GtoI, const Eigen::Vector3d& p_IinG, int camid, std::unordered_map<size_t, Eigen::Vector3d>& feats, int numpts) protected

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