bugfix: read use_hessian option from config; also make double evaluat…

…ion optional

cfg/tumvi_cam0.json

@@ -1,7 +1,7 @@
 {
   // verbose
   "simulation": false,
-  "print_timing": false,
+  "print_timing": true,
   "use_canvas": true,
   "use_debug_view": false,  // draw rejected & dropped features on canvas
   "async_run": false, // turn this off in benchmarking

misc/run_all.sh

@@ -9,9 +9,9 @@
 TUMVIROOT="/home/feixh/Data/tumvi/exported/euroc/512_16"
 
 mkdir $OUTPUT
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room1 ${1} &
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room2 ${1} &
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room3 ${1} &
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room4 ${1} &
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room5 ${1} &
-python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room6 ${1} &
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room1 ${1}
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room2 ${1}
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room3 ${1}
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room4 ${1}
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room5 ${1}
+python scripts/run_and_eval_pyxivo.py -out_dir $OUTPUT -root $TUMVIROOT -seq room6 ${1}

scripts/run_and_eval_pyxivo.py

@@ -5,7 +5,7 @@
 
 TP_ROOT = '/home/feixh/Data/tumvi/exported/euroc/512_16'
 KIF_ROOT = '/local2/Data/tumvi/exported/euroc/512_16'
-double_fusion = True
+double_fusion = False
 
 parser = argparse.ArgumentParser()
 parser.add_argument(
@@ -33,7 +33,9 @@
         os.makedirs(args.out_dir)
 
 
-    for cam_id in [0, 1]:
+    cam_ids = [0, 1] if double_fusion else [0]
+
+    for cam_id in cam_ids:
         cfg = 'cfg/tumvi_cam{}.json'.format(cam_id)
 
         copyfile(cfg, os.path.join(args.out_dir, os.path.basename(cfg)))

src/CMakeLists.txt

@@ -15,7 +15,7 @@ add_definitions(-DUSE_ONLINE_TEMPORAL_CALIB)
 add_definitions(-DUSE_ONLINE_CAMERA_CALIB)
 # if set, approximate the initial correlation between feature state and
 # group state with gradient information from depth refinement optimization
-# add_definitions(-DAPPROXIMATE_INIT_COVARIANCE)
+add_definitions(-DAPPROXIMATE_INIT_COVARIANCE)
 
 include_directories(
   ${PROJECT_SOURCE_DIR}

src/estimator.cpp

@@ -94,6 +94,7 @@ Estimator::Estimator(const Json::Value &cfg)
   use_depth_opt_ = cfg_.get("use_depth_opt", false).asBool();
   refinement_options_.two_view =
       cfg_["depth_opt"].get("two_view", false).asBool();
+  refinement_options_.use_hessian = cfg_["depth_opt"].get("use_hessian", false).asBool();
   refinement_options_.max_iters = cfg_["depth_opt"].get("max_iters", 5).asInt();
   refinement_options_.eps = cfg_["depth_opt"].get("eps", 1e-4).asDouble();
   refinement_options_.damping =

src/feature.cpp

@@ -289,23 +289,60 @@ bool Feature::RefineDepth(const SE3 &gbc,
     // auto Hinv = H.inverse();
     // Pseudo-Inverse, since H is rank 2 (3x2 matrix times 2x2 matrix times 2x3 matrix)
     Mat3 H_pinv{H.completeOrthogonalDecomposition().pseudoInverse()};
-    if (anynan(H_pinv)) return false;
+    if (anynan(H_pinv)) {
+      std::cout << "hessian as information matrix: nan in H.inv!!!" << std::endl;
+      return false;
+    }
     P_ = H_pinv;
 
 #ifdef APPROXIMATE_INIT_COVARIANCE
+    // std::cout << "approximating covariance using inverse of Hessian" << std::endl;
     // compute correlation blocks
-    Mat3 dXs_dx;
-    Vec3 Xs = this->Xs(gbc, &dXs_dx); // ref_->gsb() * gbc * this->Xc();
+    Mat3 dXc_dx;
+    Vec3 Xc = this->Xc(&dXc_dx);
+
+    SO3 Rr{ref_->gsb().R()};
+    Vec3 Tr{ref_->gsb().T()};
 
     SO3 Rbc{gbc.R()};
     Vec3 Tbc{gbc.T()};
 
+    // total rotation & translation w.r.t. body pose and alignment
+    Mat3 dWtot_dWr, dWtot_dWbc;
+    Mat3 dTtot_dWr, dTtot_dTr, dTtot_dTbc;
+    // compose and compute the Jacobians
+    auto [Rtot, Ttot] = Compose(Rr, Tr, Rbc, Tbc,
+        &dWtot_dWr, &dWtot_dWbc,
+        &dTtot_dWr, &dTtot_dTr, &dTtot_dTbc);
+    // 3D point in spatial frame (Xs) w.r.t. total rotation & translation, and 3D point in camera frame (Xc)
+    Mat3 dXs_dWtot, dXs_dTtot, dXs_dXc;
+    auto Xs = Transform(Rtot, Ttot, Xc, &dXs_dWtot, &dXs_dTtot, &dXs_dXc);
+
+    Mat3 dXs_dWr{dXs_dWtot * dWtot_dWr + dXs_dTtot * dTtot_dWr};
+    Mat3 dXs_dTr{dXs_dTtot * dTtot_dTr};
+    Mat3 dXs_dWbc{dXs_dWtot * dWtot_dWbc}; //  + dXs_dTtot * dTtot_dWbc};
+    Mat3 dXs_dTbc{dXs_dTtot * dTtot_dTbc};
+    Mat3 dXs_dx{dXs_dXc * dXc_dx};
+
+    Eigen::Matrix<number_t, 2, kGroupSize> Hr;  // dxp_d[Wr, Tr]
+    Eigen::Matrix<number_t, 2, kGroupSize> Hc;  // dxp_d[Wbc, Tbc]
+    Eigen::Matrix<number_t, 2, kFeatureSize> Hx;  // dxp_dx
+
+    // tuples of (residual, Jacobian block)
+    using ResidualJacobian = std::tuple<
+      Eigen::Matrix<number_t, 2, 1>, Eigen::Matrix<number_t, 2, kGroupSize> >;
+    std::unordered_map<int, ResidualJacobian> resH; 
+
+    cov_.clear();
     for (const auto &obs : views) {
+      Group* g{obs.g};
+      if (g->id() == ref_->id())
+        continue;
       // Feeling too lasy to derive the Jacobians on paper,
       // so I'm gonna use chain rule to compute them.
-      SO3 Rsb{obs.g->gsb().R()};
-      Vec3 Tsb{obs.g->gsb().T()};
-
+      SO3 Rsb{g->gsb().R()};
+      Vec3 Tsb{g->gsb().T()};
+      // compute the total transformation from spatial frame to new camera frame
       Mat3 dWi_dWsb, dWi_dWbc; 
       Mat3 dTi_dWsb, dTi_dTsb, dTi_dTbc;
       // [Ri, Ti] = spatial to camera transformation
@@ -314,30 +351,42 @@ bool Feature::RefineDepth(const SE3 &gbc,
           &dWi_dWsb, &dWi_dWbc, 
           &dTi_dWsb, &dTi_dTsb, &dTi_dTbc);
 
+      // transfrom from spatial frame to new camera frame
       Mat3 dXcn_dWi, dXcn_dTi, dXcn_dXs;
       Vec3 Xcn = Transform(Ri, Ti, Xs,
           &dXcn_dWi, &dXcn_dTi, &dXcn_dXs);
 
+      // intermediate Jacobians
       Mat3 dXcn_dx = dXcn_dXs * dXs_dx;
       Mat3 dXcn_dWsb = dXcn_dWi * dWi_dWsb + dXcn_dTi * dTi_dWsb;
       Mat3 dXcn_dTsb = dXcn_dTi * dTi_dTsb;
       Mat3 dXcn_dWbc = dXcn_dWi * dWi_dWbc; //  + dXcn_dTi * dTi_dWbc;
       Mat3 dXcn_dTbc = dXcn_dTi * dTi_dTbc;
 
+      // perspective projection
       Mat23 dxcn_dXcn;
       Vec2 xcn = project(Xcn, &dxcn_dXcn);
 
+      // apply distortion model
       Mat2 dxp_dxcn;
       Vec2 xp = Camera::instance()->Project(xcn, &dxp_dxcn);
 
-      Mat23 dxp_dx = dxp_dxcn * dxcn_dXcn * dXcn_dx;
-      // std::cout << dxp_dx << std::endl;
+      // fill-in Jacobian w.r.t. the group
+      Mat23 dxp_dXcn{dxp_dxcn * dxcn_dXcn};
+      std::get<0>(resH[g->id()]) << obs.xp - xp;
+      std::get<1>(resH[g->id()]) << dxp_dXcn * dXcn_dWsb, dxp_dXcn * dXcn_dTsb;
+      Mat23 dxp_dXs{dxp_dXcn * dXcn_dXs};
+
+      // update Jacobian w.r.t. the pose of the reference group
+      Hr.block<2, 3>(0, 0) += dxp_dXs * dXs_dWr;
+      Hr.block<2, 3>(0, 3) += dxp_dXs * dXs_dTr;
 
-      // H += (dxp_dx.transpose() * invC * dxp_dx) / (views.size() - 1);
-      // Vec2 res = obs.xp - xp;
-      // b += dxp_dx.transpose() * invC * res / (views.size() - 1);
+      // update Jacobian w.r.t. the pose of the camera-body alignment
+      Hc.block<2, 3>(0, 0) += dxp_dXs * dXs_dWbc + dxp_dXcn * dXcn_dWbc;
+      Hc.block<2, 3>(0, 3) += dxp_dXs * dXs_dTbc + dxp_dXcn * dXcn_dTbc;
 
-      // res_norm += res.norm() / (views.size() - 1);
+      // update Jacobian w.r.t. local parametrization of the feature
+      Hx += dxp_dXcn * dXcn_dx;
     }
 #endif
   }