V1.0 eol

README.md

@@ -63,3 +63,6 @@ If you get librealsense2.so is missing for pangolin, clear build and rebuild Pan
 > $ cmake ..  
 > $ make  
 > $ sudo make install  
+
+### Camera poses estimated for XYZ FR1 TUM data
+![Demo run](./docs/full-run.gif)

include/frame.h

@@ -32,6 +32,10 @@ struct RootInvCovAll{
     std::vector<std::vector<Eigen::Matrix3d>> cov_matrices;
 };
 
+struct system_poses{
+    std::vector<cv::Mat> relative_poses;
+    std::vector<cv::Mat> global_poses;
+};
 
 class FramePair{
     /*
@@ -78,10 +82,17 @@ class FramePair{
 
         // EigenVectors of cov matrices [For ransac]
         std::vector<std::vector<Eigen::Matrix3d>> cov_eig_vectors_im1, cov_eig_vectors_im2;
+
+        // covariance of endpoints of optimized matrix
+        std::vector<std::vector<Eigen::Matrix3d>> line1_endPt_covs, line2_endPt_covs;
 
         // Ransac object for culling outliers
         Ransac *pointRefine;
 
+        // Optimized Rot and trans
+        Eigen::Vector3d t_optim;
+        Eigen::Matrix3d R_optim;
+
         // Line Sampler in 2D
         void SampleIndices(const Eigen::MatrixXi& lines, std::vector<points2d>& sampled_lines_2d);
 
@@ -98,6 +109,5 @@ class FramePair{
 
         // ctor which populates members inside
         FramePair(const cv::Mat& rgb_image1, cv::Mat& depth_image1, cv::Mat& rgb_image2, cv::Mat& depth_image2);
-
 
 };

include/optim.h

@@ -18,22 +18,64 @@
 namespace optim
 {
 
-struct RootInvCov{
-    int idx1, idx2;
-    std::vector<Eigen::Matrix3d> cov_matrices;
-};
+    class LineData{
+    public:
+        Eigen::Vector3d A, B, u, d;
+
+        LineData(Eigen::Vector3d startPoint, Eigen::Vector3d endPoint): A(startPoint), B(endPoint){
+            Eigen::Vector3d l = B - A;
+            Eigen::Vector3d m = (A + B)/2;
 
-void compute_residual(double *points, double *error, int m, int n, void *data);
+            u = l/l.norm();
+            d = (l.cross(m))/l.norm();
+        }
+    };
 
-points3d nonlinOptimize(points3d& line3D, std::vector<Eigen::Matrix3d>& inv_cov_one_line, std::vector<Eigen::Matrix3d>& covariance_matrices, std::vector<Eigen::Matrix3d>& endPt_covs, int line_idx1, int line_idx2);
+    struct OptimizedLinesWithCov{
+        const std::vector<Eigen::Matrix3Xd> *l1, *l2;
+        const std::vector<std::vector<Eigen::Matrix3d>> *l1_cov, *l2_cov;
+        const std::vector<int> *matches;
+    };
+
+    struct RootInvCov{
+        int idx1, idx2;
+        std::vector<Eigen::Matrix3d> cov_matrices;
+    };
 
-double m_dist(const Eigen::Vector3d& X, const Eigen::Matrix3d& sigma_x, const Eigen::Vector3d& A, const Eigen::Vector3d& B);
+    void compute_residual(double *points, double *error, int m, int n, void *data);
 
-double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, 
-                      const Eigen::Vector3d& A1, const Eigen::Vector3d& B1,
-                      const Eigen::Matrix3d& cov_A1, const Eigen::Matrix3d& cov_B1,
-                      const Eigen::Vector3d& A2, const Eigen::Vector3d& B2,
-                      const Eigen::Matrix3d& cov_A2, const Eigen::Matrix3d& cov_B2
-                      );
+    points3d nonlinOptimize(points3d& line3D, std::vector<Eigen::Matrix3d>& inv_cov_one_line, std::vector<Eigen::Matrix3d>& covariance_matrices, std::vector<std::vector<Eigen::Matrix3d>>& endPt_covs, int line_idx1, int line_idx2);
+
+    double m_dist(const Eigen::Vector3d& X, const Eigen::Matrix3d& sigma_x, const Eigen::Vector3d& A, const Eigen::Vector3d& B);
+
+    double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, 
+                        const Eigen::Vector3d& A1, const Eigen::Vector3d& B1,
+                        const Eigen::Matrix3d& cov_A1, const Eigen::Matrix3d& cov_B1,
+                        const Eigen::Vector3d& A2, const Eigen::Vector3d& B2,
+                        const Eigen::Matrix3d& cov_A2, const Eigen::Matrix3d& cov_B2
+                        );
+
+
+    // Get anti-symmetric matrix given a vector
+    Eigen::Matrix3d GetCrossMatrix(const Eigen::Vector3d& vec);
+
+    // Computing rotation matrix as per the reference paper
+    Eigen::Matrix3d ComputeRotationMatrix(const LineData& l1, const LineData& l1_prime, const LineData& l2, const LineData& l2_prime);
+
+    // Computing translation as per the reference paper
+    Eigen::Vector3d ComputeTranslation(std::vector<LineData>& im1_lines, std::vector<LineData>& im2_lines, Eigen::Matrix3d& R);
+
+    double computeRtError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& A1, 
+                            const Eigen::Vector3d& B1, const Eigen::Vector3d& A2, const Eigen::Vector3d& B2);
+
+    double computeDistError(const Eigen::Matrix3d& R, const Eigen::Vector3d& t, const Eigen::Vector3d& X, 
+                            const Eigen::Vector3d& A, const Eigen::Vector3d& B);
+
+    void computeRotTransResidual(double *params, double *residuals, int n_params, int n_meas, void *data);
+
+    void optimizeRotTrans(Eigen::Matrix3d& R, Eigen::Vector3d& t, const std::vector<points3d>& im1_lines,
+                        const std::vector<points3d>& im2_lines, const std::vector<int>& matches,
+                        const std::vector<std::vector<Eigen::Matrix3d>>& im1_line_cov,
+                        const std::vector<std::vector<Eigen::Matrix3d>>& im2_line_cov, Eigen::Matrix3d& R_optim, Eigen::Vector3d& t_optim);
 
 }

include/ransac.h

@@ -10,10 +10,12 @@ Date: 28 Nov 2020
 */
 #pragma once
 
+// #include "utils.h"
 #include <Eigen/Dense>
 
 typedef Eigen::Vector3d point3d;
 typedef Eigen::Matrix3d covariance;
+typedef Eigen::Matrix<double, 3, Eigen::Dynamic> points3d;
 
 class Ransac{
 public:
@@ -35,4 +37,7 @@ class Ransac{
                                          std::vector<Eigen::Matrix3d>& updated_inv_root_covariance, const std::vector<Eigen::Matrix3d>& cov_G,
                                          const std::vector<Eigen::Matrix3d>& inv_cov_G);
 
+    std::vector<int> ransac3D(const std::vector<points3d>& optLines1, const std::vector<points3d>& optLines2, const std::vector<std::vector<Eigen::Matrix3d>>& endPtCov1,
+                             const std::vector<std::vector<Eigen::Matrix3d>> endPtCov2, Eigen::Matrix3d& R_best, Eigen::Vector3d& t_best);
+
 };

include/utils.h

@@ -43,7 +43,7 @@ namespace utils{
     void DrawLines3D(const std::vector<std::vector<cv::Point3d>>& lines);
 
     // Draw camera using a rectangular pyramid
-    void DrawSingleCamera(const cv::Mat& camera_pose, const float& w=1.0, const float& h_ratio=0.75, const float& z_ratio=0.6);
+    void DrawSingleCamera(const cv::Mat& camera_pose, const float& w=0.1, const float& h_ratio=0.75, const float& z_ratio=0.6);
 
     // Draw points in 3D
     // <TODO> Add default color information if no color is passed
@@ -57,4 +57,8 @@ namespace utils{
 
     // This draws the world coordinate frame at the world origin
     void DrawCoordinates();
+
+    // Helper to make 4d transformation matrix from R and t
+    cv::Mat MakeCameraPose(const Eigen::Matrix3d& R, const Eigen::Vector3d& t);
+
 }

include/viewer.h

@@ -18,7 +18,7 @@ Date: 19 Nov 2020
 class viewer{
     public:
         // constructor for the viewer
-        viewer(const std::string& window_name);
+        viewer(const std::string& window_name, struct system_poses* ptr_to_poses);
 
         // main render loop
         void run();
@@ -33,11 +33,17 @@ class viewer{
 
         // Mutex to guard data that needs to be plotted (current_frame)
         std::mutex viewerVarsMtx;
+
+        // Mutex to guard the poses data
+        std::mutex viewerPosesMtx;
 
     private:
         // Pointer to frame whose data we want to plot
         FramePair* current_frame;
 
+        // Pointer to the system of poses
+        struct system_poses* camera_poses;
+
         // To close viewer at end of processing
         bool stopViewer;
         std::mutex stopViewerMtx;

src/CMakeLists.txt

@@ -87,5 +87,8 @@ target_link_libraries(main PUBLIC levmar ${PROJECT_NAME} )
 # add_executable(levmar_test ${CMAKE_CURRENT_SOURCE_DIR}/levmar_test.cpp)
 target_link_libraries(main PUBLIC ${PROJECT_NAME} )
 
-# add_executable(test_ransac ${CMAKE_CURRENT_SOURCE_DIR}/test_ransac.cpp)
-# target_link_libraries(test_ransac PUBLIC ${PROJECT_NAME} )
+add_executable(test_viewer ${CMAKE_CURRENT_SOURCE_DIR}/test_viewer.cpp)
+target_link_libraries(test_viewer PUBLIC ${PROJECT_NAME} )
+
+add_executable(test_rotation ${CMAKE_CURRENT_SOURCE_DIR}/test_rotation.cpp)
+target_link_libraries(test_rotation PUBLIC ${PROJECT_NAME} )

src/frame.cpp

@@ -271,26 +271,33 @@ FramePair::FramePair(const cv::Mat& rgb_image1, cv::Mat& depth_image1, cv::Mat&
     std::cout << "Starting Im1" << std::endl;
     int Inp;
 
-    std::vector<Eigen::Matrix3d> line1_endPt_covs, line2_endPt_covs;
-
     for(int i = 0; i < rsac_points_3d_im1.size(); i++){
     // std::cout << rsac_points_3d_im1[i].cols() << " " << im1_data.cov_matrices[i].size() << std::endl;
     points3d optimized_line1 = optim::nonlinOptimize(rsac_points_3d_im1[i], im1_data.cov_matrices[i], cov_G_im1[i], line1_endPt_covs, 0, im1_data.cov_matrices[i].size()-1);
     optimized_lines_im1.push_back(optimized_line1);
     // std::cin >> Inp;
     }
+    // std::cout << line1_endPt_covs[0][0] << " \n " << line1_endPt_covs[0][1] << std::endl;
 
     std::cout << "Starting Im2" << std::endl;
     for(int i = 0; i < rsac_points_3d_im2.size(); i++){
     points3d optimized_line2 = optim::nonlinOptimize(rsac_points_3d_im2[i], im2_data.cov_matrices[i], cov_G_im2[i], line2_endPt_covs, 0, im2_data.cov_matrices[i].size()-1);
     optimized_lines_im2.push_back(optimized_line2);
     }
-    std::cout << "Matrix Size" << line1_endPt_covs.size() << " " << line2_endPt_covs.size() << std::endl;
+    // std::cin >> inp;
 
+    Eigen::Vector3d t_best;
+    Eigen::Matrix3d R_best;
+    std::vector<int> inlier_indices = pointRefine->ransac3D(optimized_lines_im1, optimized_lines_im2, line1_endPt_covs, line2_endPt_covs, R_best, t_best);
 
+    cout << "Ransac over: best initial estimates are: \n" << R_best << "\n" << t_best << endl;
 
+    // Optimize best_R and best_t
+    optim::optimizeRotTrans(R_best, t_best, optimized_lines_im1, optimized_lines_im2, inlier_indices,
+                            line1_endPt_covs, line2_endPt_covs, R_optim, t_optim);
+    cout << "I have come out of optimization" << endl;
+
+    // get Motion12 from Motion21
+    R_optim.transposeInPlace();
+    t_optim = -R_optim*t_optim;
 }
-
-    // points3d FramePair::OptimizeFrames(){
-
-    // }

src/main.cpp

@@ -16,6 +16,7 @@ This runs the full visual odometry system.
 #include <Eigen/Dense>
 #include <thread>
 #include <pangolin/pangolin.h>
+#include <opencv2/core/eigen.hpp>
 
 // Custom headers
 #include "frame.h"
@@ -35,13 +36,23 @@ int main(int argc, char* argv[])
   // Loading the data (TUMfr1)
   string data_dir     = "../data/rgbd_dataset_freiburg1_xyz/";
   string synched_file = "synched_data.txt";
+  string output_file  = "output_poses_full_run.txt";
 
-  string window_name = "ROBust LINE Visual Odometry";;
-  viewer* robline_viewer = new viewer(window_name);
+  string window_name = "ROBust LINE Visual Odometry";
+  struct system_poses *robline = new struct system_poses;
+
+  // ugly, bad, very very bad - fix later [Why? Don't want vector to be empty. So populate here instead of first image]
+  cv::Mat world_pose = cv::Mat::eye(4, 4, CV_64F);
+  robline->relative_poses.push_back(world_pose);
+  robline->global_poses.push_back(world_pose);
+
+  viewer* robline_viewer = new viewer(window_name, robline);
+
   std::thread render_thread(&viewer::run, robline_viewer);
 
   string image_pair_path, rgb_path, depth_path, rgb_time, depth_time;
   ifstream infile(data_dir + synched_file);
+  ofstream outfile(output_file);
 
   bool isFirst = true;
   cv::Mat previous_rgb, previous_depth;
@@ -50,13 +61,19 @@ int main(int argc, char* argv[])
   // Each object contains lines found in both images and matches between the frames
   std::vector<FramePair*> pairs;
 
+  int i=0;
   while(getline(infile, image_pair_path)){
+
+    // Sample every 4 frames
+    // if(i++%1) continue;
+
+    // if(i > 100) break;
+
     std::stringstream linestream(image_pair_path);
     linestream >> rgb_time >> rgb_path >> depth_time >> depth_path;
 
     cv::Mat rgb_img   = utils::ReadImage(data_dir + rgb_path, true);
     cv::Mat depth_img = utils::ReadImage(data_dir + depth_path, false);
-
     if (isFirst)
     {
       previous_rgb = rgb_img;
@@ -66,24 +83,59 @@ int main(int argc, char* argv[])
 
     else
     { 
+      cout << "Starting pair with im2_rgb: " << data_dir + rgb_path << endl;
       // Detect lines, match lines between frames, sample 2d line, reproject to 3d, remove outliers
       FramePair* fpair = new FramePair(previous_rgb, previous_depth, rgb_img, depth_img);
       // pairs.push_back(fpair);
+
+      cout << "Frame pair has been successfully created" << endl;
 
       utils::DrawSampledLines2D(fpair->rgb_image1, fpair->sampled_lines_2d_im1);
-      utils::DrawSampledLines2D(fpair->depth_image1, fpair->sampled_lines_2d_im1);
+      // utils::DrawSampledLines2D(fpair->depth_image1, fpair->sampled_lines_2d_im1);
       utils::DisplayImage(fpair->rgb_image1);
+
+      cout << "Have asked the 2d sampled drawing function" << endl;
+
+      // Update system poses
+      cv::Mat current_camera_pose = utils::MakeCameraPose(fpair->R_optim, fpair->t_optim);
+      if(fpair->t_optim.lpNorm<Eigen::Infinity>() > 0.4) continue;
+      cv::Mat current_global_pose = current_camera_pose*robline->global_poses.back();
+
+      // Lock viewer mtx
+      std::unique_lock<std::mutex> curPoseLock(robline_viewer->viewerPosesMtx);
+      robline->relative_poses.push_back(current_camera_pose);
+      robline->global_poses.push_back(current_global_pose);
+      curPoseLock.unlock();
+      // Unlock viewer mtx
+
       // Update the current vieweing frame
       robline_viewer->updateCurrentFrame(fpair);
 
-      cv::waitKey(10);
+      cout << "Updated the viewer content!" << endl;
+
+      cv::waitKey(0);
 
       // Update previous frame
       previous_rgb = rgb_img;
       previous_depth = depth_img;
+      cout << "updated prev images" << endl;
+
+      // generate required eigen data for saving output
+      Eigen::Matrix4d output_Rt;
+      cv::cv2eigen(current_global_pose, output_Rt);
+      cout << output_Rt << endl;
+      cout << current_global_pose << endl;
+      Eigen::Quaterniond q(output_Rt.block<3,3>(0,0).transpose());
+
+      // write to output file
+      outfile << rgb_time << " " << output_Rt.block<1,3>(3,0) << " " << 
+              q.x() << " " << q.y() << " " << q.z() << " " << q.w() << std::endl;
     }
   }
 
+  // close the output file
+  outfile.close();
+
   // close file
   infile.close();