00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <rtabmap/core/OdometryF2M.h>
00029 #include "rtabmap/core/Odometry.h"
00030 #include "rtabmap/core/OdometryF2F.h"
00031 #include "rtabmap/core/OdometryFovis.h"
00032 #include "rtabmap/core/OdometryViso2.h"
00033 #include "rtabmap/core/OdometryDVO.h"
00034 #include "rtabmap/core/OdometryOkvis.h"
00035 #include "rtabmap/core/OdometryORBSLAM2.h"
00036 #include "rtabmap/core/OdometryLOAM.h"
00037 #include "rtabmap/core/OdometryMSCKF.h"
00038 #include "rtabmap/core/OdometryInfo.h"
00039 #include "rtabmap/core/util3d.h"
00040 #include "rtabmap/core/util3d_mapping.h"
00041 #include "rtabmap/core/util3d_filtering.h"
00042 #include "rtabmap/utilite/ULogger.h"
00043 #include "rtabmap/utilite/UTimer.h"
00044 #include "rtabmap/utilite/UConversion.h"
00045 #include "rtabmap/utilite/UProcessInfo.h"
00046 #include "rtabmap/core/ParticleFilter.h"
00047 #include "rtabmap/core/util2d.h"
00048
00049 #include <pcl/pcl_base.h>
00050
00051 namespace rtabmap {
00052
00053 Odometry * Odometry::create(const ParametersMap & parameters)
00054 {
00055 int odomTypeInt = Parameters::defaultOdomStrategy();
00056 Parameters::parse(parameters, Parameters::kOdomStrategy(), odomTypeInt);
00057 Odometry::Type type = (Odometry::Type)odomTypeInt;
00058 return create(type, parameters);
00059 }
00060
00061 Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & parameters)
00062 {
00063 UDEBUG("type=%d", (int)type);
00064 Odometry * odometry = 0;
00065 switch(type)
00066 {
00067 case Odometry::kTypeORBSLAM2:
00068 odometry = new OdometryORBSLAM2(parameters);
00069 break;
00070 case Odometry::kTypeDVO:
00071 odometry = new OdometryDVO(parameters);
00072 break;
00073 case Odometry::kTypeViso2:
00074 odometry = new OdometryViso2(parameters);
00075 break;
00076 case Odometry::kTypeFovis:
00077 odometry = new OdometryFovis(parameters);
00078 break;
00079 case Odometry::kTypeF2F:
00080 odometry = new OdometryF2F(parameters);
00081 break;
00082 case Odometry::kTypeOkvis:
00083 odometry = new OdometryOkvis(parameters);
00084 break;
00085 case Odometry::kTypeLOAM:
00086 odometry = new OdometryLOAM(parameters);
00087 break;
00088 case Odometry::kTypeMSCKF:
00089 odometry = new OdometryMSCKF(parameters);
00090 break;
00091 default:
00092 odometry = new OdometryF2M(parameters);
00093 type = Odometry::kTypeF2M;
00094 break;
00095 }
00096 return odometry;
00097 }
00098
00099 Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
00100 _resetCountdown(Parameters::defaultOdomResetCountdown()),
00101 _force3DoF(Parameters::defaultRegForce3DoF()),
00102 _holonomic(Parameters::defaultOdomHolonomic()),
00103 guessFromMotion_(Parameters::defaultOdomGuessMotion()),
00104 _filteringStrategy(Parameters::defaultOdomFilteringStrategy()),
00105 _particleSize(Parameters::defaultOdomParticleSize()),
00106 _particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
00107 _particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
00108 _particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
00109 _particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
00110 _fillInfoData(Parameters::defaultOdomFillInfoData()),
00111 _kalmanProcessNoise(Parameters::defaultOdomKalmanProcessNoise()),
00112 _kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
00113 _imageDecimation(Parameters::defaultOdomImageDecimation()),
00114 _alignWithGround(Parameters::defaultOdomAlignWithGround()),
00115 _publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
00116 _imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
00117 _pose(Transform::getIdentity()),
00118 _resetCurrentCount(0),
00119 previousStamp_(0),
00120 distanceTravelled_(0),
00121 framesProcessed_(0)
00122 {
00123 Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
00124
00125 Parameters::parse(parameters, Parameters::kRegForce3DoF(), _force3DoF);
00126 Parameters::parse(parameters, Parameters::kOdomHolonomic(), _holonomic);
00127 Parameters::parse(parameters, Parameters::kOdomGuessMotion(), guessFromMotion_);
00128 Parameters::parse(parameters, Parameters::kOdomFillInfoData(), _fillInfoData);
00129 Parameters::parse(parameters, Parameters::kOdomFilteringStrategy(), _filteringStrategy);
00130 Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
00131 Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
00132 Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
00133 Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR);
00134 Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR);
00135 UASSERT(_particleNoiseT>0);
00136 UASSERT(_particleLambdaT>0);
00137 UASSERT(_particleNoiseR>0);
00138 UASSERT(_particleLambdaR>0);
00139 Parameters::parse(parameters, Parameters::kOdomKalmanProcessNoise(), _kalmanProcessNoise);
00140 Parameters::parse(parameters, Parameters::kOdomKalmanMeasurementNoise(), _kalmanMeasurementNoise);
00141 Parameters::parse(parameters, Parameters::kOdomImageDecimation(), _imageDecimation);
00142 Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
00143 Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
00144 Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
00145
00146 if(_imageDecimation == 0)
00147 {
00148 _imageDecimation = 1;
00149 }
00150
00151 if(_filteringStrategy == 2)
00152 {
00153
00154 particleFilters_.resize(6);
00155 for(unsigned int i = 0; i<particleFilters_.size(); ++i)
00156 {
00157 if(i<3)
00158 {
00159 particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
00160 }
00161 else
00162 {
00163 particleFilters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
00164 }
00165 }
00166 }
00167 else if(_filteringStrategy == 1)
00168 {
00169 initKalmanFilter();
00170 }
00171 }
00172
00173 Odometry::~Odometry()
00174 {
00175 for(unsigned int i=0; i<particleFilters_.size(); ++i)
00176 {
00177 delete particleFilters_[i];
00178 }
00179 particleFilters_.clear();
00180 }
00181
00182 void Odometry::reset(const Transform & initialPose)
00183 {
00184 UASSERT(!initialPose.isNull());
00185 previousVelocityTransform_.setNull();
00186 previousGroundTruthPose_.setNull();
00187 _resetCurrentCount = 0;
00188 previousStamp_ = 0;
00189 distanceTravelled_ = 0;
00190 framesProcessed_ = 0;
00191 if(_force3DoF || particleFilters_.size())
00192 {
00193 float x,y,z, roll,pitch,yaw;
00194 initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
00195
00196 if(_force3DoF)
00197 {
00198 if(z != 0.0f || roll != 0.0f || pitch != 0.0f)
00199 {
00200 UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
00201 }
00202 z = 0;
00203 roll = 0;
00204 pitch = 0;
00205 Transform pose(x, y, z, roll, pitch, yaw);
00206 _pose = pose;
00207 }
00208 else
00209 {
00210 _pose = initialPose;
00211 }
00212
00213 if(particleFilters_.size())
00214 {
00215 UASSERT(particleFilters_.size() == 6);
00216 particleFilters_[0]->init(x);
00217 particleFilters_[1]->init(y);
00218 particleFilters_[2]->init(z);
00219 particleFilters_[3]->init(roll);
00220 particleFilters_[4]->init(pitch);
00221 particleFilters_[5]->init(yaw);
00222 }
00223
00224 if(_filteringStrategy == 1)
00225 {
00226 initKalmanFilter(initialPose);
00227 }
00228 }
00229 else
00230 {
00231 _pose = initialPose;
00232 }
00233 }
00234
00235 Transform Odometry::process(SensorData & data, OdometryInfo * info)
00236 {
00237 return process(data, Transform(), info);
00238 }
00239
00240 Transform Odometry::process(SensorData & data, const Transform & guessIn, OdometryInfo * info)
00241 {
00242 UASSERT_MSG(data.id() >= 0, uFormat("Input data should have ID greater or equal than 0 (id=%d)!", data.id()).c_str());
00243
00244 if(!_imagesAlreadyRectified && !this->canProcessRawImages())
00245 {
00246 UERROR("Odometry approach chosen cannot process raw images (not rectified images). Make sure images "
00247 "are rectified, and set %s parameter back to true.",
00248 Parameters::kRtabmapImagesAlreadyRectified().c_str());
00249 }
00250
00251
00252 if(_pose.isIdentity() && _alignWithGround)
00253 {
00254 if(data.depthOrRightRaw().empty())
00255 {
00256 UWARN("\"%s\" is true but the input has no depth information, ignoring alignment with ground...", Parameters::kOdomAlignWithGround().c_str());
00257 }
00258 else
00259 {
00260 UTimer alignTimer;
00261 pcl::IndicesPtr indices(new std::vector<int>);
00262 pcl::IndicesPtr ground, obstacles;
00263 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, 1, 10, 0, indices.get());
00264 bool success = false;
00265 if(indices->size())
00266 {
00267 cloud = util3d::voxelize(cloud, indices, 0.01);
00268 util3d::segmentObstaclesFromGround<pcl::PointXYZ>(cloud, ground, obstacles, 20, M_PI/4.0f, 0.02, 200, true);
00269 if(ground->size())
00270 {
00271 pcl::ModelCoefficients coefficients;
00272 util3d::extractPlane(cloud, ground, 0.02, 100, &coefficients);
00273 if(coefficients.values.at(3) >= 0)
00274 {
00275 UWARN("Ground detected! coefficients=(%f, %f, %f, %f) time=%fs",
00276 coefficients.values.at(0),
00277 coefficients.values.at(1),
00278 coefficients.values.at(2),
00279 coefficients.values.at(3),
00280 alignTimer.ticks());
00281 }
00282 else
00283 {
00284 UWARN("Ceiling detected! coefficients=(%f, %f, %f, %f) time=%fs",
00285 coefficients.values.at(0),
00286 coefficients.values.at(1),
00287 coefficients.values.at(2),
00288 coefficients.values.at(3),
00289 alignTimer.ticks());
00290 }
00291 Eigen::Vector3f n(coefficients.values.at(0), coefficients.values.at(1), coefficients.values.at(2));
00292 Eigen::Vector3f z(0,0,1);
00293
00294 Eigen::Matrix3f R;
00295 R = Eigen::Quaternionf().setFromTwoVectors(n,z);
00296 Transform rotation(
00297 R(0,0), R(0,1), R(0,2), 0,
00298 R(1,0), R(1,1), R(1,2), 0,
00299 R(2,0), R(2,1), R(2,2), coefficients.values.at(3));
00300 this->reset(rotation);
00301 success = true;
00302 }
00303 }
00304 if(!success)
00305 {
00306 UERROR("Odometry failed to detect the ground. You have this "
00307 "error because parameter \"%s\" is true. "
00308 "Make sure the camera is seeing the ground (e.g., tilt ~30 "
00309 "degrees toward the ground).", Parameters::kOdomAlignWithGround().c_str());
00310 }
00311 }
00312 }
00313
00314
00315 double dt = previousStamp_>0.0f || (previousStamp_==0.0f && framesProcessed()==1)?data.stamp() - previousStamp_:0.0;
00316 Transform guess = dt>0.0 && guessFromMotion_ && !previousVelocityTransform_.isNull()?Transform::getIdentity():Transform();
00317 if(!(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull())))
00318 {
00319 if(guessFromMotion_ && !data.imageRaw().empty())
00320 {
00321 UERROR("Guess from motion is set but dt is invalid! Odometry is then computed without guess. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
00322 }
00323 else if(_filteringStrategy==1)
00324 {
00325 UERROR("Kalman filtering is enalbed but dt is invalid! Odometry is then computed without Kalman filtering. (dt=%f previous transform=%s)", dt, previousVelocityTransform_.prettyPrint().c_str());
00326 }
00327 dt=0;
00328 previousVelocityTransform_.setNull();
00329 }
00330 if(!previousVelocityTransform_.isNull())
00331 {
00332 if(guessFromMotion_)
00333 {
00334 if(_filteringStrategy == 1)
00335 {
00336
00337 float vx,vy,vz, vroll,vpitch,vyaw;
00338 predictKalmanFilter(dt, &vx,&vy,&vz,&vroll,&vpitch,&vyaw);
00339 guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
00340 }
00341 else
00342 {
00343 float vx,vy,vz, vroll,vpitch,vyaw;
00344 previousVelocityTransform_.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
00345 guess = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
00346 }
00347 }
00348 else if(_filteringStrategy == 1)
00349 {
00350 predictKalmanFilter(dt);
00351 }
00352 }
00353
00354 if(!guessIn.isNull())
00355 {
00356 guess = guessIn;
00357 }
00358
00359 UTimer time;
00360 Transform t;
00361 if(_imageDecimation > 1 && !data.imageRaw().empty())
00362 {
00363
00364 SensorData decimatedData = data;
00365 decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imageDecimation));
00366 decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imageDecimation));
00367 std::vector<CameraModel> cameraModels = decimatedData.cameraModels();
00368 for(unsigned int i=0; i<cameraModels.size(); ++i)
00369 {
00370 cameraModels[i] = cameraModels[i].scaled(1.0/double(_imageDecimation));
00371 }
00372 decimatedData.setCameraModels(cameraModels);
00373 StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
00374 if(stereoModel.isValidForProjection())
00375 {
00376 stereoModel.scale(1.0/double(_imageDecimation));
00377 }
00378 decimatedData.setStereoCameraModel(stereoModel);
00379
00380
00381 t = this->computeTransform(decimatedData, guess, info);
00382
00383
00384 std::vector<cv::KeyPoint> kpts = decimatedData.keypoints();
00385 double log2value = log(double(_imageDecimation))/log(2.0);
00386 for(unsigned int i=0; i<kpts.size(); ++i)
00387 {
00388 kpts[i].pt.x *= _imageDecimation;
00389 kpts[i].pt.y *= _imageDecimation;
00390 kpts[i].size *= _imageDecimation;
00391 kpts[i].octave += log2value;
00392 }
00393 data.setFeatures(kpts, decimatedData.keypoints3D(), decimatedData.descriptors());
00394
00395 if(info)
00396 {
00397 UASSERT(info->newCorners.size() == info->refCorners.size());
00398 for(unsigned int i=0; i<info->newCorners.size(); ++i)
00399 {
00400 info->refCorners[i].x *= _imageDecimation;
00401 info->refCorners[i].y *= _imageDecimation;
00402 info->newCorners[i].x *= _imageDecimation;
00403 info->newCorners[i].y *= _imageDecimation;
00404 }
00405 for(std::multimap<int, cv::KeyPoint>::iterator iter=info->words.begin(); iter!=info->words.end(); ++iter)
00406 {
00407 iter->second.pt.x *= _imageDecimation;
00408 iter->second.pt.y *= _imageDecimation;
00409 iter->second.size *= _imageDecimation;
00410 iter->second.octave += log2value;
00411 }
00412 }
00413 }
00414 else
00415 {
00416 t = this->computeTransform(data, guess, info);
00417 }
00418
00419 if(info)
00420 {
00421 info->timeEstimation = time.ticks();
00422 info->lost = t.isNull();
00423 info->stamp = data.stamp();
00424 info->interval = dt;
00425 info->transform = t;
00426 if(_publishRAMUsage)
00427 {
00428 info->memoryUsage = UProcessInfo::getMemoryUsage()/(1024*1024);
00429 }
00430
00431 if(!data.groundTruth().isNull())
00432 {
00433 if(!previousGroundTruthPose_.isNull())
00434 {
00435 info->transformGroundTruth = previousGroundTruthPose_.inverse() * data.groundTruth();
00436 }
00437 previousGroundTruthPose_ = data.groundTruth();
00438 }
00439 }
00440
00441 if(!t.isNull())
00442 {
00443 _resetCurrentCount = _resetCountdown;
00444
00445 float vx,vy,vz, vroll,vpitch,vyaw;
00446 t.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
00447
00448
00449 if(dt)
00450 {
00451 vx /= dt;
00452 vy /= dt;
00453 vz /= dt;
00454 vroll /= dt;
00455 vpitch /= dt;
00456 vyaw /= dt;
00457 }
00458
00459 if(_force3DoF || !_holonomic || particleFilters_.size() || _filteringStrategy==1)
00460 {
00461 if(_filteringStrategy == 1)
00462 {
00463 if(previousVelocityTransform_.isNull())
00464 {
00465
00466 if(dt)
00467 {
00468 initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
00469 }
00470 else
00471 {
00472 initKalmanFilter(t);
00473 }
00474 }
00475 else
00476 {
00477
00478 updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
00479 }
00480 }
00481 else if(particleFilters_.size())
00482 {
00483
00484 UASSERT(particleFilters_.size()==6);
00485 if(previousVelocityTransform_.isNull())
00486 {
00487 particleFilters_[0]->init(vx);
00488 particleFilters_[1]->init(vy);
00489 particleFilters_[2]->init(vz);
00490 particleFilters_[3]->init(vroll);
00491 particleFilters_[4]->init(vpitch);
00492 particleFilters_[5]->init(vyaw);
00493 }
00494 else
00495 {
00496 vx = particleFilters_[0]->filter(vx);
00497 vy = particleFilters_[1]->filter(vy);
00498 vyaw = particleFilters_[5]->filter(vyaw);
00499
00500 if(!_holonomic)
00501 {
00502
00503 float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
00504 if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
00505 {
00506 vy = tmpY;
00507 }
00508 else
00509 {
00510 vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
00511 }
00512 }
00513
00514 if(!_force3DoF)
00515 {
00516 vz = particleFilters_[2]->filter(vz);
00517 vroll = particleFilters_[3]->filter(vroll);
00518 vpitch = particleFilters_[4]->filter(vpitch);
00519 }
00520 }
00521
00522 if(info)
00523 {
00524 info->timeParticleFiltering = time.ticks();
00525 }
00526
00527 if(_force3DoF)
00528 {
00529 vz = 0.0f;
00530 vroll = 0.0f;
00531 vpitch = 0.0f;
00532 }
00533 }
00534 else if(!_holonomic)
00535 {
00536
00537 vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
00538 if(_force3DoF)
00539 {
00540 vz = 0.0f;
00541 vroll = 0.0f;
00542 vpitch = 0.0f;
00543 }
00544 }
00545
00546 if(dt)
00547 {
00548 t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
00549 }
00550 else
00551 {
00552 t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
00553 }
00554
00555 if(info)
00556 {
00557 info->transformFiltered = t;
00558 }
00559 }
00560
00561 if(data.stamp() == 0 && framesProcessed_ != 0)
00562 {
00563 UWARN("Null stamp detected");
00564 }
00565
00566 previousStamp_ = data.stamp();
00567 previousVelocityTransform_.setNull();
00568
00569 if(dt)
00570 {
00571 previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
00572 }
00573
00574 if(info)
00575 {
00576 distanceTravelled_ += t.getNorm();
00577 info->distanceTravelled = distanceTravelled_;
00578 }
00579 ++framesProcessed_;
00580
00581 return _pose *= t;
00582 }
00583 else if(_resetCurrentCount > 0)
00584 {
00585 UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
00586
00587 --_resetCurrentCount;
00588 if(_resetCurrentCount == 0)
00589 {
00590 UWARN("Odometry automatically reset to latest pose!");
00591 this->reset(_pose);
00592 }
00593 }
00594
00595 previousVelocityTransform_.setNull();
00596 previousStamp_ = 0;
00597
00598 return Transform();
00599 }
00600
00601 void Odometry::initKalmanFilter(const Transform & initialPose, float vx, float vy, float vz, float vroll, float vpitch, float vyaw)
00602 {
00603 UDEBUG("");
00604
00605
00606
00607
00608 int nStates = 18;
00609 int nMeasurements = 6;
00610 if(_force3DoF)
00611 {
00612 nStates = 9;
00613 nMeasurements = 3;
00614 }
00615 int nInputs = 0;
00616
00617
00618
00619
00620
00621
00622
00623
00624
00625
00626
00627
00628
00629
00630
00631
00632
00633
00634
00635 kalmanFilter_.init(nStates, nMeasurements, nInputs);
00636 cv::setIdentity(kalmanFilter_.processNoiseCov, cv::Scalar::all(_kalmanProcessNoise));
00637 cv::setIdentity(kalmanFilter_.measurementNoiseCov, cv::Scalar::all(_kalmanMeasurementNoise));
00638 cv::setIdentity(kalmanFilter_.errorCovPost, cv::Scalar::all(1));
00639
00640 float x,y,z,roll,pitch,yaw;
00641 initialPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
00642
00643 if(_force3DoF)
00644 {
00645
00646
00647
00648
00649 kalmanFilter_.measurementMatrix.at<float>(0,2) = 1;
00650 kalmanFilter_.measurementMatrix.at<float>(1,3) = 1;
00651 kalmanFilter_.measurementMatrix.at<float>(2,7) = 1;
00652
00653 kalmanFilter_.statePost.at<float>(0) = x;
00654 kalmanFilter_.statePost.at<float>(1) = y;
00655 kalmanFilter_.statePost.at<float>(6) = yaw;
00656
00657 kalmanFilter_.statePost.at<float>(2) = vx;
00658 kalmanFilter_.statePost.at<float>(3) = vy;
00659 kalmanFilter_.statePost.at<float>(7) = vyaw;
00660 }
00661 else
00662 {
00663
00664
00665
00666
00667
00668
00669
00670 kalmanFilter_.measurementMatrix.at<float>(0,3) = 1;
00671 kalmanFilter_.measurementMatrix.at<float>(1,4) = 1;
00672 kalmanFilter_.measurementMatrix.at<float>(2,5) = 1;
00673 kalmanFilter_.measurementMatrix.at<float>(3,12) = 1;
00674 kalmanFilter_.measurementMatrix.at<float>(4,13) = 1;
00675 kalmanFilter_.measurementMatrix.at<float>(5,14) = 1;
00676
00677 kalmanFilter_.statePost.at<float>(0) = x;
00678 kalmanFilter_.statePost.at<float>(1) = y;
00679 kalmanFilter_.statePost.at<float>(2) = z;
00680 kalmanFilter_.statePost.at<float>(9) = roll;
00681 kalmanFilter_.statePost.at<float>(10) = pitch;
00682 kalmanFilter_.statePost.at<float>(11) = yaw;
00683
00684 kalmanFilter_.statePost.at<float>(3) = vx;
00685 kalmanFilter_.statePost.at<float>(4) = vy;
00686 kalmanFilter_.statePost.at<float>(5) = vz;
00687 kalmanFilter_.statePost.at<float>(12) = vroll;
00688 kalmanFilter_.statePost.at<float>(13) = vpitch;
00689 kalmanFilter_.statePost.at<float>(14) = vyaw;
00690 }
00691 }
00692
00693 void Odometry::predictKalmanFilter(float dt, float * vx, float * vy, float * vz, float * vroll, float * vpitch, float * vyaw)
00694 {
00695
00696 if(_force3DoF)
00697 {
00698
00699
00700
00701
00702
00703
00704
00705
00706
00707
00708 kalmanFilter_.transitionMatrix.at<float>(0,2) = dt;
00709 kalmanFilter_.transitionMatrix.at<float>(1,3) = dt;
00710 kalmanFilter_.transitionMatrix.at<float>(2,4) = dt;
00711 kalmanFilter_.transitionMatrix.at<float>(3,5) = dt;
00712 kalmanFilter_.transitionMatrix.at<float>(0,4) = 0.5*pow(dt,2);
00713 kalmanFilter_.transitionMatrix.at<float>(1,5) = 0.5*pow(dt,2);
00714
00715 kalmanFilter_.transitionMatrix.at<float>(6,7) = dt;
00716 kalmanFilter_.transitionMatrix.at<float>(7,8) = dt;
00717 kalmanFilter_.transitionMatrix.at<float>(6,8) = 0.5*pow(dt,2);
00718 }
00719 else
00720 {
00721
00722
00723
00724
00725
00726
00727
00728
00729
00730
00731
00732
00733
00734
00735
00736
00737
00738
00739
00740 kalmanFilter_.transitionMatrix.at<float>(0,3) = dt;
00741 kalmanFilter_.transitionMatrix.at<float>(1,4) = dt;
00742 kalmanFilter_.transitionMatrix.at<float>(2,5) = dt;
00743 kalmanFilter_.transitionMatrix.at<float>(3,6) = dt;
00744 kalmanFilter_.transitionMatrix.at<float>(4,7) = dt;
00745 kalmanFilter_.transitionMatrix.at<float>(5,8) = dt;
00746 kalmanFilter_.transitionMatrix.at<float>(0,6) = 0.5*pow(dt,2);
00747 kalmanFilter_.transitionMatrix.at<float>(1,7) = 0.5*pow(dt,2);
00748 kalmanFilter_.transitionMatrix.at<float>(2,8) = 0.5*pow(dt,2);
00749
00750 kalmanFilter_.transitionMatrix.at<float>(9,12) = dt;
00751 kalmanFilter_.transitionMatrix.at<float>(10,13) = dt;
00752 kalmanFilter_.transitionMatrix.at<float>(11,14) = dt;
00753 kalmanFilter_.transitionMatrix.at<float>(12,15) = dt;
00754 kalmanFilter_.transitionMatrix.at<float>(13,16) = dt;
00755 kalmanFilter_.transitionMatrix.at<float>(14,17) = dt;
00756 kalmanFilter_.transitionMatrix.at<float>(9,15) = 0.5*pow(dt,2);
00757 kalmanFilter_.transitionMatrix.at<float>(10,16) = 0.5*pow(dt,2);
00758 kalmanFilter_.transitionMatrix.at<float>(11,17) = 0.5*pow(dt,2);
00759 }
00760
00761
00762 UDEBUG("Predict");
00763 const cv::Mat & prediction = kalmanFilter_.predict();
00764
00765 if(vx)
00766 *vx = prediction.at<float>(3);
00767 if(vy)
00768 *vy = prediction.at<float>(4);
00769 if(vz)
00770 *vz = _force3DoF?0.0f:prediction.at<float>(5);
00771 if(vroll)
00772 *vroll = _force3DoF?0.0f:prediction.at<float>(12);
00773 if(vpitch)
00774 *vpitch = _force3DoF?0.0f:prediction.at<float>(13);
00775 if(vyaw)
00776 *vyaw = prediction.at<float>(_force3DoF?7:14);
00777 }
00778
00779 void Odometry::updateKalmanFilter(float & vx, float & vy, float & vz, float & vroll, float & vpitch, float & vyaw)
00780 {
00781
00782 cv::Mat measurements;
00783 if(!_force3DoF)
00784 {
00785 measurements = cv::Mat(6,1,CV_32FC1);
00786 measurements.at<float>(0) = vx;
00787 measurements.at<float>(1) = vy;
00788 measurements.at<float>(2) = vz;
00789 measurements.at<float>(3) = vroll;
00790 measurements.at<float>(4) = vpitch;
00791 measurements.at<float>(5) = vyaw;
00792 }
00793 else
00794 {
00795 measurements = cv::Mat(3,1,CV_32FC1);
00796 measurements.at<float>(0) = vx;
00797 measurements.at<float>(1) = vy;
00798 measurements.at<float>(2) = vyaw;
00799 }
00800
00801
00802 UDEBUG("Correct");
00803 const cv::Mat & estimated = kalmanFilter_.correct(measurements);
00804
00805
00806 vx = estimated.at<float>(3);
00807 vy = estimated.at<float>(4);
00808 vz = _force3DoF?0.0f:estimated.at<float>(5);
00809 vroll = _force3DoF?0.0f:estimated.at<float>(12);
00810 vpitch = _force3DoF?0.0f:estimated.at<float>(13);
00811 vyaw = estimated.at<float>(_force3DoF?7:14);
00812 }
00813
00814 }