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; }