Beispiel #1
0
bool PinholeCamera<DISTORTION_T>::backProjectHomogeneousBatch(
    const Eigen::Matrix2Xd & imagePoints, Eigen::Matrix4Xd * directions,
    std::vector<bool> * success) const
{
  const int numPoints = imagePoints.cols();
  directions->row(3) = Eigen::VectorXd::Ones(numPoints);
  for (int i = 0; i < numPoints; ++i) {
    Eigen::Vector2d imagePoint = imagePoints.col(i);
    Eigen::Vector3d point;
    bool suc = backProject(imagePoint, &point);
    if(success)
      success->push_back(suc);
    directions->template block<3, 1>(0, i) = point;
  }
  return true;
}
Eigen::Matrix<double,4,1> estimateSimilarTransform(
    const Eigen::Matrix2Xd& pointsFrom, const Eigen::Matrix2Xd& pointsTo ) {
  Matrix<double,4,1> h(4,1);
  VectorXd indices( pointsFrom.cols() );
  for ( int i = 0; i < indices.rows(); i++ ) {
    indices(i) = i;
  }
  //Matrix3d T = computeSimilarity( pointsFrom, pointsTo ); // No ransac'n
  Matrix3d T = msac( pointsTo, pointsFrom, 20, 99, 1 );

  h(0) = T(0,0);
  h(1) = T(1,0);
  h(2) = T(2,0);
  h(3) = T(2,1);

  return h;
}
Matrix3d msac( const Eigen::Matrix2Xd& pointsFrom, const Eigen::Matrix2Xd& pointsTo,
    int maxNumTrials, double confidence, double maxDistance ) {
  double threshold = maxDistance;
  int numPts = pointsFrom.cols();
  int idxTrial = 1;
  int numTrials = maxNumTrials;
  double maxDis = threshold * numPts;
  double bestDist = maxDis;
  Matrix3d bestT;
  bestT << 1, 0, 0,   0, 1, 0,  0, 0, 1;

  int index1;
  int index2;
  // Get two random, different numbers in [0:pointsFrom.cols()-1]
  std::uniform_int_distribution<int> distribution1( 0, pointsFrom.cols()-1 );
  std::uniform_int_distribution<int> distribution2( 0, pointsFrom.cols()-2 );
  while ( idxTrial <= numTrials ) {
    // Get two random, different numbers in [0:pointsFrom.cols()-1]
    index1 = distribution1( msacGenerator );
    index2 = distribution2( msacGenerator );
    if ( index2 >= index1 )
      index2++;
    Vector2d indices( index1, index2 );

    /*std::cout << "indices: " << indices.transpose()
    << " pointsFrom.cols: " << pointsFrom.cols()
    << " pointsTo.cols: " << pointsTo.cols() << std::endl;*/

    // Get T form Calculated from this set of points
    Matrix3d T = computeTform( pointsFrom, pointsTo, indices );

    VectorXd dis = evaluateTform( pointsFrom, pointsTo, T, threshold );

    double accDis = dis.sum();

    if ( accDis < bestDist ) {
      bestDist = accDis;
      bestT = T;
    }
    idxTrial++;
  }

  VectorXd dis = evaluateTform( pointsFrom, pointsTo, bestT, threshold );
  threshold *= dis.mean();
  int numInliers = 0;
  for ( int i = 0; i < dis.rows(); i++ ){
    if ( dis(i) < threshold )
      numInliers++;
  }
  VectorXd inliers( numInliers );
  int j = 0;
  for ( int i = 0; i < dis.rows(); i++ ){
    if ( dis(i) < threshold )
      inliers(j++) = i;
  }

  Matrix3d T;
  if ( numInliers >= 2 )
    T = computeTform( pointsFrom, pointsTo, inliers );
  else
    T << 1, 0, 0,  0, 1, 0,  0, 0, 1;

  return T;
}