basically done

ch13/config/default.yaml

@@ -1,7 +1,7 @@
 %YAML:1.0
 # data
 # the tum dataset directory, change it to yours! 
-dataset_dir: /media/xiang/Data/Dataset/Kitti/dataset/sequences/02
+dataset_dir: /media/xiang/Data/Dataset/Kitti/dataset/sequences/00
 # dataset_dir: /mnt/1A9286BD92869CBF/Dataset/Kitti/dataset/sequences/02
 
 # camera intrinsics
@@ -11,8 +11,6 @@ camera.fy: 516.5
 camera.cx: 325.1
 camera.cy: 249.7
 
-camera.depth_scale: 5000
-
-num_features: 200
+num_features: 150
 num_features_init: 100
 num_features_tracking: 50

ch13/include/myslam/frontend.h

@@ -117,6 +117,8 @@ class Frontend {
     std::shared_ptr<Backend> backend_ = nullptr;
     std::shared_ptr<Viewer> viewer_ = nullptr;
 
+    SE3 relative_motion_;  // 当前帧与上一帧的相对运动，用于估计当前帧pose初值
+
     int tracking_inliers_ = 0;  // inliers, used for testing new keyframes
 
     // params

ch13/include/myslam/map.h

@@ -57,8 +57,10 @@ class Map {
     KeyframesType keyframes_;         // all key-frames
     KeyframesType active_keyframes_;  // all key-frames
 
+    Frame::Ptr current_frame_ = nullptr;
+
     // settings
-    int num_active_keyframes_ = 10;  // 激活的关键帧数量
+    int num_active_keyframes_ = 7;  // 激活的关键帧数量
 };
 }  // namespace myslam
 

ch13/include/myslam/mappoint.h

@@ -39,6 +39,7 @@ struct MapPoint {
     void AddObservation(std::shared_ptr<Feature> feature) {
         std::unique_lock<std::mutex> lck(data_mutex_);
         observations_.push_back(feature);
+        observed_times_++;
     }
 
     std::list<std::weak_ptr<Feature>> GetObs() {

ch13/src/backend.cpp

@@ -50,8 +50,8 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
     g2o::SparseOptimizer optimizer;
     optimizer.setAlgorithm(solver);
 
-    // vertex
-    std::map<unsigned long, VertexPose *> vertices; // pose 顶点，id使用Keyframe id
+    // pose 顶点，id使用Keyframe id
+    std::map<unsigned long, VertexPose *> vertices;
     unsigned long max_kf_id = 0;
     for (auto &keyframe : keyframes) {
         auto kf = keyframe.second;
@@ -63,8 +63,7 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
             max_kf_id = kf->keyframe_id_;
         }
 
-        vertices.insert(std::pair<unsigned long, VertexPose *>(kf->keyframe_id_,
-                                                               vertex_pose));
+        vertices.insert({kf->keyframe_id_, vertex_pose});
     }
 
     // landmarks, will be created when adding new edges
@@ -103,8 +102,7 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
                 v->setEstimate(landmark.second->Pos());
                 v->setId(landmark_id + max_kf_id + 1);
                 v->setMarginalized(true);
-                vertices_landmarks.insert(
-                    std::pair<unsigned long, VertexXYZ *>(landmark_id, v));
+                vertices_landmarks.insert({landmark_id, v});
                 optimizer.addVertex(v);
             }
 
@@ -116,8 +114,7 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
             auto rk = new g2o::RobustKernelHuber();
             rk->setDelta(chi2_th);
             edge->setRobustKernel(rk);
-            edges_and_features.insert(
-                std::pair<EdgeProjection *, Feature::Ptr>(edge, feat));
+            edges_and_features.insert({edge, feat});
 
             optimizer.addEdge(edge);
 
