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); } }