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 #include "gicp-fallback.h"
00026
00027 #include <fstream>
00028 #include <sstream>
00029 #include <iostream>
00030 #include <pcl/filters/voxel_grid.h>
00031 using namespace std;
00032
00033 void saveCloud(const char* filename, const pointcloud_type& pc, const int max_cnt, const bool color){
00034
00035 ofstream of;
00036 of.open(filename);
00037 assert(of.is_open());
00038
00039
00040 int write_step = 1;
00041 if (max_cnt>0 && (int)pc.points.size()>max_cnt)
00042 write_step = floor(pc.points.size()*1.0/max_cnt);
00043
00044 int cnt = 0;
00045 assert(write_step > 0);
00046
00047
00048
00049 for (unsigned int i=0; i<pc.points.size(); i += write_step)
00050 {
00051 point_type p = pc.points[i];
00052
00053 bool invalid = (isnan(p.x) || isnan(p.y) || isnan(p.z));
00054 if (invalid)
00055 continue;
00056
00057
00058 of << p.x << "\t" << p.y << "\t" << p.z;
00059 if (color) {
00060
00061 int color = *reinterpret_cast<const int*>(&p.rgb);
00062 int r = (0xff0000 & color) >> 16;
00063 int g = (0x00ff00 & color) >> 8;
00064 int b = 0x0000ff & color;
00065 of << "\t \t" << r << "\t" << g << "\t" << b << "\t" << endl;
00066 }
00067 else
00068 of << endl;
00069
00070
00071 cnt++;
00072 }
00073
00074
00075
00076
00077
00078 of.close();
00079
00080 }
00081
00082
00083 void downSample(const pointcloud_type& src, pointcloud_type& to){
00084 pcl::VoxelGrid<point_type> down_sampler;
00085 down_sampler.setLeafSize (0.01, 0.01, 0.01);
00086 pcl::PCLBase<point_type>::PointCloudConstPtr const_cloud_ptr = boost::make_shared<pointcloud_type> (src);
00087 down_sampler.setInputCloud (const_cloud_ptr);
00088 down_sampler.filter(to);
00089 ROS_INFO("gicp.cpp: Downsampling from %i to %i", (int) src.points.size(), (int) to.points.size());
00090 }
00091
00092
00093
00094 bool gicpfallback(const pointcloud_type& from, const pointcloud_type& to, Eigen::Matrix4f& transform){
00095
00096
00097
00098 FILE *fp;
00099 char f1[200];
00100 char f2[200];
00101
00102 char line[130];
00103 char cmd[200];
00104
00105 sprintf(f1, "pc1.txt");
00106 sprintf(f2, "pc2.txt");
00107
00108
00109 sprintf(cmd, "/home/endres/Phd/rospacks/rgbdslam/external/gicp/test_gicp %s %s --d_max 0.1 --debug 0",f1,f2);
00110
00111 int N = 10000;
00112
00113 saveCloud(f1,from,N);
00114 saveCloud(f2,to,N);
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124 fp = popen(cmd, "r");
00125
00126 std::vector<string> lines;
00127
00128
00129 while ( fgets( line, sizeof line, fp))
00130 {
00131 lines.push_back(line);
00132
00133 }
00134 int retval = pclose(fp);
00135
00136
00137
00138
00139 if(retval != 0){
00140 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);
00141 transform = Eigen::Matrix<float, 4, 4>::Identity();
00142 return false;
00143 }
00144
00145 int pos = 0;
00146
00147 for (unsigned int i=lines.size()-5; i<lines.size()-1; i++){
00148
00149 stringstream ss(lines.at(i));
00150 for (int j=0; j<4; j++)
00151 {
00152 ss >> line;
00153 transform(pos,j) = atof(line);
00154 }
00155
00156 pos++;
00157 }
00158
00159
00160 stringstream ss(lines.at(lines.size()-1));
00161 ss >> line;
00162 ss >> line;
00163 ss >> line;
00164
00165 int iter_cnt;
00166 iter_cnt = atoi(line);
00167
00168 return iter_cnt < 200;
00169
00170
00171
00172
00173
00174
00175 }