ch9/proj.0.3

project/0.3/CMakeLists.txt

@@ -2,8 +2,8 @@ cmake_minimum_required( VERSION 2.8 )
 project ( myslam )
 
 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.3/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_xyz
+dataset_dir: /home/ciang/dataset/rgbd_dataset_freiburg1_desk
 
 # camera intrinsics
 # fr1

project/0.3/include/myslam/g2o_types.h

@@ -49,6 +49,7 @@ class EdgeProjectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::V
 class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d, g2o::VertexSE3Expmap >
 {
 public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  // Error: measure = R*point+t
  virtual void computeError();
  virtual void linearizeOplus();
@@ -62,7 +63,7 @@ class EdgeProjectXYZRGBDPoseOnly: public g2o::BaseUnaryEdge<3, Eigen::Vector3d,
 class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap >
 {
 public:
- Camera::Ptr camera_;
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  virtual void computeError();
  virtual void linearizeOplus();
@@ -71,6 +72,7 @@ class EdgeProjectXYZ2UVPoseOnly: public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g
  virtual bool write(std::ostream& os) const {};
 
  Vector3d point_;
+ Camera* camera_;
 };
 
 }

project/0.3/include/myslam/visual_odometry.h

@@ -39,30 +39,32 @@ class VisualOdometry
 
  VOState state_; // current VO status
  Map::Ptr map_; // map with all frames and map points
- Frame::Ptr ref_; // reference frame 
+
+ Frame::Ptr ref_; // reference key-frame 
  Frame::Ptr curr_; // current frame 
 
  cv::Ptr<cv::ORB> orb_; // orb detector and computer 
  vector<cv::Point3f> pts_3d_ref_; // 3d points in reference frame 
  vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
  Mat descriptors_curr_; // descriptor in current frame 
  Mat descriptors_ref_; // descriptor in reference frame 
- vector<cv::DMatch> feature_matches_;
-
- SE3 T_c_r_estimated_; // the estimated pose of current frame 
+ vector<cv::DMatch> feature_matches_; // feature matches 
+ cv::FlannBasedMatcher matcher_flann_; // flann matcher
+
+ SE3 T_c_r_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();
@@ -74,9 +76,10 @@ class VisualOdometry
  // inner operation 
  void extractKeyPoints();
  void computeDescriptors(); 
+ void addAllKeypointsIntoMap(); 
  void featureMatching();
- void poseEstimationPnP(); 
  void setRef3DPoints();
+ void poseEstimationPnP(); 
 
  void addKeyFrame();
  bool checkEstimatedPose(); 
@@ -85,4 +88,4 @@ class VisualOdometry
 };
 }
 
-#endif // VISUALODOMETRY_H
+#endif // VISUALODOMETRY_H

project/0.3/include/myslam/visual_odometry2.h

@@ -1,91 +0,0 @@
-/*
- * <one line to give the program's name and a brief idea of what it does.>
- * Copyright (C) 2016 <copyright holder> <email>
- *
- * This program is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * This program is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this program. If not, see <http:https://www.gnu.org/licenses/>.
- *
- */
-
-#ifndef VISUALODOMETRY_H
-#define VISUALODOMETRY_H
-
-#include "myslam/common_include.h"
-#include "myslam/map.h"
-
-#include <opencv2/features2d/features2d.hpp>
-
-namespace myslam 
-{
-class VisualOdometry2
-{
-public:
- typedef shared_ptr<VisualOdometry2> Ptr;
- enum VOState {
- INITIALIZING=-1,
- OK=0,
- LOST
- };
-
- VOState state_; // current VO status
- Map::Ptr map_; // map with all frames and map points
-
- Frame::Ptr ref_; // reference key-frame 
- Frame::Ptr curr_; // current frame 
-
- cv::Ptr<cv::ORB> orb_; // orb detector and computer 
- vector<cv::Point3f> pts_3d_ref_; // 3d points in reference frame 
- vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
- Mat descriptors_curr_; // descriptor in current frame 
- Mat descriptors_ref_; // descriptor in reference frame 
- vector<cv::DMatch> feature_matches_;
- cv::FlannBasedMatcher matcher_flann_;
-
- SE3 T_c_r_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
- 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 
- VisualOdometry2();
- ~VisualOdometry2();
-
- bool addFrame( Frame::Ptr frame ); // add a new frame 
-
-protected: 
- // inner operation 
- void extractKeyPoints();
- void computeDescriptors(); 
- void addAllKeypointsIntoMap(); 
- void featureMatching();
- void setRef3DPoints();
- void poseEstimationPnP(); 
-
- void addKeyFrame();
- bool checkEstimatedPose(); 
- bool checkKeyFrame();
-
-};
-}
-
-#endif // VISUALODOMETRY_H

project/0.3/src/CMakeLists.txt

@@ -5,7 +5,7 @@ add_library( myslam SHARED
  camera.cpp
  config.cpp
  g2o_types.cpp
- visual_odometry2.cpp
+ visual_odometry.cpp
 )
 
 target_link_libraries( myslam