diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 3f88f9f8f32d218236bf70481d8638719975a0b3..4bedf4d5da4b4636c68907c55c4dff7e317360a5 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -24,7 +24,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
ScalarT prev_range = 0;
unsigned int ii = 0;
unsigned int ii_ok = 0;
- ScalarT kr = 2; //TODO: as a parameters somewhere.
+ ScalarT kr = 10; //TODO: as a parameters somewhere.
Eigen::Vector4s point_laser, point_ref;
//resize to all points case
@@ -57,7 +57,7 @@ void LaserScan::ranges2xy(Eigen::Matrix4s _device_T)
// points_(2,ii_ok) = 1;
//check jump. Min dist between consecutive points is r*sin(angle_step_). A jump occurs when this min dist is overpassed by kr times
- if ( fabs(ranges_[ii]-prev_range) > kr*ranges_[ii]*params_.angle_step_) //jump condition (kr*r*sin(a) ~ kr*r*a)
+ if ( fabs(ranges_[ii]-prev_range) > fabs(kr*ranges_[ii]*params_.angle_step_)) //jump condition (kr*r*sin(a) ~ kr*r*a)
//if ( fabs(ranges_[ii]-prev_range) > 0.5) //jump condition >0.5m
{
jumps_.push_back(ii_ok);
diff --git a/src/line_finder.cpp b/src/line_finder.cpp
index 032e2bcf34dcaa701901a1300a092cc0b2faa895..da4fe529b845b3dfbf63536a6e1f6f979f58af22 100644
--- a/src/line_finder.cpp
+++ b/src/line_finder.cpp
@@ -30,6 +30,42 @@ void LineFinder::fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) c
//_line.fit_error_ = (_points.transpose() * _line.abc_).array().abs().sum() / (_line.abc_.head(2).norm()*_points.cols());
}
+void LineFinder::pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const
+{
+ ScalarT a,b,c,qx,qy,dd;
+
+ //inits
+ a = _line(0);
+ b = _line(1);
+ c = _line(2);
+ qx = _in_pt(0)/_in_pt(2);
+ qy = _in_pt(1)/_in_pt(2);
+
+ //compute point projection to line. The formula comes from frocing:
+ // 1) _out_pt to lie on _lines, and f
+ // 2) lines l2 through _in_pt and _out_pt perpendicular to _line -> normal vectors perpendicular
+
+ if (a == 0. ) //case a=0 -> vertical line (parallel to y axis)
+ {
+ _out_pt(0) = _in_pt(0);
+ _out_pt(1) = -c/b;
+ return;
+ }
+
+ if ( b == 0. ) //case b=0 -> vertical line (parallel to y axis)
+ {
+ _out_pt(0) = -c/a;
+ _out_pt(1) = _in_pt(1);
+ return;
+ }
+
+ //general case
+ dd = -(a*a + b*b); //denominator. Can't be 0 because either a or b have to be non-zero
+ _out_pt(1) = (b*c + a*b*qx - a*a*qy) / dd;
+ _out_pt(0) = -( c + b*_out_pt(1) ) / a;
+
+}
+
// unsigned int laserscanutils::LineFinder::mergeLines(std::list<LineSegment> & _line_list)
// {
// //TODO
diff --git a/src/line_finder.h b/src/line_finder.h
index 6d888d6bcaaf558dcd4774b576890365417f747d..923d9cd7e12a5a92c61d1b16caa036dfe3f67ee4 100644
--- a/src/line_finder.h
+++ b/src/line_finder.h
@@ -45,7 +45,16 @@ class LineFinder
* \param _line: a laserscanutils::Line object of the best fitting line in the Least Squares sense
*
**/
- void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;
+ void fitLine(const Eigen::MatrixXs & _points, LineSegment & _line) const;
+
+ /** \brief Computes projection of point to line
+ *
+ * Computes the projection of _in_pt to _line. Result at _out_pt
+ * Everything in homogeneous coordinates
+ * TODO: To be moved elsewhere. No reason why this methos has to be member of LineFinder
+ *
+ **/
+ void pointProjectionToLine(const Eigen::Vector3s & _line, const Eigen::Vector3s & _in_pt, Eigen::Vector3s & _out_pt) const;
/** \brief Merges lines of a list
*
diff --git a/src/line_finder_hough.cpp b/src/line_finder_hough.cpp
index de178a474269ee14c7d31ae4c2d3855fd8e13014..092ad5c201a4e1af8a418bc1b5039148aa4edb9b 100644
--- a/src/line_finder_hough.cpp
+++ b/src/line_finder_hough.cpp
@@ -19,7 +19,7 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
this->hough_params_ = _params;
//resize hough grid accordingly
- hough_grid_rows = (unsigned int)ceil(M_PI/hough_params_.theta_step_);//[0,PI]
+ hough_grid_rows = (unsigned int)ceil(M_PI/hough_params_.theta_step_);//theta in [0,PI]
hough_grid_rows_half = (unsigned int)ceil(0.5*M_PI/hough_params_.theta_step_);//middle row index
hough_grid_cols = (unsigned int)ceil(2*hough_params_.range_max_/hough_params_.range_step_);//[-rmax,+rmax]
hough_grid_cols_half = (unsigned int)ceil(hough_params_.range_max_/hough_params_.range_step_);//middle col index
@@ -33,11 +33,11 @@ void LineFinderHough::setHoughParams(const LineFinderHoughParams & _params)
unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
std::list<laserscanutils::LineSegment> & _line_list)
{
- double theta, range;
+ ScalarT theta, range;
int kr;
LineSegment line;
- double xmax, xmin, ymax, ymin;
- Eigen::Matrix3s pt; //auxiliary point
+ ScalarT xmax, xmin, ymax, ymin;
+ Eigen::Vector3s q_pt, p_pt; //auxiliary points
Eigen::MatrixXs pts; //auxiliar array of points
std::list<Eigen::Vector3s>::iterator pts_it;
@@ -105,7 +105,7 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
line.range_ = jj*hough_params_.range_step_; //range
//set starting and ending points of the line
- //TODO: take into account normal convention. abc_ should point to the free space
+ //TODO: apply convention: (point_first_ - point_last_ , 0)^T x (a/c, b/c, 0)^T = UP ! (z>0)
xmax=-100; xmin=100; ymax=-100; ymin=100;
for (pts_it = hough_grid_.at(ii).at(jj).begin(); pts_it != hough_grid_.at(ii).at(jj).end(); pts_it++)
{
@@ -115,22 +115,23 @@ unsigned int LineFinderHough::findLines( const Eigen::MatrixXs & _points,
if (pts_it->x() < xmin) xmin = pts_it->x();
if (pts_it->y() < ymin) ymin = pts_it->y();
}
- if (ii < hough_grid_rows_half) //first and third quartile of r-theta plane
+ if (ii < hough_grid_rows_half) //theta in [0,PI/2]
{
- //TODO
- //pt << xmin,ymax,1;
- //line.point_first_ << projection of pt to line.abc_
- //pt << xmax,ymin,1;
- //line.point_last_ << projection of pt to line.abc_
- line.point_first_ << xmin,ymax,1;
- line.point_last_ << xmax,ymin,1;
-
+ q_pt << xmin,ymax,1;
+ pointProjectionToLine(line.abc_, q_pt, p_pt);
+ line.point_first_ << p_pt;
+ q_pt << xmax,ymin,1;
+ pointProjectionToLine(line.abc_, q_pt, p_pt);
+ line.point_last_ << p_pt;
}
- else //second and fourth quartile of r-theta plane
+ else //theta in [PI/2,PI]
{
- //TODO same as above
- line.point_first_ << xmin,ymin,1;
- line.point_last_ << xmax,ymax,1;
+ q_pt << xmin,ymin,1;
+ pointProjectionToLine(line.abc_, q_pt, p_pt);
+ line.point_first_ << p_pt;
+ q_pt << xmax,ymax,1;
+ pointProjectionToLine(line.abc_, q_pt, p_pt);
+ line.point_last_ << p_pt;
}
//push back the line to the list
diff --git a/src/line_segment.h b/src/line_segment.h
index fb5d8beeeb33b97f3714a52c45d19c51fcada2a5..213da308dd52bdcad8f46d4329d550c32d8bd6d1 100644
--- a/src/line_segment.h
+++ b/src/line_segment.h
@@ -21,7 +21,7 @@ class LineSegment
/** \brief Homogeneous parameterization of the line to which the segment lies on.
*
* Homogeneous parameterization of the line to which the segment lies on: (a,b,c)^T -> ax+by+c=0.
- * Vector (a,b,c)^T is the normal vector, pointing to a free surface
+ * Vector (a/c,b/c)^T is the normal vector, pointing to a free surface
*
*/
Eigen::Vector3s abc_;