int main(int argc, char** argv)
{
// Initialize ROS
ros::init (argc, argv, "seg_block");
ros::NodeHandle nh;
// --- Set Up Template Container --- //
// Load the object templates specified in the object_templates.txt file
object_templates.clear();
FeatureCloud template_cloud1;
template_cloud1.makeCube(0.03);
object_templates.push_back(template_cloud1);
FeatureCloud template_cloud2;
template_cloud2.makeCube(0.04);
object_templates.push_back(template_cloud2);
FeatureCloud template_cloud3;
template_cloud3.makeCube(0.045);
object_templates.push_back(template_cloud3);
FeatureCloud template_cloud4;
template_cloud4.makeCube(0.05);
object_templates.push_back(template_cloud4);
FeatureCloud template_cloud5;
template_cloud5.makeCube(0.055);
object_templates.push_back(template_cloud5);
for (size_t i = 0; i < object_templates.size (); ++i)
{
template_align.addTemplateCloud (object_templates[i]);
}
// Create visualizer
//visualizer_o_Ptr = pcl::visualization::PCLVisualizer::Ptr(new pcl::visualization::PCLVisualizer());
// Create a ROS publisher for the pose of the block relative to the ASUS.
pub = nh.advertise<geometry_msgs::Pose>("/block_pose", 1);
cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/filtered_cloud", 1);
// Create a ROS subscriber for the input point cloud
ros::Subscriber sub = nh.subscribe ("camera/depth_registered/points", 1, cloud_cb);
// Create a subscriber for the current state
ros::Subscriber stateSub = nh.subscribe("/control_current_state", 1, current_state_cb );
// Spin
ros::spin();
}
int sac_ia_alignment(pcl::PointCloud<pcl::PointXYZ>::Ptr prev_cloud,
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in) {
pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>(*prev_cloud));
std::cout << "Performing Sample Consensus Initial Alignment.. "
<< std::endl;
FeatureCloud targetCloud;
FeatureCloud templateCloud;
targetCloud.setInputCloud(temp_cloud);
templateCloud.setInputCloud(cloud_in);
TemplateAlignment templateAlign;
templateAlign.addTemplateCloud(templateCloud);
templateAlign.setTargetCloud(targetCloud);
Result bestAlignment;
std::cout << "entering alignment" << std::endl;
templateAlign.findBestAlignment(bestAlignment);
std::cout << "exiting alignment" << std::endl;
printf("Fitness Score: %f \n", bestAlignment.fitness_score);
Eigen::Matrix3f rotation = bestAlignment.final_transformation.block<3,3>(0, 0);
Eigen::Vector3f translation =
bestAlignment.final_transformation.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));
pcl::transformPointCloud(*templateCloud.getPointCloud(), *cloud_in,
bestAlignment.final_transformation);
std::cout << "***Initial alignment complete***" << std::endl;
}