Eigen::Matrix4f CloudMapper::ComputeInitialAlignment(Cloud::Ptr &cloud1, Cloud::Ptr &cloud2, pcl::CorrespondencesPtr &correspondences)
{
if (correspondences->empty())
{
return Eigen::Matrix4f::Identity();
}
pcl::registration::CorrespondenceRejectorSampleConsensus<PointType> sac;
sac.setInlierThreshold(0.5);
sac.setInputSource(cloud1);
sac.setInputTarget(cloud2);
sac.setInputCorrespondences(correspondences);
pcl::Correspondences inliers;
sac.getCorrespondences(inliers);
Eigen::Matrix4f transformation;
if (m_estimateScale)
{
pcl::registration::TransformationEstimationSVDScale<PointType, PointType> transformationEstimation;
transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
}
else
{
pcl::registration::TransformationEstimationSVD<PointType, PointType> transformationEstimation;
transformationEstimation.estimateRigidTransformation(*cloud1, *cloud2, inliers, transformation);
}
if (transformation == Eigen::Matrix4f::Identity() || IsNotNAN(transformation) == false)
{
std::cout << "Identity or NAN" << std::endl;
}
return transformation;
}
void InitialGuess::displayCorrespondances(pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints1, pcl::PointCloud<pcl::PointXYZI>::Ptr keypoints2, pcl::CorrespondencesPtr corres,pcl::CorrespondencesPtr inlier)
{
for(int i=1;i<corres->size();i++)
{
pcl::PointXYZI srcpt=keypoints1->points.at(corres->at(i).index_query); //corres[i].index_query);
pcl::PointXYZI tgtpt=keypoints2->points.at(corres->at(i).index_match);
std::stringstream ss2 ("line_vp_2_");
ss2<<i;
p->addLine<pcl::PointXYZI>(srcpt,tgtpt,ss2.str(),vp_2);
}
for(int i=1;i<inlier->size();i++)
{
pcl::PointXYZI srcpt=keypoints1->points.at(inlier->at(i).index_query);
pcl::PointXYZI tgtpt=keypoints2->points.at(inlier->at(i).index_match);
std::stringstream ss3 ("line_vp_3_");
ss3<<i;
p->addLine<pcl::PointXYZI>(srcpt,tgtpt,ss3.str(),vp_3);
}
p->spin();
}
void
InitialGuess::findCorrespondance(PointCloudOfFpfhPtr descriptor1, PointCloudOfFpfhPtr descriptor2, pcl::CorrespondencesPtr &corres ){
pcl::registration::CorrespondenceEstimation< pcl::FPFHSignature33, pcl::FPFHSignature33 > cer;
//pcl::registration::CorrespondenceEstimation <pcl::PointXYZRGB,pcl::PointXYZRGB > cer2;
//pcl::Correspondences corres,recpCorres;
cer.setInputCloud(descriptor1);
cer.setInputTarget(descriptor2);
cer.determineCorrespondences(*corres);
//cer.determineReciprocalCorrespondences(*corres);
cerr<<"\nFPFH Corres size "<<corres->size();
}
void
print_correspondences_and_histograms(
pcl::CorrespondencesPtr correspondences,
pcl::PointCloud<pcl::FPFHSignature33>::Ptr features1,
pcl::PointCloud<pcl::FPFHSignature33>::Ptr features2
)
{
for (size_t i = 0; i < correspondences->size(); i++) {
int q = (*correspondences)[i].index_query;
int m = (*correspondences)[i].index_match;
std::cout << "!for correspondence n." << i
<< " index_query=" << q
<< " index_match=" << m
<< endl;
cout << "!!1!!";
print_feature_descriptor(features1->points[q]);
cout << "!!2!!";
print_feature_descriptor(features2->points[q]);
}
}
/* perform 2D SURF feature matching */
void match (Mat img_1, Mat img_2, vector<KeyPoint> keypoints_1,
vector<KeyPoint> keypoints_2, vector<DMatch> &good_matches,
pcl::CorrespondencesPtr &correspondences)
{
SurfDescriptorExtractor extractor;
Mat descriptors_1, descriptors_2;
extractor.compute (img_1, keypoints_1, descriptors_1);
extractor.compute (img_2, keypoints_2, descriptors_2);
//FlannBasedMatcher matcher;
BFMatcher matcher (NORM_L2);
std::vector<DMatch> matches;
matcher.match (descriptors_1, descriptors_2, matches);
double max_dist = 0;
double min_dist = 100;
for (int i = 0; i < descriptors_1.rows; i++)
{
double dist = matches[i].distance;
if (dist < min_dist)
min_dist = dist;
if (dist > max_dist)
max_dist = dist;
}
for (int i = 0; i < descriptors_1.rows; i++)
{
// need to change the factor "2" to adapt to different cases
if (matches[i].distance < 3 * min_dist) //may adapt for changes
{
good_matches.push_back (matches[i]);
}
}
correspondences->resize (good_matches.size ());
for (unsigned cIdx = 0; cIdx < good_matches.size (); cIdx++)
{
(*correspondences)[cIdx].index_query = good_matches[cIdx].queryIdx;
(*correspondences)[cIdx].index_match = good_matches[cIdx].trainIdx;
if (0) // for debugging
{
cout << good_matches[cIdx].queryIdx << " " << good_matches[cIdx].trainIdx
<< " " << good_matches[cIdx].distance << endl;
cout << good_matches.size () << endl;
}
}
// change the constant value of SHOW_MATCHING to 1 if you want to visulize the matching result
if (SHOW_MATCHING)
{
Mat img_matches;
drawMatches (img_1, keypoints_1, img_2, keypoints_2, good_matches,
img_matches, Scalar::all (-1), Scalar::all (-1), vector<char> (),
DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
//-- Show detected matches
imshow ("Good Matches", img_matches);
waitKey (0);
}
}
void
visualize_correspondences (const PointCloudPtr points1, const PointCloudPtr keypoints1,
const PointCloudPtr points2, const PointCloudPtr keypoints2,
const pcl::CorrespondencesPtr pCorrespondences
/* std::vector<int> &correspondences ,
const std::vector<float> &correspondence_scores, int max_to_display */)
{
// We want to visualize two clouds side-by-side, so do to this, we'll make copies of the clouds and transform them
// by shifting one to the left and the other to the right. Then we'll draw lines between the corresponding points
// Create some new point clouds to hold our transformed data
PointCloudPtr points_left (new PointCloud);
PointCloudPtr keypoints_left (new PointCloud);
PointCloudPtr points_right (new PointCloud);
PointCloudPtr keypoints_right (new PointCloud);
// Shift the first clouds' points to the left
//const Eigen::Vector3f translate (0.0, 0.0, 0.3);
const Eigen::Vector3f translate (0.4, 0.0, 0.0);
const Eigen::Quaternionf no_rotation (0, 0, 0, 0);
pcl::transformPointCloud (*points1, *points_left, -translate, no_rotation);
pcl::transformPointCloud (*keypoints1, *keypoints_left, -translate, no_rotation);
// Shift the second clouds' points to the right
pcl::transformPointCloud (*points2, *points_right, translate, no_rotation);
pcl::transformPointCloud (*keypoints2, *keypoints_right, translate, no_rotation);
// Add the clouds to the visualizer
pcl::visualization::PCLVisualizer vis;
vis.addPointCloud (points_left, "points_left");
vis.addPointCloud (points_right, "points_right");
// // Compute the weakest correspondence score to display
// std::vector<float> temp (correspondence_scores);
// std::sort (temp.begin (), temp.end ());
// if (max_to_display >= temp.size ())
// max_to_display = temp.size () - 1;
// float threshold = temp[max_to_display];
// std::cout << max_to_display << std::endl;
// Draw lines between the best corresponding points
for (size_t i = 0; i < pCorrespondences->size (); ++i)
{
// if (correspondence_scores[i] > threshold)
// {
// continue; // Don't draw weak correspondences
// }
// Get the pair of points
const PointT & p_left = keypoints_left->points[(*pCorrespondences)[i].index_query];
const PointT & p_right = keypoints_right->points[(*pCorrespondences)[i].index_match];
// Generate a random (bright) color
double r = (rand() % 100);
double g = (rand() % 100);
double b = (rand() % 100);
double max_channel = std::max (r, std::max (g, b));
r /= max_channel;
g /= max_channel;
b /= max_channel;
// Generate a unique string for each line
std::stringstream ss ("line");
ss << i;
// Draw the line
vis.addLine (p_left, p_right, r, g, b, ss.str ());
}
vis.resetCamera ();
vis.spin ();
}
Eigen::Matrix4f twostepAlign (const PointCloud &src_points,
const PointCloud &tgt_points, const PointCloud &src_key,
const PointCloud &tgt_key, pcl::CorrespondencesPtr correspondences,
const Eigen::Matrix4f initial_matrix, PointCloud &output, bool initial,
double threshold)
{
Eigen::Matrix4f matrix1; // initial
Eigen::Matrix4f matrix2; //fine*initial
Eigen::Matrix4f matrix_final;
// smart pointer no need to delete it at the end
PointCloudPtr src (new PointCloud (src_points));
PointCloudPtr tgt (new PointCloud (tgt_points));
bool flag = 1;
// initial alignment
if (initial && flag == 1)
{
if (correspondences->size () < 3)
{
printf (
"The correspondence points are less than 3. The RANSAC based initial alignment will be swithched off. \n");
printf ("Please relax the threshold for SURF feature mtaching. \n");
flag = 0;
}
else
matrix1 = computeInitialAlignment (src_key, tgt_key, correspondences);
}
// Switch off the initial alignment
if (!initial || flag == 0)
{
matrix1 << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1;
}
PointCloudPtr points_transformed (new PointCloud);
pcl::transformPointCloud (*src, *points_transformed, matrix1);
// fine registration
if (FINE_FLAG)
{
matrix2 = fineAlign (points_transformed, tgt, output, threshold,initial) * matrix1; //pairwise alignment matrix
}
else // Switch off the fine alignment
{
matrix2 = matrix1;
output = *points_transformed;
}
// initial_matrix: the initial alignment refered to the first frame
// output is the transformed point cloud refered to the firs frame
matrix_final = initial_matrix * matrix2;
pcl::transformPointCloud (output, output, initial_matrix);
// print the transform for debugging
if (PRINT_TRANSFORM_PAIR)
{
cout << "Final Tranform: " << endl;
for (int k = 0; k < 3; k++)
{
for (int j = 0; j < 4; j++)
{
cout << matrix_final (k, j) << " ";
}
cout << endl;
}
}
// return the final ransfrom relative to the first frame
return matrix_final;
}
void cluster(const pcl::CorrespondencesPtr &object_world_corrs)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("cg_size" , cg_size_ , 0.01 );
nh_param.param<double>("cg_thresh", cg_thresh_, 5.0);
//
// Debug output
//
PointCloud correspondence_object;
PointCloud correspondence_world;
for (int j = 0; j < object_world_corrs->size (); ++j)
{
PointType& object_point = object_keypoints->at(object_world_corrs->at(j).index_query);
PointType& world_point = world_keypoints->at(object_world_corrs->at(j).index_match);
correspondence_object.push_back(object_point);
correspondence_world.push_back(world_point);
// cout << object_point.x << " " << object_point.y << " " << object_point.z << endl;
// cout << world_point.x << " " << world_point.y << " " << world_point.z << endl;
}
PointCloudROS pub_me_object2;
PointCloudROS pub_me_world2;
toROSMsg(correspondence_object, pub_me_object2);
toROSMsg(correspondence_world, pub_me_world2);
pub_me_object2.header.frame_id = "/object";
pub_me_world2.header.frame_id = "/world";
pub_object2.publish(pub_me_object2);
pub_world2.publish(pub_me_world2);
//
// Actual Clustering
//
cout << "... clustering ..." << endl;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (object_keypoints);
gc_clusterer.setSceneCloud (world_keypoints);
gc_clusterer.setModelSceneCorrespondences (object_world_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
//
// Output results
//
std::cout << "Object instances found: " << rototranslations.size () << std::endl;
int maximum = 0;
int best;
for (int i = 0; i < rototranslations.size (); ++i)
{
cout << "Instance "<< i << " has " << clustered_corrs[i].size () << " correspondences" << endl;
if (maximum < clustered_corrs[i].size ())
{
maximum = clustered_corrs[i].size ();
best = i;
}
}
if (rototranslations.size () > 0)
{
cout << "selecting instance " << best << " and calculating TF" << endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[best].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[best].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
// convert Eigen matricies into ROS TF message
tf::Vector3 object_offset;
tf::Quaternion object_rotation;
object_offset[0] = translation (0);
object_offset[1] = translation (1);
object_offset[2] = translation (2);
// convert rotation matrix to quaternion
Eigen::Quaternionf quaternion (rotation);
object_rotation[0] = quaternion.x();
object_rotation[1] = quaternion.y();
object_rotation[2] = quaternion.z();
object_rotation[3] = quaternion.w();
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin (object_offset);
transform.setRotation (object_rotation);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "object"));
//
// Debug output
//
PointCloud correspondence_object_cluster;
PointCloud correspondence_world_cluster;
for (int j = 0; j < clustered_corrs[best].size (); ++j)
{
PointType& model_point = object_keypoints->at(clustered_corrs[best][j].index_query);
PointType& scene_point = world_keypoints->at(clustered_corrs[best][j].index_match);
correspondence_object_cluster.push_back(model_point);
correspondence_world_cluster.push_back(scene_point);
//cout << model_point.x << " " << model_point.y << " " << model_point.z << endl;
//cout << scene_point.x << " " << scene_point.y << " " << scene_point.z << endl;
}
PointCloudROS pub_me_object;
PointCloudROS pub_me_world;
toROSMsg(correspondence_object_cluster, pub_me_object);
toROSMsg(correspondence_world_cluster, pub_me_world);
pub_me_object.header.frame_id = "/object";
pub_me_world.header.frame_id = "/world";
pub_object.publish(pub_me_object);
pub_world.publish(pub_me_world);
}
}
