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;
}
void CModelQuery2D::computePoseEstimation()
{
std::vector<cv::DMatch> & validCorr = matchingResults_;
if ( best_k_sorted_2D_ > 0 )
{
std::sort (validCorr.begin (), validCorr.end (),
sort2DCorrespondencesByDistance ());
validCorr.resize(best_k_sorted_2D_);
}
vector<cv::KeyPoint> modelKeyPoints = modelFeatures_.getKeypoints();
vector<cv::KeyPoint> queryKeyPoints = queryFeatures_.getKeypoints();
for ( unsigned i = 0; i < validCorr.size(); i++ )
{
modelPoints_.push_back(modelKeyPoints[ validCorr[i].queryIdx].pt);
queryPoints_.push_back(queryKeyPoints[ validCorr[i].trainIdx].pt);
}
vector<unsigned char> inliersMask(modelPoints_.size());
H_ = findHomography(modelPoints_ , queryPoints_, CV_RANSAC, inputParams_.ransac_inlier_threshold_2D, inliersMask );
vector<cv::DMatch> inliers;
unsigned inliersCount = 0;
for ( unsigned i = 0; i < inliersMask.size(); i++ )
{
if ( inliersMask[i] )
{
inliersCount++;
inliers.push_back(validCorr[i]);
}
}
// fill our performance data structure
perfData & pd = pGlobalPerf_->pd[descriptorType_];
pd.descID = descriptorType_;
pd.actualRotation = inputParams_.rot_deg;
pd.inlierCount = inliersCount;
pd.inlierRate = (float) inliersCount / validCorr.size();
if ( inputParams_.live_sensor ) // assumes horizontal rotation
{
pd.computedRotation = pcl::rad2deg ((atan(H_.at<double>(6)/H_.at<double>(0))));
// if live_sensor, perfData_.actualRotation is ignored.
}
else // simulated rotation
{
pd.computedRotation = pcl::rad2deg ((atan(H_.at<double>(3)/H_.at<double>(0))));
}
pd.rotEstError = abs(abs(pd.computedRotation) - abs(pd.actualRotation));
pd.averageDistance = -1.0; //## not in use
if ( inputParams_.debug_level > OUTPUT_TRANSFORMATIONS )
{
// print out our H matrix
pcl::console::print_info ("----------------------------------------------------------------------------\n");
pcl::console::print_info ("2D DescID: %d, Computed Rotation: %8.3f, actual Rotation: %8.3f\n",
pd.descID,pd.computedRotation, pd.actualRotation);
pcl::console::print_info ("inlierCount: %d, inlierRate: %8.3f\n", pd.inlierCount, pd.inlierRate);
pcl::console::print_info ("\n");
pcl::console::print_info (" | %8.3f %8.3f %8.3f | \n", H_.at<double>(0), H_.at<double>(1), H_.at<double>(2) );
pcl::console::print_info ("R = | %8.3f %8.3f %8.3f | \n", H_.at<double>(3), H_.at<double>(4), H_.at<double>(5) );
pcl::console::print_info (" | %8.3f %8.3f %8.3f | \n", H_.at<double>(6), H_.at<double>(7), H_.at<double>(8) );
pcl::console::print_info ("\n");
}
}
