change pointer into shared_ptr

GPU_version/vio/include/vio/cl_class.h

@@ -51,6 +51,7 @@ public:
 
     template<typename T>
     int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){
+        assert(buf);
         for(auto&& i:_buffers)if(i.second==id){
                 cl_int error;
                 cl::Event event;

GPU_version/vio/include/vio/depth_filter.h

@@ -39,14 +39,14 @@ struct Seed
   static int seed_counter;
   int batch_id;                //!< Batch id is the id of the keyframe for which the seed was created.
   int id;                      //!< Seed ID, only used for visualization.
-  Feature* ftr;                //!< Feature in the keyframe for which the depth should be computed.
+  std::shared_ptr<Feature> ftr;                //!< Feature in the keyframe for which the depth should be computed.
   float a;                     //!< a of Beta distribution: When high, probability of inlier is large.
   float b;                     //!< b of Beta distribution: When high, probability of outlier is large.
   float mu;                    //!< Mean of normal distribution.
   float z_range;               //!< Max range of the possible depth.
   float sigma2;                //!< Variance of normal distribution.
   Matrix2d patch_cov;          //!< Patch covariance in reference image.
-  Seed(Feature* ftr, float depth_mean, float depth_min);
+  Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min);
 };
 
 /// Depth filter implements the Bayesian Update proposed in:
@@ -61,7 +61,7 @@ public:
   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
   typedef boost::unique_lock<boost::mutex> lock_t;
-  typedef boost::function<void ( Point*, double )> callback_t;
+  typedef boost::function<void ( std::shared_ptr<Point>, double )> callback_t;
 
   /// Depth-filter config parameters
   struct Options
@@ -115,17 +115,17 @@ 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<std::shared_ptr<Seed>>& seeds);
 
   /// Return a reference to the seeds. This is NOT THREAD SAFE!
   //std::list<Seed>& getSeeds() { return seeds_; } Bug 238
-  std::list<Seed, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
+  std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>>& getSeeds() { return seeds_; }
 
   /// Bayes update of the seed, x is the measurement, tau2 the measurement uncertainty
   static bool updateSeed(
       const float x,
       const float tau2,
-      Seed* seed);
+      std::shared_ptr<Seed> seed);
 
   /// Compute the uncertainty of the measurement.
   static double computeTau(
@@ -137,7 +137,7 @@ public:
 protected:
   feature_detection::DetectorPtr feature_detector_;
   callback_t seed_converged_cb_;
-  std::list<Seed, Eigen::aligned_allocator<Seed>> seeds_;
+  std::list<std::shared_ptr<Seed>, Eigen::aligned_allocator<Seed>> seeds_;
   boost::mutex seeds_mut_;
   bool seeds_updating_halt_;            //!< Set this value to true when seeds updating should be interrupted.
   boost::thread* thread_;

GPU_version/vio/include/vio/feature.h

@@ -32,34 +32,32 @@ struct Feature
   };
 
   FeatureType type;     //!< Type can be corner or edgelet.
-  Frame* frame;         //!< Pointer to frame in which the feature was detected.
+  std::shared_ptr<Frame> frame;         //!< Pointer to frame in which the feature was detected.
   Vector2d px;          //!< Coordinates in pixels on pyramid level 0.
   Vector3d f;           //!< Unit-bearing vector of the feature.
   int level;            //!< Image pyramid level where feature was extracted.
-  Point* point;         //!< Pointer to 3D point which corresponds to the feature.
+  std::shared_ptr<Point> point;         //!< Pointer to 3D point which corresponds to the feature.
   Vector2d grad;        //!< Dominant gradient direction for edglets, normalized.
 
-  Feature(Frame* _frame, const Vector2d& _px, int _level) :
+  Feature(std::shared_ptr<Frame> _frame, const Vector2d& _px, int _level) :
     type(CORNER),
     frame(_frame),
     px(_px),
     f(frame->cam_->cam2world(px)),
     level(_level),
-    point(NULL),
     grad(1.0,0.0)
   {}
 
