mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-05 17:47:49 +08:00
df52523a0c
* OptimizerG2O: track BA outliers per observation Track rejected BA projections by word and pose, preserving a landmark's optimized estimate whenever at least one observation remains active. Add a deterministic g2o regression and focused Linux CTest coverage. * Changed some error logs to warning --------- Co-authored-by: happyman <[email protected]> Co-authored-by: matlabbe <[email protected]>
836 lines
28 KiB
C++
836 lines
28 KiB
C++
/*
|
|
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
|
All rights reserved.
|
|
|
|
Redistribution and use in source and binary forms, with or without
|
|
modification, are permitted provided that the following conditions are met:
|
|
* Redistributions of source code must retain the above copyright
|
|
notice, this list of conditions and the following disclaimer.
|
|
* Redistributions in binary form must reproduce the above copyright
|
|
notice, this list of conditions and the following disclaimer in the
|
|
documentation and/or other materials provided with the distribution.
|
|
* Neither the name of the Universite de Sherbrooke nor the
|
|
names of its contributors may be used to endorse or promote products
|
|
derived from this software without specific prior written permission.
|
|
|
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
*/
|
|
|
|
#include <rtabmap/utilite/ULogger.h>
|
|
#include <rtabmap/utilite/UStl.h>
|
|
#include <rtabmap/utilite/UMath.h>
|
|
#include <rtabmap/utilite/UConversion.h>
|
|
#include <rtabmap/core/Optimizer.h>
|
|
#include <rtabmap/core/Graph.h>
|
|
#include <rtabmap/core/util3d_transforms.h>
|
|
#include <rtabmap/core/RegistrationVis.h>
|
|
#include <set>
|
|
#include <queue>
|
|
|
|
#include <rtabmap/core/optimizer/OptimizerTORO.h>
|
|
#include <rtabmap/core/optimizer/OptimizerG2O.h>
|
|
#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
|
|
#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
|
|
#include <rtabmap/core/optimizer/OptimizerCeres.h>
|
|
|
|
namespace rtabmap {
|
|
|
|
bool Optimizer::isAvailable(Optimizer::Type type)
|
|
{
|
|
if(type == Optimizer::kTypeG2O)
|
|
{
|
|
return OptimizerG2O::available();
|
|
}
|
|
else if(type == Optimizer::kTypeGTSAM)
|
|
{
|
|
return OptimizerGTSAM::available();
|
|
}
|
|
else if(type == Optimizer::kTypeCVSBA)
|
|
{
|
|
return OptimizerCVSBA::available();
|
|
}
|
|
else if(type == Optimizer::kTypeTORO)
|
|
{
|
|
return OptimizerTORO::available();
|
|
}
|
|
else if(type == Optimizer::kTypeCeres)
|
|
{
|
|
return OptimizerCeres::available();
|
|
}
|
|
return false;
|
|
}
|
|
|
|
Optimizer * Optimizer::create(const ParametersMap & parameters)
|
|
{
|
|
int optimizerTypeInt = Parameters::defaultOptimizerStrategy();
|
|
Parameters::parse(parameters, Parameters::kOptimizerStrategy(), optimizerTypeInt);
|
|
return create((Optimizer::Type)optimizerTypeInt, parameters);
|
|
}
|
|
|
|
Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parameters)
|
|
{
|
|
UASSERT_MSG(OptimizerG2O::available() || OptimizerGTSAM::available() || OptimizerTORO::available() || OptimizerCeres::available(),
|
|
"RTAB-Map is not built with any graph optimization approach!");
|
|
|
|
if(!OptimizerTORO::available() && type == Optimizer::kTypeTORO)
|
|
{
|
|
if(OptimizerGTSAM::available())
|
|
{
|
|
UWARN("TORO optimizer not available. GTSAM will be used instead.");
|
|
type = Optimizer::kTypeGTSAM;
|
|
}
|
|
else if(OptimizerG2O::available())
|
|
{
|
|
UWARN("TORO optimizer not available. g2o will be used instead.");
|
|
type = Optimizer::kTypeG2O;
|
|
}
|
|
else if(OptimizerCeres::available())
|
|
{
|
|
UWARN("TORO optimizer not available. ceres will be used instead.");
|
|
type = Optimizer::kTypeCeres;
|
|
}
|
|
}
|
|
if(!OptimizerG2O::available() && type == Optimizer::kTypeG2O)
|
|
{
|
|
if(OptimizerGTSAM::available())
|
|
{
|
|
UWARN("g2o optimizer not available. GTSAM will be used instead.");
|
|
type = Optimizer::kTypeGTSAM;
|
|
}
|
|
else if(OptimizerTORO::available())
|
|
{
|
|
UWARN("g2o optimizer not available. TORO will be used instead.");
|
|
type = Optimizer::kTypeTORO;
|
|
}
|
|
else if(OptimizerCeres::available())
|
|
{
|
|
UWARN("g2o optimizer not available. ceres will be used instead.");
|
|
type = Optimizer::kTypeCeres;
|
|
}
|
|
}
|
|
if(!OptimizerGTSAM::available() && type == Optimizer::kTypeGTSAM)
|
|
{
|
|
if(OptimizerG2O::available())
|
|
{
|
|
UWARN("GTSAM optimizer not available. g2o will be used instead.");
|
|
type = Optimizer::kTypeG2O;
|
|
}
|
|
else if(OptimizerTORO::available())
|
|
{
|
|
UWARN("GTSAM optimizer not available. TORO will be used instead.");
|
|
type = Optimizer::kTypeTORO;
|
|
}
|
|
else if(OptimizerCeres::available())
|
|
{
|
|
UWARN("GTSAM optimizer not available. ceres will be used instead.");
|
|
type = Optimizer::kTypeCeres;
|
|
}
|
|
}
|
|
if(!OptimizerCVSBA::available() && type == Optimizer::kTypeCVSBA)
|
|
{
|
|
if(OptimizerG2O::available())
|
|
{
|
|
UWARN("CVSBA optimizer not available. g2o will be used instead.");
|
|
type = Optimizer::kTypeG2O;
|
|
}
|
|
}
|
|
if(!OptimizerCeres::available() && type == Optimizer::kTypeCeres)
|
|
{
|
|
if(OptimizerGTSAM::available())
|
|
{
|
|
UWARN("Ceres optimizer not available. gtsam will be used instead.");
|
|
type = Optimizer::kTypeGTSAM;
|
|
}
|
|
else if(OptimizerG2O::available())
|
|
{
|
|
UWARN("Ceres optimizer not available. g2o will be used instead.");
|
|
type = Optimizer::kTypeG2O;
|
|
}
|
|
else if(OptimizerTORO::available())
|
|
{
|
|
UWARN("Ceres optimizer not available. TORO will be used instead.");
|
|
type = Optimizer::kTypeTORO;
|
|
}
|
|
}
|
|
Optimizer * optimizer = 0;
|
|
switch(type)
|
|
{
|
|
case Optimizer::kTypeGTSAM:
|
|
optimizer = new OptimizerGTSAM(parameters);
|
|
break;
|
|
case Optimizer::kTypeG2O:
|
|
optimizer = new OptimizerG2O(parameters);
|
|
break;
|
|
case Optimizer::kTypeCVSBA:
|
|
optimizer = new OptimizerCVSBA(parameters);
|
|
break;
|
|
case Optimizer::kTypeCeres:
|
|
optimizer = new OptimizerCeres(parameters);
|
|
break;
|
|
case Optimizer::kTypeTORO:
|
|
default:
|
|
optimizer = new OptimizerTORO(parameters);
|
|
break;
|
|
|
|
}
|
|
return optimizer;
|
|
}
|
|
|
|
class LinkIdKey
|
|
{
|
|
public:
|
|
LinkIdKey(int id, Link::Type type) :
|
|
id_(id),
|
|
type_(type) {}
|
|
bool operator<(const LinkIdKey & k) const
|
|
{
|
|
// landmark, sort by smallest to largest landmark id, after normal links
|
|
if(id_ < 0 && k.id_ < 0)
|
|
{
|
|
return id_ > k.id_;
|
|
}
|
|
else if(id_ < 0) {
|
|
return false;
|
|
}
|
|
else if(k.id_ < 0) {
|
|
return true;
|
|
}
|
|
|
|
if(type_ == Link::kNeighbor && k.type_ != Link::kNeighbor)
|
|
{
|
|
return true;
|
|
}
|
|
else if(type_ != Link::kNeighbor && k.type_ == Link::kNeighbor)
|
|
{
|
|
return false;
|
|
}
|
|
else if(type_ == Link::kNeighborMerged && k.type_ != Link::kNeighbor && k.type_ != Link::kNeighborMerged)
|
|
{
|
|
return true;
|
|
}
|
|
else if(k.type_ == Link::kNeighborMerged && type_ != Link::kNeighbor && type_ != Link::kNeighborMerged)
|
|
{
|
|
return false;
|
|
}
|
|
else
|
|
{
|
|
// normal link, sort by smallest to largest id
|
|
return id_ < k.id_;
|
|
}
|
|
}
|
|
int id_;
|
|
Link::Type type_;
|
|
};
|
|
|
|
void Optimizer::getConnectedGraph(
|
|
int fromId,
|
|
const std::map<int, Transform> & posesIn,
|
|
const std::multimap<int, Link> & linksIn,
|
|
std::map<int, Transform> & posesOut,
|
|
std::multimap<int, Link> & linksOut) const
|
|
{
|
|
UDEBUG("IN: fromId=%d poses=%d links=%d priorsIgnored=%d landmarksIgnored=%d", fromId, (int)posesIn.size(), (int)linksIn.size(), priorsIgnored()?1:0, landmarksIgnored()?1:0);
|
|
UASSERT(fromId>0);
|
|
UASSERT_MSG(uContains(posesIn, fromId), uFormat("poses=%ld (first=%d last=%d) fromId=%d",
|
|
posesIn.size(), posesIn.empty()?0:posesIn.begin()->first, posesIn.empty()?0:posesIn.rbegin()->first, fromId).c_str());
|
|
|
|
posesOut.clear();
|
|
linksOut.clear();
|
|
|
|
std::map<LinkIdKey, Transform> nextPoses;
|
|
nextPoses.insert(std::make_pair(LinkIdKey(fromId, Link::kUndef), posesIn.find(fromId)->second));
|
|
std::multimap<int, std::pair<int, Link::Type> > biLinks;
|
|
for(std::multimap<int, Link>::const_iterator iter=linksIn.begin(); iter!=linksIn.end(); ++iter)
|
|
{
|
|
if(iter->second.from() != iter->second.to())
|
|
{
|
|
if(graph::findLink(biLinks, iter->second.from(), iter->second.to(), true, iter->second.type()) == biLinks.end())
|
|
{
|
|
biLinks.insert(std::make_pair(iter->second.from(), std::make_pair(iter->second.to(), iter->second.type())));
|
|
biLinks.insert(std::make_pair(iter->second.to(), std::make_pair(iter->second.from(), iter->second.type())));
|
|
}
|
|
}
|
|
}
|
|
|
|
while(!nextPoses.empty())
|
|
{
|
|
// Fill up all nodes before landmarks
|
|
// For nodes, fill up all neightbor nodes before loop closure ones
|
|
int currentId = nextPoses.begin()->first.id_;
|
|
Transform currentPose = nextPoses.begin()->second;
|
|
nextPoses.erase(nextPoses.begin());
|
|
|
|
if(posesOut.find(currentId) != posesOut.end()) {
|
|
// Already added from priority list
|
|
continue;
|
|
}
|
|
|
|
posesOut.insert(std::make_pair(currentId, currentPose));
|
|
|
|
// add prior links
|
|
for(std::multimap<int, Link>::const_iterator pter=linksIn.find(currentId); pter!=linksIn.end() && pter->first==currentId; ++pter)
|
|
{
|
|
if(pter->second.from() == pter->second.to() && (!priorsIgnored() || pter->second.type() != Link::kPosePrior))
|
|
{
|
|
linksOut.insert(*pter);
|
|
}
|
|
}
|
|
|
|
for(std::multimap<int, std::pair<int, Link::Type> >::const_iterator iter=biLinks.find(currentId); iter!=biLinks.end() && iter->first==currentId; ++iter)
|
|
{
|
|
int toId = iter->second.first;
|
|
Link::Type type = iter->second.second;
|
|
if(posesIn.find(toId) != posesIn.end() && (!landmarksIgnored() || toId>0))
|
|
{
|
|
std::multimap<int, Link>::const_iterator kter = graph::findLink(linksIn, currentId, toId, true, type);
|
|
UASSERT(kter!=linksIn.end());
|
|
if(!uContains(posesOut, toId))
|
|
{
|
|
const Transform & poseToIn = posesIn.at(toId);
|
|
Transform t = kter->second.from()==currentId?kter->second.transform():kter->second.transform().inverse();
|
|
Transform pose;
|
|
if(isSlam2d() && kter->second.type() == Link::kLandmark && toId>0 && (poseToIn.is3DoF() || poseToIn.is4DoF()))
|
|
{
|
|
if(poseToIn.is3DoF())
|
|
{
|
|
pose = (posesOut.at(currentId) * t).to3DoF();
|
|
}
|
|
else
|
|
{
|
|
pose = (posesOut.at(currentId) * t).to4DoF();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
pose = posesOut.at(currentId)* t;
|
|
}
|
|
|
|
nextPoses.insert(std::make_pair(LinkIdKey(toId, type), pose));
|
|
}
|
|
|
|
// only add unique links
|
|
if(graph::findLink(linksOut, currentId, toId, true, kter->second.type()) == linksOut.end())
|
|
{
|
|
if(kter->second.to() < 0)
|
|
{
|
|
// For landmarks, make sure fromId is the landmark
|
|
linksOut.insert(std::make_pair(kter->second.to(), kter->second.inverse()));
|
|
}
|
|
else
|
|
{
|
|
linksOut.insert(*kter);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
UDEBUG("OUT: poses=%d links=%d", (int)posesOut.size(), (int)linksOut.size());
|
|
}
|
|
|
|
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored, bool landmarksIgnored, float gravitySigma) :
|
|
iterations_(iterations),
|
|
slam2d_(slam2d),
|
|
covarianceIgnored_(covarianceIgnored),
|
|
epsilon_(epsilon),
|
|
robust_(robust),
|
|
priorsIgnored_(priorsIgnored),
|
|
landmarksIgnored_(landmarksIgnored),
|
|
gravitySigma_(gravitySigma)
|
|
{
|
|
}
|
|
|
|
Optimizer::Optimizer(const ParametersMap & parameters) :
|
|
iterations_(Parameters::defaultOptimizerIterations()),
|
|
slam2d_(Parameters::defaultRegForce3DoF()),
|
|
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
|
|
epsilon_(Parameters::defaultOptimizerEpsilon()),
|
|
robust_(Parameters::defaultOptimizerRobust()),
|
|
priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()),
|
|
landmarksIgnored_(Parameters::defaultOptimizerLandmarksIgnored()),
|
|
gravitySigma_(Parameters::defaultOptimizerGravitySigma())
|
|
{
|
|
parseParameters(parameters);
|
|
}
|
|
|
|
void Optimizer::parseParameters(const ParametersMap & parameters)
|
|
{
|
|
Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
|
|
Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerLandmarksIgnored(), landmarksIgnored_);
|
|
Parameters::parse(parameters, Parameters::kOptimizerGravitySigma(), gravitySigma_);
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimizeIncremental(
|
|
int rootId,
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & constraints,
|
|
std::list<std::map<int, Transform> > * intermediateGraphes,
|
|
double * finalError,
|
|
int * iterationsDone)
|
|
{
|
|
std::map<int, Transform> incGraph;
|
|
std::multimap<int, Link> incGraphLinks;
|
|
incGraph.insert(*poses.begin());
|
|
int i=0;
|
|
std::multimap<int, Link> constraintsCpy = constraints;
|
|
UDEBUG("Incremental optimization... poses=%d constraints=%d", (int)poses.size(), (int)constraints.size());
|
|
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
|
|
{
|
|
incGraph.insert(*iter);
|
|
bool hasLoopClosure = false;
|
|
for(std::multimap<int, Link>::iterator jter=constraintsCpy.lower_bound(iter->first); jter!=constraintsCpy.end() && jter->first==iter->first; ++jter)
|
|
{
|
|
UDEBUG("%d: %d -> %d type=%d", iter->first, jter->second.from(), jter->second.to(), jter->second.type());
|
|
if(jter->second.type() == Link::kNeighbor || jter->second.type() == Link::kNeighborMerged)
|
|
{
|
|
UASSERT(uContains(incGraph, iter->first));
|
|
incGraph.insert(std::make_pair(jter->second.to(), incGraph.at(iter->first) * jter->second.transform()));
|
|
incGraphLinks.insert(*jter);
|
|
}
|
|
else
|
|
{
|
|
if(!uContains(incGraph, jter->second.to()) && jter->second.to() > iter->first)
|
|
{
|
|
// node not yet in graph, switch link direction
|
|
constraintsCpy.insert(std::make_pair(jter->second.to(), jter->second.inverse()));
|
|
}
|
|
else
|
|
{
|
|
UASSERT(uContains(incGraph, jter->second.to()));
|
|
incGraphLinks.insert(*jter);
|
|
hasLoopClosure = true;
|
|
}
|
|
}
|
|
}
|
|
if(hasLoopClosure)
|
|
{
|
|
incGraph = this->optimize(incGraph.begin()->first, incGraph, incGraphLinks);
|
|
if(incGraph.empty())
|
|
{
|
|
UWARN("Failed incremental optimization... last pose added is %d", iter->first);
|
|
break;
|
|
}
|
|
}
|
|
UDEBUG("Iteration %d/%d %s", ++i, (int)poses.size(), hasLoopClosure?"*":"");
|
|
}
|
|
if(!incGraph.empty() && incGraph.size() == poses.size())
|
|
{
|
|
UASSERT(incGraphLinks.size() == constraints.size());
|
|
UASSERT(uContains(poses, rootId) && uContains(incGraph, rootId));
|
|
incGraph.at(rootId) = poses.at(rootId);
|
|
return this->optimize(rootId, incGraph, incGraphLinks, intermediateGraphes, finalError, iterationsDone);
|
|
}
|
|
|
|
UDEBUG("Failed incremental optimization");
|
|
return std::map<int, Transform>();
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimize(
|
|
int rootId,
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & edgeConstraints,
|
|
std::list<std::map<int, Transform> > * intermediateGraphes,
|
|
double * finalError,
|
|
int * iterationsDone)
|
|
{
|
|
cv::Mat covariance;
|
|
return optimize(rootId,
|
|
poses,
|
|
edgeConstraints,
|
|
covariance,
|
|
intermediateGraphes,
|
|
finalError,
|
|
iterationsDone);
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimize(
|
|
int rootId,
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & constraints,
|
|
cv::Mat & outputCovariance,
|
|
std::list<std::map<int, Transform> > * intermediateGraphes,
|
|
double * finalError,
|
|
int * iterationsDone)
|
|
{
|
|
UERROR("Optimizer %d doesn't implement optimize() method.", (int)this->type());
|
|
return std::map<int, Transform>();
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimizeBA(
|
|
int rootId,
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & links,
|
|
const std::map<int, std::vector<CameraModel> > & models,
|
|
std::map<int, cv::Point3f> & points3DMap,
|
|
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
|
|
BAOutliers * outliers)
|
|
{
|
|
if(outliers)
|
|
{
|
|
outliers->clear();
|
|
}
|
|
UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
|
|
return std::map<int, Transform>();
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimizeBA(
|
|
int rootId,
|
|
const std::map<int, Transform> & posesIn,
|
|
const std::multimap<int, Link> & links,
|
|
const std::map<int, Signature> & signatures,
|
|
std::map<int, cv::Point3f> & points3DMap,
|
|
std::map<int, std::map<int, FeatureBA> > & wordReferences,
|
|
bool rematchFeatures,
|
|
const ParametersMap & registrationParameters)
|
|
{
|
|
UDEBUG("");
|
|
std::map<int, std::vector<CameraModel> > multiModels;
|
|
std::map<int, Transform> poses;
|
|
for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
|
|
{
|
|
// Get camera model
|
|
std::vector<CameraModel> models;
|
|
if(uContains(signatures, iter->first))
|
|
{
|
|
const SensorData & s = signatures.at(iter->first).sensorData();
|
|
if(s.cameraModels().size() >= 1 && s.cameraModels().at(0).isValidForProjection())
|
|
{
|
|
models = s.cameraModels();
|
|
}
|
|
else if(!s.stereoCameraModels().empty() && s.stereoCameraModels()[0].isValidForProjection())
|
|
{
|
|
for(size_t i=0; i<s.stereoCameraModels().size(); ++i)
|
|
{
|
|
CameraModel model = s.stereoCameraModels()[i].left();
|
|
|
|
// Set Tx = -baseline*fx for stereo BA
|
|
models.push_back(CameraModel(
|
|
model.fx(),
|
|
model.fy(),
|
|
model.cx(),
|
|
model.cy(),
|
|
model.localTransform(),
|
|
-s.stereoCameraModels()[i].baseline()*model.fx(),
|
|
model.imageSize()));
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UERROR("Missing calibration for node %d", iter->first);
|
|
return std::map<int, Transform>();
|
|
}
|
|
}
|
|
else
|
|
{
|
|
UERROR("Did not find node %d in cache", iter->first);
|
|
return std::map<int, Transform>();
|
|
}
|
|
|
|
multiModels.insert(std::make_pair(iter->first, models));
|
|
poses.insert(*iter);
|
|
}
|
|
|
|
// compute correspondences
|
|
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures, false, registrationParameters);
|
|
|
|
return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
|
|
}
|
|
|
|
std::map<int, Transform> Optimizer::optimizeBA(
|
|
int rootId,
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & links,
|
|
const std::map<int, Signature> & signatures,
|
|
bool rematchFeatures,
|
|
const ParametersMap & registrationParameters)
|
|
{
|
|
std::map<int, cv::Point3f> points3DMap;
|
|
std::map<int, std::map<int, FeatureBA> > wordReferences;
|
|
return optimizeBA(rootId, poses, links, signatures, points3DMap, wordReferences, rematchFeatures, registrationParameters);
|
|
}
|
|
|
|
Transform Optimizer::optimizeBA(
|
|
const Link & link,
|
|
const CameraModel & model,
|
|
std::map<int, cv::Point3f> & points3DMap,
|
|
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
|
|
BAOutliers * outliers)
|
|
{
|
|
std::map<int, Transform> poses;
|
|
poses.insert(std::make_pair(link.from(), Transform::getIdentity()));
|
|
poses.insert(std::make_pair(link.to(), link.transform()));
|
|
std::multimap<int, Link> links;
|
|
links.insert(std::make_pair(link.from(), link));
|
|
std::map<int, std::vector<CameraModel> > models;
|
|
std::vector<CameraModel> tmp;
|
|
tmp.push_back(model);
|
|
models.insert(std::make_pair(link.from(), tmp));
|
|
models.insert(std::make_pair(link.to(), tmp));
|
|
poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
|
|
if(poses.size() == 2)
|
|
{
|
|
return poses.rbegin()->second;
|
|
}
|
|
else
|
|
{
|
|
return link.transform();
|
|
}
|
|
}
|
|
|
|
struct KeyPointCompare
|
|
{
|
|
bool operator() (const cv::KeyPoint& lhs, const cv::KeyPoint& rhs) const
|
|
{
|
|
return lhs.pt.x < rhs.pt.x || (lhs.pt.x == rhs.pt.x && lhs.pt.y < rhs.pt.y);
|
|
}
|
|
};
|
|
|
|
void Optimizer::computeBACorrespondences(
|
|
const std::map<int, Transform> & poses,
|
|
const std::multimap<int, Link> & links,
|
|
const std::map<int, Signature> & signatures,
|
|
std::map<int, cv::Point3f> & points3DMap,
|
|
std::map<int, std::map<int, FeatureBA> > & wordReferences,
|
|
bool rematchFeatures,
|
|
bool useLinkTransformAsGuess,
|
|
ParametersMap registrationParameters)
|
|
{
|
|
UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
|
|
int wordCount = 0;
|
|
int edgeWithWordsAdded = 0;
|
|
|
|
// Some defaults if not provided
|
|
registrationParameters.insert(ParametersPair(Parameters::kVisEstimationType(), "1"));
|
|
registrationParameters.insert(ParametersPair(Parameters::kVisPnPReprojError(), "5"));
|
|
registrationParameters.insert(ParametersPair(Parameters::kVisMinInliers(), "6"));
|
|
registrationParameters.insert(ParametersPair(Parameters::kVisCorNNDR(), "0.6"));
|
|
RegistrationVis reg(registrationParameters);
|
|
|
|
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
|
|
for(std::multimap<int, Link>::const_iterator iter=links.lower_bound(1); iter!=links.end(); ++iter)
|
|
{
|
|
Link link = iter->second;
|
|
if(link.to() < link.from())
|
|
{
|
|
link = link.inverse();
|
|
}
|
|
if(link.to() != link.from() &&
|
|
uContains(signatures, link.from()) &&
|
|
uContains(signatures, link.to()) &&
|
|
uContains(poses, link.from()))
|
|
{
|
|
Signature sFrom = signatures.at(link.from());
|
|
if(sFrom.getWeight() >= 0) // ignore intermediate links
|
|
{
|
|
Signature sTo = signatures.at(link.to());
|
|
|
|
if((sFrom.sensorData().cameraModels().empty() && sFrom.sensorData().stereoCameraModels().empty()) ||
|
|
(sTo.sensorData().cameraModels().empty() && sTo.sensorData().stereoCameraModels().empty()))
|
|
{
|
|
UERROR("No camera models found");
|
|
continue;
|
|
}
|
|
|
|
if(sTo.getWeight() < 0)
|
|
{
|
|
for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
|
|
sTo.getWeight() < 0 && jter!=links.end() && uContains(signatures, jter->second.to());
|
|
++jter)
|
|
{
|
|
sTo = signatures.at(jter->second.to());
|
|
}
|
|
}
|
|
|
|
if(sFrom.getWordsKpts().size() &&
|
|
sTo.getWordsKpts().size() &&
|
|
sFrom.getWords3().size())
|
|
{
|
|
if(!rematchFeatures)
|
|
{
|
|
sFrom.setWordsDescriptors(cv::Mat());
|
|
sTo.setWordsDescriptors(cv::Mat());
|
|
}
|
|
else if(sFrom.getWordsDescriptors().empty() && sTo.getWordsDescriptors().empty())
|
|
{
|
|
UWARN("Rematching features is enabled but signatures (%d and %d) don't have word descriptors!? "
|
|
"Features won't be rematched. If it is an old database, do rtabmap-reprocess "
|
|
"so that signatures contain word desriptors.",
|
|
sFrom.id(), sTo.id());
|
|
}
|
|
|
|
RegistrationInfo info;
|
|
Transform t = reg.computeTransformationMod(sFrom, sTo, useLinkTransformAsGuess?iter->second.transform():Transform(), &info);
|
|
UDEBUG("%d->%d, inliers=%d",sFrom.id(), sTo.id(), (int)info.inliersIDs.size());
|
|
|
|
if(!t.isNull())
|
|
{
|
|
if(!rematchFeatures)
|
|
{
|
|
// set descriptors for the output
|
|
if(sFrom.getWords().size() &&
|
|
sFrom.getWordsDescriptors().empty() &&
|
|
(int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
|
|
{
|
|
sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
|
|
}
|
|
if(sTo.getWords().size() &&
|
|
sTo.getWordsDescriptors().empty() &&
|
|
(int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
|
|
{
|
|
sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
|
|
}
|
|
}
|
|
|
|
Transform pose = poses.at(sFrom.id());
|
|
UASSERT(!pose.isNull());
|
|
for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
|
|
{
|
|
int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
|
|
cv::Point3f p = sFrom.getWords3()[indexFrom];
|
|
if(p.x > 0.0f) // make sure the point is valid
|
|
{
|
|
cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
|
|
int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
|
|
cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
|
|
|
|
int wordId = -1;
|
|
|
|
// find if the word is already added
|
|
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator fromIter = frameToWordMap.find(sFrom.id());
|
|
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> >::iterator toIter = frameToWordMap.find(sTo.id());
|
|
bool fromAlreadyAdded = false;
|
|
bool toAlreadyAdded = false;
|
|
if( fromIter != frameToWordMap.end() &&
|
|
fromIter->second.find(ptFrom) != fromIter->second.end())
|
|
{
|
|
wordId = fromIter->second.at(ptFrom);
|
|
fromAlreadyAdded = true;
|
|
}
|
|
if( toIter != frameToWordMap.end() &&
|
|
toIter->second.find(ptTo) != toIter->second.end())
|
|
{
|
|
wordId = toIter->second.at(ptTo);
|
|
toAlreadyAdded = true;
|
|
}
|
|
|
|
if(wordId == -1)
|
|
{
|
|
wordId = ++wordCount;
|
|
wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
|
|
|
|
points3DMap.insert(std::make_pair(wordId, util3d::transformPoint(p, pose)));
|
|
}
|
|
else
|
|
{
|
|
UASSERT(wordReferences.find(wordId) != wordReferences.end());
|
|
UASSERT(points3DMap.find(wordId) != points3DMap.end());
|
|
}
|
|
|
|
if(!fromAlreadyAdded)
|
|
{
|
|
cv::Mat descriptorFrom;
|
|
if(!sFrom.getWordsDescriptors().empty())
|
|
{
|
|
UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
|
|
descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
|
|
}
|
|
int cameraIndex = 0;
|
|
if(sFrom.sensorData().cameraModels().size()>1 || sFrom.sensorData().stereoCameraModels().size()>1)
|
|
{
|
|
float subImageWidth = sFrom.sensorData().cameraModels().size()>1?sFrom.sensorData().cameraModels()[0].imageWidth():sFrom.sensorData().stereoCameraModels()[0].left().imageWidth();
|
|
cameraIndex = int(ptFrom.pt.x / subImageWidth);
|
|
ptFrom.pt.x = ptFrom.pt.x - (subImageWidth*float(cameraIndex));
|
|
}
|
|
|
|
float depth = 0.0f;
|
|
if(!sFrom.sensorData().cameraModels().empty())
|
|
{
|
|
depth = util3d::transformPoint(p, sFrom.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
|
|
}
|
|
else
|
|
{
|
|
UASSERT(!sFrom.sensorData().stereoCameraModels().empty());
|
|
depth = util3d::transformPoint(p, sFrom.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
|
|
}
|
|
|
|
|
|
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, depth, descriptorFrom, cameraIndex)));
|
|
frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
|
|
frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
|
|
}
|
|
|
|
if(!toAlreadyAdded)
|
|
{
|
|
cv::Mat descriptorTo;
|
|
if(!sTo.getWordsDescriptors().empty())
|
|
{
|
|
UASSERT(indexTo < sTo.getWordsDescriptors().rows);
|
|
descriptorTo = sTo.getWordsDescriptors().row(indexTo);
|
|
}
|
|
|
|
int cameraIndex = 0;
|
|
if(sTo.sensorData().cameraModels().size()>1 || sTo.sensorData().stereoCameraModels().size()>1)
|
|
{
|
|
float subImageWidth = sTo.sensorData().cameraModels().size()>1?sTo.sensorData().cameraModels()[0].imageWidth():sTo.sensorData().stereoCameraModels()[0].left().imageWidth();
|
|
cameraIndex = int(ptTo.pt.x / subImageWidth);
|
|
ptTo.pt.x = ptTo.pt.x - (subImageWidth*float(cameraIndex));
|
|
}
|
|
|
|
float depth = 0.0f;
|
|
if(!sTo.getWords3().empty())
|
|
{
|
|
UASSERT(indexTo < (int)sTo.getWords3().size());
|
|
const cv::Point3f & pt = sTo.getWords3()[indexTo];
|
|
if(!sTo.sensorData().cameraModels().empty())
|
|
{
|
|
depth = util3d::transformPoint(pt, sTo.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
|
|
}
|
|
else
|
|
{
|
|
UASSERT(!sTo.sensorData().stereoCameraModels().empty());
|
|
depth = util3d::transformPoint(pt, sTo.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
|
|
}
|
|
}
|
|
|
|
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo, cameraIndex)));
|
|
frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
|
|
frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
|
|
}
|
|
}
|
|
}
|
|
++edgeWithWordsAdded;
|
|
}
|
|
else
|
|
{
|
|
UWARN("Not enough inliers (%d) between %d and %d", (int)info.inliersIDs.size(), sFrom.id(), sTo.id());
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, (int)links.size());
|
|
if(links.empty())
|
|
{
|
|
UERROR("No links found for BA?!");
|
|
}
|
|
else if(wordCount == 0)
|
|
{
|
|
UERROR("No words added for BA?!");
|
|
}
|
|
}
|
|
|
|
} /* namespace rtabmap */
|