From fa70707d8e9c8728e2d06eda0f2159ee5a12908b Mon Sep 17 00:00:00 2001 From: Xiaohan Fei Date: Thu, 5 Sep 2019 12:09:54 -0700 Subject: [PATCH] bugfix: read use_hessian option from config; also make double evaluation optional --- cfg/tumvi_cam0.json | 2 +- misc/run_all.sh | 12 +++--- scripts/run_and_eval_pyxivo.py | 6 ++- src/CMakeLists.txt | 2 +- src/estimator.cpp | 1 + src/feature.cpp | 73 ++++++++++++++++++++++++++++------ 6 files changed, 74 insertions(+), 22 deletions(-) diff --git a/cfg/tumvi_cam0.json b/cfg/tumvi_cam0.json index b82361c0..9656d348 100644 --- a/cfg/tumvi_cam0.json +++ b/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 diff --git a/misc/run_all.sh b/misc/run_all.sh index c246b339..1104c028 100755 --- a/misc/run_all.sh +++ b/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} diff --git a/scripts/run_and_eval_pyxivo.py b/scripts/run_and_eval_pyxivo.py index e630c048..4ca1f859 100644 --- a/scripts/run_and_eval_pyxivo.py +++ b/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))) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index a511895e..88229d07 100644 --- a/src/CMakeLists.txt +++ b/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} diff --git a/src/estimator.cpp b/src/estimator.cpp index 1e86f052..bfd46c1f 100644 --- a/src/estimator.cpp +++ b/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 = diff --git a/src/feature.cpp b/src/feature.cpp index 853d38f5..55edcbe2 100644 --- a/src/feature.cpp +++ b/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 Hr; // dxp_d[Wr, Tr] + Eigen::Matrix Hc; // dxp_d[Wbc, Tbc] + Eigen::Matrix Hx; // dxp_dx + + // tuples of (residual, Jacobian block) + using ResidualJacobian = std::tuple< + Eigen::Matrix, Eigen::Matrix >; + std::unordered_map 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 }