diff --git a/src/capture_laser_2D.cpp b/src/capture_laser_2D.cpp
index 1e2f6bb4589a0424dcf8ca47eca717b9c414834a..8c5749ae48c0e82a3049fd245ba9aa9cad1023ca 100644
--- a/src/capture_laser_2D.cpp
+++ b/src/capture_laser_2D.cpp
@@ -7,19 +7,24 @@
// unsigned int CaptureLaser2D::max_beam_distance = 5;//max number of beams of distance between lines to consider corner or concatenation
// double CaptureLaser2D::max_distance = 0.5;//max distance between line ends to consider corner or concatenation
-//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges):
+//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<WolfScalar>& _ranges):
// CaptureBase(_ts, _sensor_ptr, _ranges),
// ranges_(data_.data(), _ranges.size()),
// intensities_(data_.data(), 0)
//{
// laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
//}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase(_ts, _sensor_ptr), ranges_(_ranges)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts,
+ SensorBase* _sensor_ptr,
+ const std::vector<float>& _ranges)
+ :
+ CaptureBase(_ts, _sensor_ptr),
+ ranges_(_ranges)
{
laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
}
+
//CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities):
// CaptureBase(_ts, _sensor_ptr, _ranges),
// ranges_(data_.data(), _ranges.size()),
@@ -27,12 +32,19 @@ CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, c
//{
// laser_ptr_ = (SensorLaser2D*)sensor_ptr_;
//}
-CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities) :
- CaptureBase(_ts, _sensor_ptr), ranges_(_ranges), intensities_(_intensities)
+CaptureLaser2D::CaptureLaser2D(const TimeStamp & _ts,
+ SensorBase* _sensor_ptr,
+ const std::vector<float>& _ranges,
+ const std::vector<float>& _intensities)
+ :
+ CaptureBase(_ts, _sensor_ptr),
+ ranges_(_ranges),
+ intensities_(_intensities)
{
laser_ptr_ = (SensorLaser2D*) sensor_ptr_;
}
+
CaptureLaser2D::~CaptureLaser2D()
{
//
@@ -41,8 +53,7 @@ CaptureLaser2D::~CaptureLaser2D()
void CaptureLaser2D::processCapture()
{
//variables
- //std::list<Eigen::Vector4s> corners;
- std::list < laserscanutils::Corner > corners;
+ std::list <laserscanutils::Corner> corners;
//extract corners from range data
this->extractCorners(corners);
@@ -60,15 +71,16 @@ void CaptureLaser2D::processCapture()
unsigned int CaptureLaser2D::extractCorners(std::list<laserscanutils::Corner> & _corner_list) const
{
- std::list < laserscanutils::Line > line_list;
-
- laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
- return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+// std::list < laserscanutils::Line > line_list;
+//
+// laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, line_list);
+// return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), line_list, _corner_list);
+ return laserscanutils::extractCorners(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _corner_list);
}
unsigned int CaptureLaser2D::extractLines(std::list<laserscanutils::Line> & _line_list) const
{
- return laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams(), ranges_, _line_list);
+ return laserscanutils::extractLines(laser_ptr_->getScanParams(), laser_ptr_->getCornerAlgParams().line_params_, ranges_, _line_list);
}
void CaptureLaser2D::createFeatures(std::list<laserscanutils::Corner> & _corner_list)
@@ -257,9 +269,9 @@ void CaptureLaser2D::establishConstraintsMHTree()
tree.solve(ft_lk_pairs,associated_mask);
//print tree & score table
- std::cout << "-------------" << std::endl;
- tree.printTree();
- tree.printScoreTable();
+// std::cout << "-------------" << std::endl;
+// tree.printTree();
+// tree.printScoreTable();
//loop over all features
ii = 0; //runs over features
diff --git a/src/capture_laser_2D.h b/src/capture_laser_2D.h
index 3fde65d46aab37404f2bbdd88fd57b22d6429952..1c9ce106a8ed8b1951ef8ad5c3de8989873e7cf2 100644
--- a/src/capture_laser_2D.h
+++ b/src/capture_laser_2D.h
@@ -12,9 +12,10 @@
#include <eigen3/Eigen/Geometry>
//laser_scan_utils
+#include "iri-algorithms/laser_scan_utils/entities.h"
#include "iri-algorithms/laser_scan_utils/scan_basics.h"
+#include "iri-algorithms/laser_scan_utils/line_detector.h"
#include "iri-algorithms/laser_scan_utils/corner_detector.h"
-#include "iri-algorithms/laser_scan_utils/entities.h"
//wolf includes
#include "constraint_corner_2D_theta.h"
@@ -45,9 +46,9 @@ class CaptureLaser2D : public CaptureBase
// static double max_distance;//max distance between line ends to consider corner or concatenation
//Eigen::Map<Eigen::VectorXs> ranges_; // a map to the ranges inside de data vector
- std::vector<float> ranges_; // ranges vector
+ std::vector<float> ranges_; // ranges vector. Type float to match ROS LaserScan message
//Eigen::Map<Eigen::VectorXs> intensities_; // a map to the intensities inside the data vector
- std::vector<float> intensities_; // intensities vector
+ std::vector<float> intensities_; // intensities vector. Type float to match ROS LaserScan message
SensorLaser2D* laser_ptr_; //specific pointer to sensor laser 2D object
public:
@@ -56,7 +57,6 @@ class CaptureLaser2D : public CaptureBase
* Constructor with ranges
*
**/
- //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges);
CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges);
/** \brief Constructor with ranges and intensities
@@ -64,7 +64,6 @@ class CaptureLaser2D : public CaptureBase
* Constructor with ranges and intensities
*
**/
- //CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _intensities);
CaptureLaser2D(const TimeStamp & _ts, SensorBase* _sensor_ptr, const std::vector<float>& _ranges, const std::vector<float>& _intensities);
/** \brief Destructor
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index c34f025208a5c4086d13d6d83c125f7b5fdaaf5b..0d4c80f34c8a65a41ff96828ee8c8a194f6009b9 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -135,8 +135,8 @@ int main(int argc, char** argv)
string modelFileName;
//model and initial view point
- modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
- //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
+ //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj";
+ modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj";
//modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj";
devicePose.setPose(2, 8, 0.2, 0, 0, 0);
viewPoint.setPose(devicePose);
diff --git a/src/sensor_laser_2D.cpp b/src/sensor_laser_2D.cpp
index bb6ff38f30b49fcaad8b4ed4097042b5333facd5..434a43ab0ec214e556e432c3b00c5574a77c07bc 100644
--- a/src/sensor_laser_2D.cpp
+++ b/src/sensor_laser_2D.cpp
@@ -59,13 +59,13 @@ void SensorLaser2D::setScanParams(const laserscanutils::ScanParams & _params)
void SensorLaser2D::setDefaultCornerAlgParams()
{
//default parameters for corner extraction. TODO: get them from a file or static constants
- corners_alg_params_.segment_window_size = 8;
- corners_alg_params_.theta_min = 0.4;
- corners_alg_params_.theta_max_parallel = 0.1;
- corners_alg_params_.k_sigmas = 3;
- corners_alg_params_.max_beam_distance = 5;
- corners_alg_params_.max_distance = 1;
-
+ corners_alg_params_.theta_min_ = 0.4;
+ corners_alg_params_.max_distance_ = 1;
+ corners_alg_params_.line_params_.jump_dist_ut_ = 1;
+ corners_alg_params_.line_params_.window_sz_ = 8;
+ corners_alg_params_.line_params_.k_sigmas_ut_ = 3;
+ corners_alg_params_.line_params_.concatenate_angle_ut_ = 0.1;
+ corners_alg_params_.line_params_.concatenate_ii_ut_ = 5;
}
void SensorLaser2D::setCornerAlgParams(const laserscanutils::ExtractCornerParams & _corner_alg_params)