ch9/proj

project/0.1/CMakeLists.txt

@@ -1,10 +1,9 @@
 cmake_minimum_required( VERSION 2.8 )
 project ( myslam )
 
-# set( CMAKE_CXX_COMPILER "clang++" )
 set( CMAKE_CXX_COMPILER "g++" )
-set( CMAKE_BUILD_TYPE "Debug" )
-set( CMAKE_CXX_FLAGS "-std=c++11 -march=native" )
+set( CMAKE_BUILD_TYPE "Release" )
+set( CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3" )
 
 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
 set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )

project/0.1/config/default.yaml

@@ -1,7 +1,7 @@
 %YAML:1.0
 # data
 # the tum dataset directory, change it to yours! 
-dataset_dir: /home/ciang/dataset/rgbd_dataset_freiburg1_desk
+dataset_dir: /home/xiang/dataset/rgbd_dataset_freiburg1_xyz
 
 # camera intrinsics
 # fr1
@@ -19,7 +19,12 @@ camera.cy: 249.7
 camera.depth_scale: 5000
 
 # VO Parameters
-number_of_features: 500
+number_of_features: 1000
 scale_factor: 1.2
 level_pyramid: 4
-match_ratio: 2.0
+match_ratio: 2.0
+max_num_lost: 10
+min_inliers: 10
+keyframe_rotation: 0.2
+keyframe_translation: 0.2
+map_point_erase_ratio: 0.5

project/0.1/include/myslam/camera.h

@@ -31,20 +31,19 @@ class Camera
 public:
  typedef std::shared_ptr<Camera> Ptr;
  float fx_, fy_, cx_, cy_, depth_scale_;
- SE3 T_c_w_;
 
  Camera();
- Camera ( float fx, float fy, float cx, float cy, float depth_scale=0, SE3 T_c_w=SE3() ) :
- fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale ), T_c_w_ ( T_c_w ) 
+ Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :
+ fx_ ( fx ), fy_ ( fy ), cx_ ( cx ), cy_ ( cy ), depth_scale_ ( depth_scale )
  {}
 
  // coordinate transform: world, camera, pixel
- Vector3d world2camera( const Vector3d& p_w );
- Vector3d camera2world( const Vector3d& p_c );
+ Vector3d world2camera( const Vector3d& p_w, const SE3& T_c_w );
+ Vector3d camera2world( const Vector3d& p_c, const SE3& T_c_w );
  Vector2d camera2pixel( const Vector3d& p_c );
  Vector3d pixel2camera( const Vector2d& p_p, double depth=1 ); 
- Vector3d pixel2world ( const Vector2d& p_p, double depth=1 );
- Vector2d world2pixel ( const Vector3d& p_w );
+ Vector3d pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth=1 );
+ Vector2d world2pixel ( const Vector3d& p_w, const SE3& T_c_w );
 
 };
 

project/0.1/include/myslam/frame.h

@@ -37,8 +37,8 @@ class Frame
  SE3 T_c_w_; // transform from frame to world
  Camera::Ptr camera_; // Pinhole RGBD Camera model 
  Mat color_, depth_; // color and depth image 
- std::vector<cv::KeyPoint> keypoints_; // key points in image
- std::vector<MapPoint*> map_points_; // associated map points
+ // std::vector<cv::KeyPoint> keypoints_; // key points in image
+ // std::vector<MapPoint*> map_points_; // associated map points
  bool is_key_frame_; // whether a key-frame
 
 public: // data members 

project/0.1/include/myslam/g2o_types.h

@@ -21,12 +21,16 @@
 #define MYSLAM_G2O_TYPES_H
 
 #include "myslam/common_include.h"
+#include "camera.h"
 
 #include <g2o/core/base_vertex.h>
 #include <g2o/core/base_unary_edge.h>
 #include <g2o/core/block_solver.h>
 #include <g2o/core/optimization_algorithm_levenberg.h>
 #include <g2o/types/sba/types_six_dof_expmap.h>
