Merge pull request #193 from akirayou/for_pull_eigenalignfix

Eigen alignment fix

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

@@ -119,7 +119,7 @@ public:
   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 @@ public:
 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

@@ -29,6 +29,12 @@
 #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/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>::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>::iterator it=seeds_.begin();
 
   const double focal_length = frame->cam_->errorMultiplier2();
   double px_noise = 1.0;

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

@@ -177,8 +177,8 @@ void computeHomography(
     vector<Vector3d>& xyz_in_cur,
     SE3& T_cur_from_ref)
 {
-  vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
+  vector<Vector2d > uv_ref(f_ref.size());
-  vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.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 

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>