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 Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
00158 iterations_(iterations),
00159 slam2d_(slam2d),
00160 covarianceIgnored_(covarianceIgnored),
00161 epsilon_(epsilon),
00162 robust_(robust)
00163 {
00164 }
00165
00166 Optimizer::Optimizer(const ParametersMap & parameters) :
00167 iterations_(Parameters::defaultOptimizerIterations()),
00168 slam2d_(Parameters::defaultOptimizerSlam2D()),
00169 covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
00170 epsilon_(Parameters::defaultOptimizerEpsilon()),
00171 robust_(Parameters::defaultOptimizerRobust())
00172 {
00173 parseParameters(parameters);
00174 }
00175
00176 void Optimizer::parseParameters(const ParametersMap & parameters)
00177 {
00178 Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
00179 Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
00180 Parameters::parse(parameters, Parameters::kOptimizerSlam2D(), slam2d_);
00181 Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
00182 Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
00183 }
00184
00185 std::map<int, Transform> Optimizer::optimize(
00186 int rootId,
00187 const std::map<int, Transform> & poses,
00188 const std::multimap<int, Link> & constraints,
00189 std::list<std::map<int, Transform> > * intermediateGraphes,
00190 double * finalError,
00191 int * iterationsDone)
00192 {
00193 UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
00194 return std::map<int, Transform>();
00195 }
00196
00197 std::map<int, Transform> Optimizer::optimizeBA(
00198 int rootId,
00199 const std::map<int, Transform> & poses,
00200 const std::multimap<int, Link> & links,
00201 const std::map<int, Signature> & signatures)
00202 {
00203 UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
00204 return std::map<int, Transform>();
00205 }
00206
00207 void Optimizer::getConnectedGraph(
00208 int fromId,
00209 const std::map<int, Transform> & posesIn,
00210 const std::multimap<int, Link> & linksIn,
00211 std::map<int, Transform> & posesOut,
00212 std::multimap<int, Link> & linksOut,
00213 int depth)
00214 {
00215 UASSERT(depth >= 0);
00216 UASSERT(fromId>0);
00217 UASSERT(uContains(posesIn, fromId));
00218
00219 posesOut.clear();
00220 linksOut.clear();
00221
00222 std::set<int> ids;
00223 std::set<int> curentDepth;
00224 std::set<int> nextDepth;
00225 nextDepth.insert(fromId);
00226 int d = 0;
00227 std::multimap<int, int> biLinks;
00228 for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
00229 {
00230 UASSERT_MSG(graph::findLink(biLinks, iter->second.from(), iter->second.to()) == biLinks.end(),
00231 uFormat("Input links should be unique between two poses (%d->%d).",
00232 iter->second.from(), iter->second.to()).c_str());
00233 biLinks.insert(std::make_pair(iter->second.from(), iter->second.to()));
00234 biLinks.insert(std::make_pair(iter->second.to(), iter->second.from()));
00235 }
00236
00237 while((depth == 0 || d < depth) && nextDepth.size())
00238 {
00239 curentDepth = nextDepth;
00240 nextDepth.clear();
00241
00242 for(std::set<int>::iterator jter = curentDepth.begin(); jter!=curentDepth.end(); ++jter)
00243 {
00244 if(ids.find(*jter) == ids.end())
00245 {
00246 ids.insert(*jter);
00247 posesOut.insert(*posesIn.find(*jter));
00248
00249 for(std::multimap<int, int>::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter)
00250 {
00251 int nextId = iter->second;
00252 if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId))
00253 {
00254 nextDepth.insert(nextId);
00255
00256 std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, *jter, nextId);
00257 if(depth == 0 || d < depth-1)
00258 {
00259 linksOut.insert(*kter);
00260 }
00261 else if(curentDepth.find(nextId) != curentDepth.end() ||
00262 ids.find(nextId) != ids.end())
00263 {
00264 linksOut.insert(*kter);
00265 }
00266 }
00267 }
00268 }
00269 }
00270 ++d;
00271 }
00272 }
00273
00274
00275 void Optimizer::computeBACorrespondences(
00276 const std::map<int, Transform> & poses,
00277 const std::multimap<int, Link> & links,
00278 const std::map<int, Signature> & signatures,
00279 std::map<int, cv::Point3f> & points3DMap,
00280 std::map<int, std::map<int, cv::Point2f> > & wordReferences)
00281 {
00282 int wordCount = 0;
00283 int edgeWithWordsAdded = 0;
00284 for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00285 {
00286 Link link = iter->second;
00287 if(link.to() < link.from())
00288 {
00289 link = link.inverse();
00290 }
00291 if(uContains(signatures, link.from()) &&
00292 uContains(signatures, link.to()) &&
00293 uContains(poses, link.from()))
00294 {
00295 Signature sFrom = signatures.at(link.from());
00296 Signature sTo = signatures.at(link.to());
00297
00298 if(sFrom.getWords().size() &&
00299 sTo.getWords().size() &&
00300 sFrom.getWords3().size())
00301 {
00302 ParametersMap regParam;
00303 regParam.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
00304 regParam.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
00305 regParam.insert(ParametersPair(Parameters::kVisMinInliers(), "5"));
00306 regParam.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
00307 RegistrationVis reg(regParam);
00308
00309
00310
00311
00312 RegistrationInfo info;
00313 Transform t = reg.computeTransformationMod(sFrom, sTo, Transform(), &info);
00314
00315 UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
00316
00317 if(!t.isNull())
00318 {
00319 Transform pose = poses.at(sFrom.id());
00320 for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
00321 {
00322 cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
00323 if(p.x > 0.0f)
00324 {
00325 int wordId = ++wordCount;
00326
00327 p = util3d::transformPoint(p, pose);
00328 points3DMap.insert(std::make_pair(wordId, p));
00329 wordReferences.insert(std::make_pair(wordId, std::map<int, cv::Point2f>()));
00330 wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), sFrom.getWords().lower_bound(info.inliersIDs[i])->second.pt));
00331 wordReferences.at(wordId).insert(std::make_pair(sTo.id(), sTo.getWords().lower_bound(info.inliersIDs[i])->second.pt));
00332 }
00333 }
00334 ++edgeWithWordsAdded;
00335 }
00336 else
00337 {
00338 UWARN("Not enough inliers (%d) between %d and %d", info.inliersIDs.size(), sFrom.id(), sTo.id());
00339 }
00340 }
00341 }
00342 }
00343 UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
00344 }
00345
00346 }