ch9/project

project/0.1/CMakeLists.txt

@@ -27,7 +27,7 @@ set( THIRD_PARTY_LIBS
  ${Sophus_LIBRARIES}
  g2o_core g2o_stuff g2o_types_sba
 )
-############### dependencies ######################
+############### source and test ######################
 include_directories( ${PROJECT_SOURCE_DIR}/include )
 add_subdirectory( src )
 add_subdirectory( test )

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/xiang/dataset/rgbd_dataset_freiburg1_xyz
+dataset_dir: /home/ciang/dataset/rgbd_dataset_freiburg1_xyz
 
 # camera intrinsics
 # fr1
@@ -10,21 +10,4 @@ camera.fy: 516.5
 camera.cx: 325.1
 camera.cy: 249.7
 
-# fr2
-#camera.fx: 520.9
-#camera.fy: 521.0
-#camera.cx: 325.1
-#camera.cy: 249.7
-
-camera.depth_scale: 5000
-
-# VO Parameters
-number_of_features: 1000
-scale_factor: 1.2
-level_pyramid: 4
-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
+camera.depth_scale: 5000

project/0.1/include/myslam/camera.h

@@ -30,7 +30,7 @@ class Camera
 {
 public:
  typedef std::shared_ptr<Camera> Ptr;
- float fx_, fy_, cx_, cy_, depth_scale_;
+ float fx_, fy_, cx_, cy_, depth_scale_; // Camera intrinsics 
 
  Camera();
  Camera ( float fx, float fy, float cx, float cy, float depth_scale=0 ) :

project/0.1/include/myslam/frame.h

@@ -37,15 +37,13 @@ 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
- bool is_key_frame_; // whether a key-frame
 
 public: // data members 
  Frame();
  Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
  ~Frame();
 
+ // factory function
  static Frame::Ptr createFrame(); 
 
  // find the depth in depth map
@@ -54,8 +52,6 @@ class Frame
  // Get Camera Center
  Vector3d getCamCenter() const;
 
- void setPose( const SE3& T_c_w );
-
  // check if a point is in this frame 
  bool isInFrame( const Vector3d& pt_world );
 };

project/0.1/include/myslam/mappoint.h

@@ -28,20 +28,17 @@ class MapPoint
 {
 public:
  typedef shared_ptr<MapPoint> Ptr;
- unsigned long id_; // ID
- bool good_; // wheter a good point 
+ unsigned long id_; // ID
  Vector3d pos_; // Position in world
  Vector3d norm_; // Normal of viewing direction 
  Mat descriptor_; // Descriptor for matching 
-
- // 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 );
 
+ // factory function
  static MapPoint::Ptr createMapPoint();
 };
 }

project/0.1/src/CMakeLists.txt

@@ -3,9 +3,6 @@ add_library( myslam SHARED
  mappoint.cpp
  map.cpp
  camera.cpp
- config.cpp
- visual_odometry.cpp
- g2o_types.cpp
 )
 
 target_link_libraries( myslam

project/0.1/src/camera.cpp

@@ -18,18 +18,12 @@
  */
 
 #include "myslam/camera.h"
-#include <myslam/config.h>
 
 namespace myslam
 {
- 
+
 Camera::Camera()
 {
- fx_ = Config::get<float>("camera.fx");
- fy_ = Config::get<float>("camera.fy");
- cx_ = Config::get<float>("camera.cx");
- cy_ = Config::get<float>("camera.cy");
- depth_scale_ = Config::get<float>("camera.depth_scale");
 }
 
 Vector3d Camera::world2camera ( const Vector3d& p_w, const SE3& T_c_w )
@@ -45,23 +39,23 @@ Vector3d Camera::camera2world ( const Vector3d& p_c, const SE3& T_c_w )
 Vector2d Camera::camera2pixel ( const Vector3d& p_c )
 {
  return Vector2d (
-  fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) + cx_,
-  fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) + cy_
-  );
+ fx_ * p_c ( 0,0 ) / p_c ( 2,0 ) + cx_,
+ fy_ * p_c ( 1,0 ) / p_c ( 2,0 ) + cy_
+ );
 }
 
 Vector3d Camera::pixel2camera ( const Vector2d& p_p, double depth )
 {
  return Vector3d (
-  ( p_p ( 0,0 )-cx_ ) *depth/fx_,
-  ( p_p ( 1,0 )-cy_ ) *depth/fy_,
-  depth
-  );
+ ( p_p ( 0,0 )-cx_ ) *depth/fx_,
+ ( p_p ( 1,0 )-cy_ ) *depth/fy_,
+ depth
+ );
 }
 
 Vector2d Camera::world2pixel ( const Vector3d& p_w, const SE3& T_c_w )
 {
- return camera2pixel ( world2camera(p_w, T_c_w) );
+ return camera2pixel ( world2camera ( p_w, T_c_w ) );
 }
 
 Vector3d Camera::pixel2world ( const Vector2d& p_p, const SE3& T_c_w, double depth )

