reconstruct.cc
Go to the documentation of this file.
00001 #include <NDTMatcherF2F.hh>
00002 #include <NDTMatcher.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 
00010 #include "pcl/io/pcd_io.h"
00011 #include <pcl/filters/radius_outlier_removal.h>
00012 
00013 #include "pcl/features/feature.h"
00014 #include <cstdio>
00015 #include <Eigen/Eigen>
00016 #include <PointCloudUtils.hh>
00017 #include <fstream>
00018 #include <NDTMap.hh>
00019 #include <LazzyGrid.hh>
00020 
00021 using namespace std;
00022 using namespace lslgeneric;
00023 
00024 void filterDensity(pcl::PointCloud<pcl::PointXYZ> &rawCloud, pcl::PointCloud<pcl::PointXYZ> &pc)
00025 {
00026     pcl::RadiusOutlierRemoval<pcl::PointXYZ> filter;
00027     filter.setRadiusSearch(0.2);
00028     filter.setMinNeighborsInRadius(3);
00029     pcl::PointCloud<pcl::PointXYZ>::Ptr pcptr(new pcl::PointCloud<pcl::PointXYZ>());
00030     *pcptr = rawCloud;
00031     filter.setInputCloud(pcptr);
00032     filter.filter(pc);
00033 }
00034 
00035 int
00036 main (int argc, char** argv)
00037 {
00038 
00039     pcl::PointCloud<pcl::PointXYZ> prev, thisone1, thisone, cloudFinal;
00040     double []__res = {0.2, 0.4, 1, 2};
00041     std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00042     lslgeneric::NDTMatcherF2F matcher(false, false, false, resolutions);
00043     //lslgeneric::NDTMatcher matcher;
00044     Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tprev, Tloc, Treg, Tglob;
00045 
00046     pcl::PCDReader reader;
00047     Tprev.setIdentity();
00048     Tloc.setIdentity();
00049     Treg.setIdentity();
00050     Tglob.setIdentity();
00051 
00052     //we do a single scan to scan registration
00053     if(argc != 2)
00054     {
00055         cout<<"usage: ./reconstruct configFile\n";
00056         return -1;
00057     }
00058 
00059     FILE *fin = fopen(argv[1],"r");
00060     double xd,yd,zd, q1,q2,q3,q4;
00061     double xoffset=0, yoffset=0, zoffset=-0.01;
00062     string pcName;
00063     char *line = NULL;
00064     size_t len;
00065     bool first = true;
00066 
00067     while(getline(&line,&len,fin) > 0)
00068     {
00069 
00070         int n = sscanf(line,"%lf,%lf,%lf %lf",
00071                        &q1,&q2,&q3,&q4);
00072         if(n != 4)
00073         {
00074             cout<<"wrong format of pose at : "<<line<<endl;
00075             break;
00076         }
00077         if(!getline(&line,&len,fin) > 0) break;
00078         n = sscanf(line,"%lf,%lf,%lf",
00079                    &xd,&yd,&zd);
00080         if(n != 3)
00081         {
00082             cout<<"wrong format of pose at : "<<line<<endl;
00083             break;
00084         }
00085 
00086         if(!getline(&line,&len,fin) > 0) break;
00087         pcName = line;
00088         *(pcName.rbegin()) = '\0';
00089 
00090 //      thisone = lslgeneric::readVRML(pcName.c_str());
00091 
00092 
00093 //      thisone1.width = thisone1.height = 0;
00094         cout<<"reading "<<pcName<<endl;
00095         reader.read<pcl::PointXYZ>(pcName,thisone);
00096         //cout<<"filtering density..."<<thisone1.points.size()<<endl;
00097         //filterDensity(thisone1,thisone);
00098         cout<<" --> "<<thisone.points.size()<<endl;
00099         lslgeneric::writeToVRML("/home/tsv/ndt_tmp/last.wrl",thisone);
00100 
00101 
00102         Tloc = Tprev.inverse();
00103         Tprev = Eigen::Translation<double,3>(xd,yd,zd)*Eigen::AngleAxis<double>(q4,Eigen::Vector3d(q1,q2,q3));
00104         Tloc = Tloc*Tprev;
00105 
00106         transformPointCloudInPlace(Tloc,thisone);
00107         cout<<"old pose is t:"<<Tprev.translation().transpose()<<" r "<<Tprev.rotation().eulerAngles(0,1,2).transpose()<<endl;
00108         cout<<"local pose is t:"<<Tloc.translation().transpose()<<" r "<<Tloc.rotation().eulerAngles(0,1,2).transpose()<<endl;
00109 
00110         if(!first)
00111         {
00112             //register
00113             Treg.setIdentity();
00114             matcher.match(thisone,prev,Treg);
00115             cout<<"registration pose is t:"<<Treg.translation().transpose()<<" r "<<Treg.rotation().eulerAngles(0,1,2).transpose()<<endl;
00116             Tglob = Tglob*Tloc*Treg;
00117             cout<<"new global pose t:"<<Tglob.translation().transpose()<<" r "<<Tglob.rotation().eulerAngles(0,1,2).transpose()<<endl;
00118         }
00119         else
00120         {
00121             Tglob = Tloc;
00122             first = false;
00123         }
00124 
00125         //set prev to thisone
00126         prev = lslgeneric::readVRML(pcName.c_str());
00127         thisone = prev;
00128         //transform thisone
00129         transformPointCloudInPlace(Tglob,thisone);
00130 
00131         cout<<"read vrml file at "<<pcName<<endl;
00132         cloudFinal += thisone;
00133     }
00134 
00135     fclose(fin);
00136     lslgeneric::writeToVRML("/home/tsv/ndt_tmp/final.wrl",cloudFinal);
00137     lslgeneric::writeToVRML("/home/tsv/ndt_tmp/last.wrl",thisone);
00138 
00139 #if 0
00140     //completely unrelated, needed it for other debug TSV
00141     pcl::PointCloud<pcl::PointXYZ> cl;
00142     pcl::PointXYZ pt;
00143     for(double x = 0; x<2; x+=0.1)
00144     {
00145         for(double y = 0; y<2; y+=0.1)
00146         {
00147             for(double z = 0; z<2; z+=0.1)
00148             {
00149                 pt.x=x;
00150                 pt.y=y;
00151                 pt.z=z;
00152                 cl.points.push_back(pt);
00153             }
00154         }
00155     }
00156     lslgeneric::writeToVRML("/home/tsv/ndt_tmp/final2.wrl",cl);
00157     LazzyGrid pr(0.33);
00158     NDTMap ndt2( &pr );
00159     ndt2.loadPointCloud( cl );
00160     ndt2.computeNDTCells();
00161     ndt2.writeToVRML("/home/tsv/ndt_tmp/example.wrl");
00162 
00163 #endif
00164 
00165 
00166 }
00167 


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