+#include <g2o/solvers/dense/linear_solver_dense.h>
+#include <g2o/core/robust_kernel.h>
+#include <g2o/core/robust_kernel_impl.h>
 
 namespace myslam
 {
@@ -36,19 +40,37 @@ class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::V
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  virtual void computeError();
  virtual void linearizeOplus();
- virtual bool read( istream& in ){}
- virtual bool write( ostream& out) {}
+ virtual bool read( std::istream& in ){}
+ virtual bool write( std::ostream& out) const {}
 
 };
 
+// only to optimize the pose, no point
 class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap >
 {
 public:
+ // Error: measure = R*point+t
  virtual void computeError();
  virtual void linearizeOplus();
 
- virtual bool read( istream& in ){}
- virtual bool write( ostream& out) {}
+ virtual bool read( std::istream& in ){}
+ virtual bool write( std::ostream& out) const {}
+
+ Vector3d point_;
+};
+
+class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >
+{
+public:
+ Camera::Ptr camera_;
+
+ virtual void computeError();
+ virtual void linearizeOplus();
+
+ virtual bool read( std::istream& in ){}
+ virtual bool write(std::ostream& os) const {};
+
+ Vector3d point_;
 };
 
 }

project/0.1/include/myslam/mappoint.h

@@ -34,7 +34,10 @@ class MapPoint
  Vector3d norm_; // Normal of viewing direction 
  Mat descriptor_; // Descriptor for matching 
 
- list<Frame*> observed_frames_; // frames that can observe this point 
+ // list<Frame*> observed_frames_; // frames that can observe this point 
+
+ int observed_times_; // being observed by feature matching algo.
+ int correct_times_; // being an inliner in pose estimation
 
  MapPoint();
  MapPoint( long id, Vector3d position, Vector3d norm );

project/0.1/include/myslam/visual_odometry.h

@@ -47,27 +47,39 @@ class VisualOdometry
 
  cv::Ptr<cv::ORB> orb_; // orb detector and computer 
 
- vector<long> match_map_, match_curr_; // feature matching in map and current
+ vector<unsigned long> match_map_, match_curr_; // feature matching in map and current
+ SE3 pose_curr_estimated_; // the estimated pose of current frame 
+ int num_inliers_; // number of inlier features in icp
+ int num_lost_; // number of lost times
 
  // parameters 
  int num_of_features_; // number of features
  double scale_factor_; // scale in image pyramid
  int level_pyramid_; // number of pyramid levels
- float match_ratio_; // ratio for selecting good matches
-
+ float match_ratio_; // ratio for selecting good matches
+ int max_num_lost_; // max number of continuous lost times
+ int min_inliers_; // minimum inliers
+ double key_frame_min_rot; // minimal rotation of two key-frames
+ double key_frame_min_trans; // minimal translation of two key-frames
+ double map_point_erase_ratio_; // remove map point ratio
 
 public: // functions 
  VisualOdometry();
  ~VisualOdometry();
 
  bool addFrame( Frame::Ptr frame ); // add a new frame 
 
-protected: // inner operation 
+protected: 
+ // inner operation 
  void extractKeyPoints();
  void computeDescriptors(); 
  void addAllKeypointsIntoMap(); 
  void featureMatching();
- void poseEstimation3D3D(); 
+ void poseEstimationRGBD(); 
+ void addKeyFrame();
+
+ bool checkEstimatedPose(); 
+ bool checkKeyFrame();
 
 };
 }

project/0.1/src/camera.cpp

@@ -30,17 +30,16 @@ Camera::Camera()
  cx_ = Config::get<float>("camera.cx");
  cy_ = Config::get<float>("camera.cy");
  depth_scale_ = Config::get<float>("camera.depth_scale");
- T_c_w_ = SE3();
 }
 
-Vector3d Camera::world2camera ( const Vector3d& p_w )
+Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w )
 {
- return T_c_w_*p_w;
+ return T_c_w*p_w;
 }
 
-Vector3d Camera::camera2world ( const Vector3d& p_c )
+Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
 {
- return T_c_w_.inverse() *p_c;
+ return T_c_w.inverse() *p_c;
 }
 
 Vector2d Camera::camera2pixel ( const Vector3d& p_c )