@@ -127,7 +124,6 @@ void Backend::Optimize(Map::KeyframesType &keyframes,
 
     // do optimization and eliminate the outliers
     optimizer.initializeOptimization();
-    optimizer.setVerbose(true);
     optimizer.optimize(10);
 
     int cnt_outlier = 0, cnt_inlier = 0;

ch13/src/frontend.cpp

@@ -42,6 +42,10 @@ bool Frontend::AddFrame(myslam::Frame::Ptr frame) {
 }
 
 bool Frontend::Track() {
+    if (last_frame_) {
+        current_frame_->SetPose(relative_motion_ * last_frame_->Pose());
+    }
+
     int num_track_last = TrackLastFrame();
     tracking_inliers_ = EstimateCurrentPose();
 
@@ -57,6 +61,7 @@ bool Frontend::Track() {
     }
 
     InsertKeyframe();
+    relative_motion_ = current_frame_->Pose() * last_frame_->Pose().inverse();
 
     if (viewer_) viewer_->AddCurrentFrame(current_frame_);
     return true;
@@ -92,10 +97,7 @@ bool Frontend::InsertKeyframe() {
 void Frontend::SetObservationsForKeyFrame() {
     for (auto &feat : current_frame_->features_left_) {
         auto mp = feat->map_point_.lock();
-        if (mp) {
-            mp->observed_times_++;
-            mp->observations_.push_back(feat);
-        }
+        if (mp) mp->AddObservation(feat);
     }
 }
 
@@ -120,7 +122,6 @@ int Frontend::TriangulateNewPoints() {
                 auto new_map_point = MapPoint::CreateNewMappoint();
                 pworld = current_pose_Twc * pworld;
                 new_map_point->SetPos(pworld);
-                new_map_point->observed_times_ = 2;
                 new_map_point->AddObservation(
                     current_frame_->features_left_[i]);
                 new_map_point->AddObservation(
@@ -213,6 +214,9 @@ int Frontend::EstimateCurrentPose() {
               << features.size() - cnt_outlier;
     // Set pose and outlier
     current_frame_->SetPose(vertex_pose->estimate());
+
+    LOG(INFO) << "Current Pose = \n" << current_frame_->Pose().matrix();
+
     for (auto &feat : features) {
         if (feat->is_outlier_) {
             feat->map_point_.reset();
@@ -226,17 +230,29 @@ int Frontend::TrackLastFrame() {
     // use LK flow to estimate points in the right image
     std::vector<cv::Point2f> kps_last, kps_current;
     for (auto &kp : last_frame_->features_left_) {
-        kps_last.push_back(kp->position_.pt);
+        if (kp->map_point_.lock()) {
+            // use project point
+            auto mp = kp->map_point_.lock();
+            auto px =
+                camera_left_->world2pixel(mp->pos_, current_frame_->Pose());
+            kps_last.push_back(kp->position_.pt);
+            kps_current.push_back(cv::Point2f(px[0], px[1]));
+        } else {
+            kps_last.push_back(kp->position_.pt);
+            kps_current.push_back(kp->position_.pt);
+        }
     }
 
     std::vector<uchar> status;
     Mat error;
-    cv::calcOpticalFlowPyrLK(last_frame_->left_img_, current_frame_->left_img_,
-                             kps_last, kps_current, status, error);
+    cv::calcOpticalFlowPyrLK(
+        last_frame_->left_img_, current_frame_->left_img_, kps_last,
+        kps_current, status, error, cv::Size(21, 21), 3,
+        cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30,
+                         0.01),
+        cv::OPTFLOW_USE_INITIAL_FLOW);
 
     int num_good_pts = 0;
-    cv::Mat img_show;
-    cv::cvtColor(current_frame_->left_img_, img_show, CV_GRAY2BGR);
 
     for (size_t i = 0; i < status.size(); ++i) {
         if (status[i]) {
@@ -245,13 +261,8 @@ int Frontend::TrackLastFrame() {
             feature->map_point_ = last_frame_->features_left_[i]->map_point_;
             current_frame_->features_left_.push_back(feature);
             num_good_pts++;
-
-            cv::circle(img_show, kp.pt, 2, cv::Scalar(0, 250, 0), 2);
-            cv::line(img_show, kp.pt, kps_last[i], cv::Scalar(0, 250, 0), 2);
         }
     }
-    // cv::imshow("LK track", img_show);
-    // cv::waitKey();
 
     LOG(INFO) << "Find " << num_good_pts << " in the last image.";
     return num_good_pts;
@@ -283,9 +294,6 @@ int Frontend::DetectFeatures() {
                       feat->position_.pt + cv::Point2f(10, 10), 0, CV_FILLED);
     }
 
-    // cv::imshow("mask", mask);
-    // cv::waitKey();
-
     std::vector<cv::KeyPoint> keypoints;
     gftt_->detect(current_frame_->left_img_, keypoints);
     for (auto &kp : keypoints) {
@@ -315,9 +323,12 @@ int Frontend::FindFeaturesInRight() {
 
     std::vector<uchar> status;
     Mat error;
-    cv::calcOpticalFlowPyrLK(current_frame_->left_img_,
-                             current_frame_->right_img_, kps_left, kps_right,
-                             status, error);
+    cv::calcOpticalFlowPyrLK(
+        current_frame_->left_img_, current_frame_->right_img_, kps_left,
+        kps_right, status, error, cv::Size(21, 21), 3,
+        cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 30,
+                         0.01),
+        cv::OPTFLOW_USE_INITIAL_FLOW);
 
     int num_good_pts = 0;
     for (size_t i = 0; i < status.size(); ++i) {
@@ -353,7 +364,6 @@ bool Frontend::BuildInitMap() {
         if (triangulation(poses, points, pworld) && pworld[2] > 0) {
             auto new_map_point = MapPoint::CreateNewMappoint();
             new_map_point->SetPos(pworld);
-            new_map_point->observed_times_ = 2;
             new_map_point->AddObservation(current_frame_->features_left_[i]);
             new_map_point->AddObservation(current_frame_->features_right_[i]);
             current_frame_->features_left_[i]->map_point_ = new_map_point;

ch13/src/map.cpp

@@ -18,10 +18,12 @@
  */
 
 #include "myslam/map.h"
+#include "myslam/feature.h"
 
 namespace myslam {
 
 void Map::InsertKeyFrame(Frame::Ptr frame) {
+    current_frame_ = frame;
     if (keyframes_.find(frame->id_) == keyframes_.end()) {
         keyframes_.insert(make_pair(frame->keyframe_id_, frame));
         active_keyframes_.insert(make_pair(frame->keyframe_id_, frame));
@@ -46,10 +48,66 @@ void Map::InsertMapPoint(MapPoint::Ptr map_point) {
 }
 
 void Map::RemoveOldKeyframe() {
-    auto frame_to_remove_iter = active_keyframes_.begin();
-    // set landmarks as
+    if (current_frame_ == nullptr) return;
+    // 寻找与当前帧最近与最远的两个关键帧
+    double max_dis = 0, min_dis = 9999;
+    double max_kf_id = 0, min_kf_id = 0;
+    auto Twc = current_frame_->Pose().inverse();
+    for (auto& kf : active_keyframes_) {
+        if (kf.second == current_frame_) continue;
+        auto dis = (kf.second->Pose() * Twc).log().norm();
+        if (dis > max_dis) {
+            max_dis = dis;
+            max_kf_id = kf.first;
+        }
+        if (dis < min_dis) {
+            min_dis = dis;
+            min_kf_id = kf.first;
+        }
+    }
+
+    const double min_dis_th = 0.2;  // 最近阈值
+    Frame::Ptr frame_to_remove = nullptr;
+    if (min_dis < min_dis_th) {
+        // 如果存在很近的帧，优先删掉最近的
+        frame_to_remove = keyframes_.at(min_kf_id);
+    } else {
+        // 删掉最远的
+        frame_to_remove = keyframes_.at(max_kf_id);
+    }
+
+    LOG(INFO) << "remove keyframe " << frame_to_remove->keyframe_id_;
+    // remove keyframe and landmark observation
+    active_keyframes_.erase(frame_to_remove->keyframe_id_);
+    for (auto feat : frame_to_remove->features_left_) {
+        auto mp = feat->map_point_.lock();
+        if (mp) {
+            mp->RemoveObservation(feat);
+        }
+    }
+    for (auto feat : frame_to_remove->features_right_) {
+        if (feat == nullptr) continue;
+        auto mp = feat->map_point_.lock();
+        if (mp) {
+            mp->RemoveObservation(feat);
+        }
+    }
+
+    CleanMap();
 }
 
-void Map::CleanMap() {}
+void Map::CleanMap() {
+    int cnt_landmark_removed = 0;
+    for (auto iter = active_landmarks_.begin();
+         iter != active_landmarks_.end();) {
+        if (iter->second->observed_times_ == 0) {
+            iter = active_landmarks_.erase(iter);
+            cnt_landmark_removed++;
+        } else {
+            ++iter;
+        }
+    }
+    LOG(INFO) << "Removed " << cnt_landmark_removed << " active landmarks";
+}
 
 }  // namespace myslam

ch13/src/mappoint.cpp

@@ -39,6 +39,7 @@ void MapPoint::RemoveObservation(std::shared_ptr<Feature> feat) {
             observations_.erase(iter);
             feat->map_point_.reset();
             observed_times_--;
+            break;
         }
     }
 }

ch13/src/viewer.cpp

@@ -59,7 +59,7 @@ void Viewer::ThreadLoop() {
         std::unique_lock<std::mutex> lock(viewer_data_mutex_);
         if (current_frame_) {
             DrawFrame(current_frame_, green);
-            // FollowCurrentFrame(vis_camera);
+            FollowCurrentFrame(vis_camera);
 
             cv::Mat img = PlotFrameImage();
             cv::imshow("image", img);

ch13/src/visual_odometry.cpp

@@ -45,10 +45,12 @@ void VisualOdometry::Run() {
         if (Step() == false) {
             break;
         }
-
-        sleep(1);
+        usleep(10000);
     }
 
+    backend_->Stop();
+    viewer_->Close();
+
     LOG(INFO) << "VO exit";
 }