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

fixed unitialized jacobian

This commit is contained in:
Vladyslav Usenko 2019-04-19 16:50:12 +02:00
parent 16d4fd90a8
commit bc62be4922
1 changed files with 23 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
include/basalt/vi_estimator/ba_base.h
View File

@ -174,6 +174,7 @@ class BundleAdjustmentBase {
if (d_res_d_p) { if (d_res_d_p) {
Eigen::Matrix<double, 4, 3> Jpp; Eigen::Matrix<double, 4, 3> Jpp;
Jpp.setZero();
Jpp.block<3, 2>(0, 0) = T_t_h.topLeftCorner<3, 4>() * Jup; Jpp.block<3, 2>(0, 0) = T_t_h.topLeftCorner<3, 4>() * Jup;
Jpp.col(2) = T_t_h.col(3); Jpp.col(2) = T_t_h.col(3);
@ -215,6 +216,7 @@ class BundleAdjustmentBase {
if (d_res_d_p) { if (d_res_d_p) {
Eigen::Matrix<double, 4, 3> Jpp; Eigen::Matrix<double, 4, 3> Jpp;
Jpp.setZero();
Jpp.block<4, 2>(0, 0) = Jup; Jpp.block<4, 2>(0, 0) = Jup;
Jpp.col(2).setZero(); Jpp.col(2).setZero();
@ -284,28 +286,32 @@ class BundleAdjustmentBase {
continue; continue;
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_h_idx, rld.d_rel_d_h[i].transpose() * abs_h_idx, abs_h_idx,
rel_H.block<POSE_SIZE, POSE_SIZE>( rld.d_rel_d_h[i].transpose() *
POSE_SIZE * i, POSE_SIZE * j) * rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
rld.d_rel_d_h[j]); POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_h_idx, rld.d_rel_d_t[i].transpose() * abs_ti_idx, abs_h_idx,
rel_H.block<POSE_SIZE, POSE_SIZE>( rld.d_rel_d_t[i].transpose() *
POSE_SIZE * i, POSE_SIZE * j) * rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
rld.d_rel_d_h[j]); POSE_SIZE * j) *
rld.d_rel_d_h[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_h_idx, abs_tj_idx, rld.d_rel_d_h[i].transpose() * abs_h_idx, abs_tj_idx,
rel_H.block<POSE_SIZE, POSE_SIZE>( rld.d_rel_d_h[i].transpose() *
POSE_SIZE * i, POSE_SIZE * j) * rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
rld.d_rel_d_t[j]); POSE_SIZE * j) *
rld.d_rel_d_t[j]);
accum.template addH<POSE_SIZE, POSE_SIZE>( accum.template addH<POSE_SIZE, POSE_SIZE>(
abs_ti_idx, abs_tj_idx, rld.d_rel_d_t[i].transpose() * abs_ti_idx, abs_tj_idx,
rel_H.block<POSE_SIZE, POSE_SIZE>( rld.d_rel_d_t[i].transpose() *
POSE_SIZE * i, POSE_SIZE * j) * rel_H.block<POSE_SIZE, POSE_SIZE>(POSE_SIZE * i,
rld.d_rel_d_t[j]); POSE_SIZE * j) *
rld.d_rel_d_t[j]);
} }
} }
} }
@ -369,4 +375,4 @@ class BundleAdjustmentBase {
basalt::Calibration<double> calib; basalt::Calibration<double> calib;
}; };
} } // namespace basalt