use EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION and use same CXX_FLAGS for…

… rpg_svo rpg_vikit

svo/CMakeLists.txt

@@ -30,6 +30,7 @@ IF(DEBUG_OUTPUT)
 ENDIF()
 
 # Set build flags, set ARM_ARCHITECTURE environment variable on Odroid
+# Set build flags. Set IS_ARM on odroid board as environment variable
 SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
 IF(DEFINED ENV{ARM_ARCHITECTURE})
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")

svo/include/svo/depth_filter.h

@@ -116,10 +116,10 @@ class DepthFilter
   /// Can be used to compute the Next-Best-View in parallel.
   /// IMPORTANT! Make sure you hold a valid reference counting pointer to frame
   /// so it is not being deleted while you use it.
-  void getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds);
+  void getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds);
 
   /// Return a reference to the seeds. This is NOT THREAD SAFE!
-  std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }
+  std::list<Seed>& getSeeds() { return seeds_; }
 
   /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
   static void updateSeed(
@@ -137,7 +137,7 @@ class DepthFilter
 protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
-  std::list<Seed, aligned_allocator<Seed> > seeds_;
+  std::list<Seed> seeds_;
   boost::mutex seeds_mut_;
   bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
   boost::thread* thread_;

svo/include/svo/global.h

@@ -25,14 +25,16 @@
 #include <math.h>
 
 #include <Eigen/Core>
-template <class T, class Allocator =  Eigen::aligned_allocator<std::pair<const int,T>>   >
-using vectorA = std::vector< T, Allocator > ;
-template <class T, class Allocator =  Eigen::aligned_allocator<std::pair<const int,T>>   >
-using listA = std::list< T, Allocator > ;
 #include <opencv2/opencv.hpp>
 #include <sophus/se3.h>
 #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>

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.
-  vectorA<Vector3d> f_ref_;          //!< bearing vectors corresponding to the keypoints in the reference image.
-  vectorA<Vector3d> f_cur_;          //!< bearing vectors corresponding to the keypoints in the current image.
+  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<double> disparities_;      //!< disparity between first and second frame.
   vector<int> inliers_;             //!< inliers after the geometric check (e.g., Homography).
-  vectorA<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
+  vector<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,
-    vectorA<Vector3d>& f_vec);
+    vector<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,
-    vectorA<Vector3d>& f_ref,
-    vectorA<Vector3d>& f_cur,
+    vector<Vector3d>& f_ref,
+    vector<Vector3d>& f_cur,
     vector<double>& disparities);
 
 void computeHomography(
-    const vectorA<Vector3d>& f_ref,
-    const vectorA<Vector3d>& f_cur,
+    const vector<Vector3d>& f_ref,
+    const vector<Vector3d>& f_cur,
     double focal_length,
     double reprojection_threshold,
     vector<int>& inliers,
-    vectorA<Vector3d>& xyz_in_cur,
+    vector<Vector3d>& xyz_in_cur,
     SE3& T_cur_from_ref);
 
 } // namespace initialization

svo/src/depth_filter.cpp

@@ -135,7 +135,7 @@ void DepthFilter::removeKeyframe(FramePtr frame)
 {
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_);
-  std::list<Seed, aligned_allocator<Seed> >::iterator it=seeds_.begin();
+  std::list<Seed>::iterator it=seeds_.begin();
   size_t n_removed = 0;
   while(it!=seeds_.end())
   {
@@ -200,7 +200,7 @@ void DepthFilter::updateSeeds(FramePtr frame)
   // for all the seeds in every frame!
   size_t n_updates=0, n_failed_matches=0, n_seeds = seeds_.size();
   lock_t lock(seeds_mut_);
-  std::list<Seed, aligned_allocator<Seed> >::iterator it=seeds_.begin();
+  std::list<Seed>::iterator it=seeds_.begin();
 
   const double focal_length = frame->cam_->errorMultiplier2();
   double px_noise = 1.0;
@@ -296,10 +296,10 @@ void DepthFilter::clearFrameQueue()
     frame_queue_.pop();
 }
 
-void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds)
+void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
 {
   lock_t lock(seeds_mut_);
-  for(std::list<Seed, aligned_allocator<Seed> >::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
+  for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
   {
     if (it->ftr->frame == frame.get())
       seeds.push_back(*it);

svo/src/initialization.cpp

@@ -107,7 +107,7 @@ void KltHomographyInit::reset()
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vectorA<Vector3d>& f_vec)
+    vector<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,
-    vectorA<Vector3d>& f_ref,
-    vectorA<Vector3d>& f_cur,
+    vector<Vector3d>& f_ref,
+    vector<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();
-  vectorA<Vector3d>::iterator f_ref_it = f_ref.begin();
+  vector<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 vectorA<Vector3d>& f_ref,
-    const vectorA<Vector3d>& f_cur,
+    const vector<Vector3d>& f_ref,
+    const vector<Vector3d>& f_cur,
     double focal_length,
     double reprojection_threshold,
     vector<int>& inliers,
-    vectorA<Vector3d>& xyz_in_cur,
+    vector<Vector3d>& xyz_in_cur,
     SE3& T_cur_from_ref)
 {
-  vectorA<Vector2d > uv_ref(f_ref.size());
-  vectorA<Vector2d > uv_cur(f_cur.size());
+  vector<Vector2d > uv_ref(f_ref.size());
+  vector<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_ros/CMakeLists.txt

@@ -5,12 +5,19 @@ SET(CMAKE_BUILD_TYPE Release) # Release, RelWithDebInfo
 SET(CMAKE_VERBOSE_MAKEFILE OFF)
 SET(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/CMakeModules/")
 
+# Set build flags. Set IS_ARM on odroid board as environment variable
+SET(CMAKE_CXX_FLAGS "-Wall -D_LINUX -D_REENTRANT -march=native -Wno-unused-variable -Wno-unused-but-set-variable -Wno-unknown-pragmas")
+IF(DEFINED ENV{ARM_ARCHITECTURE})
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mfpu=neon -march=armv7-a")
+ELSE()
+  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -mmmx -msse -msse -msse2 -msse3 -mssse3")
+ENDIF()
 IF(CMAKE_COMPILER_IS_GNUCC)
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
 ELSE()
   SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
 ENDIF()
-
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS} -O3 -fsee -fomit-frame-pointer -fno-signed-zeros -fno-math-errno -funroll-loops")
 
 # Add catkin and required ROS packages
 FIND_PACKAGE(catkin REQUIRED COMPONENTS