Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025 #ifdef USE_ICP_BIN
00026 #define PCL_NO_PRECOMPILE
00027 #include "gicp-fallback.h"
00028
00029 #include <fstream>
00030 #include <sstream>
00031 #include <iostream>
00032 #include <pcl/filters/voxel_grid.h>
00033 #include <pcl/filters/impl/voxel_grid.hpp>
00034 #include <pcl/registration/icp.h>
00035 #include <pcl/registration/impl/icp.hpp>
00036 #include <pcl/registration/registration.h>
00037 #include <pcl/registration/impl/registration.hpp>
00038 using namespace std;
00039
00040 void saveCloud(const char* filename, const pointcloud_type& pc, const int max_cnt, const bool color){
00041
00042 ofstream of;
00043 of.open(filename);
00044 assert(of.is_open());
00045
00046
00047 int write_step = 1;
00048 if (max_cnt>0 && (int)pc.points.size()>max_cnt)
00049 write_step = floor(pc.points.size()*1.0/max_cnt);
00050
00051 int cnt = 0;
00052 assert(write_step > 0);
00053
00054
00055
00056 for (unsigned int i=0; i<pc.points.size(); i += write_step)
00057 {
00058 point_type p = pc.points[i];
00059
00060 bool invalid = (isnan(p.x) || isnan(p.y) || isnan(p.z));
00061 if (invalid)
00062 continue;
00063
00064
00065 of << p.x << "\t" << p.y << "\t" << p.z;
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077 of << endl;
00078
00079
00080 cnt++;
00081 }
00082
00083
00084
00085
00086
00087 of.close();
00088
00089 }
00090
00091
00092 void downSample(const pointcloud_type& src, pointcloud_type& to){
00093 pcl::VoxelGrid<point_type> down_sampler;
00094 down_sampler.setLeafSize (0.01, 0.01, 0.01);
00095 pcl::PCLBase<point_type>::PointCloudConstPtr const_cloud_ptr = boost::make_shared<pointcloud_type> (src);
00096 down_sampler.setInputCloud (const_cloud_ptr);
00097 down_sampler.filter(to);
00098 ROS_INFO("gicp.cpp: Downsampling from %i to %i", (int) src.points.size(), (int) to.points.size());
00099 }
00100
00101
00102
00103 bool gicpfallback(const pointcloud_type& from, const pointcloud_type& to, Eigen::Matrix4f& transform){
00104
00105
00106
00107 FILE *fp;
00108 char f1[200];
00109 char f2[200];
00110
00111 char line[130];
00112 char cmd[200];
00113
00114 sprintf(f1, "pc1.txt");
00115 sprintf(f2, "pc2.txt");
00116
00117
00118 sprintf(cmd, "/home/endres/Phd/rospacks/rgbdslam/external/gicp/test_gicp %s %s --d_max 0.1 --debug 0",f1,f2);
00119
00120 int N = 10000;
00121
00122 saveCloud(f1,from,N);
00123 saveCloud(f2,to,N);
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133 fp = popen(cmd, "r");
00134
00135 std::vector<string> lines;
00136
00137
00138 while ( fgets( line, sizeof line, fp))
00139 {
00140 lines.push_back(line);
00141
00142 }
00143 int retval = pclose(fp);
00144
00145
00146
00147
00148 if(retval != 0){
00149 ROS_ERROR_ONCE("Non-zero return value from %s: %i. Identity transformation is returned instead of GICP result.\nThis error will be reported only once.", cmd, retval);
00150 transform = Eigen::Matrix<float, 4, 4>::Identity();
00151 return false;
00152 }
00153
00154 int pos = 0;
00155
00156 for (unsigned int i=lines.size()-5; i<lines.size()-1; i++){
00157
00158 stringstream ss(lines.at(i));
00159 for (int j=0; j<4; j++)
00160 {
00161 ss >> line;
00162 transform(pos,j) = atof(line);
00163 }
00164
00165 pos++;
00166 }
00167
00168
00169 stringstream ss(lines.at(lines.size()-1));
00170 ss >> line;
00171 ss >> line;
00172 ss >> line;
00173
00174 int iter_cnt;
00175 iter_cnt = atoi(line);
00176
00177 return iter_cnt < 200;
00178
00179
00180
00181
00182
00183
00184 }
00185
00186 #endif