Eigen alignment fix

svo/include/svo/depth_filter.h

@@ -116,7 +116,7 @@ public:
   /// 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>& seeds);
+  void getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds);
 
   /// Return a reference to the seeds. This is NOT THREAD SAFE!
   std::list<Seed, aligned_allocator<Seed> >& getSeeds() { return seeds_; }

svo/include/svo/global.h

@@ -25,6 +25,10 @@
 #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>

svo/include/svo/initialization.h

@@ -44,11 +44,11 @@ public:
 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.
+  vectorA<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.
+  vectorA<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.
+  vectorA<Vector3d> xyz_in_cur_;     //!< 3D points computed during the geometric check.
   SE3 T_cur_from_ref_;              //!< computed transformation between the first two frames.
 };
 
@@ -56,7 +56,7 @@ protected:
 void detectFeatures(
     FramePtr frame,
     vector<cv::Point2f>& px_vec,
-    vector<Vector3d>& f_vec);
+    vectorA<Vector3d>& f_vec);
 
 /// Compute optical flow (Lucas Kanade) for selected keypoints.
 void trackKlt(
@@ -64,17 +64,17 @@ void trackKlt(
     FramePtr frame_cur,
     vector<cv::Point2f>& px_ref,
     vector<cv::Point2f>& px_cur,
-    vector<Vector3d>& f_ref,
+    vectorA<Vector3d>& f_ref,
-    vector<Vector3d>& f_cur,
+    vectorA<Vector3d>& f_cur,
     vector<double>& disparities);
 
 void computeHomography(
-    const vector<Vector3d>& f_ref,
+    const vectorA<Vector3d>& f_ref,
-    const vector<Vector3d>& f_cur,
+    const vectorA<Vector3d>& f_cur,
     double focal_length,
     double reprojection_threshold,
     vector<int>& inliers,
-    vector<Vector3d>& xyz_in_cur,
+    vectorA<Vector3d>& xyz_in_cur,
     SE3& T_cur_from_ref);
 
 } // namespace initialization

svo/include/svo/reprojector.h

@@ -72,7 +72,7 @@ private:
     Vector2d px;     //!< projected 2D pixel location.
     Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
   };
-  typedef std::list<Candidate, aligned_allocator<Candidate> > Cell;
+  typedef std::list<Candidate > Cell;
   typedef std::vector<Cell*> CandidateGrid;
 
   /// The grid stores a set of candidate matches. For every grid cell we try to find one match.

svo/src/depth_filter.cpp

@@ -135,7 +135,7 @@ void DepthFilter::removeKeyframe(FramePtr frame)
 {
   seeds_updating_halt_ = true;
   lock_t lock(seeds_mut_);
-  list<Seed>::iterator it=seeds_.begin();
+  std::list<Seed, aligned_allocator<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_);
-  list<Seed>::iterator it=seeds_.begin();
+  std::list<Seed, aligned_allocator<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>& seeds)
+void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed, aligned_allocator<Seed> >& seeds)
 {
   lock_t lock(seeds_mut_);
-  for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
+  for(std::list<Seed, aligned_allocator<Seed> >::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
   {
     if (it->ftr->frame == frame.get())
       seeds.push_back(*it);

svo/src/frame_handler_mono.cpp

@@ -84,7 +84,6 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp)
   // set last frame
   last_frame_ = new_frame_;
   new_frame_.reset();
-
   // finish processing
   finishFrameProcessingCommon(last_frame_->id_, res, last_frame_->nObs());
 }

svo/src/initialization.cpp

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

@@ -134,26 +134,25 @@ void Visualizer::publishMinimal(
                    cv::Point2f((*it)->px[0]+3*(*it)->grad[1], (*it)->px[1]-3*(*it)->grad[0]),
                    cv::Point2f((*it)->px[0]-3*(*it)->grad[1], (*it)->px[1]+3*(*it)->grad[0]),
                    cv::Scalar(255,0,255), 2);
-        else
+        else//point size 5x5
           cv::rectangle(img_rgb,
                         cv::Point2f((*it)->px[0]-2, (*it)->px[1]-2),
                         cv::Point2f((*it)->px[0]+2, (*it)->px[1]+2),
                         cv::Scalar(0,255,0), CV_FILLED);
       }
     }
-    if(img_pub_level_ == 1)
+    else if(img_pub_level_ == 1){//point size 3x3
       for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
         cv::rectangle(img_rgb,
                       cv::Point2f((*it)->px[0]/scale-1, (*it)->px[1]/scale-1),
                       cv::Point2f((*it)->px[0]/scale+1, (*it)->px[1]/scale+1),
                       cv::Scalar(0,255,0), CV_FILLED);
-    else
+    }else{ //point size 1x1
-      for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it)
+      for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it){
-        cv::rectangle(img_rgb,
+	cv::Vec3b &p=  img_rgb.at<cv::Vec3b>((*it)->px[1]/scale, (*it)->px[0]/scale);
-                      cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
+	p[0]=0;p[1]=255;p[2]=0;
-                      cv::Point2f((*it)->px[0]/scale, (*it)->px[1]/scale),
+      }
-                      cv::Scalar(0,255,0), CV_FILLED);
+    }
-
     cv_bridge::CvImage img_msg;
     img_msg.header = header_msg;
     img_msg.image = img_rgb;

svo_ros/src/vo_node.cpp

@@ -13,7 +13,6 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
 #include <ros/package.h>
 #include <string>
 #include <svo/frame_handler_mono.h>