CaptureLaser2D added. CaptureBase::getCorrespondenceList() implemented but not tested

src/CMakeLists.txt

@@ -20,6 +20,7 @@ SET(HDRS
     capture_base.h
     capture_relative.h
     capture_gps_fix.h
+    capture_laser_2D.h
     capture_odom_2D.h
     correspondence_base.h
     correspondence_sparse.h
@@ -51,6 +52,7 @@ SET(SRCS
     capture_base.cpp
     capture_relative.cpp
     capture_gps_fix.cpp
+    capture_laser_2D.cpp
     capture_odom_2D.cpp
     correspondence_base.cpp
     feature_base.cpp

src/capture_base.cpp

@@ -48,16 +48,26 @@ const FrameBasePtr CaptureBase::getFramePtr() const
     return upperNodePtr();
 }
 
-// FeatureBaseList & CaptureBase::getFeatureList() const
-// {
-//     return downNodeList();
-// }
-
 FeatureBaseList* CaptureBase::getFeatureListPtr()
 {
     return getDownNodeListPtr();
 }
 
+void CaptureBase::getCorrespondenceList(CorrespondenceBaseList & _corr_list)
+{
+    std::list<FeatureBaseShPtr>::iterator f_it;
+    std::list<CorrespondenceBaseShPtr>* c_list_ptr;
+    std::list<CorrespondenceBaseShPtr>::iterator c_it;
+    
+    for( f_it = down_node_list_.begin(); f_it != down_node_list_.end(); ++f_it)
+    {
+        c_list_ptr = (f_it->get())->getCorrespondenceListPtr();
+        for( c_it = c_list_ptr->begin(); c_it != c_list_ptr->end(); ++c_it)
+        {
+            _corr_list.push_back(*c_it);
+        }
+    }
+}
 
 TimeStamp CaptureBase::getTimeStamp() const
 {
@@ -102,7 +112,7 @@ void CaptureBase::setDataCovariance(const Eigen::MatrixXs& _data_cov)
 
 void CaptureBase::processCapture()
 {
-    std::cout << "... processing capture" << std::endl;
+    std::cout << "CaptureBase::processCapture()... processing capture" << std::endl;
 }
 
 void CaptureBase::printSelf(unsigned int _ntabs, std::ostream & _ost) const

src/capture_base.h

@@ -29,7 +29,7 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
         Eigen::Vector3s inverse_sensor_pose_; ///< World pose in the sensor frame: inverse of the global_pose_. TODO: use state units
         
     public:
-        CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);
+        CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr);//TODO: to be removed ??
         
         CaptureBase(const TimeStamp& _ts, const SensorBasePtr& _sensor_ptr, const Eigen::VectorXs& _data);
 
@@ -58,21 +58,21 @@ class CaptureBase : public NodeLinked<FrameBase,FeatureBase>
          **/                
         const FrameBasePtr getFramePtr() const;
         
-        /** \brief Gets a reference to feature list
+        /** \brief Gets a pointer to feature list
          * 
-         * Gets a reference to feature list
+         * Gets a pointer to feature list
          *
          **/                        
-//         FeatureBaseList & getFeatureList() const;
-        
         FeatureBaseList* getFeatureListPtr();
         
-        /** \brief Gets a const reference to correspondence list
+        /** \brief Fills the provided list with all correspondences related to this capture
          *
-         * Gets a const reference to correspondence list
+         * Fills the provided list with all correspondences related to this capture
+         * 
          *
          **/
-        const CorrespondenceBaseList getCorrespondenceList() const;
+        //const CorrespondenceBaseList getCorrespondenceList() const;
+        void getCorrespondenceList(CorrespondenceBaseList & _corr_list);//TODO: Should be const
 
         TimeStamp getTimeStamp() const;
 

src/capture_laser_2D.h

@@ -17,7 +17,7 @@ class CaptureLaser2D : public CaptureBase
          * Constructor
          * 
          **/
-        CaptureLaser2D(double _ts, const SensorLaser2D & _sensor_ptr, const Eigen::VectorXs& _ranges, const double & _std_dev);
+        CaptureLaser2D(const TimeStamp & _ts, const SensorLaser2DPtr & _sensor_ptr, const Eigen::VectorXs& _ranges);
 
         /** \brief Destructor
          * 
@@ -40,37 +40,4 @@ class CaptureLaser2D : public CaptureBase
          **/
         virtual void extractCorners();
 };
-
-////////////////////////////////
-// IMPLEMENTATION
-////////////////////////////////
-
-
-inline CaptureLaser2D::CaptureLaser2D(const FrameShPtr& _frm_ptr, const SensorShPtr& _sen_ptr, const NodeLocation _loc) :
-        Capture(_frm_ptr, _sen_ptr, _loc)
-{
-    // 
-}
-
-inline CaptureLaser2D::~CaptureLaser2D()
-{
-    //
-}
-
-inline void CaptureLaser2D::processCapture()
-{
-    extractCorners();
-}
-
-inline void CaptureLaser2D::extractCorners()
-{
-    std::cout << "Extracting corners ... " << std::endl;
-    //TODO by Andreu: create class FeatureCorner2D + main to test this method.
-    //TODO by Juan AC
-    // Laser ranges are at data_
-    // Scan size is data_.size()
-    // Corners should be created as FeatureCorner2D. Corner 3 params to be stored at FeatureBase::measurement_ 
-    // After creation, they have to be pushed back to down_node_list_ by means of the method Capture::addFeature(const FeatureShPtr& _f_ptr)  
-}
-
 #endif /* CAPTURE_LASER_2D_H_ */

src/sensor_laser_2D.cpp

 #include "sensor_laser_2D.h"
 
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax) :
-        SensorBase(LIDAR, _sp, 4)
-//         SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax})
+SensorLaser2D::SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax, WolfScalar _range_stdev) :
+        SensorBase(LIDAR, _sp, 5)
+//         SensorBase(LIDAR, _sp,{(WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev})
 {
-    params_ << (WolfScalar)(_nrays), _apert, _rmin, _rmax;
+    params_ << (WolfScalar)(_nrays), _apert, _rmin, _rmax, _range_stdev;
 }
 
 SensorLaser2D::~SensorLaser2D()

src/sensor_laser_2D.h

@@ -23,9 +23,10 @@ class SensorLaser2D : public SensorBase
          * \param _apert angular aperture [rad]
          * \param _rmin minimum range [m]
          * \param _rmax maximum range [m]
+         * \param _range_stdev range standard deviation [m]
          * 
          **/
-        SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax);
+        SensorLaser2D(const Eigen::VectorXs & _sp, unsigned int _nrays, WolfScalar _apert, WolfScalar _rmin, WolfScalar _rmax, WolfScalar _range_stdev);
         
         /** \brief Destructor
          * 

src/wolf.h

@@ -243,9 +243,10 @@ typedef CorrespondenceBaseList::iterator CorrespondenceBaseIter;
 typedef std::shared_ptr<RawBase> RawShPtr;
 typedef RawBase* RawPtr;
 
-// - Sensor
+// - Sensors
 typedef std::shared_ptr<SensorBase> SensorBaseShPtr;
 typedef SensorBase* SensorBasePtr;
+typedef SensorLaser2D* SensorLaser2DPtr;
 
 // - transSensor
 typedef std::shared_ptr<TransSensor> TransSensorShPtr;