Exemplo n.º 1
0
 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;
 }
Exemplo n.º 2
0
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;
}