i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added Bundle Adjustment options to OdometryF2M (OdomF2M/BundleAdjustment) and RegistrationVis (Vis/BundleAdjustment). Added logger thread filter. RegistrationVis: Copy back corresponding input word IDs to output words when available. Memory::computeTransform() added option to use already computed corespondences if possible (proximity detection by time uses this). Updated Optimizer::optimizeBA() interface.

Browse Source
This commit is contained in:
matlabbe
2016-11-14 19:54:31 -05:00
parent 9fbd02b06d
commit 9276607920
39 changed files with 2214 additions and 594 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
corelib/src/CameraThread.cpp
View File

@@ -117,6 +117,11 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
_bilateralSigmaR = sigmaR;
}
void CameraThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Camera");
}
void CameraThread::mainLoop()
{
UTimer totalTime;

18
corelib/src/Memory.cpp
View File

@@ -2091,7 +2091,8 @@ Transform Memory::computeTransform(
int fromId,
int toId,
Transform guess,
RegistrationInfo * info)
RegistrationInfo * info,
bool useKnownCorrespondencesIfPossible)
{
Signature * fromS = this->_getSignature(fromId);
Signature * toS = this->_getSignature(toId);
@@ -2100,7 +2101,7 @@ Transform Memory::computeTransform(
if(fromS && toS)
{
return computeTransform(*fromS, *toS, guess, info);
return computeTransform(*fromS, *toS, guess, info, useKnownCorrespondencesIfPossible);
}
else
{
@@ -2119,7 +2120,8 @@ Transform Memory::computeTransform(
Signature & fromS,
Signature & toS,
Transform guess,
RegistrationInfo * info) const
RegistrationInfo * info,
bool useKnownCorrespondencesIfPossible) const
{
Transform transform;
@@ -2172,6 +2174,12 @@ Transform Memory::computeTransform(
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
else if(useKnownCorrespondencesIfPossible)
{
// This will make RegistrationVis bypassing the correspondences computation
tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
}
if(guess.isNull() && !_registrationPipeline->isImageRequired())
{
@@ -2181,12 +2189,12 @@ Transform Memory::computeTransform(
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
}
}
else
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
}
if(!transform.isNull())

230
corelib/src/OdometryF2M.cpp
View File

@@ -63,6 +63,8 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
scanMaximumMapSize_(Parameters::defaultOdomF2MScanMaxSize()),
scanSubtractRadius_(Parameters::defaultOdomF2MScanSubtractRadius()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
bundleAdjustment_(Parameters::defaultOdomF2MBundleAdjustment()),
bundleAdjustmentMaxFrames_(Parameters::defaultOdomF2MBundleAdjustmentMaxFrames()),
regPipeline_(Registration::create(parameters)),
map_(new Signature(-1)),
lastFrame_(new Signature(1))
@@ -75,6 +77,9 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomF2MScanMaxSize(), scanMaximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomF2MScanSubtractRadius(), scanSubtractRadius_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustment(), bundleAdjustment_);
Parameters::parse(parameters, Parameters::kOdomF2MBundleAdjustmentMaxFrames(), bundleAdjustmentMaxFrames_);
bundleParameters_ = parameters;
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(scanKeyFrameThr_ >= 0.0f && scanKeyFrameThr_<=1.0f);
@@ -164,6 +169,12 @@ OdometryF2M::~OdometryF2M()
{
delete map_;
delete lastFrame_;
scansBuffer_.clear();
bundleWordReferences_.clear();
bundlePoses_.clear();
bundleLinks_.clear();
bundleModels_.clear();
bundlePoseReferences_.clear();
UDEBUG("");
}
@@ -203,6 +214,13 @@ Transform OdometryF2M::computeTransform(
delete lastFrame_;
lastFrame_ = new Signature(data);
if(bundleAdjustment_ > 0 &&
data.cameraModels().size() > 1)
{
UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
bundleAdjustment_ = 0;
}
// Generate keypoints from the new data
if(lastFrame_->sensorData().isValid())
{
@@ -219,8 +237,106 @@ Transform OdometryF2M::computeTransform(
data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
if(!transform.isNull())
{
// local bundle adjustment
if(bundleAdjustment_>0 &&
regPipeline_->isImageRequired() &&
((bundleAdjustment_==1 && Optimizer::isAvailable(Optimizer::kTypeG2O)) ||
(bundleAdjustment_==2 && Optimizer::isAvailable(Optimizer::kTypeCVSBA))) &&
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
// make sure the IDs of words in the map are not modified (Optical Flow Registration issue)
UASSERT(map_->getWords().size() && tmpMap.getWords().size());
if(map_->getWords().size() != tmpMap.getWords().size() ||
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
bundleAdjustment_ = 0;
}
else
{
UTimer bundleTime;
Optimizer * sba = Optimizer::create(bundleAdjustment_==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, bundleParameters_);
std::map<int, std::map<int, cv::Point2f> > wordReferences;
UASSERT(bundlePoses_.size());
UASSERT(bundlePoses_.size()-1 == bundleLinks_.size() && bundlePoses_.size() == bundleModels_.size());
if(bundleAdjustmentMaxFrames_ > 0)
{
std::map<int, Transform>::reverse_iterator iter = bundlePoses_.rbegin();
for(int i = 0; i<bundleAdjustmentMaxFrames_ && i < (int)bundlePoses_.size()-1; ++i, ++iter)
{
bundlePoses.insert(*iter);
UASSERT(bundleLinks_.find(iter->first) != bundleLinks_.end());
bundleLinks.insert(*bundleLinks_.find(iter->first));
UASSERT(bundleModels_.find(iter->first) != bundleModels_.end());
bundleModels.insert(*bundleModels_.find(iter->first));
}
//make sure the origin is there
bundlePoses.insert(*bundlePoses_.find(0));
bundleModels.insert(*bundleModels_.find(0));
}
else
{
bundlePoses = bundlePoses_;
bundleLinks = bundleLinks_;
bundleModels = bundleModels_;
}
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, transform, regInfo.variance, regInfo.variance)));
bundlePoses.insert(std::make_pair(lastFrame_->id(), transform));
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
{
std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(regInfo.inliersIDs[i]);
UASSERT(iter3D!=tmpMap.getWords3().end());
points3DMap.insert(*iter3D);
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(regInfo.inliersIDs[i]);
UASSERT(iter2D!=lastFrame_->getWords().end());
if(wordReferences.find(iter2D->first) == wordReferences.end())
{
UASSERT(bundleWordReferences_.find(iter2D->first) != bundleWordReferences_.end());
wordReferences.insert(*bundleWordReferences_.find(iter2D->first));
}
wordReferences.find(iter2D->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
}
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
}
UASSERT(model.isValidForProjection());
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses = sba->optimizeBA(0, bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences);
delete sba;
UDEBUG("bundleTime=%fs (poses=%d wordRef=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)bundleWordReferences_.size());
UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
UDEBUG("Local Bundle Adjustment After : %s", bundlePoses.rbegin()->second.prettyPrint().c_str());
if(!bundlePoses.rbegin()->second.isNull())
{
transform = bundlePoses.rbegin()->second;
}
}
}
// make it incremental
transform = this->getPose().inverse() * transform;
}
@@ -262,6 +378,24 @@ Transform OdometryF2M::computeTransform(
UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
if(bundleAdjustment_>0)
{
// update local map 3D points (if bundle adjustment was done)
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
UASSERT(mapPoints.count(iter->first) == 1);
mapPoints.find(iter->first)->second = iter->second;
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), 0));
uInsert(bundlePoses_, bundlePoses);
UASSERT(bundleModels.find(lastFrame_->id()) != bundleModels.end());
bundleModels_.insert(*bundleModels.find(lastFrame_->id()));
UASSERT(bundleLinks.find(lastFrame_->id()) != bundleLinks.end());
bundleLinks_.insert(*bundleLinks.find(lastFrame_->id()));
iterBundlePosesRef = bundlePoseReferences_.find(lastFrame_->id());
}
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
@@ -279,6 +413,25 @@ Transform OdometryF2M::computeTransform(
std::make_pair(iter2D->second,
std::make_pair(iter->second, iterDesc->second)))));
}
else if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), iter2D->second.pt));
}
}
}
}
}
@@ -288,6 +441,26 @@ Transform OdometryF2M::computeTransform(
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
if(bundleAdjustment_>0)
{
if(lastFrame_->getWords().count(iter->second.first) == 1)
{
UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
iterBundlePosesRef->second += 1;
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), iter->second.second.first.pt));
}
}
}
mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second.second.first, newFramePose)));
mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
@@ -307,6 +480,27 @@ Transform OdometryF2M::computeTransform(
{
if(matches.find(iter->first) == matches.end())
{
std::map<int, std::map<int, cv::Point2f> >::iterator iterRef = bundleWordReferences_.find(iter->first);
if(iterRef != bundleWordReferences_.end())
{
for(std::map<int, cv::Point2f>::iterator iterKp = iterRef->second.begin(); iterKp != iterRef->second.end(); ++iterKp)
{
if(bundlePoseReferences_.find(iterKp->first) != bundlePoseReferences_.end())
{
bundlePoseReferences_.at(iterKp->first) -= 1;
if(bundlePoseReferences_.at(iterKp->first) <= regPipeline_->getMinVisualCorrespondences())
{
bundlePoses_.erase(iterKp->first);
bundleLinks_.erase(iterKp->first);
bundleModels_.erase(iterKp->first);
bundlePoseReferences_.erase(iterKp->first);
UDEBUG("bundlePoseReferences_ erased all words from cam %d", iterKp->first);
}
}
}
bundleWordReferences_.erase(iterRef);
}
mapPoints.erase(iter++);
mapDescriptors.erase(iterMapDescriptors++);
mapWords.erase(iterMapWords++);
@@ -514,6 +708,42 @@ Transform OdometryF2M::computeTransform(
descriptors.insert(*descIter);
}
}
if(bundleAdjustment_>0)
{
// update bundleWordReferences_: used for bundle adjustment
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, cv::Point2f> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), iter->second.pt));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
}
UASSERT(model.isValidForProjection());
UASSERT_MSG(lastFrame_->id() > 0, uFormat("Input data should have ID greater than 0 when odometry bundle adjustment is enabled!").c_str());
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
bundleLinks_.insert(std::make_pair(lastFrame_->id(), Link(0, lastFrame_->id(), Link::kNeighbor, newFramePose, 0.000001, 0.00001)));
//origin
bundlePoses_.insert(std::make_pair(0, Transform::getIdentity()));
bundleModels_.insert(std::make_pair(0, model));
}
map_->setWords(words);
map_->setWords3(transformedPoints);
map_->setWordsDescriptors(descriptors);

