From bc62be4922e961139e3cb026e2003cc8f9b99442 Mon Sep 17 00:00:00 2001
From: Vladyslav Usenko <usenko.vladyslav@gmail.com>
Date: Fri, 19 Apr 2019 16:50:12 +0200
Subject: [PATCH] fixed unitialized jacobian

---
 include/basalt/vi_estimator/ba_base.h | 40 +++++++++++++++------------
 1 file changed, 23 insertions(+), 17 deletions(-)

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