Merge pull request uzh-rpg#2 from Drahflow/master

Fixes for Sophus and eigen

svo/include/svo/frame.h

@@ -46,7 +46,7 @@ class Frame : boost::noncopyable
   int                           id_;                    //!< Unique id of the frame.
   double                        timestamp_;             //!< Timestamp of when the image was recorded.
   vk::AbstractCamera*           cam_;                   //!< Camera model.
-  Sophus::SE3                   T_f_w_;                 //!< Transform (f)rame from (w)orld.
+  SE3                           T_f_w_;                 //!< Transform (f)rame from (w)orld.
   Matrix<double, 6, 6>          Cov_;                   //!< Covariance.
   ImgPyr                        img_pyr_;               //!< Image Pyramid.
   Features                      fts_;                   //!< List of features in the image.

svo/include/svo/global.h

@@ -24,17 +24,14 @@
 #include <stdio.h>
 #include <math.h>
 
+#include <Eigen/Cholesky>
+#include <Eigen/LU>
 #include <Eigen/Core>
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.hpp>
 #include <vikit/performance_monitor.h>
 #include <boost/shared_ptr.hpp>
 #include<Eigen/StdVector>
-#ifndef RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED //Guard for rpg_vikit
-#define RPG_SVO_VIKIT_IS_VECTOR_SPECIALIZED
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector3d)
-EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
-#endif
 
 #ifdef SVO_USE_ROS
   #include <ros/console.h>
@@ -71,6 +68,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 
 namespace svo
 {
+  using namespace std;
   using namespace Eigen;
   using namespace Sophus;
 
@@ -96,6 +94,7 @@ namespace svo
 
   class Frame;
   typedef boost::shared_ptr<Frame> FramePtr;
+  typedef Sophus::SE3d SE3;
 } // namespace svo
 
 #endif // SVO_GLOBAL_H_

svo/include/svo/initialization.h

@@ -44,37 +44,37 @@ class KltHomographyInit {
 protected:
   vector<cv::Point2f> px_ref_;      //!< keypoints to be tracked in reference frame.
   vector<cv::Point2f> px_cur_;      //!< tracked keypoints in current frame.
-  vector<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
-  vector<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
+  vector<Vector3d, aligned_allocator<Vector3d> > f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
+  vector<Vector3d, aligned_allocator<Vector3d> > f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
   vector<double> disparities_;      //!< disparity between first and second frame.
   vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
-  vector<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
+  vector<Vector3d, aligned_allocator<Vector3d> > xyz_in_cur_;     //!< 3D points computed during the geometric check.
   SE3 T_cur_from_ref_;              //!< computed transformation between the first two frames.
 };
 
 /// Detect Fast corners in the image.
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec);
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_vec);
 
 /// Compute optical flow (Lucas Kanade) for selected keypoints.
 void trackKlt(
     FramePtr frame_ref,
     FramePtr frame_cur,
     vector<cv::Point2f>& px_ref,
     vector<cv::Point2f>& px_cur,
-    vector<Vector3d>& f_ref,
-    vector<Vector3d>& f_cur,
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_ref,
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_cur,
     vector<double>& disparities);
 
 void computeHomography(
-    const vector<Vector3d>& f_ref,
-    const vector<Vector3d>& f_cur,
+    const vector<Vector3d, aligned_allocator<Vector3d> >& f_ref,
+    const vector<Vector3d, aligned_allocator<Vector3d> >& f_cur,
     double focal_length,
     double reprojection_threshold,
     vector<int>& inliers,
-    vector<Vector3d>& xyz_in_cur,
+    vector<Vector3d, aligned_allocator<Vector3d> >& xyz_in_cur,
     SE3& T_cur_from_ref);
 
 } // namespace initialization

svo/src/initialization.cpp

