success vo

backend/cost_function/impl/reprojection_se3_factor_impl.hpp

@@ -2,9 +2,8 @@
 
 namespace backend {
 namespace DirectLeftSE3 {
-bool ReprojectionErrorSE3::Evaluate(double const* const* parameters,
-                                    double* residuals,
-                                    double** jacobians) const {
+inline bool ReprojectionErrorSE3::Evaluate(double const* const* parameters, double* residuals,
+                                           double** jacobians) const {
   // [tx, ty, tz, qw, qx, qy, qz]
   Eigen::Map<const Eigen::Vector3d> twc(parameters[0]);
   Eigen::Map<const Eigen::Quaterniond> qwc(parameters[0] + 3);
@@ -43,8 +42,7 @@ bool ReprojectionErrorSE3::Evaluate(double const* const* parameters,
     J_lRcw.noalias() = J_lcam * Tcw.rotationMatrix();
 
     if (jacobians[0] != NULL) {
-      Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J_se3pose(
-          jacobians[0]);
+      Eigen::Map<Eigen::Matrix<double, 2, 7, Eigen::RowMajor> > J_se3pose(jacobians[0]);
       J_se3pose.setZero();
 
       J_se3pose.block<2, 3>(0, 0).noalias() = -1. * J_lRcw;

geometry/CMakeLists.txt

@@ -0,0 +1,4 @@
+add_library(geometry motion_ba/motion_ba.cpp)
+target_link_libraries(geometry tiny_ceres)
+add_executable(triangulate_test example/triangulate_test.cpp)
+target_link_libraries(triangulate_test ${OpenCV_LIBS})

geometry/example/triangulate_test.cpp

@@ -0,0 +1,8 @@
+#include <iostream>
+#include <opencv2/opencv.hpp>
+#include "geometry/triangulate/triangulate_cv.hpp"
+
+int main(int argc, char **argv) {
+  std::cout << "hello world";
+  return 0;
+}

geometry/motion_ba/motion_ba.cpp

@@ -0,0 +1,198 @@
+#include "geometry/motion_ba/motion_ba.hpp"
+
+namespace geometry {
+
+bool tceresMotionOnlyBA(
+    const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > &vunkps,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
+    const bool buse_robust, const bool bapply_l2_after_robust, const Eigen::Matrix3d &K,
+    std::vector<int> &voutliersidx) {
+  float fx = K(0, 0);
+  float fy = K(1, 1);
+  float cx = K(0, 2);
+  float cy = K(1, 2);
+  return tceresMotionOnlyBA(vunkps, vwpts, vscales, Twc, nmaxiter, chi2th, buse_robust,
+                            bapply_l2_after_robust, fx, fy, cx, cy, voutliersidx);
+}
+
+bool tceresMotionOnlyBA(
+    const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > &vunkps,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
+    const bool buse_robust, const bool bapply_l2_after_robust, const float fx, const float fy,
+    const float cx, const float cy, std::vector<int> &voutliersidx) {
+  assert(vunkps.size() == vwpts.size());
+
+  tceres::Problem problem;
+
+  double chi2thrtsq = std::sqrt(chi2th);
+
+  tceres::LossFunctionWrapper *loss_function;
+  loss_function =
+      new tceres::LossFunctionWrapper(new tceres::HuberLoss(chi2thrtsq), tceres::TAKE_OWNERSHIP);
+
+  if (!buse_robust) {
+    loss_function->Reset(NULL, tceres::TAKE_OWNERSHIP);
+  }
+
+  size_t nbkps = vunkps.size();
+
+  tceres::LocalParameterization *local_parameterization = new backend::SE3LeftParameterization();
+
+  backend::PoseParametersBlock posepar = backend::PoseParametersBlock(0, Twc);
+
+  problem.AddParameterBlock(posepar.values(), 7, local_parameterization);
+
+  std::vector<backend::DirectLeftSE3::ReprojectionErrorSE3 *> verrors_;
+  std::vector<tceres::ResidualBlockId> vrids_;
+
+  for (size_t i = 0; i < nbkps; i++) {
+    auto *f = new backend::DirectLeftSE3::ReprojectionErrorSE3(
+        vunkps[i].x(), vunkps[i].y(), fx, fy, cx, cy, vwpts.at(i), std::pow(2., vscales[i]));
+    tceres::ResidualBlockId rid = problem.AddResidualBlock(f, loss_function, posepar.values());
+
+    verrors_.push_back(f);
+    vrids_.push_back(rid);
+  }
+
+  tceres::Solver::Options options;
+  options.linear_solver_type = tceres::DENSE_SCHUR;
+  options.trust_region_strategy_type = tceres::LEVENBERG_MARQUARDT;
+
+  options.num_threads = 1;
+  options.max_num_iterations = nmaxiter;
+  options.max_solver_time_in_seconds = 0.005;
+  options.function_tolerance = 1.e-3;
+
+  options.minimizer_progress_to_stdout = false;
+
+  tceres::Solver::Summary summary;
+  tceres::Solve(options, &problem, &summary);
+
+  size_t bad = 0;
+
+  for (size_t i = 0; i < nbkps; i++) {
+    auto err = verrors_.at(i);
+    if (err->chi2err_ > chi2th || !err->isdepthpositive_) {
+      if (bapply_l2_after_robust) {
+        auto rid = vrids_.at(i);
+        problem.RemoveResidualBlock(rid);
+      }
+      voutliersidx.push_back(i);
+      bad++;
+    }
+  }
+
+  if (bad == nbkps) {
+    return false;
+  }
+
+  if (bapply_l2_after_robust && !voutliersidx.empty()) {
+    loss_function->Reset(nullptr, tceres::TAKE_OWNERSHIP);
+    tceres::Solve(options, &problem, &summary);
+  }
+
+  Twc = posepar.getPose();
+
+  return summary.IsSolutionUsable();
+}
+
+bool opencvP3PRansac(
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &bvs,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const int nmaxiter, const float errth, const float fx, const float fy, const bool boptimize,
+    Sophus::SE3d &Twc, std::vector<int> &voutliersidx) {
+  assert(bvs.size() == vwpts.size());
+
+  size_t nb3dpts = bvs.size();
+
+  if (nb3dpts < 4) {
+    return false;
+  }
+
+  voutliersidx.reserve(nb3dpts);
+
+  std::vector<cv::Point2f> cvbvs;
+  cvbvs.reserve(nb3dpts);
+
+  std::vector<cv::Point3f> cvwpts;
+  cvwpts.reserve(nb3dpts);
+
+  for (size_t i = 0; i < nb3dpts; i++) {
+    cvbvs.push_back(cv::Point2f(bvs.at(i).x() / bvs.at(i).z(), bvs.at(i).y() / bvs.at(i).z()));
+
+    cvwpts.push_back(cv::Point3f(vwpts.at(i).x(), vwpts.at(i).y(), vwpts.at(i).z()));
+  }
+
+  // Using homoegeneous pts here so no dist or calib.
+  cv::Mat D;
+  cv::Mat K = cv::Mat::eye(3, 3, CV_32F);
+
+  cv::Mat tvec, rvec;
+  cv::Mat inliers;
+
+  bool use_extrinsic_guess = false;
+  float confidence = 0.99;
+
+  float focal = (fx + fy) / 2.;
+
+  cv::solvePnPRansac(cvwpts, cvbvs, K, D, rvec, tvec, use_extrinsic_guess, nmaxiter, errth / focal,
+                     confidence, inliers, cv::SOLVEPNP_ITERATIVE);
+
+  if (inliers.rows == 0) {
+    return false;
+  }
+
+  int k = 0;
+  for (size_t i = 0; i < nb3dpts; i++) {
+    if (inliers.at<int>(k) == (int)i) {
+      k++;
+      if (k == inliers.rows) {
+        k = 0;
+      }
+    } else {
+      voutliersidx.push_back(i);
+    }
+  }
+
+  if (voutliersidx.size() >= nb3dpts - 5) {
+    return false;
+  }
+
+  if (boptimize) {
+    use_extrinsic_guess = true;
+
+    // Filter outliers
+    std::vector<cv::Point2f> in_cvbvs;
+    in_cvbvs.reserve(inliers.rows);
+
+    std::vector<cv::Point3f> in_cvwpts;
+    in_cvwpts.reserve(inliers.rows);
+
+    for (int i = 0; i < inliers.rows; i++) {
+      in_cvbvs.push_back(cvbvs.at(inliers.at<int>(i)));
+      in_cvwpts.push_back(cvwpts.at(inliers.at<int>(i)));
+    }
+
+    cv::solvePnP(in_cvwpts, in_cvbvs, K, D, rvec, tvec, use_extrinsic_guess,
+                 cv::SOLVEPNP_ITERATIVE);
+  }
+
+  // Store the resulting pose
+  cv::Mat cvR;
+  cv::Rodrigues(rvec, cvR);
+
+  Eigen::Vector3d tcw;
+  Eigen::Matrix3d Rcw;
+
+  cv::cv2eigen(cvR, Rcw);
+  cv::cv2eigen(tvec, tcw);
+
+  Twc.translation() = -1. * Rcw.transpose() * tcw;
+  Twc.setRotationMatrix(Rcw.transpose());
+
+  return true;
+}
+
+}  // namespace geometry

geometry/motion_ba/motion_ba.hpp

@@ -0,0 +1,37 @@
+#ifndef VIO_HELLO_WORLD_MOTION_BA_HPP
+#define VIO_HELLO_WORLD_MOTION_BA_HPP
+
+#include "backend/cost_function/reprojection_se3_factor.hpp"
+#include "backend/parameter_block/point_parameter_block.hpp"
+#include "backend/parameter_block/se3_parameter_block.hpp"
+#include "backend/parameterization/se3left_parametrization.hpp"
+#include "opencv2/core/eigen.hpp"
+#include "opencv2/opencv.hpp"
+#include "tceres/loss_function.h"
+#include "tceres/problem.h"
+#include "tceres/solver.h"
+
+namespace geometry {
+
+bool tceresMotionOnlyBA(
+    const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > &vunkps,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
+    const bool buse_robust, const bool bapply_l2_after_robust, const float fx, const float fy,
+    const float cx, const float cy, std::vector<int> &voutliersidx);
+
+bool tceresMotionOnlyBA(
+    const std::vector<Eigen::Vector2d, Eigen::aligned_allocator<Eigen::Vector2d> > &vunkps,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const std::vector<int> &vscales, Sophus::SE3d &Twc, const int nmaxiter, const float chi2th,
+    const bool buse_robust, const bool bapply_l2_after_robust, const Eigen::Matrix3d &K,
+    std::vector<int> &voutliersidx);
+
+bool opencvP3PRansac(
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &bvs,
+    const std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &vwpts,
+    const int nmaxiter, const float errth, const float fx, const float fy, const bool boptimize,
+    Sophus::SE3d &Twc, std::vector<int> &voutliersidx);
+}  // namespace geometry
+
+#endif  // VIO_HELLO_WORLD_MOTION_BA_HPP

geometry/triangulate/triangulate_cv.hpp

@@ -0,0 +1,39 @@
+#ifndef VIO_HELLO_WORLD_TRIANGULATE_CV_HPP
+#define VIO_HELLO_WORLD_TRIANGULATE_CV_HPP
+#include <Eigen/Core>
+#include <opencv2/opencv.hpp>
+
+#include "sophus/se3.hpp"
+
+namespace geometry {
+inline Eigen::Vector3d OpencvTriangulate(const Sophus::SE3d &Tlr, const Eigen::Vector3d &bvl,
+                                         const Eigen::Vector3d &bvr) {
+  std::vector<cv::Point2f> lpt, rpt;
+  lpt.push_back(cv::Point2f(bvl.x() / bvl.z(), bvl.y() / bvl.z()));
+  rpt.push_back(cv::Point2f(bvr.x() / bvr.z(), bvr.y() / bvr.z()));
+
+  cv::Matx34f P0 = cv::Matx34f(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0);
+
+  Sophus::SE3d Tcw = Tlr.inverse();
+  Eigen::Matrix3d R = Tcw.rotationMatrix();
+  Eigen::Vector3d t = Tcw.translation();
+
+  cv::Matx34f P1 = cv::Matx34f(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1),
+                               R(2, 0), R(2, 1), R(2, 2), t(2));
+
+  cv::Mat campt;
+  cv::triangulatePoints(P0, P1, lpt, rpt, campt);
+
+  if (campt.col(0).at<float>(3) != 1.) {
+    campt.col(0) /= campt.col(0).at<float>(3);
+  }
+
+  Eigen::Vector3d pt(campt.col(0).at<float>(0), campt.col(0).at<float>(1),
+                     campt.col(0).at<float>(2));
+
+  return pt;
+}
+
+}  // namespace geometry
+
+#endif  // VIO_HELLO_WORLD_TRIANGULATE_CV_HPP

geometry/triangulate/triangulate_impl.hpp

@@ -0,0 +1,4 @@
+#ifndef VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP
+#define VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP
+
+#endif  // VIO_HELLO_WORLD_TRIANGULATE_IMPL_HPP

vio_hw/CMakeLists.txt

@@ -43,6 +43,7 @@ add_library(vio_hw ${SOURCE_FILES})
 
 target_link_libraries(vio_hw
         feature_extraction
+        geometry
         tiny_ceres
         feature_tracking
         ${OpenCV_LIBS}

vio_hw/internal/frame.hpp

@@ -33,6 +33,18 @@ class Frame
   void UpdateKeypoint(const int lmid, const cv::Point2f &pt);
   void RemoveKeypointById(const int lmid);
   Keypoint GetKeypointById(const int lmid) const;
+  void ComputeStereoKeypoint(const cv::Point2f &pt, Keypoint &kp);
+  std::vector<Keypoint> getKeypointsStereo() const;
+  void RemoveStereoKeypointById(const int lmid);
+  std::vector<Keypoint> GetKeypoints2d() const;
+  void UpdateKeypointStereo(const int lmid, const cv::Point2f &pt);
+  cv::Point2f ProjCamToRightImage(const Eigen::Vector3d &pt) const;
+  Eigen::Vector3d ProjCamToWorld(const Eigen::Vector3d &pt) const;
+  Sophus::SE3d GetTwc() const;
+  Sophus::SE3d GetTcw() const;
+  void SetTwc(const Sophus::SE3d &Twc);
+  cv::Point2f ProjCamToImage(const Eigen::Vector3d &pt) const;
+  void TurnKeypoint3d(const int lmid);
   // For using frame in ordered containers
   bool operator<(const Frame &f) const { return id_ < f.id_; }
 
@@ -53,7 +65,7 @@ class Frame
 
   // Pose (T cam -> world), (T world -> cam)
   Sophus::SE3d Twc_, Tcw_;
-
+  Sophus::SE3d Twb_, Tbw_;
   /* TODO
   Set a vector of calib ptrs to handle any multicam system.
   Each calib ptr should contain an extrinsic parametrization with a common

vio_hw/internal/keyframe.hpp

@@ -18,8 +18,10 @@ struct Keyframe
   Keyframe(int kfid, const cv::Mat &imleftraw)
       : kfid_(kfid), imleftraw_(imleftraw.clone()), is_stereo_(false) {}
 
-  Keyframe(int kfid, const cv::Mat &imleftraw,
-           const std::vector<cv::Mat> &vpyrleft,
+  Keyframe(int kfid, const cv::Mat &imleftraw, const cv::Mat &imright_raw)
+      : kfid_(kfid), imleftraw_(imleftraw.clone()), imrightraw_(imright_raw.clone()), is_stereo_(true) {}
+
+  Keyframe(int kfid, const cv::Mat &imleftraw, const std::vector<cv::Mat> &vpyrleft,
            const std::vector<cv::Mat> &vpyrright)
       : kfid_(kfid),
         imleftraw_(imleftraw.clone()),
@@ -37,8 +39,7 @@ struct Keyframe
   void displayInfo() {
     std::cout << "\n\n Keyframe struct object !  Info : id #" << kfid_
               << " - is stereo : " << is_stereo_;
-    std::cout << " - imleft size : " << imleft_.size
-              << " - imright size : " << imright_.size;
+    std::cout << " - imleft size : " << imleft_.size << " - imright size : " << imright_.size;
     std::cout << " - pyr left size : " << vpyr_imleft_.size()
               << " - pyr right size : " << vpyr_imright_.size() << "\n\n";
   }