proj

project/0.2/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
@@ -13,11 +13,11 @@ camera.cy: 249.7
 camera.depth_scale: 5000
 
 # VO paras
-number_of_features: 800
+number_of_features: 300
 scale_factor: 1.2
 level_pyramid: 8
 match_ratio: 2.0
 max_num_lost: 10
 min_inliers: 10
 keyframe_rotation: 0.1
-keyframe_translation: 0.1
+keyframe_translation: 0.1

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 "Release" )
-set( CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3" )
+set( CMAKE_BUILD_TYPE "Debug" )
+set( CMAKE_CXX_FLAGS "-std=c++11 -march=native" )
 
 list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )
 set( EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin )

project/0.3/config/default.yaml

@@ -19,7 +19,7 @@ camera.cy: 249.7
 camera.depth_scale: 5000
 
 # VO Parameters
-number_of_features: 1500
+number_of_features: 800
 scale_factor: 1.2
 level_pyramid: 4
 match_ratio: 2.0

project/0.3/include/myslam/common_include.h

@@ -30,6 +30,8 @@ using Eigen::Vector3d;
 
 // for Sophus
 #include <sophus/se3.h>
+#include <sophus/so3.h>
+using Sophus::SO3;
 using Sophus::SE3;
 
 // for cv

project/0.3/include/myslam/mappoint.h

@@ -34,7 +34,7 @@ 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

project/0.3/include/myslam/visual_odometry.h

@@ -42,13 +42,14 @@ class VisualOdometry
  Frame::Ptr ref_; // reference frame 
  Frame::Ptr curr_; // current frame 
 
- vector<cv::KeyPoint> keypoints_curr_;
- Mat descriptors_curr_;
-
  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_;
 
- vector<unsigned long> match_map_, match_curr_; // feature matching in map and current
- SE3 pose_curr_estimated_; // the estimated pose of current frame 
+ 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
 
@@ -59,9 +60,9 @@ class VisualOdometry
  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();
@@ -73,15 +74,15 @@ class VisualOdometry
  // inner operation 
  void extractKeyPoints();
  void computeDescriptors(); 
- void addAllKeypointsIntoMap(); 
  void featureMatching();
- void poseEstimationRGBD(); 
- void addKeyFrame();
+ void poseEstimationPnP(); 
+ void setRef3DPoints();
 
+ void addKeyFrame();
  bool checkEstimatedPose(); 
  bool checkKeyFrame();
 
 };
 }
 
-#endif // VISUALODOMETRY_H
+#endif // VISUALODOMETRY_H

project/0.3/include/myslam/visual_odometry2.h

@@ -0,0 +1,91 @@
+/*
+ * <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

@@ -4,8 +4,8 @@ add_library( myslam SHARED
  map.cpp
  camera.cpp
  config.cpp
- visual_odometry.cpp
  g2o_types.cpp
+ visual_odometry2.cpp
 )
 
 target_link_libraries( myslam