-  Feature(Frame* _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
+  Feature(std::shared_ptr<Frame> _frame, const Vector2d& _px, const Vector3d& _f, int _level) :
     type(CORNER),
     frame(_frame),
     px(_px),
     f(_f),
     level(_level),
-    point(NULL),
     grad(1.0,0.0)
   {}
 
-  Feature(Frame* _frame, Point* _point, const Vector2d& _px, const Vector3d& _f, int _level) :
+  Feature(std::shared_ptr<Frame> _frame, std::shared_ptr<Point> _point, const Vector2d& _px, const Vector3d& _f, int _level) :
     type(CORNER),
     frame(_frame),
     px(_px),

GPU_version/vio/include/vio/feature_detection.h

@@ -57,7 +57,7 @@ public:
   };
 
   virtual void detect(
-      Frame* frame,
+          std::shared_ptr<Frame> frame,
       const ImgPyr& img_pyr,
       const double detection_threshold,
       Features& fts,
@@ -101,7 +101,7 @@ public:
   virtual ~FastDetector() {}
 
   virtual void detect(
-      Frame* frame,
+      std::shared_ptr<Frame> frame,
       const ImgPyr& img_pyr,
       const double detection_threshold,
       Features& fts,

GPU_version/vio/include/vio/frame.h

@@ -33,7 +33,7 @@ namespace vio {
     class Point;
     struct Feature;
 
-    typedef list<Feature*> Features;
+    typedef list<std::shared_ptr<Feature>> Features;
     typedef vector<cv::Mat> ImgPyr;
 
 
@@ -51,7 +51,7 @@ namespace vio {
         Matrix<double, 3, 3>          Cov_;                   //!< Covariance.
         ImgPyr                        img_pyr_;               //!< Image Pyramid.
         Features                      fts_;                   //!< List of features in the image.
-        vector<Feature*>              key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
+        vector<std::shared_ptr<Feature>>  key_pts_;               //!< Five features and associated 3D points which are used to detect if two frames have overlapping field of view.
         bool                          is_keyframe_;           //!< Was this frames selected as keyframe?
         g2oFrameSE3*                  v_kf_;                  //!< Temporary pointer to the g2o node object of the keyframe.
         int                           last_published_ts_;     //!< Timestamp of last publishing.
@@ -66,7 +66,7 @@ namespace vio {
         void setKeyframe();
 
         /// Add a feature to the image
-        void addFeature(Feature* ftr);
+        void addFeature(std::shared_ptr<Feature> ftr);
 
         /// The KeyPoints are those five features which are closest to the 4 image corners
         /// and to the center and which have a 3D point assigned. These points are used
@@ -74,10 +74,10 @@ namespace vio {
         void setKeyPoints();
 
         /// Check if we can select five better key-points.
-        void checkKeyPoints(Feature* ftr);
+        void checkKeyPoints(std::shared_ptr<Feature> ftr);
 
         /// If a point is deleted, we must remove the corresponding key-point.
-        void removeKeyPoint(Feature* ftr);
+        void removeKeyPoint(std::shared_ptr<Feature> ftr);
 
         /// Return number of point observations.
         inline size_t nObs() const { return fts_.size(); }

GPU_version/vio/include/vio/global.h

@@ -45,6 +45,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d)
 
 namespace vio
 {
+
     using namespace Eigen;
     using namespace Sophus;
 
@@ -128,8 +129,6 @@ namespace vio
             SE3 tem=SE3(q.toRotationMatrix(),Vector3d(T2_->translation()(0), 0.0,T2_->translation()(1)));
             return tem;
         }
-        ~SE2_5(){
-        }
         bool empty() const{
             return T2_ == nullptr ? true : false;
         }
@@ -139,7 +138,7 @@ namespace vio
     };
 
     class Frame;
-    typedef boost::shared_ptr<Frame> FramePtr;
+    typedef std::shared_ptr<Frame> FramePtr;
 } // namespace vio
 
 #endif // SVO_GLOBAL_H_

GPU_version/vio/include/vio/map.h

@@ -32,7 +32,7 @@ class Seed;
 class MapPointCandidates
 {
 public:
-  typedef pair<Point*, Feature*> PointCandidate;
+  typedef pair<std::shared_ptr<Point>, std::shared_ptr<Feature>> PointCandidate;
   typedef list<PointCandidate> PointCandidateList;
 
   /// The depth-filter is running in a parallel thread and fills the canidate list.
@@ -42,19 +42,19 @@ public:
   /// Candidate points are created from converged seeds.
   /// Until the next keyframe, these points can be used for reprojection and pose optimization.
   PointCandidateList candidates_;
-  list< Point* > trash_points_;
+  list< std::shared_ptr<Point> > trash_points_;
 
   MapPointCandidates();
   ~MapPointCandidates();
 
   /// Add a candidate point.
-  void newCandidatePoint(Point* point, double depth_sigma2);
+  void newCandidatePoint(std::shared_ptr<Point>  point, double depth_sigma2);
 
   /// Adds the feature to the frame and deletes candidate from list.
   void addCandidatePointToFrame(FramePtr frame);
 
   /// Remove a candidate point from the list of candidates.
-  bool deleteCandidatePoint(Point* point);
+  bool deleteCandidatePoint(std::shared_ptr<Point> point);
 
   /// Remove all candidates that belong to a frame.
   void removeFrameCandidates(FramePtr frame);
@@ -72,7 +72,7 @@ class Map : boost::noncopyable
 {
 public:
   list< FramePtr > keyframes_;          //!< List of keyframes in the map.
-  list< Point* > trash_points_;         //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
+  list< std::shared_ptr<Point> > trash_points_;         //!< A deleted point is moved to the trash bin. Now and then this is cleaned. One reason is that the visualizer must remove the points also.
   MapPointCandidates point_candidates_;
 
 
@@ -83,16 +83,16 @@ public:
   void reset();
 
   /// Delete a point in the map and remove all references in keyframes to it.
-  void safeDeletePoint(Point* pt);
+  void safeDeletePoint(std::shared_ptr<Point> pt);
 
   /// Moves the point to the trash queue which is cleaned now and then.
-  void deletePoint(Point* pt);
+  void deletePoint(std::shared_ptr<Point> pt);
 
   /// Moves the frame to the trash queue which is cleaned now and then.
   bool safeDeleteFrame(FramePtr frame);
 
   /// Remove the references between a point and a frame.
-  void removePtFrameRef(Frame* frame, Feature* ftr);
+  void removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr);
 
   /// Add a new keyframe to the map.
   void addKeyframe(FramePtr new_keyframe);
@@ -128,8 +128,8 @@ namespace map_debug {
 
 void mapStatistics(Map* map);
 void mapValidation(Map* map, int id);
-void frameValidation(Frame* frame, int id);
+void frameValidation(FramePtr frame, int id);
-void pointValidation(Point* point, int id);
+void pointValidation(std::shared_ptr<Point> point, int id);
 
 } // namespace map_debug
 } // namespace vio

GPU_version/vio/include/vio/matcher.h

@@ -99,7 +99,7 @@ public:
   double h_inv_;                //!< hessian of 1d image alignment along epipolar line
   int search_level_;
   bool reject_;
-  Feature* ref_ftr_;
+  std::shared_ptr<Feature> ref_ftr_;
   Vector2d px_cur_;
 
   Matcher() = default;

GPU_version/vio/include/vio/point.h

@@ -53,7 +53,7 @@ public:
   Vector3d                    normal_;                  //!< Surface normal at point.
   Matrix3d                    normal_information_;      //!< Inverse covariance matrix of normal estimation.
   bool                        normal_set_;              //!< Flag whether the surface normal was estimated or not.
-  list<Feature*>              obs_;                     //!< References to keyframes which observe the point.
+  list<std::shared_ptr<Feature>>              obs_;                     //!< References to keyframes which observe the point.
   size_t                      n_obs_;                   //!< Number of obervations: Keyframes AND successful reprojections in intermediate frames.
   g2oPoint*                   v_pt_;                    //!< Temporary pointer to the point-vertex in g2o during bundle adjustment.
   int                         last_published_ts_;       //!< Timestamp of last publishing.
@@ -64,23 +64,23 @@ public:
   int                         last_structure_optim_;    //!< Timestamp of last point optimization
 
   Point(const Vector3d& pos);
-  Point(const Vector3d& pos, Feature* ftr);
+  Point(const Vector3d& pos, std::shared_ptr<Feature> ftr);
   ~Point();
 
   /// Add a reference to a frame.
-  void addFrameRef(Feature* ftr);
+  void addFrameRef(std::shared_ptr<Feature> ftr);
 
   /// Remove reference to a frame.
-  bool deleteFrameRef(Frame* frame);
+  bool deleteFrameRef(FramePtr frame);
 
   /// Initialize point normal. The inital estimate will point towards the frame.
   void initNormal();
 
   /// Check whether mappoint has reference to a frame.
-  Feature* findFrameRef(Frame* frame);
+  std::shared_ptr<Feature> findFrameRef(FramePtr frame);
 
   /// Get Frame with similar viewpoint.
-  bool getCloseViewObs(const Vector2d& pos, Feature*& obs, int id=0) const;
+  bool getCloseViewObs(const Vector2d& pos, std::shared_ptr<Feature>& obs, int id=0) const;
 
   /// Get number of observations.
   inline size_t nRefs() const { return obs_.size(); }

GPU_version/vio/include/vio/reprojector.h

@@ -79,9 +79,9 @@ private:
   /// will search a maching feature in the image.
   struct Candidate {
     EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-    Point* pt;       //!< 3D point.
+    std::shared_ptr<Point> pt;       //!< 3D point.
     Vector2d px;     //!< projected 2D pixel location.
-    Candidate(Point* pt, Vector2d& px) : pt(pt), px(px) {}
+    Candidate(std::shared_ptr<Point> pt, Vector2d& px) : pt(pt), px(px) {}
   };
   typedef std::list<Candidate > Cell;
   typedef std::vector<Cell*> CandidateGrid;
@@ -104,7 +104,7 @@ private:
   void initializeGrid(vk::AbstractCamera* cam);
   void resetGrid();
   bool reprojectCell(Cell& cell, FramePtr frame);
-  bool reprojectPoint(FramePtr frame, Point* point);
+  bool reprojectPoint(FramePtr frame, std::shared_ptr<Point>  point);
 };
 
 } // namespace vio

GPU_version/vio/include/vio/visualizer.h

@@ -35,7 +35,7 @@ class Point;
 class Map;
 class FrameHandlerMono;
 
-typedef boost::shared_ptr<Frame> FramePtr;
+typedef std::shared_ptr<Frame> FramePtr;
 
 /// This class bundles all functions to publish visualisation messages.
 class Visualizer

GPU_version/vio/src/depth_filter.cpp

@@ -37,7 +37,7 @@ namespace vio {
     int Seed::batch_counter = 0;
     int Seed::seed_counter = 0;
 
-    Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
+    Seed::Seed(std::shared_ptr<Feature> ftr, float depth_mean, float depth_min) :
             batch_id(batch_counter),
             id(seed_counter++),
             ftr(ftr),
@@ -148,16 +148,14 @@ namespace vio {
         fprintf(log_,"[%s] init seeds\n",vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
         feature_detector_->setExistingFeatures(frame->fts_);
-        feature_detector_->detect(frame.get(), frame->img_pyr_,
+        feature_detector_->detect(frame, frame->img_pyr_,
                                   Config::triangMinCornerScore(), new_features);
 
         // initialize a seed for every new feature
         seeds_updating_halt_ = true;
         seeds_mut_.lock();
         ++Seed::batch_counter;
-        std::for_each(new_features.begin(), new_features.end(), [&](Feature* ftr){
+        for(auto&& ftr:new_features)seeds_.push_back(make_shared<Seed>(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
-            seeds_.push_back(Seed(ftr, new_keyframe_mean_depth_, new_keyframe_min_depth_));
-        });
 
 #if VIO_DEBUG
         fprintf(log_,"[%s] DepthFilter: Initialized %d new seeds\n",vio::time_in_HH_MM_SS_MMM().c_str(),new_features.size());
@@ -173,11 +171,11 @@ namespace vio {
         fprintf(log_,"[%s] remove key frame\n",vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
         seeds_mut_.lock();
-        std::list<Seed>::iterator it=seeds_.begin();
+        std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
         size_t n_removed = 0;
         while(it!=seeds_.end())
         {
-            if(it->ftr->frame->id_ == frame->id_)
+            if((*it)->ftr->frame->id_ == frame->id_)
             {
                 it = seeds_.erase(it);
                 ++n_removed;
@@ -240,7 +238,7 @@ namespace vio {
         // for all the seeds in every frame!
         size_t n_updates=0, n_failed_matches=0;
         lock_t lock(seeds_mut_);
-        std::list<Seed>::iterator it=seeds_.begin();
+        std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin();
 #if VIO_DEBUG
         fprintf(log_,"[%s] update seed\n",vio::time_in_HH_MM_SS_MMM().c_str());
 #endif
@@ -253,19 +251,19 @@ namespace vio {
             if(seeds_updating_halt_)
                 return;
 
-            if(it->ftr->frame == NULL){
+            if((*it)->ftr->frame == NULL){
                 it = seeds_.erase(it);
                 continue;
             }
             // check if point is visible in the current image
-            SE2 T(frame->T_f_w_.pitch()-it->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+it->ftr->frame->T_f_w_.inverse().translation());
+            SE2 T(frame->T_f_w_.pitch()-(*it)->ftr->frame->T_f_w_.pitch(),frame->T_f_w_.se2().translation()+(*it)->ftr->frame->T_f_w_.inverse().translation());
             ///TODO add 15 degrees roll orientation
             Quaterniond q;
             q = AngleAxisd(0.0, Vector3d::UnitX())
                 * AngleAxisd(atan2(T.so2().unit_complex().imag(),T.so2().unit_complex().real()), Vector3d::UnitY())
                 * AngleAxisd(0.0, Vector3d::UnitZ());
             SE3 T_cur_ref(q.toRotationMatrix(),Vector3d(T.translation()(0), 0.0,T.translation()(1)));
-            const Vector3d xyz_f(T_cur_ref*(1.0/it->mu * it->ftr->f) );
+            const Vector3d xyz_f(T_cur_ref*(1.0/(*it)->mu * (*it)->ftr->f) );
 #if VIO_DEBUG
             fprintf(log_,"[%s]  If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
 #endif
@@ -278,8 +276,8 @@ namespace vio {
                 continue;
             }
 
-            float z_min = it->mu + sqrt(it->sigma2);
+            float z_min = (*it)->mu + sqrt((*it)->sigma2);
-            float z_max = max(it->mu - sqrt(it->sigma2), 0.00000001f);
+            float z_max = max((*it)->mu - sqrt((*it)->sigma2), 0.00000001f);
 
             if(z_min < 0){
                 ++it;
@@ -288,18 +286,18 @@ namespace vio {
 
             double z;
             if(!matcher_.findEpipolarMatchDirect(
-                    *it->ftr->frame, *frame, *it->ftr, 1.0/it->mu, 1.0/z_min, 1.0/z_max, z))
+                    *(*it)->ftr->frame, *frame, *(*it)->ftr, 1.0/(*it)->mu, 1.0/z_min, 1.0/z_max, z))
             {
-                it->b++; // increase outlier probability when no match was found
+                (*it)->b++; // increase outlier probability when no match was found
                 ++it;
                 ++n_failed_matches;
                 continue;
             }
-            double tau = computeTau(T_cur_ref.inverse(), it->ftr->f, z, px_error_angle);
+            double tau = computeTau(T_cur_ref.inverse(), (*it)->ftr->f, z, px_error_angle);
             double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));
 
             // update the estimate
-            if(!updateSeed(1.0/z, tau_inverse*tau_inverse, &*it)){
+            if(!updateSeed(1.0/z, tau_inverse*tau_inverse, *it)){
                 ++it;
                 ++n_failed_matches;
                 continue;
@@ -313,13 +311,13 @@ namespace vio {
             }
 
             // if the seed has converged, we initialize a new candidate point and remove the seed
-            if(sqrt(it->sigma2) < it->z_range/options_.seed_convergence_sigma2_thresh)
+            if(sqrt((*it)->sigma2) < (*it)->z_range/options_.seed_convergence_sigma2_thresh)
             {
-                assert(it->ftr->point == NULL); // TODO this should not happen anymore
+                assert((*it)->ftr->point == NULL); // TODO this should not happen anymore
-                Vector3d xyz_world(it->ftr->frame->getSE3Inv() * (it->ftr->f/it->mu));
+                Vector3d xyz_world((*it)->ftr->frame->getSE3Inv() * ((*it)->ftr->f/(*it)->mu));
-                Point* point = new Point(xyz_world, it->ftr);
+                std::shared_ptr<Point> point = std::make_shared<Point>(xyz_world, (*it)->ftr);
-                it->ftr->point = point;
+                (*it)->ftr->point = point;
-                seed_converged_cb_(point, it->sigma2); // put in candidate list
+                seed_converged_cb_(point, (*it)->sigma2); // put in candidate list
                 it = seeds_.erase(it);
             }
             else if(isnan(z_min))
@@ -337,16 +335,16 @@ namespace vio {
             frame_queue_.pop();
     }
 
-    void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<Seed>& seeds)
+    void DepthFilter::getSeedsCopy(const FramePtr& frame, std::list<std::shared_ptr<Seed>>& seeds)
     {
         lock_t lock(seeds_mut_);
-        for(std::list<Seed>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
+        for(std::list<std::shared_ptr<Seed>>::iterator it=seeds_.begin(); it!=seeds_.end(); ++it)
         {
-            if (it->ftr->frame->id_ == frame->id_)
+            if ((*it)->ftr->frame->id_ == frame->id_)
                 seeds.push_back(*it);
         }
     }
-    bool DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)
+    bool DepthFilter::updateSeed(const float x, const float tau2, std::shared_ptr<Seed> seed)
     {
         float norm_scale = sqrt(seed->sigma2 + tau2);
         if(std::isnan(norm_scale))

GPU_version/vio/src/feature_detection.cpp

@@ -41,11 +41,11 @@ void AbstractDetector::resetGrid()
 
 void AbstractDetector::setExistingFeatures(const Features& fts)
 {
-  std::for_each(fts.begin(), fts.end(), [&](Feature* i){
+  for(auto&& i:fts){
     grid_occupancy_.at(
         static_cast<int>(i->px[1]/cell_size_)*grid_n_cols_
         + static_cast<int>(i->px[0]/cell_size_)) = true;
-  });
+  };
 }
 
 void AbstractDetector::setGridOccpuancy(const Vector2d& px)
@@ -66,7 +66,7 @@ FastDetector::FastDetector(
 }
 
 void FastDetector::detect(
-    Frame* frame,
+    std::shared_ptr<Frame> frame,
     const ImgPyr& img_pyr,
     const double detection_threshold,
     Features& fts,
@@ -102,7 +102,7 @@ void FastDetector::detect(
           corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
           // Create feature for every corner that has high enough corner score
           if(corners.at(k).score > detection_threshold){
-              fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
+              fts.push_back(make_shared<Feature>(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
               if(descriptors)keypoints.push_back(cv::KeyPoint(fast_corners[i].x, fast_corners[i].y, 1));
           }
       }

GPU_version/vio/src/frame.cpp

@@ -40,7 +40,7 @@ Frame::Frame(vk::AbstractCamera* cam, const cv::Mat& img, double timestamp) :
 
 Frame::~Frame()
 {
-  std::for_each(fts_.begin(), fts_.end(), [&](Feature* i){delete i;});
+  for(auto&& f:fts_)f.reset();
 }
 
 void Frame::initFrame(const cv::Mat& img)
@@ -50,7 +50,7 @@ void Frame::initFrame(const cv::Mat& img)
     throw std::runtime_error("Frame: provided image has not the same size as the camera model or image is not grayscale");
 
   // Set keypoints to NULL
-  std::for_each(key_pts_.begin(), key_pts_.end(), [&](Feature* ftr){ ftr=NULL; });
+  for(auto&& ftr:key_pts_)ftr.reset();
 
   // Build Image Pyramid
   frame_utils::createImgPyramid(img, max(Config::nPyrLevels(), Config::kltMaxLevel()+1), img_pyr_);
@@ -62,7 +62,7 @@ void Frame::setKeyframe()
   setKeyPoints();
 }
 
-void Frame::addFeature(Feature* ftr)
+void Frame::addFeature(std::shared_ptr<Feature> ftr)
 {
   fts_.push_back(ftr);
 }
@@ -73,11 +73,10 @@ void Frame::setKeyPoints()
     if(key_pts_[i] != NULL)
       if(key_pts_[i]->point == NULL)
         key_pts_[i] = NULL;
-
+  for(auto&& ftr:fts_)if(ftr->point != NULL) checkKeyPoints(ftr);
-  std::for_each(fts_.begin(), fts_.end(), [&](Feature* ftr){ if(ftr->point != NULL) checkKeyPoints(ftr); });
 }
 
-void Frame::checkKeyPoints(Feature* ftr)
+void Frame::checkKeyPoints(std::shared_ptr<Feature> ftr)
 {
   const int cu = cam_->width()/2;
   const int cv = cam_->height()/2;
@@ -123,7 +122,7 @@ void Frame::checkKeyPoints(Feature* ftr)
   }
 }
 
-void Frame::removeKeyPoint(Feature* ftr)
+void Frame::removeKeyPoint(std::shared_ptr<Feature> ftr)
 {
     bool found = false;
     if(key_pts_.size()!=5 ){

GPU_version/vio/src/frame_handler_base.cpp

@@ -123,16 +123,15 @@ void FrameHandlerBase::optimizeStructure(
     size_t max_n_pts,
     int max_iter)
 {
-  deque<Point*> pts;
+  deque<std::shared_ptr<Point>> pts;
   for(auto&& it:frame->fts_){
       if(it->point!=NULL)pts.push_back(it->point);
   }
   //max_n_pts = min(max_n_pts, pts.size());
   //nth_element(pts.begin(), pts.begin() + max_n_pts, pts.end(), ptLastOptimComparator);
-  for(deque<Point*>::iterator it=pts.begin(); it!=pts.begin(); ++it)
+  for(auto&& pt:pts){
-  {
+      pt->optimize(max_iter);
-    (*it)->optimize(max_iter);
+      pt->last_frame_overlap_id_= frame->id_;
-    (*it)->last_structure_optim_ = frame->id_;
   }
 }
 

GPU_version/vio/src/frame_handler_mono.cpp

@@ -81,8 +81,7 @@ void FrameHandlerMono::initialize()
   feature_detection::DetectorPtr feature_detector(
       new feature_detection::FastDetector(
           cam_->width(), cam_->height(), Config::gridSize(),gpu_fast_, Config::nPyrLevels()));
-  DepthFilter::callback_t depth_filter_cb = boost::bind(
+  DepthFilter::callback_t depth_filter_cb = boost::bind(&MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
-      &MapPointCandidates::newCandidatePoint, &map_.point_candidates_, _1, _2);
   depth_filter_ = new DepthFilter(feature_detector, depth_filter_cb);
   depth_filter_->startThread();
 }
@@ -103,7 +102,7 @@ void FrameHandlerMono::addImage(const cv::Mat& img, const double timestamp,const
   overlap_kfs_.clear();
 
   // create new frame
-  new_frame_=boost::make_shared<Frame>(cam_, img.clone(), timestamp);
+  new_frame_=std::make_shared<Frame>(cam_, img.clone(), timestamp);
   time_=time;
   // process frame
   UpdateResult res = RESULT_FAILURE;

GPU_version/vio/src/initialization.cpp

@@ -89,13 +89,13 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
         if(frame_cur->cam_->isInFrame(px_cur.cast<int>(), 10) && frame_ref_->cam_->isInFrame(px_ref.cast<int>(), 10))
         {
             Vector3d pos = frame_cur->getSE3Inv() * (xyz_in_cur_[*it]/* *scale */);
-            Point* new_point = new Point(pos);
+            std::shared_ptr<Point> new_point = std::make_shared<Point>(pos);
 
-            Feature* ftr_cur(new Feature(frame_cur.get(), new_point, px_cur, f_cur_[*it], 0));
+            std::shared_ptr<Feature> ftr_cur=std::make_shared<Feature>(frame_cur, new_point, px_cur, f_cur_[*it], 0);
             frame_cur->addFeature(ftr_cur);
             new_point->addFrameRef(ftr_cur);
 
-            Feature* ftr_ref(new Feature(frame_ref_.get(), new_point, px_ref, f_ref_[*it], 0));
+            std::shared_ptr<Feature> ftr_ref=std::make_shared<Feature>(frame_ref_, new_point, px_ref, f_ref_[*it], 0);
             frame_ref_->addFeature(ftr_ref);
             new_point->addFrameRef(ftr_ref);
         }
@@ -127,14 +127,14 @@ void detectFeatures(
   Features new_features;
   feature_detection::FastDetector detector(
       frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast,Config::nPyrLevels());
-  detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), new_features);
+  detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), new_features);
   // now for all maximum corners, initialize a new seed
   px_vec.clear();
   f_vec.clear();
   for(auto&& ftr:new_features){
       px_vec.push_back(cv::Point2f(ftr->px[0], ftr->px[1]));
       f_vec.push_back(ftr->f);
-      delete ftr;
+      ftr.reset();
   }
 }
 void trackKlt(

GPU_version/vio/src/map.cpp

@@ -20,6 +20,7 @@
 #include <vio/frame.h>
 #include <vio/feature.h>
 #include <boost/bind.hpp>
+#include <vio/for_it.hpp>
 
 namespace vio {
 
@@ -28,7 +29,6 @@ Map::Map() {}
 Map::~Map()
 {
   reset();
-  SVO_INFO_STREAM("Map destructed");
 }
 
 void Map::reset()
@@ -42,19 +42,18 @@ bool Map::safeDeleteFrame(FramePtr frame)
 {
   if(syn_==true)return false;
   bool found = false;
-  for(auto it=keyframes_.begin(), ite=keyframes_.end(); it!=ite; ++it)
+  size_t position;
-  {
+  for(auto&& keyframe:_for(keyframes_)){
-    if(it->get()->id_ == frame->id_)
+      if(keyframe.item->id_==frame->id_){
-    {
+          for(auto&& fts:keyframe.item->fts_)removePtFrameRef(keyframe.item, fts);
-      std::for_each((*it)->fts_.begin(), (*it)->fts_.end(), [&](Feature* ftr){
+          found = true;
-        removePtFrameRef(it->get(), ftr);
+          position=keyframe.index;
-      });
+          break;
-      keyframes_.erase(it);
+      }
-      found = true;
-      break;
-    }
   }
-
+  std::list<std::shared_ptr<Frame>>::iterator left=keyframes_.begin();
+  std::advance(left,position);
+  keyframes_.erase(left);
   point_candidates_.removeFrameCandidates(frame);
 
   if(found)
@@ -64,40 +63,37 @@ bool Map::safeDeleteFrame(FramePtr frame)
   return false;
 }
 
-void Map::removePtFrameRef(Frame* frame, Feature* ftr)
+void Map::removePtFrameRef(FramePtr frame, std::shared_ptr<Feature> ftr)
 {
   if(ftr->point == NULL)
     return; // mappoint may have been deleted in a previous ref. removal
-  Point* pt = ftr->point;
+  if(ftr->point->obs_.size() <= 2)
-  delete ftr->point;
-  if(pt->obs_.size() <= 2)
   {
     // If the references list of mappoint has only size=2, delete mappoint
-    safeDeletePoint(pt);
+    safeDeletePoint(ftr->point);
     return;
   }
-  pt->deleteFrameRef(frame);  // Remove reference from map_point
+  ftr->point->deleteFrameRef(frame);  // Remove reference from map_point
   frame->removeKeyPoint(ftr); // Check if mp was keyMp in keyframe
 }
 
-void Map::safeDeletePoint(Point* pt)
+void Map::safeDeletePoint(std::shared_ptr<Point> pt)
 {
   // Delete references to mappoints in all keyframes
   if(pt->obs_.size()>1){
       for(auto&& ftr:pt->obs_){
-          ftr->frame->removeKeyPoint(ftr);
+          if(ftr->frame)ftr->frame->removeKeyPoint(ftr);
-          delete ftr->point;
+          ftr->point.reset();
       }
   }else{
-      delete pt->obs_.back()->point;
+      pt->obs_.back()->point.reset();
   }
   pt->obs_.clear();
-
   // Delete mappoint
   deletePoint(pt);
 }
 
-void Map::deletePoint(Point* pt)
+void Map::deletePoint(std::shared_ptr<Point> pt)
 {
   pt->type_ = Point::TYPE_DELETED;
   trash_points_.push_back(pt);
@@ -210,9 +206,7 @@ void Map::transform(const Matrix2d& R, const Vector2d& t, const double& s)
 
 void Map::emptyTrash()
 {
-  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& pt){
+  for(auto&& t:trash_points_)t.reset();
-    delete pt;
-  });
   trash_points_.clear();
   point_candidates_.emptyTrash();
 }
@@ -225,7 +219,7 @@ MapPointCandidates::~MapPointCandidates()
   reset();
 }
 
-void MapPointCandidates::newCandidatePoint(Point* point, double depth_sigma2)
+void MapPointCandidates::newCandidatePoint(std::shared_ptr<Point> point, double depth_sigma2)
 {
   point->type_ = Point::TYPE_CANDIDATE;
   boost::unique_lock<boost::mutex> lock(mut_);
@@ -238,7 +232,7 @@ void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
   PointCandidateList::iterator it=candidates_.begin();
   while(it != candidates_.end())
   {
-    if(it->first->obs_.front()->frame == frame.get())
+    if(it->first->obs_.front()->frame == frame)
     {
       // insert feature in the frame
       it->first->type_ = Point::TYPE_UNKNOWN;
@@ -251,7 +245,7 @@ void MapPointCandidates::addCandidatePointToFrame(FramePtr frame)
   }
 }
 
-bool MapPointCandidates::deleteCandidatePoint(Point* point)
+bool MapPointCandidates::deleteCandidatePoint(std::shared_ptr<Point>  point)
 {
   boost::unique_lock<boost::mutex> lock(mut_);
   for(auto it=candidates_.begin(), ite=candidates_.end(); it!=ite; ++it)
@@ -286,8 +280,8 @@ void MapPointCandidates::reset()
 {
   boost::unique_lock<boost::mutex> lock(mut_);
   std::for_each(candidates_.begin(), candidates_.end(), [&](PointCandidate& c){
-    delete c.first;
+     c.first.reset();
-    delete c.second;
+     c.second.reset();
   });
   candidates_.clear();
 }
@@ -296,16 +290,14 @@ void MapPointCandidates::deleteCandidate(PointCandidate& c)
 {
   // camera-rig: another frame might still be pointing to the candidate point
   // therefore, we can't delete it right now.
-  delete c.second;
+  c.second.reset();
   c.first->type_ = Point::TYPE_DELETED;
   trash_points_.push_back(c.first);
 }
 
 void MapPointCandidates::emptyTrash()
 {
-  std::for_each(trash_points_.begin(), trash_points_.end(), [&](Point*& p){
+  for(auto&& point:trash_points_)point.reset();
-    delete p;
-  });
   trash_points_.clear();
 }
 
@@ -313,11 +305,12 @@ namespace map_debug {
 
 void mapValidation(Map* map, int id)
 {
-  for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
+  for(auto&& it:map->keyframes_)
-    frameValidation(it->get(), id);
+    frameValidation(it, id);
+
 }
 
-void frameValidation(Frame* frame, int id)
+void frameValidation(FramePtr frame, int id)
 {
   for(auto it = frame->fts_.begin(); it!=frame->fts_.end(); ++it)
   {
@@ -338,7 +331,7 @@ void frameValidation(Frame* frame, int id)
         printf("ERROR DataValidation %i: KeyPoints not correct!\n", id);
 }
 
-void pointValidation(Point* point, int id)
+void pointValidation(std::shared_ptr<Point> point, int id)
 {
   for(auto it=point->obs_.begin(); it!=point->obs_.end(); ++it)
   {
@@ -363,7 +356,7 @@ void mapStatistics(Map* map)
   // compute average number of observations that each point has
   size_t n_frame_obs(0);
   size_t n_pts(0);
-  std::set<Point*> points;
+  std::set<std::shared_ptr<Point>> points;
   for(auto it=map->keyframes_.begin(); it!=map->keyframes_.end(); ++it)
   {
     for(auto ftr=(*it)->fts_.begin(); ftr!=(*it)->fts_.end(); ++ftr)

GPU_version/vio/src/point.cpp

@@ -38,7 +38,7 @@ Point::Point(const Vector3d& pos) :
   last_structure_optim_(0)
 {}
 
-Point::Point(const Vector3d& pos, Feature* ftr) :
+Point::Point(const Vector3d& pos, std::shared_ptr<Feature> ftr) :
   id_(point_counter_++),
   pos_(pos),
   normal_set_(false),
@@ -57,14 +57,14 @@ Point::Point(const Vector3d& pos, Feature* ftr) :
 Point::~Point()
 {}
 
-void Point::addFrameRef(Feature* ftr)
+void Point::addFrameRef(std::shared_ptr<Feature> ftr)
 {
 
   obs_.push_front(ftr);
   ++n_obs_;
 }
 
-Feature* Point::findFrameRef(Frame* frame)
+std::shared_ptr<Feature> Point::findFrameRef(FramePtr frame)
 {
     boost::unique_lock<boost::mutex> lock(point_mut_);
   for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
@@ -73,7 +73,7 @@ Feature* Point::findFrameRef(Frame* frame)
   return NULL;    // no keyframe found
 }
 
-bool Point::deleteFrameRef(Frame* frame)
+bool Point::deleteFrameRef(FramePtr frame)
 {
     boost::unique_lock<boost::mutex> lock(point_mut_);
   for(auto it=obs_.begin(), ite=obs_.end(); it!=ite; ++it)
@@ -94,7 +94,7 @@ bool Point::deleteFrameRef(Frame* frame)
 void Point::initNormal()
 {
   assert(!obs_.empty());
-  const Feature* ftr = obs_.back();
+  const std::shared_ptr<Feature> ftr = obs_.back();
   assert(ftr->frame != NULL);
   normal_ = ftr->frame->se3().rotation_matrix().transpose()*(-ftr->f);
   double x=pow(20/(pos_-Vector3d(ftr->frame->pos()(0),0.0,ftr->frame->pos()(1))).norm(),2);
@@ -102,7 +102,7 @@ void Point::initNormal()
   normal_set_ = true;
 }
 
-bool Point::getCloseViewObs(const Vector2d& framepos, Feature*& ftr,int id) const
+bool Point::getCloseViewObs(const Vector2d& framepos, std::shared_ptr<Feature>& ftr,int id) const
 {
     boost::unique_lock<boost::mutex> lock(point_mut_);
     // TODO: get frame with same point of view AND same pyramid level!

GPU_version/vio/src/reprojector.cpp

@@ -78,7 +78,7 @@ namespace vio {
         cv::Ptr<cv::BFMatcher> matcher = cv::BFMatcher::create(cv::NORM_HAMMING, true);
         feature_detection::FastDetector detector(
                 frame->img().cols, frame->img().rows, Config::gridSize(), gpu_fast_,Config::nPyrLevels());
-        detector.detect(frame.get(), frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
+        detector.detect(frame, frame->img_pyr_, Config::triangMinCornerScore(), keypoints_cur,&descriptors_cur);
         list<pair<FramePtr, double> > close_kfs;
         map_.getCloseKeyframes(frame, close_kfs);
         if (!last_frame->fts_.empty())
@@ -103,7 +103,7 @@ namespace vio {
                 for (auto &&match:matches) {
                     if (match.queryIdx != f.index)continue;
                     if(keypoints_cur.size() < match.trainIdx)continue;
-                    list<Feature*>::iterator point=keypoints_cur.begin();
+                    list<std::shared_ptr<Feature>>::iterator point=keypoints_cur.begin();
                     std::advance(point,match.trainIdx);
                     if(!(*point))continue;
                     if (f.item->point == NULL){
@@ -113,9 +113,10 @@ namespace vio {
                         assert(grid_.cells.at(k) != nullptr);
                         Vector2d px((int) (*point)->px.x(),
                                     (int) (*point)->px.y());
-                        frame->fts_.push_back(new Feature(it_frame.item.first.get(),
+                        frame->fts_.push_back(std::make_shared<Feature>(it_frame.item.first,
-                                                          new Point(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
+                                                                        std::make_shared<Point>(it_frame.item.first->cam_->cam2world(f.item->px.x(), f.item->px.y()),f.item),
-                                px,it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
+                                px,
+                                it_frame.item.first->cam_->cam2world(f.item->px),(*point)->level));
                         frame->fts_.back()->point->last_frame_overlap_id_=it_frame.item.first->id_;
                         grid_.cells.at(k)->push_back(Candidate(frame->fts_.back()->point, px));
                         overlap_kfs.back().second++;
@@ -187,7 +188,7 @@ namespace vio {
             if(it->pt->type_ == Point::TYPE_UNKNOWN && it->pt->n_succeeded_reproj_ > 10)
                 it->pt->type_ = Point::TYPE_GOOD;
 
-            Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);
+            std::shared_ptr<Feature> new_feature = std::make_shared<Feature>(frame, it->px, matcher_.search_level_);
             frame->addFeature(new_feature);
             // Here we add a reference in the feature to the 3D point, the other way
             // round is only done if this frame is selected as keyframe.
@@ -215,7 +216,7 @@ namespace vio {
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 // TODO original implementation
-    bool Reprojector::reprojectPoint(FramePtr frame, Point *point) {
+    bool Reprojector::reprojectPoint(FramePtr frame, std::shared_ptr<Point> point) {
         Vector2d px(frame->w2c(point->pos_));
         if (frame->cam_->isInFrame(px.cast<int>(), 8)) // 8px is the patch size in the matcher
         {