RawLaser::Point2DVector RawLaser::cartesian() const
{
Point2DVector points;
for (size_t i = 0; i < _ranges.size(); ++i) {
const double& r = _ranges[i];
if (r < _laserParams.maxRange) {
double alpha = _laserParams.firstBeamAngle + i * _laserParams.angularStep;
points.push_back(Eigen::Vector2d(cos(alpha) * r, sin(alpha) * r));
}
}
return points;
}
Point2DVector GeneratePoints()
{
Point2DVector points;
// Model y = 2*x + 5 with some noise
for(unsigned int i = 0; i < 50; ++i)
{
float x = static_cast<float>(i);
Eigen::Vector2d point;
point(0) = x;
point(1) = 2.0f * x + 5.0 + drand48()/10.0f;
points.push_back(point);
}
return points;
}