Go to the documentation of this file.00001 #include<hrl_phri_2011/pcl_features.h>
00002
00003 void computeNormals(const PCRGB::Ptr& in_pc, const KDTree::Ptr& in_kd_tree, PCNormals::Ptr& out_normals)
00004 {
00005 pcl::MovingLeastSquares<PRGB, pcl::Normal> mls;
00006 PCRGB mls_points;
00007 mls.setInputCloud(in_pc);
00008 mls.setPolynomialFit(true);
00009 mls.setSearchMethod(in_kd_tree);
00010 mls.setSearchRadius(0.02);
00011 mls.setOutputNormals(out_normals);
00012 mls.reconstruct(mls_points);
00013 }
00014
00015 void computeFPFH(const PCRGB::Ptr& in_pc, FPFeatures::Ptr& out_features)
00016 {
00017 KDTree::Ptr kd_tree(new pcl::KdTreeFLANN<PRGB> ());
00018 PCNormals::Ptr normals(new PCNormals());
00019 computeNormals(in_pc, kd_tree, normals);
00020 pcl::FPFHEstimation<PRGB, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
00021 fpfh_est.setInputCloud(in_pc);
00022 fpfh_est.setInputNormals(normals);
00023 fpfh_est.setSearchMethod(kd_tree);
00024 fpfh_est.setRadiusSearch(0.02);
00025 fpfh_est.compute(*out_features);
00026 }
00027
00028 void computeSACRegistration(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc)
00029 {
00030 ROS_INFO("IN ALIGN");
00031 FPFeatures::Ptr target_features(new FPFeatures());
00032 FPFeatures::Ptr source_features(new FPFeatures());
00033 computeFPFH(target_pc, target_features);
00034 computeFPFH(source_pc, source_features);
00035 ROS_INFO("OUT FEATS");
00036 pcl::SampleConsensusInitialAlignment<PRGB, PRGB, pcl::FPFHSignature33> sac_ia;
00037 sac_ia.setInputTarget(target_pc);
00038 sac_ia.setTargetFeatures(target_features);
00039 sac_ia.setInputCloud(source_pc);
00040 sac_ia.setSourceFeatures(source_features);
00041 PCRGB reg_out;
00042 sac_ia.align(reg_out);
00043 ROS_INFO("OUT ALIGN1");
00044 Eigen::Affine3d sac_tf;
00045 sac_tf.matrix() = sac_ia.getFinalTransformation().cast<double>();
00046 PCRGB tf_pc;
00047 transformPC(*source_pc, tf_pc, sac_tf);
00048 tf_pc += *target_pc;
00049 ROS_INFO("OUT ALIGN, %f %f %f", sac_tf.matrix()(0, 3), sac_tf.matrix()(1, 3), sac_tf.matrix()(2, 3));
00050 reg_out += *target_pc;
00051 ROS_INFO("in pub");
00052 pubLoop(tf_pc, "/pub_pc");
00053 }
00054
00055 void computeICPRegistration(const PCRGB::Ptr& target_pc, const PCRGB::Ptr& source_pc,
00056 Eigen::Affine3d& tf_mat, int max_iters, double color_weight)
00057 {
00058 pcl::IterativeClosestPointNonLinear<PRGB, PRGB> icp;
00059 boost::shared_ptr<pcl::DefaultPointRepresentation<PRGB> const> pt_rep(new pcl::DefaultPointRepresentation<PRGB>(color_weight, color_weight, color_weight));
00060 icp.setPointRepresentation(pt_rep);
00061 icp.setInputTarget(target_pc);
00062 icp.setInputCloud(source_pc);
00063 icp.setMaximumIterations(max_iters);
00064 PCRGB tmp_pc;
00065 icp.align(tmp_pc);
00066 tf_mat = icp.getFinalTransformation().cast<double>();
00067 }