/**
* @brief The 'regression2D' method can be used to fit a line to a given point set.
* @param points_begin set begin iterator
* @param points_end set end iterator
* @param fit_start the start of the line fit
* @param fit_end the set termintating iterator position
* @param line the parameterized line to work with
*/
inline void regression2D(const std::vector<LaserBeam>::const_iterator &points_begin, const std::vector<LaserBeam>::const_iterator &points_end,
Eigen::ParametrizedLine<double, 2> &line)
{
std::vector<LaserBeam>::const_iterator back_it = points_end;
--back_it;
Eigen::Vector2d front (points_begin->posX(), points_end->posY());
Eigen::Vector2d back (back_it->posX(), back_it->posY());
unsigned int size = std::distance(points_begin, points_end);
Eigen::MatrixXd A(size, 2);
Eigen::VectorXd b(size);
A.setOnes();
Eigen::Vector2d dxy = (front - back).cwiseAbs();
bool solve_for_x = dxy.x() > dxy.y();
if(solve_for_x) {
/// SOLVE FOR X
int i = 0;
for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
{
A(i,1) = it->posX();
b(i) = it->posY();
}
} else {
/// SOLVE FOR Y
int i = 0;
for(std::vector<LaserBeam>::const_iterator it = points_begin ; it != points_end ; ++it, ++i)
{
A(i,1) = it->posY();
b(i) = it->posX();
}
}
Eigen::VectorXd weights = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);
double alpha = weights(0);
double beta = weights(1);
Eigen::Vector2d from;
Eigen::Vector2d to;
if(solve_for_x) {
from(0) = 0.0;
from(1) = alpha;
to(0) = 1.0;
to(1) = alpha + beta;
} else {
from(0) = alpha;
from(1) = 0.0;
to(0) = alpha + beta;
to(1) = 1.0;
}
Eigen::Vector2d fit_start;
Eigen::Vector2d fit_end;
line = Eigen::ParametrizedLine<double, 2>(from, (to - from).normalized());
fit_start = line.projection(front);
fit_end = line.projection(back);
line = Eigen::ParametrizedLine<double, 2>(fit_start, (fit_end - fit_start));
}
/**
* @brief Calculate the distance between two Eigen vectors given by iterators.
* @param first the first Eigen Vector2d
* @param second the second Eigen Vector2d
* @return the distance
*/
inline double distance(const std::vector<LaserBeam>::const_iterator &first, const std::vector<LaserBeam>::const_iterator &second)
{
return (Eigen::Vector2d(first->posX(), first->posY()) - Eigen::Vector2d(second->posX(), second->posY())).norm();
}