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

Support void runs

This commit is contained in:
Mateo de Mayo 2022-05-06 16:05:11 -03:00
parent 4b4bacb951
commit b90e7e87fc
1 changed files with 6 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/vi_estimator/sqrt_keypoint_vio.cpp
View File

@ -159,12 +159,14 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
const Vec3 gyro_cov = calib.dicrete_time_gyro_noise_std().array().square();
typename ImuData<Scalar>::Ptr data = popFromImuDataQueue();
BASALT_ASSERT_MSG(data, "first IMU measurment is nullptr");
data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
bool run = data != nullptr; // End VIO otherwise
if (run) {
data->accel = calib.calib_accel_bias.getCalibrated(data->accel);
data->gyro = calib.calib_gyro_bias.getCalibrated(data->gyro);
}
while (true) {
while (run) {
vision_data_queue.pop(curr_frame);
if (config.vio_enforce_realtime) {