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/utilite/ULogger.h>
00029 #include <rtabmap/utilite/UStl.h>
00030 #include <rtabmap/utilite/UMath.h>
00031 #include <rtabmap/utilite/UConversion.h>
00032 #include <rtabmap/core/Optimizer.h>
00033 #include <rtabmap/core/Graph.h>
00034 #include <rtabmap/core/util3d_transforms.h>
00035 #include <rtabmap/core/RegistrationVis.h>
00036 #include <set>
00037 #include <queue>
00038
00039 #include <rtabmap/core/OptimizerTORO.h>
00040 #include <rtabmap/core/OptimizerG2O.h>
00041 #include <rtabmap/core/OptimizerGTSAM.h>
00042 #include <rtabmap/core/OptimizerCVSBA.h>
00043
00044 namespace rtabmap {
00045
00046 bool Optimizer::isAvailable(Optimizer::Type type)
00047 {
00048 if(type == Optimizer::kTypeG2O)
00049 {
00050 return OptimizerG2O::available();
00051 }
00052 else if(type == Optimizer::kTypeGTSAM)
00053 {
00054 return OptimizerGTSAM::available();
00055 }
00056 else if(type == Optimizer::kTypeCVSBA)
00057 {
00058 return OptimizerCVSBA::available();
00059 }
00060 else if(type == Optimizer::kTypeTORO)
00061 {
00062 return OptimizerTORO::available();
00063 }
00064 return false;
00065 }
00066
00067 Optimizer * Optimizer::create(const ParametersMap & parameters)
00068 {
00069 int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
00070 Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
00071 return create((Optimizer::Type)optimizerTypeInt, parameters);
00072 }
00073
00074 Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
00075 {
00076 UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available(),
00077 "RTAB-Map is not built with any graph optimization approach!");
00078
00079 if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
00080 {
00081 if(OptimizerGTSAM::available())
00082 {
00083 UWARN("TORO optimizer not available. GTSAM will be used instead.");
00084 type = Optimizer::kTypeGTSAM;
00085 }
00086 else if(OptimizerG2O::available())
00087 {
00088 UWARN("TORO optimizer not available. g2o will be used instead.");
00089 type = Optimizer::kTypeG2O;
00090 }
00091 }
00092 if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
00093 {
00094 if(OptimizerTORO::available())
00095 {
00096 UWARN("g2o optimizer not available. TORO will be used instead.");
00097 type = Optimizer::kTypeTORO;
00098 }
00099 else if(OptimizerGTSAM::available())
00100 {
00101 UWARN("g2o optimizer not available. GTSAM will be used instead.");
00102 type = Optimizer::kTypeGTSAM;
00103 }
00104 }
00105 if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
00106 {
00107 if(OptimizerTORO::available())
00108 {
00109 UWARN("GTSAM optimizer not available. TORO will be used instead.");
00110 type = Optimizer::kTypeTORO;
00111 }
00112 else if(OptimizerG2O::available())
00113 {
00114 UWARN("GTSAM optimizer not available. g2o will be used instead.");
00115 type = Optimizer::kTypeG2O;
00116 }
00117 }
00118 if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
00119 {
00120 if(OptimizerTORO::available())
00121 {
00122 UWARN("CVSBA optimizer not available. TORO will be used instead.");
00123 type = Optimizer::kTypeTORO;
00124 }
00125 else if(OptimizerGTSAM::available())
00126 {
00127 UWARN("CVSBA optimizer not available. GTSAM will be used instead.");
00128 type = Optimizer::kTypeGTSAM;
00129 }
00130 else if(OptimizerG2O::available())
00131 {
00132 UWARN("CVSBA optimizer not available. g2o will be used instead.");
00133 type = Optimizer::kTypeG2O;
00134 }
00135 }
00136 Optimizer * optimizer = 0;
00137 switch(type)
00138 {
00139 case Optimizer::kTypeGTSAM:
00140 optimizer = new OptimizerGTSAM(parameters);
00141 break;
00142 case Optimizer::kTypeG2O:
00143 optimizer = new OptimizerG2O(parameters);
00144 break;
00145 case Optimizer::kTypeCVSBA:
00146 optimizer = new OptimizerCVSBA(parameters);
00147 break;
00148 case Optimizer::kTypeTORO:
00149 default:
00150 optimizer = new OptimizerTORO(parameters);
00151 break;
00152
00153 }
00154 return optimizer;
00155 }
00156
00157 void Optimizer::getConnectedGraph(
00158 int fromId,
00159 const std::map<int, Transform> & posesIn,
00160 const std::multimap<int, Link> & linksIn,
00161 std::map<int, Transform> & posesOut,
00162 std::multimap<int, Link> & linksOut,
00163 int depth)
00164 {
00165 UASSERT(depth >= 0);
00166 UASSERT(fromId>0);
00167 UASSERT(uContains(posesIn, fromId));
00168
00169 posesOut.clear();
00170 linksOut.clear();
00171
00172 std::set<int> ids;
00173 std::set<int> curentDepth;
00174 std::set<int> nextDepth;
00175 nextDepth.insert(fromId);
00176 int d = 0;
00177 std::multimap<int, int> biLinks;
00178 for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
00179 {
00180 UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
00181 uFormat("Input links should be unique between two poses (%d->%d).",
00182 iter->second.from(), iter->second.to()).c_str());
00183 biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
00184 if(iter->second.from() != iter->second.to())
00185 {
00186 biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
00187 }
00188 }
00189
00190 while((depth == 0 || d < depth) && nextDepth.size())
00191 {
00192 curentDepth = nextDepth;
00193 nextDepth.clear();
00194
00195 for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
00196 {
00197 if(ids.find(*jter) == ids.end())
00198 {
00199 ids.insert(*jter);
00200 posesOut.insert(*posesIn.find(*jter));
00201
00202 for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
00203 {
00204 int nextId = iter->second;
00205 if(uContains(posesIn, nextId))
00206 {
00207 if(ids.find(nextId) == ids.end())
00208 {
00209 nextDepth.insert(nextId);
00210
00211 std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
00212 if(depth == 0 || d < depth-1)
00213 {
00214 linksOut.insert(*kter);
00215 }
00216 else if(curentDepth.find(nextId) != curentDepth.end() ||
00217 ids.find(nextId) != ids.end())
00218 {
00219 linksOut.insert(*kter);
00220 }
00221 }
00222 else if(*jter == nextId)
00223 {
00224 std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
00225 linksOut.insert(*kter);
00226 }
00227 }
00228 }
00229 }
00230 }
00231 ++d;
00232 }
00233 }
00234
00235 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) :
00236 iterations_(iterations),
00237 slam2d_(slam2d),
00238 covarianceIgnored_(covarianceIgnored),
00239 epsilon_(epsilon),
00240 robust_(robust),
00241 priorsIgnored_(priorsIgnored)
00242 {
00243 }
00244
00245 Optimizer::Optimizer(const ParametersMap & parameters) :
00246 iterations_(Parameters::defaultOptimizerIterations()),
00247 slam2d_(Parameters::defaultRegForce3DoF()),
00248 covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
00249 epsilon_(Parameters::defaultOptimizerEpsilon()),
00250 robust_(Parameters::defaultOptimizerRobust()),
00251 priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored())
00252 {
00253 parseParameters(parameters);
00254 }
00255
00256 void Optimizer::parseParameters(const ParametersMap & parameters)
00257 {
00258 Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
00259 Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
00260 Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
00261 Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
00262 Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
00263 Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
00264 }
00265
00266 std::map<int, Transform> Optimizer::optimizeIncremental(
00267 int rootId,
00268 const std::map<int, Transform> & poses,
00269 const std::multimap<int, Link> & constraints,
00270 std::list<std::map<int, Transform> > * intermediateGraphes,
00271 double * finalError,
00272 int * iterationsDone)
00273 {
00274 std::map<int, Transform> incGraph;
00275 std::multimap<int, Link> incGraphLinks;
00276 incGraph.insert(*poses.begin());
00277 int i=0;
00278 std::multimap<int, Link> constraintsCpy = constraints;
00279 UDEBUG("Incremental optimization... poses=%d comstraints=%d", (int)poses.size(), (int)constraints.size());
00280 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00281 {
00282 incGraph.insert(*iter);
00283 bool hasLoopClosure = false;
00284 for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
00285 {
00286 UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
00287 if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
00288 {
00289 UASSERT(uContains(incGraph, iter->first));
00290 incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
00291 incGraphLinks.insert(*jter);
00292 }
00293 else
00294 {
00295 if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
00296 {
00297
00298 constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
00299 }
00300 else
00301 {
00302 UASSERT(uContains(incGraph, jter->second.to()));
00303 incGraphLinks.insert(*jter);
00304 hasLoopClosure = true;
00305 }
00306 }
00307 }
00308 if(hasLoopClosure)
00309 {
00310 incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
00311 if(incGraph.empty())
00312 {
00313 UWARN("Failed incremental optimization...");
00314 break;
00315 }
00316 }
00317 UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
00318 }
00319 if(!incGraph.empty() && incGraph.size() == poses.size())
00320 {
00321 UASSERT(incGraphLinks.size() == constraints.size());
00322 UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
00323 incGraph.at(rootId) = poses.at(rootId);
00324 return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
00325 }
00326
00327 UDEBUG("Failed incremental optimization");
00328 return std::map<int, Transform>();
00329 }
00330
00331 std::map<int, Transform> Optimizer::optimize(
00332 int rootId,
00333 const std::map<int, Transform> & poses,
00334 const std::multimap<int, Link> & edgeConstraints,
00335 std::list<std::map<int, Transform> > * intermediateGraphes,
00336 double * finalError,
00337 int * iterationsDone)
00338 {
00339 cv::Mat covariance;
00340 return optimize(rootId,
00341 poses,
00342 edgeConstraints,
00343 covariance,
00344 intermediateGraphes,
00345 finalError,
00346 iterationsDone);
00347 }
00348
00349 std::map<int, Transform> Optimizer::optimize(
00350 int rootId,
00351 const std::map<int, Transform> & poses,
00352 const std::multimap<int, Link> & constraints,
00353 cv::Mat & outputCovariance,
00354 std::list<std::map<int, Transform> > * intermediateGraphes,
00355 double * finalError,
00356 int * iterationsDone)
00357 {
00358 UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
00359 return std::map<int, Transform>();
00360 }
00361
00362 std::map<int, Transform> Optimizer::optimizeBA(
00363 int rootId,
00364 const std::map<int, Transform> & poses,
00365 const std::multimap<int, Link> & links,
00366 const std::map<int, CameraModel> & models,
00367 std::map<int, cv::Point3f> & points3DMap,
00368 const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
00369 std::set<int> * outliers)
00370 {
00371 UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
00372 return std::map<int, Transform>();
00373 }
00374
00375 std::map<int, Transform> Optimizer::optimizeBA(
00376 int rootId,
00377 const std::map<int, Transform> & poses,
00378 const std::multimap<int, Link> & links,
00379 const std::map<int, Signature> & signatures)
00380 {
00381 UDEBUG("");
00382 std::map<int, CameraModel> models;
00383 for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00384 {
00385
00386 CameraModel model;
00387 if(uContains(signatures, iter->first))
00388 {
00389 if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
00390 {
00391 model = signatures.at(iter->first).sensorData().cameraModels()[0];
00392 }
00393 else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
00394 {
00395 model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
00396
00397
00398 model = CameraModel(
00399 model.fx(),
00400 model.fy(),
00401 model.cx(),
00402 model.cy(),
00403 model.localTransform(),
00404 -signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
00405 }
00406 else
00407 {
00408 UERROR("Missing calibration for node %d", iter->first);
00409 return std::map<int, Transform>();
00410 }
00411 }
00412 else
00413 {
00414 UERROR("Did not find node %d in cache", iter->first);
00415 return std::map<int, Transform>();
00416 }
00417
00418 UASSERT(model.isValidForProjection());
00419
00420 models.insert(std::make_pair(iter->first, model));
00421 }
00422
00423
00424 std::map<int, cv::Point3f> points3DMap;
00425 std::map<int, std::map<int, cv::Point3f> > wordReferences;
00426 this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
00427
00428 return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
00429 }
00430
00431 Transform Optimizer::optimizeBA(
00432 const Link & link,
00433 const CameraModel & model,
00434 std::map<int, cv::Point3f> & points3DMap,
00435 const std::map<int, std::map<int, cv::Point3f> > & wordReferences,
00436 std::set<int> * outliers)
00437 {
00438 std::map<int, Transform> poses;
00439 poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
00440 poses.insert(std::make_pair(link.to(), link.transform()));
00441 std::multimap<int, Link> links;
00442 links.insert(std::make_pair(link.from(), link));
00443 std::map<int, CameraModel> models;
00444 models.insert(std::make_pair(link.from(), model));
00445 models.insert(std::make_pair(link.to(), model));
00446 poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
00447 if(poses.size() == 2)
00448 {
00449 return poses.rbegin()->second;
00450 }
00451 else
00452 {
00453 return link.transform();
00454 }
00455 }
00456
00457 void Optimizer::computeBACorrespondences(
00458 const std::map<int, Transform> & poses,
00459 const std::multimap<int, Link> & links,
00460 const std::map<int, Signature> & signatures,
00461 std::map<int, cv::Point3f> & points3DMap,
00462 std::map<int, std::map<int, cv::Point3f> > & wordReferences)
00463 {
00464 UDEBUG("");
00465 int wordCount = 0;
00466 int edgeWithWordsAdded = 0;
00467 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00468 {
00469 Link link = iter->second;
00470 if(link.to() < link.from())
00471 {
00472 link = link.inverse();
00473 }
00474 if(uContains(signatures, link.from()) &&
00475 uContains(signatures, link.to()) &&
00476 uContains(poses, link.from()))
00477 {
00478 Signature sFrom = signatures.at(link.from());
00479 if(sFrom.getWeight() >= 0)
00480 {
00481 Signature sTo = signatures.at(link.to());
00482 if(sTo.getWeight() < 0)
00483 {
00484 for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
00485 sTo.getWeight() < 0 && jter!=links.end() && uContains(signatures, jter->second.to());
00486 ++jter)
00487 {
00488 sTo = signatures.at(jter->second.to());
00489 }
00490 }
00491
00492 if(sFrom.getWords().size() &&
00493 sTo.getWords().size() &&
00494 sFrom.getWords3().size())
00495 {
00496 ParametersMap regParam;
00497 regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
00498 regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
00499 regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
00500 regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
00501 RegistrationVis reg(regParam);
00502
00503
00504
00505
00506 RegistrationInfo info;
00507 Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
00508
00509 UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
00510
00511 if(!t.isNull())
00512 {
00513 Transform pose = poses.at(sFrom.id());
00514 UASSERT(!pose.isNull());
00515 for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
00516 {
00517 cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
00518 if(p.x > 0.0f)
00519 {
00520 int wordId = ++wordCount;
00521
00522 wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point3f>()));
00523
00524 cv::Point2f pt = sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt;
00525 wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), cv::Point3f(pt.x, pt.y, p.x)));
00526
00527
00528 pt = sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt;
00529 float depth = 0.0f;
00530 std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
00531 if( iterTo!=sTo.getWords3().end() &&
00532 iterTo->second.x > 0)
00533 {
00534 depth = iterTo->second.x;
00535 }
00536 wordReferences.at(wordId).insert(std::make_pair(sTo.id(), cv::Point3f(pt.x, pt.y, depth)));
00537
00538 p = util3d::transformPoint(p, pose);
00539 points3DMap.insert(std::make_pair(wordId, p));
00540 }
00541 }
00542 ++edgeWithWordsAdded;
00543 }
00544 else
00545 {
00546 UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
00547 }
00548 }
00549 }
00550 }
00551 }
00552 UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
00553 }
00554
00555 }