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