@@ -107,7 +107,7 @@ void KltHomographyInit::reset()
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec)
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_vec)
 {
   Features new_features;
   feature_detection::FastDetector detector(
@@ -129,8 +129,8 @@ void trackKlt(
     FramePtr frame_cur,
     vector<cv::Point2f>& px_ref,
     vector<cv::Point2f>& px_cur,
-    vector<Vector3d>& f_ref,
-    vector<Vector3d>& f_cur,
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_ref,
+    vector<Vector3d, aligned_allocator<Vector3d> >& f_cur,
     vector<double>& disparities)
 {
   const double klt_win_size = 30.0;
@@ -148,7 +148,7 @@ void trackKlt(
 
   vector<cv::Point2f>::iterator px_ref_it = px_ref.begin();
   vector<cv::Point2f>::iterator px_cur_it = px_cur.begin();
-  vector<Vector3d>::iterator f_ref_it = f_ref.begin();
+  vector<Vector3d, aligned_allocator<Vector3d> >::iterator f_ref_it = f_ref.begin();
   f_cur.clear(); f_cur.reserve(px_cur.size());
   disparities.clear(); disparities.reserve(px_cur.size());
   for(size_t i=0; px_ref_it != px_ref.end(); ++i)
@@ -169,16 +169,16 @@ void trackKlt(
 }
 
 void computeHomography(
-    const vector<Vector3d>& f_ref,
-    const vector<Vector3d>& f_cur,
+    const vector<Vector3d, aligned_allocator<Vector3d> >& f_ref,
+    const vector<Vector3d, aligned_allocator<Vector3d> >& f_cur,
     double focal_length,
     double reprojection_threshold,
     vector<int>& inliers,
-    vector<Vector3d>& xyz_in_cur,
+    vector<Vector3d, aligned_allocator<Vector3d> >& xyz_in_cur,
     SE3& T_cur_from_ref)
 {
-  vector<Vector2d > uv_ref(f_ref.size());
-  vector<Vector2d > uv_cur(f_cur.size());
+  vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
+  vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
   for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
   {
     uv_ref[i] = vk::project2d(f_ref[i]);

svo/test/test_depth_filter.cpp

@@ -119,7 +119,7 @@ void DepthFilterTest::testReconstruction(
     cv::Mat img(cv::imread(img_name, 0));
     assert(!img.empty());
 
-    Sophus::SE3 T_w_f(it->q_, it->t_);
+    Sophus::SE3d T_w_f(it->q_, it->t_);
     if(i == 0)
     {
       // create reference frame and load ground truth depthmap

svo/test/test_matcher.cpp

@@ -53,8 +53,8 @@ class MatcherTest {
     Eigen::Vector3d t_w_cur(0.5673, 0.5641, 2.0000);
     Eigen::Quaterniond q_w_ref(0.0, 0.8227, 0.2149, 0.0);
     Eigen::Quaterniond q_w_cur(0.0, 0.8235, 0.2130, 0.0);
-    frame_ref_->T_f_w_ = Sophus::SE3(q_w_ref, t_w_ref).inverse();
-    frame_cur_->T_f_w_ = Sophus::SE3(q_w_cur, t_w_cur).inverse();
+    frame_ref_->T_f_w_ = Sophus::SE3d(q_w_ref, t_w_ref).inverse();
+    frame_cur_->T_f_w_ = Sophus::SE3d(q_w_cur, t_w_cur).inverse();
 
     // load ground-truth depth
     vk::blender_utils::loadBlenderDepthmap(dataset_dir+"/depth/frame_000002_0.depth", *cam_, depth_ref_);
@@ -152,7 +152,7 @@ void MatcherTest::testWarpAffine()
   svo::Matcher matcher;
   double depth = 1.0;
 
-  Sophus::SE3 T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse());
+  Sophus::SE3d T_cur_ref(frame_cur_->T_f_w_*frame_ref_->T_f_w_.inverse());
   Eigen::Vector2d px_cur(frame_cur_->cam_->world2cam(T_cur_ref*(ref_ftr_->f*depth)));
 
   // compute reference patch

svo/test/test_sparse_img_align.cpp

@@ -68,7 +68,7 @@ void SparseImgAlignTest::testSequence(
   std::vector<vk::blender_utils::file_format::ImageNameAndPose>::iterator iter = sequence.begin();
   std::list<double> translation_error;
 
-  Sophus::SE3 T_prev_w, T_prevgt_w;
+  Sophus::SE3d T_prev_w, T_prevgt_w;
   std::string trace_dir(svo::test_utils::getTraceDir());
   std::string trace_name(trace_dir + "/sparse_img_align_" + experiment_name + "_trans_estimate.txt");
   std::ofstream ofs(trace_name.c_str());
@@ -82,8 +82,8 @@ void SparseImgAlignTest::testSequence(
     assert(!img.empty());
 
     // load pose
-    Sophus::SE3 T_w_gt(iter->q_, iter->t_);
-    Sophus::SE3 T_gt_w = T_w_gt.inverse(); // ground-truth
+    Sophus::SE3d T_w_gt(iter->q_, iter->t_);
+    Sophus::SE3d T_gt_w = T_w_gt.inverse(); // ground-truth
 
     if(i==0)
     {
@@ -123,7 +123,7 @@ void SparseImgAlignTest::testSequence(
     img_align.run(frame_ref_, frame_cur_);
 
     // compute error
-    Sophus::SE3 T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse();
+    Sophus::SE3d T_f_gt = frame_cur_->T_f_w_ * T_gt_w.inverse();
     translation_error.push_back(T_f_gt.translation().norm());
     printf("[%3.i] time = %f ms \t |t| = %f \t translation error = %f\n",
            i, t.stop()*1000, (frame_ref_->T_f_w_*T_gt_w.inverse()).translation().norm(),