Fix part of the BA bug. Improve code. The left bug is: I can only opt…

…imize camera pose, not mappoints. Optimizing mappoints ruins the estimated trajectory.

config/config.yaml

@@ -2,13 +2,10 @@
 
 # ===== Settings for displaying
 MAX_NUM_IMAGES: 300
-PCL_WAIT_FOR_KEY_PRESS: 0 # If 1, PCL Viewer will stop and wait for any of your keypress before continueing.
-USE_BA: 0                 # Use bundle adjustment for camera and points in single frame. 1 for true, 0 for false
-NUM_FRAMES_FOR_BA: 1      # [Still have bug. Can only be set as 1.]
-DRAW_GROUND_TRUTH_TRAJ: 1 # Ground truth traj's color is set as green. Estimated is set as white.
-
+PCL_WAIT_FOR_KEY_PRESS: "false" # If true, PCL Viewer will stop and wait for any of your keypress before continueing.
+DRAW_GROUND_TRUTH_TRAJ: "true" # Ground truth traj's color is set as green. Estimated is set as white.
 GROUND_TRUTH_TRAJ_FILENAME: /home/feiyu/Desktop/slam/my_vo/my2/data/test_data/cam_traj_truth.txt
-STORE_CAM_TRAJ: /home/feiyu/Desktop/slam/my_vo/my2/data/test_data/cam_traj.txt
+FILENAME_FOR_RESULT_TRAJECTORY: /home/feiyu/Desktop/slam/my_vo/my2/data/test_data/cam_traj.txt
 
 # ===== Dataset and camera intrinsics
 
@@ -68,4 +65,9 @@ MIN_PIXEL_DIST_FOR_INIT_VO: 70
 MIN_DIST_BETWEEN_KEYFRAME: 0.08
 MIN_ROTATED_ANGLE: 0.08
 
-
+# ------------------- Optimization -------------------
+USE_BA: "true"                 # Use bundle adjustment for camera and points in single frame. 1 for true, 0 for false
+MAX_NUM_FRAMES_FOR_BA: 15      # <= 20. I set the "BUFF_SIZE" in "vo.h" as 20, so only previous 20 frames are stored.
+information_matrix: "1.0 0.0 0.0 1.0"
+FIX_MAP_PTS: "true" # TO DEBUG: If I set it to true and optimize both camera pose and map points, there is huge error.
+UPDATE_MAP_PTS: "false"

doc/log.md

@@ -0,0 +1,12 @@
+
+
+---------------------------------------
+02-02
+Bundle adjustment has bug, I have to fix all points in order to get good result (and it indeed is better result compared to not using BA).
+Is the bug due to following reasons:
+* Is there a camera pose with 0 connection to the map point?  
+    No.
+* Is the graph all connected, or piece wise connected?    
+    No. In my test cases, all are connected.
+* Wrong information matrix?  
+    No. I've tested [x, 0; 0, 1], with x=1e-5 and 1e5. However, both are wrong.

include/my_basics/basics.h

@@ -19,13 +19,20 @@ namespace my_basics
 {
 using namespace std;
 
+bool str2bool(const string &s);
+
 // Convert int to string, and fill it with zero before the number to make it specified width
 string int2str(int num, int width, char char_to_fill='0');
 
+// Convert string into a vector of doubles
+vector<double> str2vecdouble(const string &pointLine);
+
 // Please sort v1 and v2 first, then this functions returns the intersection of two vector.
 vector<int> getIntersection(vector<int> v1, vector<int> v2);
 
 
+
+
 } // namespace my_basics
 
 #endif

include/my_basics/config.h

