int VoslamRigidTransformation::estimateVisual3DRigidTransformation(
const std::vector<carmen_vector_2D_t>& current_features,
const std::vector<carmen_vector_2D_t>& previous_features,
// const unsigned short* depth1,
// const unsigned short* depth2,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pointCloudPtr1,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr& pointCloudPtr2,
Eigen::Matrix<float, 4, 4>& H)
{
correspondences.reset(new pcl::Correspondences);
correspondences->clear();
correspondences->resize(current_features.size());
matchesWith3DValidData(current_features, previous_features,pointCloudPtr1,pointCloudPtr2);
for(int i = 0; i < correspondences->size(); i++)
{
printf("%d %d %f", (*correspondences)[i].index_query, (*correspondences)[i].index_query, (*correspondences)[i].distance);
}
corrsRejectorSAC.setInputCloud(pointCloudPtr1);
corrsRejectorSAC.setInputTarget(pointCloudPtr2);
corrsRejectorSAC.setInlierThreshold(0.05);
corrsRejectorSAC.setMaximumIterations(500);
corrsRejectorSAC.setInputCorrespondences(correspondences);
// boost::shared_ptr<pcl::Correspondences> correspondencesRejSAC (new pcl::Correspondences);
// corrsRejectorSAC.getCorrespondences(*correspondencesRejSAC);
H = corrsRejectorSAC.getBestTransformation();
//Return the number of RANSAC inliers
// return correspondencesRejSAC->size();
}
int Visual3DRigidTransformationEstimator_RANSAC::estimateVisual3DRigidTransformation(
const std::vector<cv::Point2f>& points2d1,
const std::vector<cv::Point2f>& points2d2,
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloudPtr1,
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloudPtr2,
Eigen::Matrix4f& H)
{
correspondences.reset(new pcl::Correspondences);
correspondences->clear();
correspondences->resize(points2d1.size());
matchesWith3DValidData(points2d1,points2d2,pointCloudPtr1,pointCloudPtr2);
corrsRejectorSAC.setInputCloud(pointCloudPtr1);
corrsRejectorSAC.setTargetCloud(pointCloudPtr2);
corrsRejectorSAC.setInlierThreshold(0.05);
corrsRejectorSAC.setMaxIterations(500);
corrsRejectorSAC.setInputCorrespondences(correspondences);
boost::shared_ptr<pcl::Correspondences> correspondencesRejSAC (new pcl::Correspondences);
corrsRejectorSAC.getCorrespondences(*correspondencesRejSAC);
H = corrsRejectorSAC.getBestTransformation();
//Return the number of RANSAC inliers
return correspondencesRejSAC->size();
}
int Visual3DRigidTransformationEstimator_SVD::estimateVisual3DRigidTransformation(
const std::vector<cv::Point2f>& points2d1,
const std::vector<cv::Point2f>& points2d2,
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloudPtr1,
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& pointCloudPtr2,
Eigen::Matrix4f& H)
{
correspondences.clear();
correspondences.resize(points2d1.size());
matchesWith3DValidData(points2d1,points2d2,pointCloudPtr1,pointCloudPtr2);
transformationEstimator.estimateRigidTransformation(*pointCloudPtr1,*pointCloudPtr2,correspondences,H);
//Return -1 to let the user know that this function returns nothing useful
return -1;
}