5
corelib/src/OdometryThread.cpp
View File

@@ -74,6 +74,11 @@ void OdometryThread::handleEvent(UEvent * event)
}
}
void OdometryThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Odometry");
}
void OdometryThread::mainLoopKill()
{
_dataAdded.release();

174
corelib/src/Optimizer.cpp
View File

@@ -154,56 +154,6 @@ Optimizer * Optimizer::create(Optimizer::Type type, const ParametersMap & parame
return optimizer;
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust)
{
}
Optimizer::Optimizer(const ParametersMap & parameters) :
iterations_(Parameters::defaultOptimizerIterations()),
slam2d_(Parameters::defaultOptimizerSlam2D()),
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust())
{
parseParameters(parameters);
}
void Optimizer::parseParameters(const ParametersMap & parameters)
{
Parameters::parse(parameters, Parameters::kOptimizerIterations(), iterations_);
Parameters::parse(parameters, Parameters::kOptimizerVarianceIgnored(), covarianceIgnored_);
Parameters::parse(parameters, Parameters::kOptimizerSlam2D(), slam2d_);
Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_);
Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_);
}
std::map<int, Transform> Optimizer::optimize(
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)
{
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, Signature> & signatures)
{
UERROR("Optimizer %d doesn't implement optimizeBA() method.", (int)this->type());
return std::map<int, Transform>();
}
void Optimizer::getConnectedGraph(
int fromId,
const std::map<int, Transform> & posesIn,
@@ -271,6 +221,129 @@ void Optimizer::getConnectedGraph(
}
}
Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) :
iterations_(iterations),
slam2d_(slam2d),
covarianceIgnored_(covarianceIgnored),
epsilon_(epsilon),
robust_(robust)
{
}
Optimizer::Optimizer(const ParametersMap & parameters) :
iterations_(Parameters::defaultOptimizerIterations()),
slam2d_(Parameters::defaultRegForce3DoF()),
covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()),
epsilon_(Parameters::defaultOptimizerEpsilon()),
robust_(Parameters::defaultOptimizerRobust())
{
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_);
}
std::map<int, Transform> Optimizer::optimize(
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)
{
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, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
{
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> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
{
UDEBUG("");
std::map<int, CameraModel> models;
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
// Get camera model
CameraModel model;
if(uContains(signatures, iter->first))
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
}
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>();
}
UASSERT(model.isValidForProjection());
models.insert(std::make_pair(iter->first, model));
}
// compute correspondences
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point2f> > wordReferences;
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences);
return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
}
Transform Optimizer::optimizeBA(
const Link & link,
const CameraModel & model,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences)
{
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, CameraModel> models;
models.insert(std::make_pair(link.from(), model));
models.insert(std::make_pair(link.to(), model));
poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences);
if(poses.size() == 2)
{
return poses.rbegin()->second;
}
else
{
return link.transform();
}
}
void Optimizer::computeBACorrespondences(
const std::map<int, Transform> & poses,
@@ -279,6 +352,7 @@ void Optimizer::computeBACorrespondences(
std::map<int, cv::Point3f> & points3DMap,
std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
{
UDEBUG("");
int wordCount = 0;
int edgeWithWordsAdded = 0;
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)

137
corelib/src/OptimizerCVSBA.cpp
View File

@@ -57,7 +57,9 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>)
{
#ifdef RTABMAP_CVSBA
// run sba optimization
@@ -73,100 +75,72 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
params.verbose=ULogger::level() <= ULogger::kInfo;
sba.setParams(params);
std::map<int, Transform> frames = poses;
std::vector<cv::Mat> cameraMatrix(frames.size()); //nframes
std::vector<cv::Mat> R(frames.size()); //nframes
std::vector<cv::Mat> T(frames.size()); //nframes
std::vector<cv::Mat> distCoeffs(frames.size()); //nframes
std::vector<cv::Mat> cameraMatrix(poses.size()); //nframes
std::vector<cv::Mat> R(poses.size()); //nframes
std::vector<cv::Mat> T(poses.size()); //nframes
std::vector<cv::Mat> distCoeffs(poses.size()); //nframes
std::map<int, int> frameIdToIndex;
std::map<int, CameraModel> models;
int oi=0;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
CameraModel model;
if(uContains(signatures, iter->first))
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
frameIdToIndex.insert(std::make_pair(iter->first, oi));
cameraMatrix[oi] = iterModel->second.K();
if(iterModel->second.D().cols != 5)
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
}
else
{
UERROR("Missing calibration for node %d", iter->first);
}
distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
}
else
{
UERROR("Did not find node %d in cache", iter->first);
distCoeffs[oi] = iterModel->second.D();
}
if(model.isValidForProjection())
{
frameIdToIndex.insert(std::make_pair(iter->first, oi));
Transform t = (iter->second * iterModel->second.localTransform()).inverse();
cameraMatrix[oi] = model.K();
if(model.D().cols != 5)
{
distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
}
else
{
distCoeffs[oi] = model.D();
}
R[oi] = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
++oi;
Transform t = (iter->second * model.localTransform()).inverse();
R[oi] = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
++oi;
models.insert(std::make_pair(iter->first, model));
UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
++iter;
}
else
{
frames.erase(iter++);
}
UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
}
cameraMatrix.resize(oi);
R.resize(oi);
T.resize(oi);
distCoeffs.resize(oi);
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
UDEBUG("points=%d frames=%d", (int)wordReferences.size(), (int)frames.size());
std::vector<cv::Point3f> points(wordReferences.size()); //npoints
std::vector<std::vector<cv::Point2f> > imagePoints(frames.size()); //nframes -> npoints
std::vector<std::vector<int> > visibility(frames.size()); //nframes -> npoints
for(unsigned int i=0; i<frames.size(); ++i)
UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
std::vector<cv::Point3f> points(points3DMap.size()); //npoints
std::vector<std::vector<cv::Point2f> > imagePoints(poses.size()); //nframes -> npoints
std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
for(unsigned int i=0; i<poses.size(); ++i)
{
imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
visibility[i].resize(wordReferences.size(), 0);
}
int i=0;
for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
points[i] = points3DMap.at(iter->first);
points[i] = kter->second;
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.find(kter->first);
if(iter != wordReferences.end())
{
imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
visibility[frameIdToIndex.at(jter->first)][i] = 1;
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
{
imagePoints[frameIdToIndex.at(jter->first)][i] = jter->second;
visibility[frameIdToIndex.at(jter->first)][i] = 1;
}
}
}
++i;
}
@@ -184,7 +158,8 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
//update poses
i=0;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); ++iter)
std::map<int, Transform> newPoses = poses;
for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
@@ -192,12 +167,28 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
iter->second = (models.at(iter->first).localTransform() * t).inverse();
if(this->isSlam2d())
{
t = (models.at(iter->first).localTransform() * t).inverse();
t = iter->second.inverse() * t;
iter->second *= t.to3DoF();
}
else
{
iter->second = (models.at(iter->first).localTransform() * t).inverse();
}
++i;
}
return frames;
//update 3D points
i=0;
for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
kter->second = points[i++];
}
return newPoses;
#else
UERROR("RTAB-Map is not built with cvsba!");

