void testPointPlaneICP()
{
Random rnd( 0 );
Matrix4f dstToSrc = Matrix4f::translation( 0.1f, -0.42f, 0.2f ) * Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 20.f ) );
Matrix4f srcToDstGT = dstToSrc.inverse();
int nPoints = 1024;
std::vector< Vector3f > srcPoints( nPoints );
std::vector< Vector3f > dstPoints( nPoints );
std::vector< Vector3f > dstNormals( nPoints );
for( size_t i = 0; i < dstPoints.size(); ++i )
{
dstPoints[i] = Vector3f( rnd.nextFloat(), rnd.nextFloat(), rnd.nextFloat() );
dstNormals[i] = Sampling::areaSampleSphere( rnd.nextFloat(), rnd.nextFloat() );
srcPoints[i] = dstToSrc.transformPoint( dstPoints[i] );
}
PointPlaneICP icp( 6, 0.01f );
Matrix4f initialGuess = Matrix4f::identity();
Matrix4f mSolution;
bool succeeded = icp.align( srcPoints, dstPoints, dstNormals, initialGuess, mSolution );
Matrix4f diff = srcToDstGT.inverse() - mSolution;
diff.print();
}
bool PatternDetector::refineMatchesWithHomography
(
const std::vector<cv::KeyPoint>& queryKeypoints,
const std::vector<cv::KeyPoint>& trainKeypoints,
float reprojectionThreshold,
std::vector<cv::DMatch>& matches,
cv::Mat& homography
)
{
const int minNumberMatchesAllowed = 8;
if (matches.size() < minNumberMatchesAllowed)
return false;
// Prepare data for cv::findHomography
std::vector<cv::Point2f> srcPoints(matches.size());
std::vector<cv::Point2f> dstPoints(matches.size());
for (size_t i = 0; i < matches.size(); i++)
{
srcPoints[i] = trainKeypoints[matches[i].trainIdx].pt;
dstPoints[i] = queryKeypoints[matches[i].queryIdx].pt;
}
// Find homography matrix and get inliers mask
std::vector<unsigned char> inliersMask(srcPoints.size());
homography = cv::findHomography(srcPoints,
dstPoints,
CV_FM_RANSAC,
reprojectionThreshold,
inliersMask);
std::vector<cv::DMatch> inliers;
for (size_t i=0; i<inliersMask.size(); i++)
{
if (inliersMask[i])
inliers.push_back(matches[i]);
}
matches.swap(inliers);
return matches.size() > minNumberMatchesAllowed;
}