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/Odometry.h"
00029 #include "rtabmap/core/OdometryInfo.h"
00030 #include "rtabmap/core/Memory.h"
00031 #include "rtabmap/core/Signature.h"
00032 #include "rtabmap/core/util3d.h"
00033 #include "rtabmap/core/VWDictionary.h"
00034 #include "rtabmap/utilite/ULogger.h"
00035 #include "rtabmap/utilite/UTimer.h"
00036 #include "rtabmap/utilite/UConversion.h"
00037 #include <opencv2/calib3d/calib3d.hpp>
00038
00039 #if _MSC_VER
00040 #define ISFINITE(value) _finite(value)
00041 #else
00042 #define ISFINITE(value) std::isfinite(value)
00043 #endif
00044
00045 namespace rtabmap {
00046
00047 OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
00048 Odometry(parameters),
00049 _localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
00050 _memory(0)
00051 {
00052 Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
00053
00054 ParametersMap customParameters;
00055 customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
00056 customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), this->getRoiRatios()));
00057 customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0"));
00058 customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
00059 customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
00060 customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
00061 int nn = Parameters::defaultOdomBowNNType();
00062 float nndr = Parameters::defaultOdomBowNNDR();
00063 int featureType = Parameters::defaultOdomFeatureType();
00064 int maxFeatures = Parameters::defaultOdomMaxFeatures();
00065 Parameters::parse(parameters, Parameters::kOdomBowNNType(), nn);
00066 Parameters::parse(parameters, Parameters::kOdomBowNNDR(), nndr);
00067 Parameters::parse(parameters, Parameters::kOdomFeatureType(), featureType);
00068 Parameters::parse(parameters, Parameters::kOdomMaxFeatures(), maxFeatures);
00069 customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
00070 customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
00071 customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
00072 customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxFeatures)));
00073
00074
00075 int subPixWinSize = Parameters::defaultOdomSubPixWinSize();
00076 int subPixIterations = Parameters::defaultOdomSubPixIterations();
00077 double subPixEps = Parameters::defaultOdomSubPixEps();
00078 Parameters::parse(parameters, Parameters::kOdomSubPixWinSize(), subPixWinSize);
00079 Parameters::parse(parameters, Parameters::kOdomSubPixIterations(), subPixIterations);
00080 Parameters::parse(parameters, Parameters::kOdomSubPixEps(), subPixEps);
00081 customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
00082 customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
00083 customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
00084
00085
00086 for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
00087 {
00088 std::string group = uSplit(iter->first, '/').front();
00089 if(group.compare("SURF") == 0 ||
00090 group.compare("SIFT") == 0 ||
00091 group.compare("BRIEF") == 0 ||
00092 group.compare("FAST") == 0 ||
00093 group.compare("ORB") == 0 ||
00094 group.compare("FREAK") == 0 ||
00095 group.compare("GFTT") == 0 ||
00096 group.compare("BRISK") == 0)
00097 {
00098 customParameters.insert(*iter);
00099 }
00100 }
00101
00102 _memory = new Memory(customParameters);
00103 if(!_memory->init("", false, ParametersMap()))
00104 {
00105 UERROR("Error initializing the memory for BOW Odometry.");
00106 }
00107 }
00108
00109 OdometryBOW::~OdometryBOW()
00110 {
00111 delete _memory;
00112 UDEBUG("");
00113 }
00114
00115
00116 void OdometryBOW::reset(const Transform & initialPose)
00117 {
00118 Odometry::reset(initialPose);
00119 _memory->init("", false, ParametersMap());
00120 localMap_.clear();
00121 }
00122
00123
00124 Transform OdometryBOW::computeTransform(
00125 const SensorData & data,
00126 OdometryInfo * info)
00127 {
00128 UTimer timer;
00129 Transform output;
00130
00131 if(info)
00132 {
00133 info->type = 0;
00134 }
00135
00136 double variance = 0;
00137 int inliers = 0;
00138 int correspondences = 0;
00139 int nFeatures = 0;
00140
00141 const Signature * previousSignature = _memory->getLastWorkingSignature();
00142 if(_memory->update(data))
00143 {
00144 const Signature * newSignature = _memory->getLastWorkingSignature();
00145 if(newSignature)
00146 {
00147 nFeatures = (int)newSignature->getWords().size();
00148 if(this->isInfoDataFilled() && info)
00149 {
00150 info->words = newSignature->getWords();
00151 }
00152 }
00153
00154 if(previousSignature && newSignature)
00155 {
00156 Transform transform;
00157 if((int)localMap_.size() >= this->getMinInliers())
00158 {
00159 if(this->isPnPEstimationUsed())
00160 {
00161 if((int)newSignature->getWords().size() >= this->getMinInliers())
00162 {
00163
00164 std::vector<int> ids = uListToVector(uUniqueKeys(newSignature->getWords()));
00165 std::vector<cv::Point3f> objectPoints(ids.size());
00166 std::vector<cv::Point2f> imagePoints(ids.size());
00167 int oi=0;
00168 std::vector<int> matches(ids.size());
00169 for(unsigned int i=0; i<ids.size(); ++i)
00170 {
00171 if(localMap_.count(ids[i]) == 1)
00172 {
00173 pcl::PointXYZ pt = localMap_.find(ids[i])->second;
00174 objectPoints[oi].x = pt.x;
00175 objectPoints[oi].y = pt.y;
00176 objectPoints[oi].z = pt.z;
00177 imagePoints[oi] = newSignature->getWords().find(ids[i])->second.pt;
00178 matches[oi++] = ids[i];
00179 }
00180 }
00181
00182 objectPoints.resize(oi);
00183 imagePoints.resize(oi);
00184 matches.resize(oi);
00185
00186 if(this->isInfoDataFilled() && info)
00187 {
00188 info->wordMatches.insert(info->wordMatches.end(), matches.begin(), matches.end());
00189 }
00190 correspondences = (int)matches.size();
00191
00192 if((int)matches.size() >= this->getMinInliers())
00193 {
00194
00195 cv::Mat K = (cv::Mat_<double>(3,3) <<
00196 data.fx(), 0, data.cx(),
00197 0, data.fy()>0?data.fy():data.fx(), data.cy(),
00198 0, 0, 1);
00199 Transform guess = (this->getPose() * data.localTransform()).inverse();
00200 cv::Mat R = (cv::Mat_<double>(3,3) <<
00201 (double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
00202 (double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
00203 (double)guess.r31(), (double)guess.r32(), (double)guess.r33());
00204 cv::Mat rvec(1,3, CV_64FC1);
00205 cv::Rodrigues(R, rvec);
00206 cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
00207 std::vector<int> inliersV;
00208 cv::solvePnPRansac(objectPoints,
00209 imagePoints,
00210 K,
00211 cv::Mat(),
00212 rvec,
00213 tvec,
00214 true,
00215 this->getIterations(),
00216 this->getPnPReprojError(),
00217 0,
00218 inliersV,
00219 this->getPnPFlags());
00220
00221 inliers = (int)inliersV.size();
00222 if((int)inliersV.size() >= this->getMinInliers())
00223 {
00224 cv::Rodrigues(rvec, R);
00225 Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
00226 R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
00227 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
00228
00229
00230 transform = (data.localTransform() * pnp * this->getPose()).inverse();
00231
00232 UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
00233
00234
00235 std::vector<float> errorSqrdDists(inliersV.size());
00236 for(unsigned int i=0; i<inliersV.size(); ++i)
00237 {
00238 std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
00239 UASSERT(iter != newSignature->getWords3().end());
00240 const cv::Point3f & objPt = objectPoints[inliersV[i]];
00241 pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
00242 errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
00243 }
00244 std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
00245 double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
00246 variance = 2.1981 * median_error_sqr;
00247 }
00248 else
00249 {
00250 UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), this->getMinInliers());
00251 }
00252
00253 if(this->isInfoDataFilled() && info && inliersV.size())
00254 {
00255 info->wordInliers.resize(inliersV.size());
00256 for(unsigned int i=0; i<inliersV.size(); ++i)
00257 {
00258 info->wordInliers[i] = matches[inliersV[i]];
00259 }
00260 }
00261 }
00262 else
00263 {
00264 UWARN("Not enough correspondences (%d < %d)", correspondences, this->getMinInliers());
00265 }
00266 }
00267 else
00268 {
00269 UWARN("Not enough features in the new image (%d < %d)", (int)newSignature->getWords().size(), this->getMinInliers());
00270 }
00271 }
00272 else
00273 {
00274 if((int)newSignature->getWords3().size() >= this->getMinInliers())
00275 {
00276 pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>);
00277 pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>);
00278
00279
00280
00281
00282 std::set<int> uniqueCorrespondences;
00283 util3d::findCorrespondences(
00284 localMap_,
00285 newSignature->getWords3(),
00286 *inliers1,
00287 *inliers2,
00288 0,
00289 &uniqueCorrespondences);
00290
00291 UDEBUG("localMap=%d, new=%d, unique correspondences=%d", (int)localMap_.size(), (int)newSignature->getWords3().size(), (int)uniqueCorrespondences.size());
00292
00293 if(this->isInfoDataFilled() && info)
00294 {
00295 info->wordMatches.insert(info->wordMatches.end(), uniqueCorrespondences.begin(), uniqueCorrespondences.end());
00296 }
00297
00298 correspondences = (int)inliers1->size();
00299 if((int)inliers1->size() >= this->getMinInliers())
00300 {
00301
00302 std::vector<int> inliersV;
00303 Transform t = util3d::transformFromXYZCorrespondences(
00304 inliers2,
00305 inliers1,
00306 this->getInlierDistance(),
00307 this->getIterations(),
00308 this->getRefineIterations()>0, 3.0, this->getRefineIterations(),
00309 &inliersV,
00310 &variance);
00311
00312 inliers = (int)inliersV.size();
00313 if(!t.isNull() && inliers >= this->getMinInliers())
00314 {
00315
00316 transform = this->getPose().inverse() * t;
00317
00318 UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
00319 }
00320 else
00321 {
00322 UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
00323 }
00324
00325 if(this->isInfoDataFilled() && info && inliersV.size())
00326 {
00327 info->wordInliers.resize(inliersV.size());
00328 for(unsigned int i=0; i<inliersV.size(); ++i)
00329 {
00330 info->wordInliers[i] = info->wordMatches[inliersV[i]];
00331 }
00332 }
00333 }
00334 else
00335 {
00336 UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
00337 }
00338 }
00339 else
00340 {
00341 UWARN("Not enough 3D features in the new image (%d < %d)", (int)newSignature->getWords3().size(), this->getMinInliers());
00342 }
00343 }
00344 }
00345 else
00346 {
00347 UWARN("Local map too small!? (%d < %d)", (int)localMap_.size(), this->getMinInliers());
00348 }
00349
00350 if(transform.isNull())
00351 {
00352 _memory->deleteLocation(newSignature->id());
00353 }
00354 else
00355 {
00356 output = transform;
00357
00358 while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
00359 {
00360 int nodeId = *_memory->getStMem().begin();
00361 std::list<int> removedPts;
00362 _memory->deleteLocation(nodeId, &removedPts);
00363 for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
00364 {
00365 localMap_.erase(*iter);
00366 }
00367 }
00368
00369 if(_localHistoryMaxSize == 0 && localMap_.size() > 0 && localMap_.size() > newSignature->getWords3().size())
00370 {
00371 UERROR("Local map should have only words of the last added signature here! (size=%d, max history size=%d, newWords=%d)",
00372 (int)localMap_.size(), _localHistoryMaxSize, (int)newSignature->getWords3().size());
00373 }
00374
00375
00376 std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
00377 Transform t = this->getPose()*output;
00378 for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
00379 {
00380
00381 if(newSignature->getWords3().count(*iter) == 1)
00382 {
00383
00384 if(localMap_.find(*iter) == localMap_.end())
00385 {
00386 const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
00387 if(pcl::isFinite(pt))
00388 {
00389 pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
00390 localMap_.insert(std::make_pair(*iter, pt2));
00391 }
00392 }
00393 }
00394 else
00395 {
00396 localMap_.erase(*iter);
00397 }
00398 }
00399 }
00400 }
00401 else if(!previousSignature && newSignature)
00402 {
00403 localMap_.clear();
00404
00405 int count = 0;
00406 std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
00407 if((int)uniques.size() >= this->getMinInliers())
00408 {
00409 output.setIdentity();
00410
00411 Transform t = this->getPose();
00412 for(std::list<int>::iterator iter = uniques.begin(); iter!=uniques.end(); ++iter)
00413 {
00414
00415 if(newSignature->getWords3().count(*iter) == 1)
00416 {
00417 const pcl::PointXYZ & pt = newSignature->getWords3().find(*iter)->second;
00418 if(pcl::isFinite(pt))
00419 {
00420 pcl::PointXYZ pt2 = util3d::transformPoint(pt, t);
00421 localMap_.insert(std::make_pair(*iter, pt2));
00422 }
00423 else
00424 {
00425 ++count;
00426 }
00427 }
00428 }
00429 }
00430 else
00431 {
00432
00433 _memory->deleteLocation(newSignature->id());
00434 }
00435 UDEBUG("uniques=%d, pt not finite = %d", (int)uniques.size(),count);
00436 }
00437
00438 _memory->emptyTrash();
00439 }
00440
00441 if(info)
00442 {
00443 info->variance = variance;
00444 info->inliers = inliers;
00445 info->matches = correspondences;
00446 info->features = nFeatures;
00447 info->localMapSize = (int)localMap_.size();
00448 }
00449
00450 UINFO("Odom update time = %fs lost=%s features=%d inliers=%d/%d variance=%f local_map=%d dict=%d nodes=%d",
00451 timer.elapsed(),
00452 output.isNull()?"true":"false",
00453 nFeatures,
00454 inliers,
00455 correspondences,
00456 variance,
00457 (int)localMap_.size(),
00458 (int)_memory->getVWDictionary()->getVisualWords().size(),
00459 (int)_memory->getStMem().size());
00460 return output;
00461 }
00462
00463 }