diff --git a/data/euroc_ds_calib.json b/data/euroc_ds_calib.json index 83e1872..210fd34 100644 --- a/data/euroc_ds_calib.json +++ b/data/euroc_ds_calib.json @@ -184,10 +184,26 @@ 0.0 ], "imu_update_rate": 200.0, - "accel_noise_std": 0.016, - "gyro_noise_std": 0.000282, - "accel_bias_std": 0.001, - "gyro_bias_std": 0.0001, + "accel_noise_std": [ + 0.016, + 0.016, + 0.016 + ], + "gyro_noise_std": [ + 0.000282, + 0.000282, + 0.000282 + ], + "accel_bias_std": [ + 0.001, + 0.001, + 0.001 + ], + "gyro_bias_std": [ + 0.0001, + 0.0001, + 0.0001 + ], "T_mocap_world": { "px": 0.0, "py": 0.0, diff --git a/data/euroc_eucm_calib.json b/data/euroc_eucm_calib.json index 9973a89..e23a5bb 100644 --- a/data/euroc_eucm_calib.json +++ b/data/euroc_eucm_calib.json @@ -184,10 +184,10 @@ 0.0 ], "imu_update_rate": 200.0, - "accel_noise_std": 0.016, - "gyro_noise_std": 0.000282, - "accel_bias_std": 0.001, - "gyro_bias_std": 0.0001, + "accel_noise_std": [0.016, 0.016, 0.016], + "gyro_noise_std": [0.000282, 0.000282, 0.000282], + "accel_bias_std": [0.001, 0.001, 0.001], + "gyro_bias_std": [0.0001, 0.0001, 0.0001], "T_mocap_world": { "px": 0.0, "py": 0.0, diff --git a/data/tumvi_512_ds_calib.json b/data/tumvi_512_ds_calib.json index 1f5d4b8..a9a441c 100644 --- a/data/tumvi_512_ds_calib.json +++ b/data/tumvi_512_ds_calib.json @@ -80,10 +80,10 @@ 0.0 ], "imu_update_rate": 200.0, - "accel_noise_std": 0.016, - "gyro_noise_std": 0.000282, - "accel_bias_std": 0.001, - "gyro_bias_std": 0.0001, + "accel_noise_std": [0.016, 0.016, 0.016], + "gyro_noise_std": [0.000282, 0.000282, 0.000282], + "accel_bias_std": [0.001, 0.001, 0.001], + "gyro_bias_std": [0.0001, 0.0001, 0.0001], "T_mocap_world": { "px": 0.0, "py": 0.0, diff --git a/data/tumvi_512_eucm_calib.json b/data/tumvi_512_eucm_calib.json index 50010a8..98d2cff 100644 --- a/data/tumvi_512_eucm_calib.json +++ b/data/tumvi_512_eucm_calib.json @@ -80,10 +80,10 @@ 0.0 ], "imu_update_rate": 200.0, - "accel_noise_std": 0.016, - "gyro_noise_std": 0.000282, - "accel_bias_std": 0.001, - "gyro_bias_std": 0.0001, + "accel_noise_std": [0.016, 0.016, 0.016], + "gyro_noise_std": [0.000282, 0.000282, 0.000282], + "accel_bias_std": [0.001, 0.001, 0.001], + "gyro_bias_std": [0.0001, 0.0001, 0.0001], "T_mocap_world": { "px": 0.0, "py": 0.0, diff --git a/include/basalt/optimization/linearize.h b/include/basalt/optimization/linearize.h index dccb264..2fe0cf7 100644 --- a/include/basalt/optimization/linearize.h +++ b/include/basalt/optimization/linearize.h @@ -122,8 +122,8 @@ struct LinearizeBase { bool opt_imu_scale; Scalar pose_var_inv; - Scalar gyro_var_inv; - Scalar accel_var_inv; + Vector3 gyro_var_inv; + Vector3 accel_var_inv; Scalar mocap_var_inv; Scalar huber_thresh; diff --git a/include/basalt/optimization/spline_linearize.h b/include/basalt/optimization/spline_linearize.h index 131b2cc..f5d4f12 100644 --- a/include/basalt/optimization/spline_linearize.h +++ b/include/basalt/optimization/spline_linearize.h @@ -235,9 +235,9 @@ struct LinearizeSplineOpt : public LinearizeBase { // std::cerr << "sline.maxTime() " << spline.maxTime() << std::endl; // std::cerr << "================================" << std::endl; - const Scalar& accel_var_inv = this->common_data.accel_var_inv; + const Vector3& accel_var_inv = this->common_data.accel_var_inv; - error += accel_var_inv * residual.squaredNorm(); + error += residual.transpose() * accel_var_inv.asDiagonal() * residual; size_t start_bias = this->common_data.bias_block_offset + ACCEL_BIAS_OFFSET; @@ -255,39 +255,45 @@ struct LinearizeSplineOpt : public LinearizeBase { accum.template addH( start_i, start_j, - accel_var_inv * J.d_val_d_knot[i].transpose() * + J.d_val_d_knot[i].transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[j]); } accum.template addH( start_bias, start_i, - accel_var_inv * J_bias.transpose() * J.d_val_d_knot[i]); + J_bias.transpose() * accel_var_inv.asDiagonal() * + J.d_val_d_knot[i]); if (this->common_data.opt_g) { accum.template addH( start_g, start_i, - accel_var_inv * J_g.transpose() * J.d_val_d_knot[i]); + J_g.transpose() * accel_var_inv.asDiagonal() * J.d_val_d_knot[i]); } - accum.template addB( - start_i, accel_var_inv * J.d_val_d_knot[i].transpose() * residual); + accum.template addB(start_i, J.d_val_d_knot[i].transpose() * + accel_var_inv.asDiagonal() * + residual); } accum.template addH( - start_bias, start_bias, accel_var_inv * J_bias.transpose() * J_bias); + start_bias, start_bias, + J_bias.transpose() * accel_var_inv.asDiagonal() * J_bias); if (this->common_data.opt_g) { accum.template addH( - start_g, start_bias, accel_var_inv * J_g.transpose() * J_bias); + start_g, start_bias, + J_g.transpose() * accel_var_inv.asDiagonal() * J_bias); accum.template addH( - start_g, start_g, accel_var_inv * J_g.transpose() * J_g); + start_g, start_g, + J_g.transpose() * accel_var_inv.asDiagonal() * J_g); } accum.template addB( - start_bias, accel_var_inv * J_bias.transpose() * residual); + start_bias, + J_bias.transpose() * accel_var_inv.asDiagonal() * residual); if (this->common_data.opt_g) { - accum.template addB(start_g, - accel_var_inv * J_g.transpose() * residual); + accum.template addB( + start_g, J_g.transpose() * accel_var_inv.asDiagonal() * residual); } } } @@ -305,7 +311,7 @@ struct LinearizeSplineOpt : public LinearizeBase { BASALT_ASSERT(t_ns >= spline->minTimeNs()); BASALT_ASSERT(t_ns <= spline->maxTimeNs()); - const Scalar& gyro_var_inv = this->common_data.gyro_var_inv; + const Vector3& gyro_var_inv = this->common_data.gyro_var_inv; Vector3 residual = spline->gyroResidual( t_ns, pm.data, this->common_data.calibration->calib_gyro_bias, &J, @@ -323,7 +329,7 @@ struct LinearizeSplineOpt : public LinearizeBase { // std::cerr << "pm.data " << pm.data.transpose() << std::endl; // std::cerr << "t_ns " << t_ns << std::endl; - error += gyro_var_inv * residual.squaredNorm(); + error += residual.transpose() * gyro_var_inv.asDiagonal() * residual; size_t start_bias = this->common_data.bias_block_offset + GYRO_BIAS_OFFSET; @@ -344,19 +350,23 @@ struct LinearizeSplineOpt : public LinearizeBase { accum.template addH( start_i, start_j, - gyro_var_inv * J.d_val_d_knot[i].transpose() * J.d_val_d_knot[j]); + J.d_val_d_knot[i].transpose() * gyro_var_inv.asDiagonal() * + J.d_val_d_knot[j]); } accum.template addH( start_bias, start_i, - gyro_var_inv * J_bias.transpose() * J.d_val_d_knot[i]); - accum.template addB( - start_i, gyro_var_inv * J.d_val_d_knot[i].transpose() * residual); + J_bias.transpose() * gyro_var_inv.asDiagonal() * J.d_val_d_knot[i]); + accum.template addB(start_i, J.d_val_d_knot[i].transpose() * + gyro_var_inv.asDiagonal() * + residual); } accum.template addH( - start_bias, start_bias, gyro_var_inv * J_bias.transpose() * J_bias); + start_bias, start_bias, + J_bias.transpose() * gyro_var_inv.asDiagonal() * J_bias); accum.template addB( - start_bias, gyro_var_inv * J_bias.transpose() * residual); + start_bias, + J_bias.transpose() * gyro_var_inv.asDiagonal() * residual); } } @@ -732,9 +742,9 @@ struct ComputeErrorSplineOpt : public LinearizeBase { t, pm.data, this->common_data.calibration->calib_accel_bias, *(this->common_data.g)); - const Scalar& accel_var_inv = this->common_data.accel_var_inv; + const Vector3& accel_var_inv = this->common_data.accel_var_inv; - error += accel_var_inv * residual.squaredNorm(); + error += residual.transpose() * accel_var_inv.asDiagonal() * residual; } } @@ -748,12 +758,12 @@ struct ComputeErrorSplineOpt : public LinearizeBase { BASALT_ASSERT(t_ns >= spline->minTimeNs()); BASALT_ASSERT(t_ns <= spline->maxTimeNs()); - const Scalar& gyro_var_inv = this->common_data.gyro_var_inv; + const Vector3& gyro_var_inv = this->common_data.gyro_var_inv; Vector3 residual = spline->gyroResidual( t_ns, pm.data, this->common_data.calibration->calib_gyro_bias); - error += gyro_var_inv * residual.squaredNorm(); + error += residual.transpose() * gyro_var_inv.asDiagonal() * residual; } } diff --git a/include/basalt/optimization/spline_optimize.h b/include/basalt/optimization/spline_optimize.h index 3318b60..e85b6fe 100644 --- a/include/basalt/optimization/spline_optimize.h +++ b/include/basalt/optimization/spline_optimize.h @@ -324,8 +324,10 @@ class SplineOptimization { ccd.opt_g = true; ccd.pose_var_inv = pose_var_inv; - ccd.gyro_var_inv = 1.0 / (calib->gyro_noise_std * calib->gyro_noise_std); - ccd.accel_var_inv = 1.0 / (calib->accel_noise_std * calib->accel_noise_std); + ccd.gyro_var_inv = + calib->dicreete_time_gyro_noise_std().array().square().inverse(); + ccd.accel_var_inv = + calib->dicreete_time_accel_noise_std().array().square().inverse(); ccd.mocap_var_inv = pose_var_inv; } diff --git a/src/calibration/cam_imu_calib.cpp b/src/calibration/cam_imu_calib.cpp index 4a2dc5e..f39f3b7 100644 --- a/src/calibration/cam_imu_calib.cpp +++ b/src/calibration/cam_imu_calib.cpp @@ -651,10 +651,10 @@ void CamImuCalib::loadDataset() { calib_opt->loadCalib(cache_path); - calib_opt->calib->accel_noise_std = imu_noise[0]; - calib_opt->calib->gyro_noise_std = imu_noise[1]; - calib_opt->calib->accel_bias_std = imu_noise[2]; - calib_opt->calib->gyro_bias_std = imu_noise[3]; + calib_opt->calib->accel_noise_std.setConstant(imu_noise[0]); + calib_opt->calib->gyro_noise_std.setConstant(imu_noise[1]); + calib_opt->calib->accel_bias_std.setConstant(imu_noise[2]); + calib_opt->calib->gyro_bias_std.setConstant(imu_noise[3]); } calib_opt->resetMocapCalib(); diff --git a/src/device/rs_t265.cpp b/src/device/rs_t265.cpp index 5bc5377..80d19b4 100644 --- a/src/device/rs_t265.cpp +++ b/src/device/rs_t265.cpp @@ -139,13 +139,6 @@ void RsT265Device::start() { data->accel = accel_interpolated; data->gyro = gyro_data.data; - const double accel_noise_std = - calib->dicreete_time_accel_noise_std(); - const double gyro_noise_std = calib->dicreete_time_gyro_noise_std(); - - data->accel_cov.setConstant(accel_noise_std * accel_noise_std); - data->gyro_cov.setConstant(gyro_noise_std * gyro_noise_std); - if (imu_data_queue) imu_data_queue->push(data); } @@ -253,13 +246,15 @@ std::shared_ptr> RsT265Device::exportCalibration() { // std::cout << "Gyro Bias\n" << gyro_bias_full << std::endl; - calib->gyro_noise_std = std::sqrt(std::max( - std::max(intrinsics.noise_variances[0], intrinsics.noise_variances[1]), - intrinsics.noise_variances[2])); + calib->gyro_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0], + intrinsics.noise_variances[1], + intrinsics.noise_variances[2]) + .cwiseSqrt(); - calib->gyro_bias_std = std::sqrt(std::max( - std::max(intrinsics.bias_variances[0], intrinsics.bias_variances[1]), - intrinsics.bias_variances[2])); + calib->gyro_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0], + intrinsics.bias_variances[1], + intrinsics.bias_variances[2]) + .cwiseSqrt(); // std::cout << "Gyro noise var: " << intrinsics.noise_variances[0] // << " bias var: " << intrinsics.bias_variances[0] << std::endl; @@ -281,13 +276,15 @@ std::shared_ptr> RsT265Device::exportCalibration() { // std::cout << "Gyro Bias\n" << accel_bias_full << std::endl; - calib->accel_noise_std = sqrt(std::max( - std::max(intrinsics.noise_variances[0], intrinsics.noise_variances[1]), - intrinsics.noise_variances[2])); + calib->accel_noise_std = Eigen::Vector3d(intrinsics.noise_variances[0], + intrinsics.noise_variances[1], + intrinsics.noise_variances[2]) + .cwiseSqrt(); - calib->accel_bias_std = sqrt(std::max( - std::max(intrinsics.bias_variances[0], intrinsics.bias_variances[1]), - intrinsics.bias_variances[2])); + calib->accel_bias_std = Eigen::Vector3d(intrinsics.bias_variances[0], + intrinsics.bias_variances[1], + intrinsics.bias_variances[2]) + .cwiseSqrt(); // std::cout << "Accel noise var: " << intrinsics.noise_variances[0] // << " bias var: " << intrinsics.bias_variances[0] << std::endl; diff --git a/src/mapper_sim_naive.cpp b/src/mapper_sim_naive.cpp index 926c123..29528b6 100644 --- a/src/mapper_sim_naive.cpp +++ b/src/mapper_sim_naive.cpp @@ -174,15 +174,6 @@ int main(int argc, char** argv) { load_data(cam_calib_path); - std::normal_distribution<> obs_noise_dist{0, obs_std_dev}; - std::normal_distribution<> gyro_noise_dist{ - 0, calib.dicreete_time_gyro_noise_std()}; - std::normal_distribution<> accel_noise_dist{ - 0, calib.dicreete_time_accel_noise_std()}; - - std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std}; - std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std}; - gen_data(); setup_vio(); @@ -198,11 +189,6 @@ int main(int argc, char** argv) { data->accel = noisy_accel[i]; data->gyro = noisy_gyro[i]; - data->accel_cov.setConstant(calib.dicreete_time_accel_noise_std() * - calib.dicreete_time_accel_noise_std()); - data->gyro_cov.setConstant(calib.dicreete_time_gyro_noise_std() * - calib.dicreete_time_gyro_noise_std()); - vio->addIMUToQueue(data); } diff --git a/src/vi_estimator/keypoint_vio.cpp b/src/vi_estimator/keypoint_vio.cpp index fdf571b..5848ac4 100644 --- a/src/vi_estimator/keypoint_vio.cpp +++ b/src/vi_estimator/keypoint_vio.cpp @@ -75,10 +75,8 @@ KeypointVioEstimator::KeypointVioEstimator( std::cout << "marg_H\n" << marg_H << std::endl; - gyro_bias_weight.setConstant(1.0 / - (calib.gyro_bias_std * calib.gyro_bias_std)); - accel_bias_weight.setConstant(1.0 / - (calib.accel_bias_std * calib.accel_bias_std)); + gyro_bias_weight = calib.gyro_bias_std.array().square().inverse(); + accel_bias_weight = calib.accel_bias_std.array().square().inverse(); max_states = std::numeric_limits::max(); max_kfs = std::numeric_limits::max(); @@ -114,6 +112,10 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, OpticalFlowResult::Ptr prev_frame, curr_frame; IntegratedImuMeasurement::Ptr meas; + Eigen::Vector3d accel_cov, gyro_cov; + accel_cov = calib.dicreete_time_accel_noise_std().array().square(); + gyro_cov = calib.dicreete_time_gyro_noise_std().array().square(); + ImuData::Ptr data; imu_data_queue.pop(data); @@ -165,7 +167,8 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, } while (data->t_ns <= curr_frame->t_ns) { - meas->integrate(*data); + meas->integrate(*data, calib.dicreete_time_accel_noise_std(), + calib.dicreete_time_gyro_noise_std()); imu_data_queue.pop(data); if (!data.get()) break; } @@ -173,7 +176,7 @@ void KeypointVioEstimator::initialize(const Eigen::Vector3d& bg, if (meas->get_start_t_ns() + meas->get_dt_ns() < curr_frame->t_ns) { int64_t tmp = data->t_ns; data->t_ns = curr_frame->t_ns; - meas->integrate(*data); + meas->integrate(*data, accel_cov, gyro_cov); data->t_ns = tmp; } } diff --git a/src/vio.cpp b/src/vio.cpp index 630c59c..c9fcf05 100644 --- a/src/vio.cpp +++ b/src/vio.cpp @@ -165,12 +165,6 @@ void feed_imu() { data->accel = vio_dataset->get_accel_data()[i].data; data->gyro = vio_dataset->get_gyro_data()[i].data; - const double accel_noise_std = calib.dicreete_time_accel_noise_std(); - const double gyro_noise_std = calib.dicreete_time_gyro_noise_std(); - - data->accel_cov.setConstant(accel_noise_std * accel_noise_std); - data->gyro_cov.setConstant(gyro_noise_std * gyro_noise_std); - vio->imu_data_queue.push(data); } vio->imu_data_queue.push(nullptr); diff --git a/src/vio_sim.cpp b/src/vio_sim.cpp index f291782..af29be8 100644 --- a/src/vio_sim.cpp +++ b/src/vio_sim.cpp @@ -209,11 +209,6 @@ int main(int argc, char** argv) { data->accel = noisy_accel[i]; data->gyro = noisy_gyro[i]; - data->accel_cov.setConstant(calib.dicreete_time_accel_noise_std() * - calib.dicreete_time_accel_noise_std()); - data->gyro_cov.setConstant(calib.dicreete_time_gyro_noise_std() * - calib.dicreete_time_gyro_noise_std()); - vio->addIMUToQueue(data); } @@ -600,12 +595,12 @@ void compute_projections() { void gen_data() { std::normal_distribution<> gyro_noise_dist{ - 0, calib.dicreete_time_gyro_noise_std()}; + 0, calib.dicreete_time_gyro_noise_std()[0]}; std::normal_distribution<> accel_noise_dist{ - 0, calib.dicreete_time_accel_noise_std()}; + 0, calib.dicreete_time_accel_noise_std()[0]}; - std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std}; - std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std}; + std::normal_distribution<> gyro_bias_dist{0, calib.gyro_bias_std[0]}; + std::normal_distribution<> accel_bias_dist{0, calib.accel_bias_std[0]}; for (size_t i = 0; i < calib.intrinsics.size(); i++) { images.emplace_back(); diff --git a/test/src/test_vio.cpp b/test/src/test_vio.cpp index 7f72f65..c36e97a 100644 --- a/test/src/test_vio.cpp +++ b/test/src/test_vio.cpp @@ -41,6 +41,10 @@ TEST(VioTestSuite, ImuNullspace2Test) { state0.bias_gyro = bg; state0.bias_accel = ba; + Eigen::Vector3d accel_cov, gyro_cov; + accel_cov.setConstant(accel_std_dev * accel_std_dev); + gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + int64_t dt_ns = 1e7; for (int64_t t_ns = dt_ns / 2; t_ns < int64_t(1e8); // gt_spline.maxTimeNs() - int64_t(1e9); @@ -63,12 +67,9 @@ TEST(VioTestSuite, ImuNullspace2Test) { data.gyro[1] += gyro_noise_dist(gen); data.gyro[2] += gyro_noise_dist(gen); - data.accel_cov.setConstant(accel_std_dev * accel_std_dev); - data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); - data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; - imu_meas.integrate(data); + imu_meas.integrate(data, accel_cov, gyro_cov); } state1.t_ns = imu_meas.get_dt_ns(); @@ -166,6 +167,10 @@ TEST(VioTestSuite, ImuNullspace3Test) { state0.bias_gyro = bg; state0.bias_accel = ba; + Eigen::Vector3d accel_cov, gyro_cov; + accel_cov.setConstant(accel_std_dev * accel_std_dev); + gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); + int64_t dt_ns = 1e7; for (int64_t t_ns = dt_ns / 2; t_ns < int64_t(1e9); // gt_spline.maxTimeNs() - int64_t(1e9); @@ -188,12 +193,9 @@ TEST(VioTestSuite, ImuNullspace3Test) { data.gyro[1] += gyro_noise_dist(gen); data.gyro[2] += gyro_noise_dist(gen); - data.accel_cov.setConstant(accel_std_dev * accel_std_dev); - data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); - data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; - imu_meas1.integrate(data); + imu_meas1.integrate(data, accel_cov, gyro_cov); } basalt::IntegratedImuMeasurement imu_meas2(imu_meas1.get_dt_ns(), bg, ba); @@ -218,12 +220,9 @@ TEST(VioTestSuite, ImuNullspace3Test) { data.gyro[1] += gyro_noise_dist(gen); data.gyro[2] += gyro_noise_dist(gen); - data.accel_cov.setConstant(accel_std_dev * accel_std_dev); - data.gyro_cov.setConstant(gyro_std_dev * gyro_std_dev); - data.t_ns = t_ns + dt_ns / 2; // measurement in the middle of the interval; - imu_meas2.integrate(data); + imu_meas2.integrate(data, accel_cov, gyro_cov); } state1.t_ns = imu_meas1.get_dt_ns(); diff --git a/thirdparty/basalt-headers b/thirdparty/basalt-headers index 62ad5cb..d169d69 160000 --- a/thirdparty/basalt-headers +++ b/thirdparty/basalt-headers @@ -1 +1 @@ -Subproject commit 62ad5cbac6bc8c906eeb9c8dad2f2581c1576fec +Subproject commit d169d697f8902ca684e41887eee3bedf91176e56