Skip to content
Snippets Groups Projects
Commit 9969fce3 authored by acoromin's avatar acoromin
Browse files

Multi-hypothesis Tree for data association ready. Tested with iri_wolf ros node

parent fc331204
No related branches found
No related tags found
No related merge requests found
......@@ -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
......
......@@ -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
......
......@@ -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);
......
......@@ -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)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment