Merge pull request ethz-asl#119 from nicolov/nicolov-fix-segfaults

Fix segfaults on Ubuntu 16.04
clssucceed · Jun 19, 2017 · f93f369 · f93f369
2 parents 19f4414 + 46730e8
commit f93f369
Show file tree

Hide file tree

Showing 10 changed files with 21 additions and 15 deletions.
diff --git a/include/rovio/FeatureCoordinates.hpp b/include/rovio/FeatureCoordinates.hpp
@@ -313,6 +313,10 @@ class FeatureCoordinates{
   float getDepthUncertaintyTau(const V3D& C1rC1C2, const float d, const float px_error_angle);
 };
 
+// Convenience type to help deal with alignment issues
+// https://eigen.tuxfamily.org/dox-devel/group__TopicStlContainers.html
+using FeatureCoordinatesVec = std::vector<FeatureCoordinates, Eigen::aligned_allocator<FeatureCoordinates>>;
+
 }
 
 

diff --git a/include/rovio/FeatureManager.hpp b/include/rovio/FeatureManager.hpp
@@ -281,7 +281,7 @@ class FeatureSetManager{
   // @todo work more on bearing vectors (in general)
   // @todo add corner motion dependency
   // @todo check inFrame, only if COVARIANCE not too large
-  std::unordered_set<unsigned int> addBestCandidates(const std::vector<FeatureCoordinates>& candidates, const ImagePyramid<nLevels>& pyr, const int camID, const double initTime,
+  std::unordered_set<unsigned int> addBestCandidates(const FeatureCoordinatesVec& candidates, const ImagePyramid<nLevels>& pyr, const int camID, const double initTime,
                                                      const int l1, const int l2, const int maxAddedFeature, const int nDetectionBuckets, const double scoreDetectionExponent,
                                                      const double penaltyDistance, const double zeroDistancePenalty, const bool requireMax, const float minScore){
     std::unordered_set<unsigned int> newFeatureIDs;

diff --git a/include/rovio/ImagePyramid.hpp b/include/rovio/ImagePyramid.hpp
@@ -113,8 +113,10 @@ class ImagePyramid{
    * @param l2         - Output pyramid level.
    * @return the corresponding pixel coordinates on pyramid level l2.
    */
-  void levelTranformCoordinates(const FeatureCoordinates& cIn,FeatureCoordinates& cOut,const int l1, const int l2) const{
+  FeatureCoordinates levelTranformCoordinates(const FeatureCoordinates& cIn, const int l1, const int l2) const{
     assert(l1<n_levels && l2<n_levels && l1>=0 && l2>=0);
+
+    FeatureCoordinates cOut;
     cOut.set_c((centers_[l1]-centers_[l2])*pow(0.5,l2)+cIn.get_c()*pow(0.5,l2-l1));
     if(cIn.mpCamera_ != nullptr){
       if(cIn.com_warp_c()){
@@ -123,6 +125,7 @@ class ImagePyramid{
     }
     cOut.camID_ = -1;
     cOut.mpCamera_ = nullptr;
+    return cOut;
   }
 
   /** \brief Extract FastCorner coordinates
@@ -132,7 +135,7 @@ class ImagePyramid{
    * @param detectionThreshold - Detection threshold of the used cv::FastFeatureDetector.
    *                             See http://docs.opencv.org/trunk/df/d74/classcv_1_1FastFeatureDetector.html
    */
-  void detectFastCorners(std::vector<FeatureCoordinates>& candidates, int l, int detectionThreshold) const{
+  void detectFastCorners(FeatureCoordinatesVec & candidates, int l, int detectionThreshold) const{
     std::vector<cv::KeyPoint> keypoints;
 #if (CV_MAJOR_VERSION < 3)
     cv::FastFeatureDetector feature_detector_fast(detectionThreshold, true);
@@ -141,11 +144,10 @@ class ImagePyramid{
     auto feature_detector_fast = cv::FastFeatureDetector::create(detectionThreshold, true);
     feature_detector_fast->detect(imgs_[l], keypoints);
 #endif
-    FeatureCoordinates c;
     candidates.reserve(candidates.size()+keypoints.size());
     for (auto it = keypoints.cbegin(), end = keypoints.cend(); it != end; ++it) {
-      levelTranformCoordinates(FeatureCoordinates(cv::Point2f(it->pt.x, it->pt.y)),c,l,0);
-      candidates.push_back(c);
+      candidates.push_back(
+              levelTranformCoordinates(FeatureCoordinates(cv::Point2f(it->pt.x, it->pt.y)),l,0));
     }
   }
 };

diff --git a/include/rovio/ImgUpdate.hpp b/include/rovio/ImgUpdate.hpp
@@ -243,7 +243,7 @@ ImgOutlierDetection<typename FILTERSTATE::mtState>,false>{
   mutable MultilevelPatch<mtState::nLevels_,mtState::patchSize_> mlpTemp2_;
   mutable FeatureCoordinates alignedCoordinates_;
   mutable FeatureCoordinates tempCoordinates_;
-  mutable std::vector<FeatureCoordinates> candidates_;
+  mutable FeatureCoordinatesVec candidates_;
   mutable cv::Point2f c_temp_;
   mutable Eigen::Matrix2d c_J_;
   mutable Eigen::Matrix2d A_red_;

diff --git a/include/rovio/MultilevelPatch.hpp b/include/rovio/MultilevelPatch.hpp
@@ -189,8 +189,7 @@ class MultilevelPatch{
    */
   static bool isMultilevelPatchInFrame(const ImagePyramid<nLevels>& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const bool withBorder = false){
     if(!c.isInFront() || !c.com_warp_c()) return false;
-    FeatureCoordinates coorTemp;
-    pyr.levelTranformCoordinates(c,coorTemp,0,l);
+    const auto coorTemp = pyr.levelTranformCoordinates(c,0,l);
     return Patch<patchSize>::isPatchInFrame(pyr.imgs_[l],coorTemp,withBorder);
   }
 
@@ -204,8 +203,7 @@ class MultilevelPatch{
    */
   void extractMultilevelPatchFromImage(const ImagePyramid<nLevels>& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const bool withBorder = false){
     for(unsigned int i=0;i<=l;i++){
-      FeatureCoordinates coorTemp;
-      pyr.levelTranformCoordinates(c,coorTemp,0,i);
+      const auto coorTemp = pyr.levelTranformCoordinates(c,0,i);
       isValidPatch_[i] = true;
       patches_[i].extractPatchFromImage(pyr.imgs_[i],coorTemp,withBorder);
     }

diff --git a/include/rovio/MultilevelPatchAlignment.hpp b/include/rovio/MultilevelPatchAlignment.hpp
@@ -119,7 +119,6 @@ class MultilevelPatchAlignment {
     }
     affInv = c.get_warp_c().inverse();
     int numLevel = 0;
-    FeatureCoordinates c_level;
     const int halfpatch_size = patch_size/2;
     float wTot = 0;
     float mean_x = 0;
@@ -136,7 +135,7 @@ class MultilevelPatchAlignment {
       mlpError_.isValidPatch_[l] = false;
     }
     for(int l = l1; l <= l2; l++){
-      pyr.levelTranformCoordinates(c,c_level,0,l);
+      const auto c_level = pyr.levelTranformCoordinates(c,0,l);
       if(mp.isValidPatch_[l] && extractedPatches_[l].isPatchInFrame(pyr.imgs_[l],c_level,false)){
         mp.patches_[l].computeGradientParameters();
         if(mp.patches_[l].validGradientParameters_){

diff --git a/include/rovio/featureTracker.hpp b/include/rovio/featureTracker.hpp
@@ -228,7 +228,7 @@ class FeatureTrackerNode{
 
     // Get new features, if there are too little valid MultilevelPatchFeature%s in the MultilevelPatchSet.
     if(fsm_.getValidCount() < min_feature_count_){
-      std::vector<FeatureCoordinates> candidates;
+      FeatureCoordinatesVec candidates;
       ROS_INFO_STREAM(" Adding keypoints");
       const double t1 = (double) cv::getTickCount();
       for(int l=l1;l<=l2;l++){

diff --git a/lightweight_filtering b/lightweight_filtering
diff --git a/src/rovio_node.cpp b/src/rovio_node.cpp
@@ -29,6 +29,8 @@
 
 #include <memory>
 
+#include <Eigen/StdVector>
+
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wunused-parameter"
 #include <ros/ros.h>

diff --git a/src/rovio_rosbag_loader.cpp b/src/rovio_rosbag_loader.cpp
@@ -33,6 +33,7 @@
 #include <iostream>
 #include <locale>
 #include <string>
+#include <Eigen/StdVector>
 #include "rovio/RovioFilter.hpp"
 #include "rovio/RovioNode.hpp"
 #include <boost/foreach.hpp>