diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 8582571f4b82b89b174b62e9c134457adebcaa53..d37f3fd9595734fc24b71bb63f6436178c313fad 100644
--- a/src/CMakeLists.txt
+++ b/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
diff --git a/src/capture_base.cpp b/src/capture_base.cpp
index de303e1b79467dd8b57757565046f8734729c665..3de5889327899272420ae6446c6e5a06eea04437 100644
--- a/src/capture_base.cpp
+++ b/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
diff --git a/src/capture_base.h b/src/capture_base.h
index 0ec6c867426325a4af1cf6570269ad7c5567a659..62c7760a97155a34b6d507233b8e949118a4bb2d 100644
--- a/src/capture_base.h
+++ b/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;
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index ed167e4b34b611e5a7e98fc663a1b04e38906153..342b672123b328d3794910484cb0d48487cdd43a 100644
--- a/src/capture_laser_2D.h
+++ b/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_ */
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index e0c09ed0fcec798069e197aa4fb4859be75e6e79..6b670fc7d2453d40095a6587e19cb0add078950b 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -1,10 +1,10 @@
#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()
diff --git a/src/sensor_laser_2D.h b/src/sensor_laser_2D.h
index 0a9fd1b9189cc17a535b03ca2be27391391390fd..a8ee94d2397ab9c0fa364536f2ad7582882c5b56 100644
--- a/src/sensor_laser_2D.h
+++ b/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
*
diff --git a/src/wolf.h b/src/wolf.h
index d30fbde7fff8c5d355652114efa7d4c082d0efd0..aec9f17a5139b65c0856041d18409a6176f23c7d 100644
--- a/src/wolf.h
+++ b/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;