24 #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
25 #include <boost/serialization/export.hpp>
40 cout <<
"biasAccCovariance:\n[\n" << biasAccCovariance <<
"\n]"
42 cout <<
"biasOmegaCovariance:\n[\n" << biasOmegaCovariance <<
"\n]"
44 cout <<
"biasAccOmegaInt:\n[\n" << biasAccOmegaInt <<
"\n]"
65 cout <<
" preintMeasCov [ " << preintMeasCov_ <<
" ]" << endl;
79 preintMeasCov_.setZero();
84 const gtsam::Matrix6& Q_init) {
87 p().biasAccOmegaInt = Q_init;
88 preintMeasCov_.setZero();
93 #define D_R_R(H) (H)->block<3,3>(0,0)
94 #define D_R_t(H) (H)->block<3,3>(0,3)
95 #define D_R_v(H) (H)->block<3,3>(0,6)
96 #define D_t_R(H) (H)->block<3,3>(3,0)
97 #define D_t_t(H) (H)->block<3,3>(3,3)
98 #define D_t_v(H) (H)->block<3,3>(3,6)
99 #define D_v_R(H) (H)->block<3,3>(6,0)
100 #define D_v_t(H) (H)->block<3,3>(6,3)
101 #define D_v_v(H) (H)->block<3,3>(6,6)
102 #define D_a_a(H) (H)->block<3,3>(9,9)
103 #define D_g_g(H) (H)->block<3,3>(12,12)
109 throw std::runtime_error(
110 "PreintegratedCombinedMeasurements::integrateMeasurement: dt <=0");
125 Matrix3 theta_H_omega =
C.topRows<3>();
126 Matrix3 pos_H_acc =
B.middleRows<3>(3);
127 Matrix3 vel_H_acc =
B.bottomRows<3>();
129 Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
130 Matrix3 pos_H_biasAccInit = -pos_H_acc;
131 Matrix3 vel_H_biasAccInit = -vel_H_acc;
136 F.block<9, 9>(0, 0) =
A;
137 F.block<3, 3>(0, 12) = theta_H_omega;
138 F.block<3, 3>(3, 9) = pos_H_acc;
139 F.block<3, 3>(6, 9) = vel_H_acc;
140 F.block<6, 6>(9, 9) = I_6x6;
143 preintMeasCov_ =
F * preintMeasCov_ *
F.transpose();
147 const Matrix3& aCov =
p().accelerometerCovariance;
148 const Matrix3& wCov =
p().gyroscopeCovariance;
149 const Matrix3& iCov =
p().integrationCovariance;
150 const Matrix6& bInitCov =
p().biasAccOmegaInt;
157 const Matrix3& bInitCov11 = bInitCov.block<3, 3>(0, 0) /
dt;
158 const Matrix3& bInitCov12 = bInitCov.block<3, 3>(0, 3) /
dt;
159 const Matrix3& bInitCov21 = bInitCov.block<3, 3>(3, 0) /
dt;
160 const Matrix3& bInitCov22 = bInitCov.block<3, 3>(3, 3) /
dt;
163 D_R_R(&G_measCov_Gt) =
164 (theta_H_omega * (wCov /
dt) * theta_H_omega.transpose())
166 (theta_H_biasOmegaInit * bInitCov22 * theta_H_biasOmegaInit.transpose());
168 D_t_t(&G_measCov_Gt) =
169 (pos_H_acc * (aCov /
dt) * pos_H_acc.transpose())
170 + (pos_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose())
173 D_v_v(&G_measCov_Gt) =
174 (vel_H_acc * (aCov /
dt) * vel_H_acc.transpose())
175 + (vel_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
177 D_a_a(&G_measCov_Gt) =
dt *
p().biasAccCovariance;
178 D_g_g(&G_measCov_Gt) =
dt *
p().biasOmegaCovariance;
181 D_R_t(&G_measCov_Gt) =
182 theta_H_biasOmegaInit * bInitCov21 * pos_H_biasAccInit.transpose();
183 D_R_v(&G_measCov_Gt) =
184 theta_H_biasOmegaInit * bInitCov21 * vel_H_biasAccInit.transpose();
185 D_t_R(&G_measCov_Gt) =
186 pos_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
187 D_t_v(&G_measCov_Gt) =
188 (pos_H_acc * (aCov /
dt) * vel_H_acc.transpose()) +
189 (pos_H_biasAccInit * bInitCov11 * vel_H_biasAccInit.transpose());
190 D_v_R(&G_measCov_Gt) =
191 vel_H_biasAccInit * bInitCov12 * theta_H_biasOmegaInit.transpose();
192 D_v_t(&G_measCov_Gt) =
193 (vel_H_acc * (aCov /
dt) * pos_H_acc.transpose()) +
194 (vel_H_biasAccInit * bInitCov11 * pos_H_biasAccInit.transpose());
196 preintMeasCov_.noalias() += G_measCov_Gt;
205 Base(noiseModel::
Gaussian::Covariance(pim.preintMeasCov_), pose_i, vel_i,
206 pose_j, vel_j, bias_i, bias_j), _PIM_(pim) {
211 return std::static_pointer_cast<gtsam::NonlinearFactor>(
218 cout << (
s.empty() ?
s :
s +
"\n") <<
"CombinedImuFactor("
219 << keyFormatter(this->key<1>()) <<
"," << keyFormatter(this->key<2>()) <<
","
220 << keyFormatter(this->key<3>()) <<
"," << keyFormatter(this->key<4>()) <<
","
221 << keyFormatter(this->key<5>()) <<
"," << keyFormatter(this->key<6>())
242 Matrix6 Hbias_i, Hbias_j;
244 H6 ? &Hbias_j : 0, H5 ? &Hbias_i : 0).vector();
246 Matrix96 D_r_pose_i, D_r_pose_j, D_r_bias_i;
251 bias_i, H1 ? &D_r_pose_i : 0, H2 ? &D_r_vel_i : 0, H3 ? &D_r_pose_j : 0,
252 H4 ? &D_r_vel_j : 0, H5 ? &D_r_bias_i : 0);
257 H1->block<9, 6>(0, 0) = D_r_pose_i;
259 H1->block<6, 6>(9, 0).
setZero();
263 H2->block<9, 3>(0, 0) = D_r_vel_i;
265 H2->block<6, 3>(9, 0).
setZero();
269 H3->block<9, 6>(0, 0) = D_r_pose_j;
271 H3->block<6, 6>(9, 0).
setZero();
275 H4->block<9, 3>(0, 0) = D_r_vel_j;
277 H4->block<6, 3>(9, 0).
setZero();
281 H5->block<9, 6>(0, 0) = D_r_bias_i;
283 H5->block<6, 6>(9, 0) = Hbias_i;
287 H6->block<9, 6>(0, 0).
setZero();
289 H6->block<6, 6>(9, 0) = Hbias_j;
300 f._PIM_.print(
"combined preintegrated measurements:\n");
301 os <<
" noise model sigmas: " <<
f.noiseModel_->sigmas().transpose();