ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added pure stereo to simulator

This commit is contained in:
Vladyslav Usenko 2019-08-23 12:01:40 +02:00
parent 27b9377285
commit d4da96119f
2 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/vi_estimator/keypoint_vo.cpp
View File

@ -99,6 +99,8 @@ void KeypointVoEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_SIZE); marg_order.abs_order_map[t_ns] = std::make_pair(0, POSE_SIZE);
marg_order.total_size = POSE_SIZE; marg_order.total_size = POSE_SIZE;
marg_order.items = 1; marg_order.items = 1;
initialize(bg, ba);
} }
void KeypointVoEstimator::initialize(const Eigen::Vector3d& bg, void KeypointVoEstimator::initialize(const Eigen::Vector3d& bg,

20
src/vio_sim.cpp
View File

@ -112,7 +112,7 @@ std::string marg_data_path;
// VIO vars // VIO vars
basalt::Calibration<double> calib; basalt::Calibration<double> calib;
basalt::KeypointVioEstimator::Ptr vio; basalt::VioEstimatorBase::Ptr vio;
// Visualization vars // Visualization vars
std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map; std::unordered_map<int64_t, basalt::VioVisualizationData::Ptr> vis_map;
@ -154,6 +154,8 @@ pangolin::Var<bool> continue_btn("ui.continue", true, false, true);
Button align_step_btn("ui.align_se3", &alignButton); Button align_step_btn("ui.align_se3", &alignButton);
bool use_imu = true;
int main(int argc, char** argv) { int main(int argc, char** argv) {
srand(1); srand(1);
@ -176,6 +178,7 @@ int main(int argc, char** argv) {
"Path to result file where the system will write RMSE ATE."); "Path to result file where the system will write RMSE ATE.");
app.add_option("--num-points", NUM_POINTS, "Number of points in simulation."); app.add_option("--num-points", NUM_POINTS, "Number of points in simulation.");
app.add_option("--use-imu", use_imu, "Use IMU.");
try { try {
app.parse(argc, argv); app.parse(argc, argv);
@ -209,10 +212,10 @@ int main(int argc, char** argv) {
data->accel = noisy_accel[i]; data->accel = noisy_accel[i];
data->gyro = noisy_gyro[i]; data->gyro = noisy_gyro[i];
vio->addIMUToQueue(data); vio->imu_data_queue.push(data);
} }
vio->addIMUToQueue(nullptr); vio->imu_data_queue.push(nullptr);
std::cout << "Finished t0" << std::endl; std::cout << "Finished t0" << std::endl;
}); });
@ -234,10 +237,10 @@ int main(int argc, char** argv) {
} }
} }
vio->addVisionToQueue(data); vio->vision_data_queue.push(data);
} }
vio->addVisionToQueue(nullptr); vio->vision_data_queue.push(nullptr);
std::cout << "Finished t1" << std::endl; std::cout << "Finished t1" << std::endl;
}); });
@ -875,13 +878,12 @@ void setup_vio() {
basalt::VioConfig config; basalt::VioConfig config;
vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config)); // vio.reset(new basalt::KeypointVioEstimator(0.0001, g, calib, config));
vio = basalt::VioEstimatorFactory::getVioEstimator(
config, calib, 0.0001, basalt::constants::g, use_imu);
vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(), vio->initialize(t_init_ns, T_w_i_init, vel_w_i_init, gt_gyro_bias.front(),
gt_accel_bias.front()); gt_accel_bias.front());
vio->setMaxStates(3);
vio->setMaxKfs(5);
// int iteration = 0; // int iteration = 0;
vio_data_log.Clear(); vio_data_log.Clear();
error_data_log.Clear(); error_data_log.Clear();