35 _imu = std::make_shared<IMU>();
36 _imu->set_local_id(current_id);
37 _variables.push_back(_imu);
38 current_id += _imu->size();
43 _calib_imu_dw = std::make_shared<Vec>(6);
44 _calib_imu_da = std::make_shared<Vec>(6);
45 if (options.
imu_model == StateOptions::ImuModel::KALIBR) {
47 Eigen::Matrix<double, 6, 1> _imu_default = Eigen::Matrix<double, 6, 1>::Zero();
48 _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0;
49 _calib_imu_dw->set_value(_imu_default);
50 _calib_imu_dw->set_fej(_imu_default);
51 _calib_imu_da->set_value(_imu_default);
52 _calib_imu_da->set_fej(_imu_default);
55 Eigen::Matrix<double, 6, 1> _imu_default = Eigen::Matrix<double, 6, 1>::Zero();
56 _imu_default << 1.0, 0.0, 0.0, 1.0, 0.0, 1.0;
57 _calib_imu_dw->set_value(_imu_default);
58 _calib_imu_dw->set_fej(_imu_default);
59 _calib_imu_da->set_value(_imu_default);
60 _calib_imu_da->set_fej(_imu_default);
62 _calib_imu_tg = std::make_shared<Vec>(9);
63 _calib_imu_GYROtoIMU = std::make_shared<JPLQuat>();
64 _calib_imu_ACCtoIMU = std::make_shared<JPLQuat>();
68 _calib_imu_dw->set_local_id(current_id);
69 _variables.push_back(_calib_imu_dw);
70 current_id += _calib_imu_dw->size();
73 _calib_imu_da->set_local_id(current_id);
74 _variables.push_back(_calib_imu_da);
75 current_id += _calib_imu_da->size();
79 _calib_imu_tg->set_local_id(current_id);
80 _variables.push_back(_calib_imu_tg);
81 current_id += _calib_imu_tg->size();
86 if (options.
imu_model == StateOptions::ImuModel::KALIBR) {
87 _calib_imu_GYROtoIMU->set_local_id(current_id);
88 _variables.push_back(_calib_imu_GYROtoIMU);
89 current_id += _calib_imu_GYROtoIMU->size();
91 _calib_imu_ACCtoIMU->set_local_id(current_id);
92 _variables.push_back(_calib_imu_ACCtoIMU);
93 current_id += _calib_imu_ACCtoIMU->size();
98 _calib_dt_CAMtoIMU = std::make_shared<Vec>(1);
99 if (_options.do_calib_camera_timeoffset) {
100 _calib_dt_CAMtoIMU->set_local_id(current_id);
101 _variables.push_back(_calib_dt_CAMtoIMU);
102 current_id += _calib_dt_CAMtoIMU->size();
106 for (
int i = 0; i < _options.num_cameras; i++) {
109 auto pose = std::make_shared<PoseJPL>();
112 auto intrin = std::make_shared<Vec>(8);
115 _calib_IMUtoCAM.insert({i, pose});
116 _cam_intrinsics.insert({i, intrin});
119 if (_options.do_calib_camera_pose) {
120 pose->set_local_id(current_id);
121 _variables.push_back(pose);
122 current_id += pose->size();
126 if (_options.do_calib_camera_intrinsics) {
127 intrin->set_local_id(current_id);
128 _variables.push_back(intrin);
129 current_id += intrin->size();
134 _Cov = std::pow(1e-3, 2) * Eigen::MatrixXd::Identity(current_id, current_id);
137 if (_options.do_calib_imu_intrinsics) {
138 _Cov.block(_calib_imu_dw->id(), _calib_imu_dw->id(), 6, 6) = std::pow(0.005, 2) * Eigen::Matrix<double, 6, 6>::Identity();
139 _Cov.block(_calib_imu_da->id(), _calib_imu_da->id(), 6, 6) = std::pow(0.008, 2) * Eigen::Matrix<double, 6, 6>::Identity();
140 if (_options.do_calib_imu_g_sensitivity) {
141 _Cov.block(_calib_imu_tg->id(), _calib_imu_tg->id(), 9, 9) = std::pow(0.005, 2) * Eigen::Matrix<double, 9, 9>::Identity();
143 if (_options.imu_model == StateOptions::ImuModel::KALIBR) {
144 _Cov.block(_calib_imu_GYROtoIMU->id(), _calib_imu_GYROtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity();
146 _Cov.block(_calib_imu_ACCtoIMU->id(), _calib_imu_ACCtoIMU->id(), 3, 3) = std::pow(0.005, 2) * Eigen::Matrix3d::Identity();
149 if (_options.do_calib_camera_timeoffset) {
150 _Cov(_calib_dt_CAMtoIMU->id(), _calib_dt_CAMtoIMU->id()) = std::pow(0.01, 2);
152 if (_options.do_calib_camera_pose) {
153 for (
int i = 0; i < _options.num_cameras; i++) {
154 _Cov.block(_calib_IMUtoCAM.at(i)->id(), _calib_IMUtoCAM.at(i)->id(), 3, 3) = std::pow(0.005, 2) * Eigen::MatrixXd::Identity(3, 3);
155 _Cov.block(_calib_IMUtoCAM.at(i)->id() + 3, _calib_IMUtoCAM.at(i)->id() + 3, 3, 3) =
156 std::pow(0.015, 2) * Eigen::MatrixXd::Identity(3, 3);
159 if (_options.do_calib_camera_intrinsics) {
160 for (
int i = 0; i < _options.num_cameras; i++) {
161 _Cov.block(_cam_intrinsics.at(i)->id(), _cam_intrinsics.at(i)->id(), 4, 4) = std::pow(1.0, 2) * Eigen::MatrixXd::Identity(4, 4);
162 _Cov.block(_cam_intrinsics.at(i)->id() + 4, _cam_intrinsics.at(i)->id() + 4, 4, 4) =
163 std::pow(0.005, 2) * Eigen::MatrixXd::Identity(4, 4);