project/0.1/src/frame.cpp

@@ -22,13 +22,13 @@
 namespace myslam
 {
 Frame::Frame()
-: id_(-1), time_stamp_(-1), camera_(nullptr), is_key_frame_(false)
+: id_(-1), time_stamp_(-1), camera_(nullptr)
 {
 
 }
 
 Frame::Frame ( long id, double time_stamp, SE3 T_c_w, Camera::Ptr camera, Mat color, Mat depth )
-: id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth), is_key_frame_(false)
+: id_(id), time_stamp_(time_stamp), T_c_w_(T_c_w), camera_(camera), color_(color), depth_(depth)
 {
 
 }
@@ -70,11 +70,6 @@ double Frame::findDepth ( const cv::KeyPoint& kp )
  return -1.0;
 }
 
-void Frame::setPose ( const SE3& T_c_w )
-{
- T_c_w_ = T_c_w;
-}
-
 
 Vector3d Frame::getCamCenter() const
 {
@@ -84,10 +79,9 @@ Vector3d Frame::getCamCenter() const
 bool Frame::isInFrame ( const Vector3d& 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;
+ if ( p_cam(2,0)<0 ) 
+   return false;
  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 
  && pixel(1,0)<color_.rows;

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), observed_times_(0), correct_times_(0)
+: id_(-1), pos_(Vector3d(0,0,0)), norm_(Vector3d(0,0,0)), observed_times_(0), correct_times_(0)
 {
 
 }
 
 MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm )
-: id_(id), pos_(position), norm_(norm), good_(true), observed_times_(0), correct_times_(0)
+: id_(id), pos_(position), norm_(norm), observed_times_(0), correct_times_(0)
 {
 
 }

project/0.1/test/CMakeLists.txt

@@ -1,2 +0,0 @@
-add_executable( run_vo run_vo.cpp )
-target_link_libraries( run_vo myslam )

project/0.3/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required( VERSION 2.8 )
+project ( myslam )
+
+set( CMAKE_CXX_COMPILER "g++" )
+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 )
+set( LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib )
+
+############### dependencies ######################
+# Eigen
+include_directories( "/usr/include/eigen3" )
+# OpenCV
+find_package( OpenCV 3.1 REQUIRED )
+include_directories( ${OpenCV_INCLUDE_DIRS} )
+# Sophus 
+find_package( Sophus REQUIRED )
+include_directories( ${Sophus_INCLUDE_DIRS} )
+# G2O
+find_package( G2O REQUIRED )
+include_directories( ${G2O_INCLUDE_DIRS} )
+
+set( THIRD_PARTY_LIBS 
+ ${OpenCV_LIBS}
+ ${Sophus_LIBRARIES}
+ g2o_core g2o_stuff g2o_types_sba
+)
+############### dependencies ######################
+include_directories( ${PROJECT_SOURCE_DIR}/include )
+add_subdirectory( src )
+add_subdirectory( test )

project/0.3/cmake_modules/FindCSparse.cmake

@@ -0,0 +1,25 @@
+# Look for csparse; note the difference in the directory specifications!
+FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h
+ PATHS
+ /usr/include/suitesparse
+ /usr/include
+ /opt/local/include
+ /usr/local/include
+ /sw/include
+ /usr/include/ufsparse
+ /opt/local/include/ufsparse
+ /usr/local/include/ufsparse
+ /sw/include/ufsparse
+ )
+
+FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse
+ PATHS
+ /usr/lib
+ /usr/local/lib
+ /opt/local/lib
+ /sw/lib
+ )
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(CSPARSE DEFAULT_MSG
+ CSPARSE_INCLUDE_DIR CSPARSE_LIBRARY)

