scanmatcherprocessor.cpp
Go to the documentation of this file.
00001 #include <iostream>
00002 #include "scanmatcherprocessor.h"
00003 #include "eig3.h"
00004 
00005 //#define SCANMATHCERPROCESSOR_DEBUG
00006 namespace GMapping {
00007 
00008 using namespace std;
00009 
00010 ScanMatcherProcessor::ScanMatcherProcessor(const ScanMatcherMap& m)
00011   : m_map(m.getCenter(), m.getWorldSizeX(), m.getWorldSizeY(), m.getResolution()), 
00012     m_pose(0,0,0){
00013   m_regScore=300;
00014   m_critScore=.5*m_regScore;
00015   m_maxMove=1;
00016   m_beams=0;
00017   m_computeCovariance=false;
00018   //m_eigenspace=gsl_eigen_symmv_alloc(3);
00019   useICP=false;
00020 }
00021 
00022 
00023 ScanMatcherProcessor::ScanMatcherProcessor 
00024   (double xmin, double ymin, double xmax, double ymax, double delta, double patchdelta):
00025   m_map(Point((xmax+xmin)*.5, (ymax+ymin)*.5), xmax-xmin, ymax-ymin, patchdelta), m_pose(0,0,0){
00026         m_regScore=300;
00027         m_critScore=.5*m_regScore;
00028         m_maxMove=1;
00029         m_beams=0;
00030         m_computeCovariance=false;
00031         //m_eigenspace=gsl_eigen_symmv_alloc(3);
00032         useICP=false;
00033 }
00034 
00035 ScanMatcherProcessor::~ScanMatcherProcessor (){
00036         //gsl_eigen_symmv_free(m_eigenspace);
00037 }
00038 
00039 
00040 void ScanMatcherProcessor::setSensorMap(const SensorMap& smap, std::string sensorName){
00041         m_sensorMap=smap;
00042         
00043         /*
00044          Construct the angle table for the sensor
00045          
00046          FIXME has to be extended to more than one laser... 
00047         */
00048         
00049         SensorMap::const_iterator laser_it=m_sensorMap.find(sensorName);
00050         assert(laser_it!=m_sensorMap.end());
00051         const RangeSensor* rangeSensor=dynamic_cast<const RangeSensor*>((laser_it->second));
00052         assert(rangeSensor && rangeSensor->beams().size());
00053         
00054         m_beams=static_cast<unsigned int>(rangeSensor->beams().size());
00055         double* angles=new double[rangeSensor->beams().size()];
00056         for (unsigned int i=0; i<m_beams; i++){
00057                 angles[i]=rangeSensor->beams()[i].pose.theta;
00058         }
00059         m_matcher.setLaserParameters(m_beams, angles, rangeSensor->getPose());
00060         delete [] angles;
00061         
00062         
00063 }
00064 
00065 void ScanMatcherProcessor::init(){
00066         m_first=true;
00067         m_pose=OrientedPoint(0,0,0);
00068         m_count=0;
00069 }
00070 
00071 void ScanMatcherProcessor::processScan(const RangeReading & reading){
00073         OrientedPoint relPose=reading.getPose();
00074         if (!m_count){
00075                 m_odoPose=relPose;
00076         }
00077 
00078         //compute the move in the scan m_matcher
00079         //reference frame
00080 
00081         OrientedPoint move=relPose-m_odoPose;
00082 
00083         double dth=m_odoPose.theta-m_pose.theta;
00084         // cout << "rel-move x="<< move.x <<  " y=" << move.y << " theta=" << move.theta << endl;
00085 
00086         double lin_move=move*move;
00087         if (lin_move>m_maxMove){
00088                 cerr << "Too big jump in the log file: " << lin_move << endl;
00089                 cerr << "relPose=" << relPose.x << " " <<relPose.y << endl;
00090                 cerr << "ignoring" << endl;
00091                 return;
00092                 //assert(0);
00093                 dth=0;
00094                 move.x=move.y=move.theta=0;
00095         }
00096         
00097         double s=sin(dth), c=cos(dth);
00098         OrientedPoint dPose;
00099         dPose.x=c*move.x-s*move.y;
00100         dPose.y=s*move.x+c*move.y;
00101         dPose.theta=move.theta;
00102 
00103 #ifdef SCANMATHCERPROCESSOR_DEBUG
00104         cout << "abs-move x="<< dPose.x <<  " y=" << dPose.y << " theta=" << dPose.theta << endl;
00105 #endif
00106         m_pose=m_pose+dPose;
00107         m_pose.theta=atan2(sin(m_pose.theta), cos(m_pose.theta));
00108 
00109 #ifdef SCANMATHCERPROCESSOR_DEBUG
00110         cout << "StartPose: x="
00111         << m_pose.x << " y=" << m_pose.y << " theta=" << m_pose.theta << endl;
00112 #endif
00113 
00114         m_odoPose=relPose; //update the past pose for the next iteration
00115 
00116 
00117         //FIXME here I assume that everithing is referred to the center of the robot,
00118         //while the offset of the laser has to be taken into account
00119         
00120         assert(reading.size()==m_beams);
00121 /*      
00122         double * plainReading = new double[m_beams];
00123 #ifdef SCANMATHCERPROCESSOR_DEBUG
00124         cout << "PackedReadings ";
00125 #endif
00126         for(unsigned int i=0; i<m_beams; i++){
00127                 plainReading[i]=reading[i];
00128 #ifdef SCANMATHCERPROCESSOR_DEBUG
00129                 cout << plainReading[i] << " ";
00130 #endif
00131         }
00132 */
00133         double * plainReading = new double[m_beams];
00134         reading.rawView(plainReading, m_map.getDelta());
00135         
00136         
00137 #ifdef SCANMATHCERPROCESSOR_DEBUG
00138         cout << endl;
00139 #endif
00140         //the final stuff: scan match the pose
00141         double score=0;
00142         OrientedPoint newPose=m_pose;
00143         if (m_count){
00144                 if(m_computeCovariance){
00145                         ScanMatcher::CovarianceMatrix cov;
00146                         score=m_matcher.optimize(newPose, cov, m_map, m_pose, plainReading);
00147                         /*
00148                         gsl_matrix* m=gsl_matrix_alloc(3,3);
00149                         gsl_matrix_set(m,0,0,cov.xx); gsl_matrix_set(m,0,1,cov.xy); gsl_matrix_set(m,0,2,cov.xt);
00150                         gsl_matrix_set(m,1,0,cov.xy); gsl_matrix_set(m,1,1,cov.yy); gsl_matrix_set(m,1,2,cov.yt);
00151                         gsl_matrix_set(m,2,0,cov.xt); gsl_matrix_set(m,2,1,cov.yt); gsl_matrix_set(m,2,2,cov.tt);
00152                         gsl_matrix* evec=gsl_matrix_alloc(3,3);
00153                         gsl_vector* eval=gsl_vector_alloc(3);
00154                         */
00155                         double m[3][3];
00156                         double evec[3][3];
00157                         double eval[3];
00158                         m[0][0] = cov.xx; 
00159                         m[0][1] = cov.xy; 
00160                         m[0][2] = cov.xt;
00161                         m[1][0] = cov.xy;
00162                         m[1][1] = cov.yy;
00163                         m[1][2] = cov.yt;
00164                         m[2][0] = cov.xt;
00165                         m[2][1] = cov.yt;
00166                         m[2][2] = cov.tt;
00167 
00168                         //gsl_eigen_symmv (m, eval,  evec, m_eigenspace);
00169                         eigen_decomposition(m,evec,eval);
00170 #ifdef SCANMATHCERPROCESSOR_DEBUG
00171                         //cout << "evals=" << gsl_vector_get(eval, 0) <<  " " << gsl_vector_get(eval, 1)<< " " << gsl_vector_get(eval, 2)<<endl;
00172                         cout << "evals=" << eval[0] <<  " " << eval[1]<< " " << eval[2]<<endl;
00173 #endif
00174                         //gsl_matrix_free(m);
00175                         //gsl_matrix_free(evec);
00176                         //gsl_vector_free(eval);
00177                 } else {
00178                         if (useICP){
00179                                 cerr << "USING ICP" << endl;
00180                                 score=m_matcher.icpOptimize(newPose, m_map, m_pose, plainReading);
00181                         }else
00182                                 score=m_matcher.optimize(newPose, m_map, m_pose, plainReading);
00183                 }
00184                 
00185                 
00186         }
00187         //...and register the scan
00188         if (!m_count || score<m_regScore){
00189 #ifdef SCANMATHCERPROCESSOR_DEBUG
00190                 cout << "Registering" << endl;
00191 #endif
00192                 m_matcher.invalidateActiveArea();
00193                 if (score<m_critScore){
00194 #ifdef SCANMATHCERPROCESSOR_DEBUG
00195                         cout << "New Scan added, using odo pose" << endl;
00196 #endif
00197                         m_matcher.registerScan(m_map, m_pose, plainReading);
00198                 } else {
00199                         m_matcher.registerScan(m_map, newPose, plainReading);
00200 #ifdef SCANMATHCERPROCESSOR_DEBUG
00201                         cout << "New Scan added, using matched pose" << endl;
00202 #endif
00203                 }       
00204         }
00205 
00206 #ifdef SCANMATHCERPROCESSOR_DEBUG
00207         cout << " FinalPose: x="
00208                 << newPose.x << " y=" << newPose.y << " theta=" << newPose.theta << endl;
00209         cout << "score=" << score << endl;
00210 #endif
00211         m_pose=newPose;
00212         delete [] plainReading;
00213         m_count++;
00214 }
00215 
00216 void ScanMatcherProcessor::setMatchingParameters
00217         (double urange, double range, double sigma, int kernsize, double lopt, double aopt, int iterations, bool computeCovariance){
00218         m_matcher.setMatchingParameters(urange, range,  sigma, kernsize, lopt, aopt, iterations);
00219         m_computeCovariance=computeCovariance;
00220 }
00221 
00222 void ScanMatcherProcessor::setRegistrationParameters(double regScore, double critScore){
00223         m_regScore=regScore;
00224         m_critScore=critScore;
00225 }
00226                 
00227 
00228 OrientedPoint ScanMatcherProcessor::getPose() const{
00229         return m_pose;
00230 }
00231 
00232 };
00233 


openslam_gmapping
Author(s): Giorgio Grisetti, Cyrill Stachniss, Wolfram Burgard
autogenerated on Fri Aug 28 2015 11:56:21