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