@@ -60,14 +59,14 @@ Vector3d Camera::pixel2camera ( const Vector2d& p_p, double depth )
  );
 }
 
-Vector2d Camera::world2pixel ( const Vector3d& p_w )
+Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w )
 {
- return camera2pixel ( T_c_w_*p_w );
+ return camera2pixel ( world2camera(p_w, T_c_w) );
 }
 
-Vector3d Camera::pixel2world ( const Vector2d& p_p, double depth )
+Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth )
 {
- return camera2world ( pixel2camera ( p_p, depth ) );
+ return camera2world ( pixel2camera ( p_p, depth ), T_c_w );
 }
 
 

project/0.1/src/frame.cpp

@@ -73,7 +73,6 @@ double Frame::findDepth ( const cv::KeyPoint& kp )
 void Frame::setPose ( const SE3& T_c_w )
 {
  T_c_w_ = T_c_w;
- camera_->T_c_w_ = T_c_w_;
 }
 
 
@@ -84,10 +83,10 @@ Vector3d Frame::getCamCenter() const
 
 bool Frame::isInFrame ( const Vector3d& pt_world )
 {
- Vector3d p_cam = camera_->world2camera(pt_world);
+ Vector3d p_cam = camera_->world2camera( pt_world, T_c_w_ );
  // cout<<"P_cam = "<<p_cam.transpose()<<endl;
  if ( p_cam(2,0)<0 ) return false;
- Vector2d pixel = camera_->world2pixel( pt_world );
+ Vector2d pixel = camera_->world2pixel( pt_world, T_c_w_ );
  // cout<<"P_pixel = "<<pixel.transpose()<<endl<<endl;
  return pixel(0,0)>0 && pixel(1,0)>0 
  && pixel(0,0)<color_.cols 

project/0.1/src/g2o_types.cpp

@@ -1 +1,117 @@
-#include "myslam/g2o_types.h"
+#include "myslam/g2o_types.h"
+
+namespace myslam
+{
+void EdgeProjectXYZRGBD::computeError()
+{
+ const g2o::VertexSBAPointXYZ* point = static_cast<const g2o::VertexSBAPointXYZ*> ( _vertices[0] );
+ const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[1] );
+ _error = _measurement - pose->estimate().map ( point->estimate() );
+}
+
+void EdgeProjectXYZRGBD::linearizeOplus()
+{
+ g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap *> ( _vertices[1] );
+ g2o::SE3Quat T ( pose->estimate() );
+ g2o::VertexSBAPointXYZ* point = static_cast<g2o::VertexSBAPointXYZ*> ( _vertices[0] );
+ Eigen::Vector3d xyz = point->estimate();
+ Eigen::Vector3d xyz_trans = T.map ( xyz );
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double z = xyz_trans[2];
+
+ _jacobianOplusXi = - T.rotation().toRotationMatrix();
+
+ _jacobianOplusXj ( 0,0 ) = 0;
+ _jacobianOplusXj ( 0,1 ) = -z;
+ _jacobianOplusXj ( 0,2 ) = y;
+ _jacobianOplusXj ( 0,3 ) = -1;
+ _jacobianOplusXj ( 0,4 ) = 0;
+ _jacobianOplusXj ( 0,5 ) = 0;
+
+ _jacobianOplusXj ( 1,0 ) = z;
+ _jacobianOplusXj ( 1,1 ) = 0;
+ _jacobianOplusXj ( 1,2 ) = -x;
+ _jacobianOplusXj ( 1,3 ) = 0;
+ _jacobianOplusXj ( 1,4 ) = -1;
+ _jacobianOplusXj ( 1,5 ) = 0;
+
+ _jacobianOplusXj ( 2,0 ) = -y;
+ _jacobianOplusXj ( 2,1 ) = x;
+ _jacobianOplusXj ( 2,2 ) = 0;
+ _jacobianOplusXj ( 2,3 ) = 0;
+ _jacobianOplusXj ( 2,4 ) = 0;
+ _jacobianOplusXj ( 2,5 ) = -1;
+}
+
+void EdgeProjectXYZRGBDPoseOnly::computeError()
+{
+ const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
+ _error = _measurement - pose->estimate().map ( point_ );
+}
+
+void EdgeProjectXYZRGBDPoseOnly::linearizeOplus()
+{
+ g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
+ g2o::SE3Quat T ( pose->estimate() );
+ Vector3d xyz_trans = T.map ( point_ );
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double z = xyz_trans[2];
+
+ _jacobianOplusXi ( 0,0 ) = 0;
+ _jacobianOplusXi ( 0,1 ) = -z;
+ _jacobianOplusXi ( 0,2 ) = y;
+ _jacobianOplusXi ( 0,3 ) = -1;
+ _jacobianOplusXi ( 0,4 ) = 0;
+ _jacobianOplusXi ( 0,5 ) = 0;
+
+ _jacobianOplusXi ( 1,0 ) = z;
+ _jacobianOplusXi ( 1,1 ) = 0;
+ _jacobianOplusXi ( 1,2 ) = -x;
+ _jacobianOplusXi ( 1,3 ) = 0;
+ _jacobianOplusXi ( 1,4 ) = -1;
+ _jacobianOplusXi ( 1,5 ) = 0;
+
+ _jacobianOplusXi ( 2,0 ) = -y;
+ _jacobianOplusXi ( 2,1 ) = x;
+ _jacobianOplusXi ( 2,2 ) = 0;
+ _jacobianOplusXi ( 2,3 ) = 0;
+ _jacobianOplusXi ( 2,4 ) = 0;
+ _jacobianOplusXi ( 2,5 ) = -1;
+}
+
+void EdgeProjectXYZ2UVPoseOnly::computeError()
+{
+ const g2o::VertexSE3Expmap* pose = static_cast<const g2o::VertexSE3Expmap*> ( _vertices[0] );
+ _error = _measurement - camera_->camera2pixel ( 
+ pose->estimate().map(point_) );
+}
+
+void EdgeProjectXYZ2UVPoseOnly::linearizeOplus()
+{
+ g2o::VertexSE3Expmap* pose = static_cast<g2o::VertexSE3Expmap*> ( _vertices[0] );
+ g2o::SE3Quat T ( pose->estimate() );
+ Vector3d xyz_trans = T.map ( point_ );
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double z = xyz_trans[2];
+ double z_2 = z*z;
+
+ _jacobianOplusXi ( 0,0 ) = x*y/z_2 *camera_->fx_;
+ _jacobianOplusXi ( 0,1 ) = - ( 1+ ( x*x/z_2 ) ) *camera_->fx_;
+ _jacobianOplusXi ( 0,2 ) = y/z * camera_->fx_;
+ _jacobianOplusXi ( 0,3 ) = -1./z * camera_->fx_;
+ _jacobianOplusXi ( 0,4 ) = 0;
+ _jacobianOplusXi ( 0,5 ) = x/z_2 * camera_->fx_;
+
+ _jacobianOplusXi ( 1,0 ) = ( 1+y*y/z_2 ) *camera_->fy_;
+ _jacobianOplusXi ( 1,1 ) = -x*y/z_2 *camera_->fy_;
+ _jacobianOplusXi ( 1,2 ) = -x/z *camera_->fy_;
+ _jacobianOplusXi ( 1,3 ) = 0;
+ _jacobianOplusXi ( 1,4 ) = -1./z *camera_->fy_;
+ _jacobianOplusXi ( 1,5 ) = y/z_2 *camera_->fy_;
+}
+
+
+}

project/0.1/src/mappoint.cpp

@@ -24,13 +24,13 @@ namespace myslam
 {
 
 MapPoint::MapPoint()
-: id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), good_(true)
+: id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), good_(true), observed_times_(0), correct_times_(0)
 {
 
 }
 
 MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm )
-: id_(id), pos_(position), norm_(norm), good_(true)
+: id_(id), pos_(position), norm_(norm), good_(true), observed_times_(0), correct_times_(0)
 {
 
 }