@@ -16,23 +16,33 @@ using namespace std;
 
 class Config
 {
-  private:
-    static std::shared_ptr<Config> config_;
-    cv::FileStorage file_;
-
-    Config() {} // private constructor makes a singleton
-  public:
-    ~Config(); // close the file when deconstructing
-
-    // set a new config file
-    static void setParameterFile(const std::string &filename);
-
-    // access the parameter values
-    template <typename T>
-    static T get(const std::string &key)
-    {
-        return T(Config::config_->file_[key]);
-    }
+private:
+  static std::shared_ptr<Config> config_;
+  cv::FileStorage file_;
+
+  Config() {} // private constructor makes a singleton
+public:
+  ~Config(); // close the file when deconstructing
+
+  // set a new config file
+  static void setParameterFile(const std::string &filename);
+
+  // access the parameter values
+  template <typename T>
+  static T get(const std::string &key)
+  {
+    return T(Config::config_->file_[key]);
+  }
+
+  static bool getBool(const std::string &key)
+  {
+    string val = string(Config::config_->file_[key]);
+    // cout << "getBool:" << val << endl;
+    if (val == "true" || val == "True")
+      return true; // If I combine this into the above template, the program throws error: could not convert ‘true’ from ‘bool’ to ‘std::__cxx11::basic_string<char>’
+    else
+      return false;
+  }
 };
 } // namespace my_basics
 

include/my_optimization/g2o_ba.h

@@ -31,7 +31,8 @@ void bundleAdjustment(
     const Mat& K,
     unordered_map<int, Point3f*> &pts_3d,
     vector<Mat*> &v_camera_g2o_poses,
-    bool fix_map_pts, bool update_map_pts);
+    const Mat &information_matrix,
+    bool fix_map_pts=false, bool update_map_pts=true);
 
 } // namespace my_optimization
 #endif

include/my_slam/vo.h

@@ -10,6 +10,7 @@
 
 // my
 
+#include "my_basics/basics.h"
 #include "my_basics/io.h"
 #include "my_basics/config.h"
 #include "my_basics/opencv_funcs.h"
