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