00001 #include <NDTMatcher.hh>
00002 #include <NDTMatcherF2F.hh>
00003 #include <NDTMap.hh>
00004 #include <OctTree.hh>
00005 #include <AdaptiveOctTree.hh>
00006 #include "ros/ros.h"
00007 #include "pcl/point_cloud.h"
00008 #include "sensor_msgs/PointCloud2.h"
00009 #include "pcl/io/pcd_io.h"
00010 #include "pcl/features/feature.h"
00011 #include "pcl/registration/icp.h"
00012 #include "pcl/filters/voxel_grid.h"
00013
00014 #include <cstdio>
00015 #include <Eigen/Eigen>
00016 #include <PointCloudUtils.hh>
00017 #include <fstream>
00018
00019 #define DEBUG_COVARIANCE 0
00020
00021 using namespace std;
00022
00023 float ranf()
00024 {
00025 return (float)rand()/(float)RAND_MAX;
00026 }
00027
00028 float box_muller(float m, float s)
00029 {
00030
00031 float x1, x2, w, y1;
00032 static float y2;
00033 static int use_last = 0;
00034
00035 if (use_last)
00036 {
00037 y1 = y2;
00038 use_last = 0;
00039 }
00040 else
00041 {
00042 do
00043 {
00044 x1 = 2.0 * ranf() - 1.0;
00045 x2 = 2.0 * ranf() - 1.0;
00046 w = x1 * x1 + x2 * x2;
00047 }
00048 while ( w >= 1.0 );
00049
00050 w = sqrt( (-2.0 * log( w ) ) / w );
00051 y1 = x1 * w;
00052 y2 = x2 * w;
00053 use_last = 1;
00054 }
00055
00056 return( m + y1 * s );
00057 }
00058
00059
00060 bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed, pcl::PointCloud<pcl::PointXYZ> &moving,
00061 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
00062 {
00063
00064 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
00065 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
00066 pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
00067 pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );
00068
00069 pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00070 gr1.setLeafSize(0.1,0.1,0.1);
00071 gr2.setLeafSize(0.1,0.1,0.1);
00072
00073 gr1.setInputCloud(m);
00074 gr2.setInputCloud(f);
00075
00076 cloud_in->height = 1;
00077 cloud_in->width = cloud_in->points.size();
00078 cloud_out->height = 1;
00079 cloud_out->width = cloud_out->points.size();
00080 cloud_in->is_dense = false;
00081 cloud_out->is_dense = false;
00082
00083 gr1.filter(*cloud_in);
00084 gr2.filter(*cloud_out);
00085
00086 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
00087
00088 icp.setMaximumIterations(1000);
00089
00090 icp.setInputCloud(cloud_in);
00091 icp.setInputTarget(cloud_out);
00092
00093 icp.setRANSACOutlierRejectionThreshold (2);
00094 icp.setMaxCorrespondenceDistance(10);
00095 icp.setTransformationEpsilon(0.00001);
00096
00097
00098
00099 pcl::PointCloud<pcl::PointXYZ> Final;
00100 icp.align(Final);
00101
00102
00103
00104
00105
00106
00107
00108 Tout = (icp.getFinalTransformation()).cast<double>();
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119 return icp.hasConverged();
00120
00121 }
00122
00123 int
00124 main (int argc, char** argv)
00125 {
00126 std::ofstream logger ("/home/tsv/ndt_tmp/covariance.m");
00127 cout.precision(15);
00128
00129 pcl::PointCloud<pcl::PointXYZ> prev, curr, currHere, currHere2;
00130 char prevName[500], currName[500];
00131 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> prevPose, currPose;
00132
00133 double __res[] = {0.2, 0.4, 1, 2};
00134 std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00135 lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
00136 lslgeneric::NDTMatcher matcherP2F;
00137
00138 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Pr, R;
00139 struct timeval tv_now;
00140
00141 gettimeofday(&tv_now, NULL);
00142 srand(tv_now.tv_usec);
00143
00144 int N_SAMPLES = 100;
00145
00146 if(argc != 2)
00147 {
00148 std::cout<<"Usage: "<<argv[0]<<" configFile\n";
00149 return -1;
00150 }
00151
00152 FILE *fin = fopen(argv[1],"r");
00153 double xd,yd,zd, x,y,z,w, ts;
00154 string prefix;
00155 char *line = NULL;
00156 size_t len;
00157 bool first = true;
00158 double randX, randY, randZ, randRoll, randPitch, randYaw;
00159 double dev_x = 0.3 ,dev_y=0.3 ,dev_z =0.3 ,dev_roll =0.1,dev_pitch =0.1,dev_yaw=0.1;
00160
00161 int ctr = 0;
00162
00163
00164 int n = getline(&line,&len,fin);
00165 if(n <=0 ) return -1;
00166 prefix = line;
00167 *(prefix.rbegin()) = '\0';
00168
00169 while(getline(&line,&len,fin) > 0)
00170 {
00171
00172 int n = sscanf(line,"%lf %lf %lf %lf %lf %lf %lf %lf",
00173 &ts,&xd,&yd,&zd,&x,&y,&z,&w);
00174 if(n != 8)
00175 {
00176 cout<<"wrong format of pose at : "<<line<<endl;
00177 break;
00178 }
00179
00180 if(first)
00181 {
00182
00183 prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
00184 Eigen::Quaternion<double>(w,x,y,z);
00185 first = false;
00186 ctr++;
00187 continue;
00188 }
00189
00190
00191 currPose = Eigen::Translation<double,3>(xd,yd,zd)*
00192 Eigen::Quaternion<double>(w,x,y,z);
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215
00216
00217
00218
00219
00220
00221 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> P = prevPose.inverse()*currPose;
00222
00223 cout<<"testing scan "<<ctr-1<<" to "<<ctr<<endl;
00224 #if DEBUG_COVARIANCE
00225 cout<<"prev pose : "<<prevPose.translation().transpose()<<" rpy "<<prevPose.rotation().eulerAngles(0,1,2).transpose()<<endl;
00226 cout<<"curr pose : "<<currPose.translation().transpose()<<" rpy "<<r<<" "<<p<<" "<<y<<endl;
00227 cout<<"gt difference: "<<P.translation().transpose()<<" rpy "<<P.rotation().eulerAngles(0,1,2).transpose()<<endl;
00228 #endif
00229
00230 for( int i=0; i<N_SAMPLES; i++)
00231 {
00232 cout<<"itr: "<<i<<endl;
00233 Pr.setIdentity();
00234 snprintf(prevName,499,"%s%03d_%03d.wrl",prefix.c_str(),ctr-1,i);
00235 snprintf(currName,499,"%s%03d_%03d.wrl",prefix.c_str(),ctr,i);
00236 prev = lslgeneric::readVRML(prevName);
00237 curr = lslgeneric::readVRML(currName);
00238
00239
00240 randX = box_muller(0,dev_x);
00241 randY = box_muller(0,dev_y);
00242 randZ = box_muller(0,dev_z);
00243 randRoll = box_muller(0,dev_roll);
00244 randPitch = box_muller(0,dev_pitch);
00245 randYaw = box_muller(0,dev_yaw);
00246
00247 currPose = Eigen::Translation<double,3>(xd+randX,yd+randY,zd+randZ)*
00248 Eigen::Quaternion<double>(w,x,y,z)*
00249 Eigen::AngleAxis<double>(randRoll,Eigen::Vector3d::UnitX())*
00250 Eigen::AngleAxis<double>(randPitch,Eigen::Vector3d::UnitY())*
00251 Eigen::AngleAxis<double>(randYaw,Eigen::Vector3d::UnitZ());
00252
00253
00254
00255
00256
00257
00258
00259
00260 Pr = prevPose.inverse()*currPose;
00261
00262 #if DEBUG_COVARIANCE
00263 cout<<"curr pose: "<<currPose.translation().transpose()<<" "<<r+randRoll<<" "<<p+randPitch<<" "<<y+randYaw<<endl;
00264 cout<<"diff: "<<Pr.translation().transpose()<<" rpy "<<Pr.rotation().eulerAngles(0,1,2).transpose()<<endl;
00265 #endif
00266 bool ret;
00267 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> offset;
00268
00269
00270
00271
00272
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285
00286
00287
00288
00289 currHere = lslgeneric::transformPointCloud(Pr,curr);
00290 ret = matcherF2F.match(prev,currHere,R);
00291 #if DEBUG_COVARIANCE
00292 currHere2 = lslgeneric::transformPointCloud(R,currHere);
00293 char fname[50];
00294 snprintf(fname,49,"/home/tsv/ndt_tmp/c_%03d.wrl",ctr);
00295 FILE *fout = fopen(fname,"w");
00296 fprintf(fout,"#VRML V2.0 utf8\n");
00297 lslgeneric::writeToVRML(fout,currHere2,Eigen::Vector3d(0,1,0));
00298 lslgeneric::writeToVRML(fout,currHere,Eigen::Vector3d(1,0,0));
00299 lslgeneric::writeToVRML(fout,prev,Eigen::Vector3d(1,1,1));
00300 fclose(fout);
00301 cout<<"registration: "<<R.translation().transpose()<<" rpy "<<R.rotation().eulerAngles(0,1,2).transpose()<<endl;
00302 #endif
00303
00304 offset = P.inverse()*(R*Pr);
00305
00306 logger<<"ndt2ndt("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
00307
00308
00309 }
00310
00311 Eigen::Matrix<double,6,6> cov;
00312
00313
00314 matcherF2F.covariance(prev,curr,P,cov);
00315 logger<<"COVndt2ndt("<<ctr<<",:,:) = ["<<cov.row(0)<<";\n"<<cov.row(1)<<";\n"<<cov.row(2)<<";\n"<<cov.row(3)<<";\n"<<cov.row(4)<<";\n"<<cov.row(5)<<"];\n";
00316
00317 prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
00318 Eigen::Quaternion<double>(w,x,y,z);
00319 ctr++;
00320 }
00321
00322 }
00323
00324