@@ -67,7 +68,7 @@ class VisualOdometry
   VisualOdometry();
   void addFrame(my_slam::Frame::Ptr frame);
   void pushFrameToBuff(Frame::Ptr frame){
-    const int BUFF_SIZE=10;
+    const int BUFF_SIZE=20;
     frames_buff_.push_back(frame);
     if(frames_buff_.size()>BUFF_SIZE)
       frames_buff_.pop_front();

src/basics.cpp

@@ -1,22 +1,52 @@
 
 
 #include "my_basics/basics.h"
+#include <string>
+#include <vector>
+#include <iterator>
+#include <sstream>
+#include <assert.h>
 
+namespace my_basics
+{
+bool str2bool(const string &s)
+{
+    if (s == "true" || s == "True")
+    {
+        return true;
+    }
+    else if (s == "false" || s == "False")
+    {
+        return false;
+    }
+    else
+    {
+        cout << "my Warning: wrong string to convert to bool" << endl;
+        assert(0);
+    }
+}
 
-namespace my_basics{
-
-
-string int2str(int num, int width, char char_to_fill){
-    std::stringstream ss;
+string int2str(int num, int width, char char_to_fill)
+{
+    stringstream ss;
     ss << std::setw(width) << std::setfill(char_to_fill) << num;
     return ss.str();
 }
 
+vector<double> str2vecdouble(const string &pointLine)
+{
+    std::istringstream iss(pointLine);
+
+    return vector<double>{
+        std::istream_iterator<double>(iss),
+        std::istream_iterator<double>()};
+}
+
 vector<int> getIntersection(vector<int> v1, vector<int> v2)
 {
     int comb_len = v1.size() + v2.size();
-    std::vector<int> v(comb_len);
-    std::vector<int>::iterator it;
+    vector<int> v(comb_len);
+    vector<int>::iterator it;
     it = std::set_intersection(
         v1.begin(), v1.end(),
         v2.begin(), v2.end(),
@@ -25,5 +55,4 @@ vector<int> getIntersection(vector<int> v1, vector<int> v2)
     return v;
 }
 
-
-}
+} // namespace my_basics

src/g2o_ba.cpp

@@ -24,6 +24,11 @@
 namespace my_optimization
 {
 
+Eigen::Matrix2d mat2eigen(const cv::Mat &mat){
+    Eigen::Matrix2d mat_eigen;
+    mat_eigen<<mat.at<double>(0,0), mat.at<double>(0,1), mat.at<double>(1,0), mat.at<double>(1,1);
+    return mat_eigen;
+}
 void optimizeSingleFrame(
     const vector<Point2f*> &points_2d,
     const Mat &K,
@@ -171,6 +176,7 @@ void bundleAdjustment(
     const Mat &K,
     unordered_map<int, Point3f *> &pts_3d,
     vector<Mat *> &v_camera_g2o_poses,
+    const Mat &information_matrix,
     bool fix_map_pts, bool update_map_pts)
 {
 
@@ -239,7 +245,10 @@ void bundleAdjustment(
     printf("Number of frames = %d, 3d points = %d\n", num_frames, vertex_id - num_frames);
 
     // -- Add edges, which define the error/cost function.
+
+    // Set information matrix
     int edge_id = 0;
+    Eigen::Matrix2d information_matrix_eigen =mat2eigen(information_matrix);
     for (int ith_frame = 0; ith_frame < num_frames; ith_frame++)
     {
         int num_pts_2d = v_pts_2d[ith_frame].size();
@@ -257,8 +266,8 @@ void bundleAdjustment(
 
             edge->setMeasurement(Eigen::Vector2d(p->x, p->y));
             edge->setParameterId(0, 0);
-            edge->setInformation(Eigen::Matrix2d::Identity());
-            // edge->setRobustKernel( new g2o::RobustKernelHuber() );
+            edge->setInformation(information_matrix_eigen);
+            edge->setRobustKernel( new g2o::RobustKernelHuber() );
             optimizer.addEdge(edge);
         }
     }

src/vo.cpp

@@ -210,22 +210,30 @@ void VisualOdometry::poseEstimationPnP()
 // bundle adjustment
 void VisualOdometry::callBundleAdjustment()
 {
-    static const int USE_BA = my_basics::Config::get<int>("USE_BA");
-    const int NUM_FRAMES_FOR_BA = my_basics::Config::get<int>("NUM_FRAMES_FOR_BA");
-    ;
+    // Read settings from config.yaml
+    static const bool USE_BA = my_basics::Config::getBool("USE_BA");
+    static const int MAX_NUM_FRAMES_FOR_BA = my_basics::Config::get<int>("MAX_NUM_FRAMES_FOR_BA");
+    static const vector<double> im = my_basics::str2vecdouble(
+        my_basics::Config::get<string>("information_matrix"));
+    static const bool FIX_MAP_PTS = my_basics::Config::getBool("FIX_MAP_PTS");
+    static const bool UPDATE_MAP_PTS =my_basics::Config::getBool("UPDATE_MAP_PTS");
+    cout << FIX_MAP_PTS << UPDATE_MAP_PTS << endl;
+
     if (USE_BA != 1)
     {
         printf("\nNot using bundle adjustment ... \n");
         return;
     }
     printf("\nCalling bundle adjustment ... \n");
-
-    // Param
+    
+    // Set params
     const int TOTAL_FRAMES = frames_buff_.size();
+    const int NUM_FRAMES_FOR_BA = min(MAX_NUM_FRAMES_FOR_BA, TOTAL_FRAMES-1);
+    const static cv::Mat information_matrix=(cv::Mat_<double>(2,2)<<im[0],im[1],im[2],im[3]);
 
     // Measurement (which is fixed; truth)
-    vector<vector<Point2f *>> v_pts_2d(NUM_FRAMES_FOR_BA, vector<Point2f *>());
-    vector<vector<int>> v_pts_2d_to_3d_idx(NUM_FRAMES_FOR_BA, vector<int>());
+    vector<vector<Point2f *>> v_pts_2d;
+    vector<vector<int>> v_pts_2d_to_3d_idx;
 
     // Things to to optimize
     unordered_map<int, Point3f *> um_pts_3d;
@@ -234,15 +242,22 @@ void VisualOdometry::callBundleAdjustment()
 
     // Set up input vars
     int ith_frame = 0;
-    for (int frame_id = TOTAL_FRAMES - 1; frame_id >= TOTAL_FRAMES - NUM_FRAMES_FOR_BA;
-         frame_id--, ith_frame++)
+    for (int ith_frame_in_buff = TOTAL_FRAMES - 1; ith_frame_in_buff >= TOTAL_FRAMES - NUM_FRAMES_FOR_BA;
+         ith_frame_in_buff--, ith_frame++)
     {
-        Frame::Ptr frame = frames_buff_[frame_id];
+        Frame::Ptr frame = frames_buff_[ith_frame_in_buff];
+        int num_mappt_in_frame = frame->inliers_to_mappt_connections_.size();
+        if(num_mappt_in_frame<3){
+            continue; // Too few mappoints. Not optimizing this frame
+        }
+        // printf("Frame id: %d, num map points = %d\n", frame->id_, num_mappt_in_frame);
+        v_pts_2d.push_back(vector<Point2f *>());
+        v_pts_2d_to_3d_idx.push_back(vector<int>());
+
         // Get camera poses
         v_camera_poses.push_back(&frame->T_w_c_);
 
         // Iterate through this camera's mappoints
-        int num_mappt_in_frame = frame->inliers_to_mappt_connections_.size();
         for (unordered_map<int, PtConn>::iterator ite = frame->inliers_to_mappt_connections_.begin();
              ite != frame->inliers_to_mappt_connections_.end(); ite++)
         {
@@ -263,21 +278,21 @@ void VisualOdometry::callBundleAdjustment()
         }
     }
     // Bundle Adjustment
-    const bool fix_map_pts = false, update_map_pts = false;
     Mat pose_src = getPosFromT(curr_->T_w_c_);
     if (1)
     {
         my_optimization::bundleAdjustment(
             v_pts_2d, v_pts_2d_to_3d_idx, curr_->camera_->K_,
             um_pts_3d, v_camera_poses,
-            fix_map_pts, update_map_pts);
+            information_matrix,
+            FIX_MAP_PTS, UPDATE_MAP_PTS);
     }
     else
     {
         my_optimization::optimizeSingleFrame(
             v_pts_2d[0], curr_->camera_->K_,
             v_pts_3d_only_in_curr, curr_->T_w_c_,
-            fix_map_pts, update_map_pts); // Update pts_3d and curr_->T_w_c_
+            FIX_MAP_PTS, UPDATE_MAP_PTS); // Update pts_3d and curr_->T_w_c_
     }
 
     // Print result

src_main/run_vo.cpp

@@ -76,7 +76,7 @@ int main(int argc, char **argv)
 
     // -- Prepare Pcl display
     PclViewer::Ptr pcl_displayer = setUpPclDisplay(); // Prepare pcl display
-    bool DRAW_GROUND_TRUTH_TRAJ =  my_basics::Config::get<int>("DRAW_GROUND_TRUTH_TRAJ")==1;
+    bool DRAW_GROUND_TRUTH_TRAJ =  my_basics::Config::getBool("DRAW_GROUND_TRUTH_TRAJ");
 
     // -- Prepare opencv display
     cv::namedWindow(IMAGE_WINDOW_NAME, cv::WINDOW_AUTOSIZE);
@@ -106,8 +106,8 @@ int main(int argc, char **argv)
         // Display
         bool cv2_draw_good = drawResultByOpenCV(rgb_img, frame, vo);
         bool pcl_draw_good = drawResultByPcl(vo, frame, pcl_displayer, DRAW_GROUND_TRUTH_TRAJ);
-        static const int PCL_WAIT_FOR_KEY_PRESS = my_basics::Config::get<int>("PCL_WAIT_FOR_KEY_PRESS");
-        if (PCL_WAIT_FOR_KEY_PRESS == 1)
+        static const bool PCL_WAIT_FOR_KEY_PRESS = my_basics::Config::getBool("PCL_WAIT_FOR_KEY_PRESS");
+        if (PCL_WAIT_FOR_KEY_PRESS)
             waitPclKeyPress(pcl_displayer);
 
         // Return
@@ -119,8 +119,8 @@ int main(int argc, char **argv)
     }
 
     // Save camera trajectory
-    const string STORE_CAM_TRAJ= my_basics::Config::get<string>("STORE_CAM_TRAJ");
-    writePoseToFile(STORE_CAM_TRAJ, cam_pose_history);
+    const string FILENAME_FOR_RESULT_TRAJECTORY= my_basics::Config::get<string>("FILENAME_FOR_RESULT_TRAJECTORY");
+    writePoseToFile(FILENAME_FOR_RESULT_TRAJECTORY, cam_pose_history);
 
     // Wait for user close
     while (!pcl_displayer->wasStopped())