00001 #include <iostream>
00002 #include "scanmatcherprocessor.h"
00003 #include "eig3.h"
00004
00005
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
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
00032 useICP=false;
00033 }
00034
00035 ScanMatcherProcessor::~ScanMatcherProcessor (){
00036
00037 }
00038
00039
00040 void ScanMatcherProcessor::setSensorMap(const SensorMap& smap, std::string sensorName){
00041 m_sensorMap=smap;
00042
00043
00044
00045
00046
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
00079
00080
00081 OrientedPoint move=relPose-m_odoPose;
00082
00083 double dth=m_odoPose.theta-m_pose.theta;
00084
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
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;
00115
00116
00117
00118
00119
00120 assert(reading.size()==m_beams);
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
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
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
00149
00150
00151
00152
00153
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
00169 eigen_decomposition(m,evec,eval);
00170 #ifdef SCANMATHCERPROCESSOR_DEBUG
00171
00172 cout << "evals=" << eval[0] << " " << eval[1]<< " " << eval[2]<<endl;
00173 #endif
00174
00175
00176
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
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