166
corelib/src/OptimizerG2O.cpp
View File

@@ -235,7 +235,7 @@ std::map<int, Transform> OptimizerG2O::optimize(
int vertigoVertexId = poses.rbegin()->first+1;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->first;
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
@@ -547,14 +547,16 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, Signature> & signatures)
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, cv::Point2f> > & wordReferences) // <ID words, IDs frames + keypoint>
{
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_G2O
UDEBUG("Optimizing graph...");
optimizedPoses.clear();
if(links.size()>=1 && poses.size()>=2 && iterations() > 0)
if(links.size()>=1 && poses.size()>=2 && iterations() > 0 && models.size() == poses.size())
{
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(ULogger::level()==ULogger::kDebug);
@@ -593,40 +595,14 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
optimizer.setAlgorithm(new g2o::OptimizationAlgorithmLevenberg(solver_ptr));
}
std::map<int, Transform> frames = poses;
UDEBUG("fill poses to g2o...");
std::map<int, CameraModel> models;
for(std::map<int, Transform>::iterator iter=frames.begin(); iter!=frames.end(); )
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); )
{
// Get camera model
CameraModel model;
if(uContains(signatures, iter->first))
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
}
else
{
UERROR("Missing calibration for node %d", iter->first);
return optimizedPoses;
}
}
else
{
UERROR("Did not find node %d in cache", iter->first);
return optimizedPoses;
}
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
UASSERT(model.isValidForProjection());
models.insert(std::make_pair(iter->first, model));
Transform camPose = iter->second * model.localTransform();
Transform camPose = iter->second * iterModel->second.localTransform();
//iter->second = (iter->second * model.localTransform()).inverse();
UDEBUG("%d t=%s", iter->first, camPose.prettyPrint().c_str());
@@ -636,7 +612,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
Eigen::Affine3d a = camPose.toEigen3d();
g2o::SBACam cam(Eigen::Quaterniond(a.rotation()), a.translation());
cam.setKcam(model.fx(), model.fy(), model.cx(), model.cy(), 0);
cam.setKcam(iterModel->second.fx(), iterModel->second.fy(), iterModel->second.cx(), iterModel->second.cy(), 0);
vCam->setEstimate(cam);
if(iter->first == rootId)
{
@@ -652,18 +628,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
UDEBUG("fill edges to g2o and associate each 3D point to all frames observing it...");
for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
{
Link link = iter->second;
if(link.to() < link.from())
{
link = link.inverse();
}
if(uContains(signatures, link.from()) &&
uContains(signatures, link.to()) &&
uContains(frames, link.from()) &&
uContains(frames, link.to()))
if(uContains(poses, iter->second.from()) &&
uContains(poses, iter->second.to()))
{
// add edge
int id1 = iter->first;
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());
@@ -703,47 +672,48 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point2f> > wordReferences; // <ID words, IDs frames + keypoint>
this->computeBACorrespondences(frames, links, signatures, points3DMap, wordReferences);
UDEBUG("fill 3D points to g2o...");
int stepVertexId = frames.rbegin()->first+1;
for(std::map<int, std::map<int, cv::Point2f> >::iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
int stepVertexId = poses.rbegin()->first+1;
for(std::map<int, std::map<int, cv::Point2f> >::const_iterator iter = wordReferences.begin(); iter!=wordReferences.end(); ++iter)
{
const cv::Point3f & pt3d = points3DMap.at(iter->first);
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
vpt3d->setId(stepVertexId + iter->first);
vpt3d->setMarginalized(true);
optimizer.addVertex(vpt3d);
// set observations
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
if(points3DMap.find(iter->first) != points3DMap.end())
{
int camId = jter->first;
const cv::Point3f & pt3d = points3DMap.at(iter->first);
g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
const cv::Point2f & pt = jter->second;
vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
vpt3d->setId(stepVertexId + iter->first);
vpt3d->setMarginalized(true);
optimizer.addVertex(vpt3d);
Eigen::Matrix<double,2,1> obs;
obs << pt.x, pt.y;
UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
e->setVertex(0, vpt3d);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
if(robustKernel)
// set observations
for(std::map<int, cv::Point2f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
e->setRobustKernel(new g2o::RobustKernelHuber);
}
int camId = jter->first;
if(poses.find(camId) != poses.end())
{
const cv::Point2f & pt = jter->second;
optimizer.addEdge(e);
Eigen::Matrix<double,2,1> obs;
obs << pt.x, pt.y;
//UDEBUG("Added observation pt=%d to cam=%d (%f,%f)", vpt3d->id(), camId, pt.x, pt.y);
g2o::EdgeProjectP2MC* e = new g2o::EdgeProjectP2MC();
e->setVertex(0, vpt3d);
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(camId)));
e->setMeasurement(obs);
e->setInformation(Eigen::Matrix2d::Identity() / pixelVariance_);
if(robustKernel)
{
e->setRobustKernel(new g2o::RobustKernelHuber);
}
optimizer.addEdge(e);
}
}
}
}
@@ -809,23 +779,55 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
return optimizedPoses;
}
// update poses
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
if(v)
{
Transform t = Transform::fromEigen3d(v->estimate());
UDEBUG("%d t=%s", iter->first, t.prettyPrint().c_str());
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
//UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
UASSERT_MSG(!t.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
// FIXME: is there a way that we can add the 2D constraint directly in SBA?
if(this->isSlam2d())
{
// get transform between old and new pose
t = iter->second.inverse() * t;
optimizedPoses.insert(std::pair<int, Transform>(iter->first, iter->second * t.to3DoF()));
}
else
{
optimizedPoses.insert(std::pair<int, Transform>(iter->first, t));
}
}
else
{
UERROR("Vertex %d not found!?", iter->first);
UERROR("Vertex (pose) %d not found!?", iter->first);
}
}
//update points3D
for(std::map<int, cv::Point3f>::iterator iter = points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
const g2o::VertexSBAPointXYZ* v = (const g2o::VertexSBAPointXYZ*)optimizer.vertex(stepVertexId + iter->first);
if(v)
{
cv::Point3f p(v->estimate()[0], v->estimate()[1], v->estimate()[2]);
//UDEBUG("%d from=%f,%f,%f to=%f,%f,%f", iter->first, iter->second.x, iter->second.y, iter->second.z, p.x, p.y, p.z);
iter->second = p;
}
else
{
UERROR("Vertex (point3D) %d not found!?", iter->first);
}
}
}
else if(poses.size() > 1 && poses.size() != models.size())
{
UERROR("This method should be called with size of poses = size camera models!");
}
else if(poses.size() == 1 || iterations() <= 0)
{
@@ -893,7 +895,7 @@ bool OptimizerG2O::saveGraph(
Eigen::Quaternionf q = iter->second.transform().getQuaternionf();
fprintf(file, "%s %d %d%s %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
prefix.c_str(),
iter->first,
iter->second.from(),
iter->second.to(),
suffix.c_str(),
iter->second.transform().x(),

2
corelib/src/OptimizerGTSAM.cpp
View File

@@ -126,7 +126,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
int switchCounter = poses.rbegin()->first+1;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->first;
int id1 = iter->second.from();
int id2 = iter->second.to();
UASSERT(!iter->second.transform().isNull());

6
corelib/src/OptimizerTORO.cpp
View File

@@ -124,7 +124,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
int id1 = iter->first;
int id1 = iter->second.from();
int id2 = iter->second.to();
AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
@@ -152,7 +152,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
int id1 = iter->first;
int id1 = iter->second.from();
int id2 = iter->second.to();
AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
@@ -356,7 +356,7 @@ bool OptimizerTORO::saveGraph(
float x,y,z, yaw,pitch,roll;
iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
iter->first,
iter->second.from(),
iter->second.to(),
x,
y,

7
corelib/src/Parameters.cpp
View File

@@ -224,6 +224,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.11.12
removedParameters_.insert(std::make_pair("Optimizer/Slam2D", std::make_pair(true, Parameters::kRegForce3DoF())));
// 0.11.10 typos
removedParameters_.insert(std::make_pair("Grid/FlatObstaclesDetected", std::make_pair(true, Parameters::kGridFlatObstacleDetected())));
@@ -332,8 +335,8 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("RGBD/OptimizeEpsilon", std::make_pair(true, Parameters::kOptimizerEpsilon())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeIterations", std::make_pair(true, Parameters::kOptimizerIterations())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeRobust", std::make_pair(true, Parameters::kOptimizerRobust())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D", std::make_pair(true, Parameters::kOptimizerSlam2D())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2d", std::make_pair(true, Parameters::kOptimizerSlam2D())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2D", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeSlam2d", std::make_pair(true, Parameters::kRegForce3DoF())));
removedParameters_.insert(std::make_pair("RGBD/OptimizeVarianceIgnored", std::make_pair(true, Parameters::kOptimizerVarianceIgnored())));
removedParameters_.insert(std::make_pair("Stereo/WinSize", std::make_pair(true, Parameters::kStereoWinWidth())));

176
corelib/src/RegistrationVis.cpp
View File

@@ -34,6 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/VWDictionary.h>
#include <rtabmap/core/util2d.h>
#include <rtabmap/core/Features2d.h>
#include <rtabmap/core/VisualWord.h>
#include <rtabmap/core/Optimizer.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
@@ -62,7 +65,8 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_flowEps(Parameters::defaultVisCorFlowEps()),
_flowMaxLevel(Parameters::defaultVisCorFlowMaxLevel()),
_nndr(Parameters::defaultVisCorNNDR()),
_guessWinSize(Parameters::defaultVisCorGuessWinSize())
_guessWinSize(Parameters::defaultVisCorGuessWinSize()),
_bundleAdjustment(Parameters::defaultVisBundleAdjustment())
{
_featureParameters = Parameters::getDefaultParameters();
uInsert(_featureParameters, ParametersPair(Parameters::kKpNNStrategy(), _featureParameters.at(Parameters::kVisCorNNType())));
@@ -100,6 +104,8 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), _flowMaxLevel);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), _nndr);
Parameters::parse(parameters, Parameters::kVisCorGuessWinSize(), _guessWinSize);
Parameters::parse(parameters, Parameters::kVisBundleAdjustment(), _bundleAdjustment);
uInsert(_bundleParameters, parameters);
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
@@ -247,6 +253,7 @@ Transform RegistrationVis::computeTransformationImpl(
cv::Mat imageFrom = fromSignature.sensorData().imageRaw();
cv::Mat imageTo = toSignature.sensorData().imageRaw();
std::vector<int> orignalWordsFromIds;
if(fromSignature.getWords().empty())
{
if(fromSignature.sensorData().keypoints().empty())
@@ -283,7 +290,14 @@ Transform RegistrationVis::computeTransformationImpl(
}
else
{
kptsFrom = uValues(fromSignature.getWords());
kptsFrom.resize(fromSignature.getWords().size());
orignalWordsFromIds.resize(fromSignature.getWords().size());
int i=0;
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
{
kptsFrom[i] = iter->second;
orignalWordsFromIds[i++] = iter->first;
}
}
std::multimap<int, cv::KeyPoint> wordsFrom;
@@ -363,6 +377,7 @@ Transform RegistrationVis::computeTransformationImpl(
UASSERT(kptsFrom.size() == kptsFrom3D.size());
std::vector<cv::KeyPoint> kptsTo(kptsFrom.size());
std::vector<cv::Point3f> kptsFrom3DKept(kptsFrom3D.size());
std::vector<int> orignalWordsFromIdsCpy = orignalWordsFromIds;
int ki = 0;
for(unsigned int i=0; i<status.size(); ++i)
{
@@ -370,11 +385,19 @@ Transform RegistrationVis::computeTransformationImpl(
uIsInBounds(cornersTo[i].x, 0.0f, float(imageTo.cols)) &&
uIsInBounds(cornersTo[i].y, 0.0f, float(imageTo.rows)))
{
if(orignalWordsFromIdsCpy.size())
{
orignalWordsFromIds[ki] = orignalWordsFromIdsCpy[i];
}
kptsFrom[ki] = cv::KeyPoint(cornersFrom[i], 1);
kptsFrom3DKept[ki] = kptsFrom3D[i];
kptsTo[ki++] = cv::KeyPoint(cornersTo[i], 1);
}
}
if(orignalWordsFromIds.size())
{
orignalWordsFromIds.resize(ki);
}
kptsFrom.resize(ki);
kptsTo.resize(ki);
kptsFrom3DKept.resize(ki);
@@ -390,12 +413,13 @@ Transform RegistrationVis::computeTransformationImpl(
UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3DKept[i]));
wordsTo.insert(std::make_pair(i, kptsTo[i]));
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
wordsTo.insert(std::make_pair(id, kptsTo[i]));
if(kptsTo3D.size())
{
words3To.insert(std::make_pair(i, kptsTo3D[i]));
words3To.insert(std::make_pair(id, kptsTo3D[i]));
}
}
toSignature.sensorData().setFeatures(kptsTo, cv::Mat());
@@ -407,8 +431,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(util3d::isFinite(kptsFrom3D[i]))
{
wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
words3From.insert(std::make_pair(i, kptsFrom3D[i]));
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
}
}
toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
@@ -705,9 +730,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("");
// Process results (Nearest Neighbor Distance Ratio)
int matchedID = descriptorsFrom.rows+descriptorsTo.rows;
int newToId = descriptorsFrom.rows;
int notMatchedFromId = 0;
std::map<int,int> addedWordsFrom; //<id, index>
std::map<int, int> duplicates; //<fromId, toId>
int newWords = 0;
@@ -759,7 +782,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(matchedIndex >= 0)
{
matchedIndex = projectedIndexToDescIndex[matchedIndex];
int id = matchedID++;
int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
{
@@ -811,11 +834,11 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
{
wordsFrom.insert(std::make_pair(notMatchedFromId, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(notMatchedFromId, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(notMatchedFromId, kptsFrom3D[i]));
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(i)));
words3From.insert(std::make_pair(id, kptsFrom3D[i]));
++notMatchedFromId;
++addWordsFromNotMatched;
}
}
@@ -866,9 +889,15 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("");
// match between all descriptors
VWDictionary dictionary(_featureParameters);
std::list<int> fromWordIds = dictionary.addNewWords(descriptorsFrom, 1);
std::list<int> toWordIds;
std::list<int> fromWordIds;
for(unsigned int i=0; i<kptsFrom.size(); ++i)
{
int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
dictionary.addWord(new VisualWord(id, descriptorsFrom.row(i), 1));
fromWordIds.push_back(id);
}
std::list<int> toWordIds;
if(descriptorsTo.rows)
{
dictionary.update();
@@ -954,8 +983,8 @@ Transform RegistrationVis::computeTransformationImpl(
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
{
// A to B
const Signature * signatureA;
const Signature * signatureB;
Signature * signatureA;
Signature * signatureB;
if(dir == 0)
{
signatureA = &fromSignature;
@@ -1140,6 +1169,115 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
UDEBUG("from->from=%s", transforms[1].prettyPrint().c_str());
}
if(_bundleAdjustment > 0 &&
_estimationType < 2 &&
!transforms[0].isNull() &&
inliers[0].size() &&
fromSignature.getWords3().size() &&
toSignature.getWords().size() &&
fromSignature.sensorData().cameraModels().size() <= 1 &&
toSignature.sensorData().cameraModels().size() <= 1)
{
UASSERT(fromSignature.sensorData().stereoCameraModel().isValidForProjection() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
std::map<int, Transform> poses;
std::multimap<int, Link> links;
std::map<int, CameraModel> models;
std::map<int, cv::Point3f> points3DMap;
std::map<int, std::map<int, cv::Point2f> > wordReferences;
const CameraModel & cameraModelFrom = fromSignature.sensorData().stereoCameraModel().isValidForProjection()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
const CameraModel & cameraModelTo = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
models.insert(std::make_pair(1, cameraModelFrom));
models.insert(std::make_pair(2, cameraModelTo));
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, transforms[0]));
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], variances[0], variances[0])));
if(!transforms[1].isNull() && inliers[1].size())
{
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], variances[1], variances[1])));
}
for(unsigned int i=0; i<inliers[0].size(); ++i)
{
points3DMap.insert(*fromSignature.getWords3().find(inliers[0][i]));
std::map<int, cv::Point2f> ptMap;
/*if(fromSignature.getWords().size())
{
ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[0][i])->second.pt));
}*/
if(toSignature.getWords().size())
{
ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[0][i])->second.pt));
}
wordReferences.insert(std::make_pair(inliers[0][i], ptMap));
}
for(unsigned int i=0; i<inliers[1].size(); ++i)
{
std::multimap<int, cv::Point3f>::const_iterator iter = fromSignature.getWords3().find(inliers[1][i]);
if(iter!=fromSignature.getWords3().end())
{
std::map<int, std::map<int, cv::Point2f> >::iterator jter = wordReferences.find(inliers[1][i]);
if(jter == wordReferences.end())
{
points3DMap.insert(*fromSignature.getWords3().find(inliers[1][i]));
std::map<int, cv::Point2f> ptMap;
if(fromSignature.getWords().size())
{
ptMap.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
}
if(toSignature.getWords().size())
{
ptMap.insert(std::make_pair(2, toSignature.getWords().find(inliers[1][i])->second.pt));
}
wordReferences.insert(std::make_pair(inliers[1][i], ptMap));
}
else
{
if(jter->second.find(1) == jter->second.end())
{
jter->second.insert(std::make_pair(1, fromSignature.getWords().find(inliers[1][i])->second.pt));
}
if(jter->second.find(2) == jter->second.end())
{
jter->second.insert(std::make_pair(1, toSignature.getWords().find(inliers[1][i])->second.pt));
}
}
}
}
Optimizer * sba = Optimizer::create(_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
std::map<int, Transform> optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences);
delete sba;
//update transform
if(optimizedPoses.size() == 2 &&
!optimizedPoses.begin()->second.isNull() &&
!optimizedPoses.rbegin()->second.isNull())
{
transforms[0] = optimizedPoses.rbegin()->second;
transforms[1].setNull();
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
std::multimap<int, cv::Point3f> cpyWordsTo3 = toSignature.getWords3();
Transform invT = transforms[0].inverse();
for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
{
cpyWordsFrom3.find(iter->first)->second = iter->second;
if(cpyWordsTo3.find(iter->first) != cpyWordsTo3.end())
{
cpyWordsTo3.find(iter->first)->second = util3d::transformPoint(iter->second, invT);
}
}
fromSignature.setWords3(cpyWordsFrom3);
toSignature.setWords3(cpyWordsTo3);*/
}
}
if(!transforms[1].isNull())
{
if(transforms[0].isNull())

5
corelib/src/Rtabmap.cpp
View File

@@ -1184,7 +1184,8 @@ bool Rtabmap::process(
guess = newPose.inverse() * _optimizedPoses.at(*iter);
}
Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info);
// For proximity by time, correspondences should be already enough precise, so don't recompute them
Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info, true);
if(!transform.isNull())
{
@@ -1217,7 +1218,7 @@ bool Rtabmap::process(
}
timeProximityByTimeDetection = timer.ticks();
UINFO("timeLocalTimeDetection=%fs", timeProximityByTimeDetection);
UINFO("timeProximityByTimeDetection=%fs", timeProximityByTimeDetection);
//============================================================
// Bayes filter update

1
corelib/src/RtabmapThread.cpp
View File

@@ -171,6 +171,7 @@ void RtabmapThread::publishMap(bool optimized, bool full, bool graphOnly) const
void RtabmapThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Rtabmap");
if(_rtabmap == 0)
{
UERROR("Cannot start rtabmap thread if no rtabmap object is set! Stopping the thread...");

11
corelib/src/SensorData.cpp
View File

@@ -553,7 +553,16 @@ void SensorData::uncompressData(
cv::Mat * groundCellsRaw,
cv::Mat * obstacleCellsRaw)
{
UDEBUG("%d", this->id());
UDEBUG("%d data(%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0);
if(imageRaw == 0 &&
depthRaw == 0 &&
laserScanRaw == 0 &&
userDataRaw == 0 &&
groundCellsRaw == 0 &&
obstacleCellsRaw == 0)
{
return;
}
uncompressDataConst(
imageRaw,
depthRaw,

4
corelib/src/VWDictionary.cpp
View File

@@ -1194,6 +1194,10 @@ void VWDictionary::addWord(VisualWord * vw)
{
_unusedWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
}
if(_lastWordId < vw->id())
{
_lastWordId = vw->id();
}
}
}

2
corelib/src/util3d_motion_estimation.cpp
View File

@@ -35,8 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util3d_correspondences.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/OptimizerG2O.h"
#if CV_MAJOR_VERSION < 3
#include "opencv/solvepnp.h"
#endif