test_covariance.cc
Go to the documentation of this file.
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()           /* ranf() is uniform in 0..1 */
00024 {
00025     return (float)rand()/(float)RAND_MAX;
00026 }
00027 
00028 float box_muller(float m, float s)  /* normal random variate generator */
00029 {
00030     /* mean m, standard deviation s */
00031     float x1, x2, w, y1;
00032     static float y2;
00033     static int use_last = 0;
00034 
00035     if (use_last)               /* use value from previous call */
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     //cout<<"max itr are "<<icp.getMaximumIterations()<<endl;
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 //    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
00097 //    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
00098 //    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
00099     pcl::PointCloud<pcl::PointXYZ> Final;
00100     icp.align(Final);
00101 
00102 
00103 //    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
00104 //      icp.getFitnessScore() << std::endl;
00105 //    std::cout << icp.getFinalTransformation() << std::endl;
00106 
00107     //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
00108     Tout = (icp.getFinalTransformation()).cast<double>();
00109 
00110     /*    char fname[50];
00111         snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00112         FILE *fout = fopen(fname,"w");
00113         fprintf(fout,"#VRML V2.0 utf8\n");
00114         lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
00115         lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
00116         lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
00117         fclose(fout);
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     //get first line
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             //set previous pose to curent pose
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         r = M_PI*r/180;
00196         p = M_PI*p/180;
00197             //due to the way we collected data, offset by 90, shouldn't matter
00198         y = y+90;
00199         y = M_PI*y/180;
00200 
00201         if(first) {
00202             //set previous pose to curent pose
00203             prevPose =Eigen::Translation<double,3>(xd,yd,zd)*
00204                       Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX())*
00205                       Eigen::AngleAxis<double>(p,Eigen::Vector3d::UnitY())*
00206                       Eigen::AngleAxis<double>(y,Eigen::Vector3d::UnitZ());
00207             first = false;
00208             ctr++;
00209             continue;
00210         }
00211 
00212 
00213         currPose =Eigen::Translation<double,3>(xd,yd,zd)*
00214             Eigen::AngleAxis<double>(r,Eigen::Vector3d::UnitX())*
00215             Eigen::AngleAxis<double>(p,Eigen::Vector3d::UnitY())*
00216             Eigen::AngleAxis<double>(y,Eigen::Vector3d::UnitZ());
00217         */
00218 
00219 
00220         //compute ground truth relative pose
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             //draw random numbers
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             //compute current pose
00253             /*currPose =Eigen::Translation<double,3>(xd+randX,yd+randY,zd+randZ)*
00254             Eigen::AngleAxis<double>(r+randRoll,Eigen::Vector3d::UnitX())*
00255             Eigen::AngleAxis<double>(p+randPitch,Eigen::Vector3d::UnitY())*
00256             Eigen::AngleAxis<double>(y+randYaw,Eigen::Vector3d::UnitZ());
00257             */
00258 
00259             //compute relative pose
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                     //point2NDT
00270                     //transform curr scan by Pr
00271                     currHere = lslgeneric::transformPointCloud(Pr,curr);
00272                     ret = matcherP2F.match(prev,currHere,R);
00273                     //compute P[-](Pr[+]R)
00274                     offset = P.inverse()*(R*Pr);
00275                     //that's the error! log it.
00276                     logger<<"pnt2ndt("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
00277 
00278                     //ICP
00279                     //transform curr scan by Pr
00280                     currHere = lslgeneric::transformPointCloud(Pr,curr);
00281                     ret = matchICP(prev,currHere,R);
00282                     //compute P[-](Pr[+]R)
00283                     offset = P.inverse()*(R*Pr);
00284                     //that's the error! log it.
00285                     logger<<"icp("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
00286             */
00287             //NDT2NDT
00288             //transform curr scan by Pr
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             //compute P[-](Pr[+]R)
00304             offset = P.inverse()*(R*Pr);
00305             //that's the error! log it.
00306             logger<<"ndt2ndt("<<ctr<<","<<i+1<<",:) = ["<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<"];\n";
00307 //          cout<<offset.translation().transpose()<<" "<<offset.rotation().eulerAngles(0,1,2).transpose()<<endl;
00308 
00309         }
00310 
00311         Eigen::Matrix<double,6,6> cov;
00312 //      matcherP2F.covariance(prev,curr,P,cov);
00313 //      logger<<"COVpnt2ndt("<<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";
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 


ndt_registration
Author(s): Todor Stoyanov, Jari Saarinen, Henrik Andreasson
autogenerated on Mon Oct 6 2014 03:19:30