Go to the documentation of this file.00001 #ifdef USE_PCL_ICP
00002
00003 #ifdef HEMACLOUDS
00004 #define PCL_NO_PRECOMPILE
00005 #endif
00006 #include <iostream>
00007 #include "parameter_server.h"
00008 #include <pcl/io/pcd_io.h>
00009 #include <pcl/point_types.h>
00010 #include <pcl/registration/icp_nl.h>
00011 #include <pcl/registration/icp.h>
00012 #include <pcl/registration/impl/icp_nl.hpp>
00013 #include <pcl/registration/impl/icp.hpp>
00014 #include <pcl/kdtree/impl/kdtree_flann.hpp>
00015
00016
00017 #include <pcl/filters/filter.h>
00018 #include "scoped_timer.h"
00019
00020 void filterCloud(const pointcloud_type& cloud_in, pointcloud_type& cloud_out, int desired_size){
00021 ScopedTimer s(__FUNCTION__);
00022 cloud_out.clear();
00023
00024 std::vector<int> non_nan_indices;
00025 non_nan_indices.reserve(cloud_in.size());
00026 for (unsigned int i=0; i<cloud_in.size(); i++ ){
00027 if (!isnan(cloud_in.points.at(i).z))
00028 non_nan_indices.push_back(i);
00029 }
00030 ROS_INFO("Nonnans in cloud: %zu", non_nan_indices.size());
00031
00032
00033 cloud_out.reserve(desired_size+1);
00034 float step = non_nan_indices.size()/static_cast<float>(desired_size);
00035 step = step < 1.0 ? 1.0 : step;
00036 for (float i=0; i<non_nan_indices.size(); i+=step ){
00037 unsigned int index = non_nan_indices.at(static_cast<unsigned int>(i));
00038 cloud_out.push_back(cloud_in.points.at(index));
00039 }
00040 cloud_out.width=cloud_out.size();
00041 cloud_out.height=1;
00042 cloud_out.is_dense=true;
00043 ROS_INFO("Subsampled cloud to: %zu", cloud_out.size());
00044
00045 }
00046
00047 Eigen::Matrix4f icpAlignment(pointcloud_type::Ptr cloud_1, pointcloud_type::Ptr cloud_2, Eigen::Matrix4f initial_guess){
00048 ScopedTimer s(__FUNCTION__);
00049 pcl::IterativeClosestPoint<point_type, point_type>* icp = NULL;
00050 std::string icp_method = ParameterServer::instance()->get<std::string>("icp_method") ;
00051 if(icp_method == "icp"){
00052 icp = new pcl::IterativeClosestPoint<point_type, point_type>();
00053 } else if (icp_method == "icp_nl"){
00054 icp = new pcl::IterativeClosestPointNonLinear<point_type, point_type>();
00055 } else{
00056 ROS_WARN("Unknown icp method \"%s\". Using regular icp", icp_method.c_str());
00057 icp = new pcl::IterativeClosestPoint<point_type, point_type>();
00058 }
00059
00060
00061 pointcloud_type::Ptr filtered_in(new pointcloud_type);
00062 pointcloud_type::Ptr filtered_out(new pointcloud_type);
00063 std::vector<int> indices_nonused;
00064
00065 icp->setInputSource(cloud_1);
00066 icp->setInputTarget(cloud_2);
00067
00068 icp->setMaxCorrespondenceDistance(0.05);
00069
00070 icp->setMaximumIterations(50);
00071
00072 icp->setTransformationEpsilon(1e-8);
00073
00074 icp->setEuclideanFitnessEpsilon(1);
00075
00076 pointcloud_type cloud_registered;
00077 icp->align(cloud_registered, initial_guess);
00078
00079 Eigen::Matrix4f transformation = icp->getFinalTransformation();
00080 std::cout << "has converged:" << icp->hasConverged() << " score: " << icp->getFitnessScore() << std::endl;
00081 if(icp->hasConverged()){
00082 delete icp;
00083 return transformation;
00084 } else {
00085 delete icp;
00086 return initial_guess;
00087 }
00088
00089 }
00090
00091 #endif