project/0.3/cmake_modules/FindG2O.cmake

@@ -0,0 +1,113 @@
+# Find the header files
+
+FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h
+ ${G2O_ROOT}/include
+ $ENV{G2O_ROOT}/include
+ $ENV{G2O_ROOT}
+ /usr/local/include
+ /usr/include
+ /opt/local/include
+ /sw/local/include
+ /sw/include
+ NO_DEFAULT_PATH
+ )
+
+# Macro to unify finding both the debug and release versions of the
+# libraries; this is adapted from the OpenSceneGraph FIND_LIBRARY
+# macro.
+
+MACRO(FIND_G2O_LIBRARY MYLIBRARY MYLIBRARYNAME)
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ${G2O_ROOT}/lib/Debug
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Debug
+ $ENV{G2O_ROOT}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY("${MYLIBRARY}_DEBUG"
+ NAMES "g2o_${MYLIBRARYNAME}_d"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "g2o_${MYLIBRARYNAME}"
+ PATHS
+ ${G2O_ROOT}/lib/Release
+ ${G2O_ROOT}/lib
+ $ENV{G2O_ROOT}/lib/Release
+ $ENV{G2O_ROOT}/lib
+ NO_DEFAULT_PATH
+ )
+
+ FIND_LIBRARY(${MYLIBRARY}
+ NAMES "g2o_${MYLIBRARYNAME}"
+ PATHS
+ ~/Library/Frameworks
+ /Library/Frameworks
+ /usr/local/lib
+ /usr/local/lib64
+ /usr/lib
+ /usr/lib64
+ /opt/local/lib
+ /sw/local/lib
+ /sw/lib
+ )
+
+ IF(NOT ${MYLIBRARY}_DEBUG)
+ IF(MYLIBRARY)
+ SET(${MYLIBRARY}_DEBUG ${MYLIBRARY})
+ ENDIF(MYLIBRARY)
+ ENDIF( NOT ${MYLIBRARY}_DEBUG)
+
+ENDMACRO(FIND_G2O_LIBRARY LIBRARY LIBRARYNAME)
+
+# Find the core elements
+FIND_G2O_LIBRARY(G2O_STUFF_LIBRARY stuff)
+FIND_G2O_LIBRARY(G2O_CORE_LIBRARY core)
+
+# Find the CLI library
+FIND_G2O_LIBRARY(G2O_CLI_LIBRARY cli)
+
+# Find the pluggable solvers
+FIND_G2O_LIBRARY(G2O_SOLVER_CHOLMOD solver_cholmod)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE solver_csparse)
+FIND_G2O_LIBRARY(G2O_SOLVER_CSPARSE_EXTENSION csparse_extension)
+FIND_G2O_LIBRARY(G2O_SOLVER_DENSE solver_dense)
+FIND_G2O_LIBRARY(G2O_SOLVER_PCG solver_pcg)
+FIND_G2O_LIBRARY(G2O_SOLVER_SLAM2D_LINEAR solver_slam2d_linear)
+FIND_G2O_LIBRARY(G2O_SOLVER_STRUCTURE_ONLY solver_structure_only)
+FIND_G2O_LIBRARY(G2O_SOLVER_EIGEN solver_eigen)
+
+# Find the predefined types
+FIND_G2O_LIBRARY(G2O_TYPES_DATA types_data)
+FIND_G2O_LIBRARY(G2O_TYPES_ICP types_icp)
+FIND_G2O_LIBRARY(G2O_TYPES_SBA types_sba)
+FIND_G2O_LIBRARY(G2O_TYPES_SCLAM2D types_sclam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SIM3 types_sim3)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM2D types_slam2d)
+FIND_G2O_LIBRARY(G2O_TYPES_SLAM3D types_slam3d)
+
+# G2O solvers declared found if we found at least one solver
+SET(G2O_SOLVERS_FOUND "NO")
+IF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+ SET(G2O_SOLVERS_FOUND "YES")
+ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER_PCG OR G2O_SOLVER_SLAM2D_LINEAR OR G2O_SOLVER_STRUCTURE_ONLY OR G2O_SOLVER_EIGEN)
+
+# G2O itself declared found if we found the core libraries and at least one solver
+SET(G2O_FOUND "NO")
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ SET(G2O_FOUND "YES")
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)