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;
}
Eigen::MatrixXd ComputeP_NormalizedDLT(const Point2DVector& points2D, const Point3DVector& points3D)
{
unsigned int numberOfPoints = points2D.size();
if(points3D.size() != numberOfPoints)
{
std::stringstream ss;
ss << "ComputeP_NormalizedDLT: The number of 2D points (" << points2D.size()
<< ") must match the number of 3D points (" << points3D.size() << ")!" << std::endl;
throw std::runtime_error(ss.str());
}
// std::cout << "ComputeP_NormalizedDLT: 2D points: " << std::endl;
// for(Point2DVector::const_iterator iter = points2D.begin(); iter != points2D.end(); ++iter)
// {
// Point2DVector::value_type p = *iter;
// std::cout << p[0] << " " << p[1] << std::endl;
// }
// std::cout << "ComputeP_NormalizedDLT: 3D points: " << std::endl;
// for(Point3DVector::const_iterator iter = points3D.begin(); iter != points3D.end(); ++iter)
// {
// Point3DVector::value_type p = *iter;
// std::cout << p[0] << " " << p[1] << " " << p[2] << std::endl;
// }
Eigen::MatrixXd similarityTransform2D = ComputeNormalizationTransform<Eigen::Vector2d>(points2D);
Eigen::MatrixXd similarityTransform3D = ComputeNormalizationTransform<Eigen::Vector3d>(points3D);
// std::cout << "Computed similarity transforms:" << std::endl;
// std::cout << "similarityTransform2D: " << similarityTransform2D << std::endl;
// std::cout << "similarityTransform3D: " << similarityTransform3D << std::endl;
// The (, Eigen::VectorXd()) below are only required when using gnu++0x, it seems to be a bug in Eigen
Point2DVector transformed2DPoints(numberOfPoints, Eigen::Vector2d());
Point3DVector transformed3DPoints(numberOfPoints, Eigen::Vector3d());
for(unsigned int i = 0; i < numberOfPoints; ++i)
{
Eigen::VectorXd point2Dhomogeneous = points2D[i].homogeneous();
Eigen::VectorXd point2Dtransformed = similarityTransform2D * point2Dhomogeneous;
transformed2DPoints[i] = point2Dtransformed.hnormalized();
Eigen::VectorXd point3Dhomogeneous = points3D[i].homogeneous();
Eigen::VectorXd point3Dtransformed = similarityTransform3D * point3Dhomogeneous;
transformed3DPoints[i] = point3Dtransformed.hnormalized();
//transformed2DPoints[i] = (similarityTransform2D * points2D[i].homogeneous()).hnormalized();
//transformed3DPoints[i] = (similarityTransform3D * points3D[i].homogeneous()).hnormalized();
}
// std::cout << "Transformed points." << std::endl;
// Compute the Camera Projection Matrix
Eigen::MatrixXd A(2*numberOfPoints,12);
for(unsigned int i = 0; i < numberOfPoints; ++i)
{
// First row/equation from the ith correspondence
unsigned int row = 2*i;
A(row, 0) = 0;
A(row, 1) = 0;
A(row, 2) = 0;
A(row, 3) = 0;
A(row, 4) = transformed3DPoints[i](0);
A(row, 5) = transformed3DPoints[i](1);
A(row, 6) = transformed3DPoints[i](2);
A(row, 7) = 1;
A(row, 8) = -transformed2DPoints[i](1) * transformed3DPoints[i](0);
A(row, 9) = -transformed2DPoints[i](1) * transformed3DPoints[i](1);
A(row, 10) = -transformed2DPoints[i](1) * transformed3DPoints[i](2);
A(row, 11) = -transformed2DPoints[i](1);
// Second row/equation from the ith correspondence
row = 2*i+1;
A(row, 0) = transformed3DPoints[i](0);
A(row, 1) = transformed3DPoints[i](1);
A(row, 2) = transformed3DPoints[i](2);
A(row, 3) = 1;
A(row, 4) = 0;
A(row, 5) = 0;
A(row, 6) = 0;
A(row, 7) = 0;
A(row, 8) = -transformed2DPoints[i](0) * transformed3DPoints[i](0);
A(row, 9) = -transformed2DPoints[i](0) * transformed3DPoints[i](1);
A(row, 10) = -transformed2DPoints[i](0) * transformed3DPoints[i](2);
A(row, 11) = -transformed2DPoints[i](0);
}
// std::cout << "A: " << A << std::endl;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
Eigen::MatrixXd lastColumnOfV = V.col(11);
Eigen::MatrixXd P = Reshape(lastColumnOfV, 3, 4);
// Denormalization
P = similarityTransform2D.inverse()*P*similarityTransform3D; // 3x3 * 3x4 * 4x4 = 4x4
return P;
}