project/0.4

project/0.2/include/myslam/mappoint.h

@@ -33,7 +33,7 @@ class MapPoint
  Vector3d norm_; // Normal of viewing direction 
  Mat descriptor_; // Descriptor for matching 
  int observed_times_; // being observed by feature matching algo.
- int correct_times_; // being an inliner in pose estimation
+ int matched_times_; // being an inliner in pose estimation
 
  MapPoint();
  MapPoint( long id, Vector3d position, Vector3d norm );

project/0.3/include/myslam/mappoint.h

@@ -37,7 +37,7 @@ class MapPoint
  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
+ int matched_times_; // being an inliner in pose estimation
 
  MapPoint();
  MapPoint( long id, Vector3d position, Vector3d norm );

project/0.3/include/myslam/visual_odometry.h

@@ -51,7 +51,7 @@ class VisualOdometry
  vector<cv::DMatch> feature_matches_; // feature matches 
  cv::FlannBasedMatcher matcher_flann_; // flann matcher
 
- SE3 T_c_r_estimated_; // the estimated pose of current frame 
+ SE3 T_c_w_estimated_; // the estimated pose of current frame 
  int num_inliers_; // number of inlier features in icp
  int num_lost_; // number of lost times
 

project/0.4/config/default.yaml

@@ -19,12 +19,12 @@ camera.cy: 249.7
 camera.depth_scale: 5000
 
 # VO Parameters
-number_of_features: 800
+number_of_features: 200
 scale_factor: 1.2
 level_pyramid: 4
 match_ratio: 2.0
 max_num_lost: 10
-min_inliers: 10
+min_inliers: 30
 keyframe_rotation: 0.1
 keyframe_translation: 0.1
-map_point_erase_ratio: 0.5
+map_point_erase_ratio: 0.1

project/0.4/include/myslam/mappoint.h

@@ -20,6 +20,8 @@
 #ifndef MAPPOINT_H
 #define MAPPOINT_H
 
+#include "myslam/common_include.h"
+
 namespace myslam
 {
 
@@ -29,20 +31,36 @@ class MapPoint
 public:
  typedef shared_ptr<MapPoint> Ptr;
  unsigned long id_; // ID
+ static unsigned long factory_id_; // factory id
  bool good_; // wheter a good point 
  Vector3d pos_; // Position in world
  Vector3d norm_; // Normal of viewing direction 
  Mat descriptor_; // Descriptor for matching 
 
  list<Frame*> observed_frames_; // key-frames that can observe this point 
 
- int observed_times_; // being observed by feature matching algo.
- int correct_times_; // being an inliner in pose estimation
+ int matched_times_;  // being an inliner in pose estimation
+ int visible_times_;
 
  MapPoint();
- MapPoint( long id, Vector3d position, Vector3d norm );
+ MapPoint( 
+ unsigned long id, 
+ const Vector3d& position, 
+ const Vector3d& norm, 
+ Frame* frame=nullptr, 
+ const Mat& descriptor=Mat() 
+ );
+
+ inline cv::Point3f getPositionCV() const {
+ return cv::Point3f( pos_(0,0), pos_(1,0), pos_(2,0) );
+ }
 
  static MapPoint::Ptr createMapPoint();
+ static MapPoint::Ptr createMapPoint( 
+ const Vector3d& pos_world, 
+ const Vector3d& norm_,
+ const Mat& descriptor,
+ Frame* frame );
 };
 }
 

project/0.4/include/myslam/visual_odometry.h

@@ -47,12 +47,11 @@ class VisualOdometry
  vector<cv::KeyPoint> keypoints_curr_; // keypoints in current frame
  Mat descriptors_curr_; // descriptor in current frame 
 
- vector<cv::Point3f> mappt_cand; // candidate map points 
-
- vector<cv::DMatch> feature_matches_; // feature matches 
  cv::FlannBasedMatcher matcher_flann_; // flann matcher
+ vector<MapPoint::Ptr> match_3dpts_;
+ vector<int> match_2dkp_index_;
 
- SE3 T_c_r_estimated_; // the estimated pose of current frame 
+ SE3 T_c_w_estimated_; // the estimated pose of current frame 
  int num_inliers_; // number of inlier features in icp
  int num_lost_; // number of lost times
 
@@ -79,8 +78,10 @@ class VisualOdometry
  void computeDescriptors(); 
  void featureMatching();
  void poseEstimationPnP(); 
+ void optimizeMap();
 
  void addKeyFrame();
+ void addMapPoints();
  bool checkEstimatedPose(); 
  bool checkKeyFrame();
 

project/0.4/src/mappoint.cpp

@@ -24,23 +24,35 @@ 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)), good_(true), visible_times_(0), matched_times_(0)
 {
 
 }
 
-MapPoint::MapPoint ( long id, Vector3d position, Vector3d norm )
-: id_(id), pos_(position), norm_(norm), good_(true), observed_times_(0), correct_times_(0)
+MapPoint::MapPoint ( long unsigned int id, const Vector3d& position, const Vector3d& norm, Frame* frame, const Mat& descriptor )
+: id_(id), pos_(position), norm_(norm), good_(true), visible_times_(1), matched_times_(1), descriptor_(descriptor)
 {
-
+ observed_frames_.push_back(frame);
 }
 
 MapPoint::Ptr MapPoint::createMapPoint()
 {
- static long factory_id = 0;
  return MapPoint::Ptr( 
- new MapPoint( factory_id++, Vector3d(0,0,0), Vector3d(0,0,0) )
+ new MapPoint( factory_id_++, Vector3d(0,0,0), Vector3d(0,0,0) )
  );
 }
 
+MapPoint::Ptr MapPoint::createMapPoint ( 
+ const Vector3d& pos_world, 
+ const Vector3d& norm, 
+ const Mat& descriptor, 
+ Frame* frame )
+{
+ return MapPoint::Ptr( 
+ new MapPoint( factory_id_++, pos_world, norm, frame, descriptor )
+ );
+}
+
+unsigned long MapPoint::factory_id_ = 0;
+
 }