i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 14:00:20 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
74c0095c2c17e3688421d6802a913a2bea86e7ca
rtabmap/corelib/src/odometry/OdometryMono.cpp
T
matlabbe 9cbe84e445 Support intermediate nodes with memory management enabled (#1687) 
* Support intermediate nodes with memory management enabled

* Adding fix from #1686

* Fixed odomCache overlay shown in mapping mode

* Fixed local retrieval with intermediate nodes

* bumping version for updating API. Fixed getting graph error.

* Disable planning if intermediate nodes are there. Updated usage of Kp/BadSignRatio to support intermediate nodes.
2026-04-19 16:08:03 -07:00

880 lines
32 KiB
C++
Raw Blame History

/*
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/core/odometry/OdometryMono.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Memory.h"
#include "rtabmap/core/Signature.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_motion_estimation.h"
#include "rtabmap/core/util3d.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d_features.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/Optimizer.h"
#include "rtabmap/core/Stereo.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/video/tracking.hpp>
#include <pcl/common/centroid.h>
namespace rtabmap {
OdometryMono::OdometryMono(const rtabmap::ParametersMap & parameters) :
Odometry(parameters),
flowWinSize_(Parameters::defaultVisCorFlowWinSize()),
flowIterations_(Parameters::defaultVisCorFlowIterations()),
flowEps_(Parameters::defaultVisCorFlowEps()),
flowMaxLevel_(Parameters::defaultVisCorFlowMaxLevel()),
minInliers_(Parameters::defaultVisMinInliers()),
iterations_(Parameters::defaultVisIterations()),
pnpReprojError_(Parameters::defaultVisPnPReprojError()),
pnpFlags_(Parameters::defaultVisPnPFlags()),
pnpRefineIterations_(Parameters::defaultVisPnPRefineIterations()),
localHistoryMaxSize_(Parameters::defaultOdomF2MMaxSize()),
initMinFlow_(Parameters::defaultOdomMonoInitMinFlow()),
initMinTranslation_(Parameters::defaultOdomMonoInitMinTranslation()),
minTranslation_(Parameters::defaultOdomMonoMinTranslation()),
fundMatrixReprojError_(Parameters::defaultVhEpRansacParam1()),
fundMatrixConfidence_(Parameters::defaultVhEpRansacParam2()),
maxVariance_(Parameters::defaultOdomMonoMaxVariance()),
keyFrameThr_(0.95)
{
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), flowWinSize_);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), flowIterations_);
Parameters::parse(parameters, Parameters::kVisCorFlowEps(), flowEps_);
Parameters::parse(parameters, Parameters::kVisCorFlowMaxLevel(), flowMaxLevel_);
Parameters::parse(parameters, Parameters::kVisMinInliers(), minInliers_);
UASSERT(minInliers_ >= 1);
Parameters::parse(parameters, Parameters::kVisIterations(), iterations_);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), pnpReprojError_);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), pnpFlags_);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), pnpRefineIterations_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), localHistoryMaxSize_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinFlow(), initMinFlow_);
Parameters::parse(parameters, Parameters::kOdomMonoInitMinTranslation(), initMinTranslation_);
Parameters::parse(parameters, Parameters::kOdomMonoMinTranslation(), minTranslation_);
Parameters::parse(parameters, Parameters::kOdomMonoMaxVariance(), maxVariance_);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kVhEpRansacParam1(), fundMatrixReprojError_);
Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
// Setup memory
ParametersMap customParameters;
std::string roi = Parameters::defaultVisRoiRatios();
Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
customParameters.insert(ParametersPair(Parameters::kMemDepthAsMask(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpRoiRatios(), roi));
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kMemSTMSize(), "0"));
customParameters.insert(ParametersPair(Parameters::kMemNotLinkedNodesKept(), "false"));
customParameters.insert(ParametersPair(Parameters::kKpTfIdfLikelihoodUsed(), "false"));
int nn = Parameters::defaultVisCorNNType();
float nndr = Parameters::defaultVisCorNNDR();
int featureType = Parameters::defaultVisFeatureType();
int maxFeatures = Parameters::defaultVisMaxFeatures();
Parameters::parse(parameters, Parameters::kVisCorNNType(), nn);
Parameters::parse(parameters, Parameters::kVisCorNNDR(), nndr);
Parameters::parse(parameters, Parameters::kVisFeatureType(), featureType);
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), maxFeatures);
customParameters.insert(ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(nn)));
customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(featureType)));
customParameters.insert(ParametersPair(Parameters::kKpMaxFeatures(), uNumber2Str(maxFeatures)));
int subPixWinSize = Parameters::defaultVisSubPixWinSize();
int subPixIterations = Parameters::defaultVisSubPixIterations();
double subPixEps = Parameters::defaultVisSubPixEps();
Parameters::parse(parameters, Parameters::kVisSubPixWinSize(), subPixWinSize);
Parameters::parse(parameters, Parameters::kVisSubPixIterations(), subPixIterations);
Parameters::parse(parameters, Parameters::kVisSubPixEps(), subPixEps);
customParameters.insert(ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(subPixWinSize)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(subPixIterations)));
customParameters.insert(ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(subPixEps)));
// add only feature stuff
for(ParametersMap::const_iterator iter=parameters.begin(); iter!=parameters.end(); ++iter)
{
if(Parameters::isFeatureParameter(iter->first))
{
customParameters.insert(*iter);
}
}
memory_ = new Memory(customParameters);
if(!memory_->init("", false, ParametersMap()))
{
UERROR("Error initializing the memory for Mono Odometry.");
}
feature2D_ = Feature2D::create(parameters);
}
OdometryMono::~OdometryMono()
{
delete memory_;
delete feature2D_;
}
void OdometryMono::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
memory_->init("", false, ParametersMap());
localMap_.clear();
firstFrameGuessCorners_.clear();
keyFrameWords3D_.clear();
keyFramePoses_.clear();
keyFrameModels_.clear();
keyFrameLinks_.clear();
}
Transform OdometryMono::computeTransform(SensorData & data, const Transform & guess, OdometryInfo * info)
{
Transform output;
if(data.imageRaw().empty())
{
UERROR("Image empty! Cannot compute odometry...");
return output;
}
if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection()) ||
(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
{
UERROR("Odometry cannot be done without calibration or on multi-camera!");
return output;
}
CameraModel cameraModel;
if(data.stereoCameraModels().size())
{
cameraModel = data.stereoCameraModels()[0].left();
// Set Tx for stereo BA
cameraModel = CameraModel(cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
cameraModel.localTransform(),
-data.stereoCameraModels()[0].baseline()*cameraModel.fx(),
cameraModel.imageSize());
}
else
{
cameraModel = data.cameraModels()[0];
}
std::vector<CameraModel> newModel;
newModel.push_back(cameraModel);
UTimer timer;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
// convert to grayscale
if(data.imageRaw().channels() > 1 || data.rightRaw().channels() > 1)
{
cv::Mat newFrame = data.imageRaw();
cv::Mat newFrameRight = data.rightRaw();
if(data.imageRaw().channels() > 1)
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
if(data.rightRaw().channels() > 1)
cv::cvtColor(data.rightRaw(), newFrameRight, cv::COLOR_BGR2GRAY);
if(!data.stereoCameraModels().empty())
{
data.setStereoImage(newFrame, newFrameRight, data.stereoCameraModels());
}
else
{
data.setRGBDImage(newFrame, newFrameRight, data.cameraModels());
}
}
if(!localMap_.empty())
{
UDEBUG("RUNNING");
//PnP
UDEBUG("PnP");
if(this->isInfoDataFilled() && info)
{
info->type = 0;
}
// generate kpts
if(memory_->update(data))
{
UDEBUG("");
bool newPtsAdded = false;
const Signature * newS = memory_->getLastWorkingSignature(false);
UDEBUG("newWords=%d", (int)newS->getWords().size());
nFeatures = (int)newS->getWords().size();
if((int)newS->getWords().size() > minInliers_)
{
cv::Mat K = cameraModel.K();
Transform pnpGuess = ((this->getPose() * (guess.isNull()?Transform::getIdentity():guess)) * cameraModel.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)pnpGuess.r11(), (double)pnpGuess.r12(), (double)pnpGuess.r13(),
(double)pnpGuess.r21(), (double)pnpGuess.r22(), (double)pnpGuess.r23(),
(double)pnpGuess.r31(), (double)pnpGuess.r32(), (double)pnpGuess.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)pnpGuess.x(), (double)pnpGuess.y(), (double)pnpGuess.z());
std::vector<cv::Point3f> objectPoints;
std::vector<cv::Point2f> imagePoints;
std::vector<int> matches;
UDEBUG("compute PnP from optical flow");
std::vector<int> ids = uKeys(localMap_);
objectPoints = uValues(localMap_);
// compute last projection
UDEBUG("project points to previous image");
std::vector<cv::Point2f> prevImagePoints;
const Signature * prevS = memory_->getSignature(*(++memory_->getStMem().rbegin()));
Transform prevGuess = (keyFramePoses_.at(prevS->id()) * cameraModel.localTransform()).inverse();
cv::Mat prevR = (cv::Mat_<double>(3,3) <<
(double)prevGuess.r11(), (double)prevGuess.r12(), (double)prevGuess.r13(),
(double)prevGuess.r21(), (double)prevGuess.r22(), (double)prevGuess.r23(),
(double)prevGuess.r31(), (double)prevGuess.r32(), (double)prevGuess.r33());
cv::Mat prevRvec(1,3, CV_64FC1);
cv::Rodrigues(prevR, prevRvec);
cv::Mat prevTvec = (cv::Mat_<double>(1,3) << (double)prevGuess.x(), (double)prevGuess.y(), (double)prevGuess.z());
cv::projectPoints(objectPoints, prevRvec, prevTvec, K, cv::Mat(), prevImagePoints);
// compute current projection
UDEBUG("project points to current image");
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePoints);
//filter points not in the image and set guess from unique correspondences
std::vector<cv::Point3f> objectPointsTmp(objectPoints.size());
std::vector<cv::Point2f> refCorners(objectPoints.size());
std::vector<cv::Point2f> newCorners(objectPoints.size());
matches.resize(objectPoints.size());
int oi=0;
for(unsigned int i=0; i<objectPoints.size(); ++i)
{
if(uIsInBounds(int(imagePoints[i].x), 0, newS->sensorData().imageRaw().cols) &&
uIsInBounds(int(imagePoints[i].y), 0, newS->sensorData().imageRaw().rows) &&
uIsInBounds(int(prevImagePoints[i].x), 0, prevS->sensorData().imageRaw().cols) &&
uIsInBounds(int(prevImagePoints[i].y), 0, prevS->sensorData().imageRaw().rows))
{
refCorners[oi] = prevImagePoints[i];
newCorners[oi] = imagePoints[i];
if(localMap_.count(ids[i]) == 1)
{
if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
{
// set guess if unique
refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
}
if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
{
// set guess if unique
newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
}
}
objectPointsTmp[oi] = objectPoints[i];
matches[oi] = ids[i];
++oi;
}
}
objectPointsTmp.resize(oi);
refCorners.resize(oi);
newCorners.resize(oi);
matches.resize(oi);
// Refine imagePoints using optical flow
std::vector<unsigned char> statusFlowInliers;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
prevS->sensorData().imageRaw(),
newS->sensorData().imageRaw(),
refCorners,
newCorners,
statusFlowInliers,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
objectPoints.resize(statusFlowInliers.size());
imagePoints.resize(statusFlowInliers.size());
std::vector<int> matchesTmp(statusFlowInliers.size());
oi = 0;
for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
{
if(statusFlowInliers[i] &&
uIsInBounds(int(newCorners[i].x), 0, newS->sensorData().imageRaw().cols) &&
uIsInBounds(int(newCorners[i].y), 0, newS->sensorData().imageRaw().rows))
{
objectPoints[oi] = objectPointsTmp[i];
imagePoints[oi] = newCorners[i];
matchesTmp[oi] = matches[i];
++oi;
if(this->isInfoDataFilled() && info)
{
cv::KeyPoint kpt;
if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
{
kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
}
kpt.pt = newCorners[i];
info->words.insert(std::make_pair(matches[i], kpt));
}
}
}
UDEBUG("Flow inliers= %d/%d", oi, (int)statusFlowInliers.size());
objectPoints.resize(oi);
imagePoints.resize(oi);
matchesTmp.resize(oi);
matches = matchesTmp;
if(this->isInfoDataFilled() && info)
{
info->reg.matchesIDs.insert(info->reg.matchesIDs.end(), matches.begin(), matches.end());
}
correspondences = (int)matches.size();
if((int)matches.size() < minInliers_)
{
UWARN("not enough matches (%d < %d)...", (int)matches.size(), minInliers_);
}
else
{
//PnPRansac
std::vector<int> inliersV;
util3d::solvePnPRansac(
objectPoints,
imagePoints,
K,
cv::Mat(),
rvec,
tvec,
true,
iterations_,
pnpReprojError_,
0, // min inliers
inliersV,
pnpFlags_,
pnpRefineIterations_);
UDEBUG("inliers=%d/%d", (int)inliersV.size(), (int)objectPoints.size());
inliers = (int)inliersV.size();
if((int)inliersV.size() < minInliers_)
{
UWARN("PnP not enough inliers (%d < %d), rejecting the transform...", (int)inliersV.size(), minInliers_);
}
else
{
cv::Mat R(3,3,CV_64FC1);
cv::Rodrigues(rvec, R);
Transform pnp = Transform(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
output = this->getPose().inverse() * pnp.inverse() * cameraModel.localTransform().inverse();
if(this->isInfoDataFilled() && info && inliersV.size())
{
info->reg.inliersIDs.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->reg.inliersIDs[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
}
}
// compute variance, which is the rms of reprojection errors
cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
std::vector<cv::Point2f> imagePointsReproj;
cv::projectPoints(objectPoints, rvec, tvec, K, cv::Mat(), imagePointsReproj);
float err = 0.0f;
for(unsigned int i=0; i<inliersV.size(); ++i)
{
err += uNormSquared(imagePoints.at(inliersV[i]).x - imagePointsReproj.at(inliersV[i]).x, imagePoints.at(inliersV[i]).y - imagePointsReproj.at(inliersV[i]).y);
}
UASSERT(uIsFinite(err));
covariance *= std::sqrt(err/float(inliersV.size()));
Link newLink(keyFramePoses_.rbegin()->first, newS->id(), Link::kNeighbor, output, covariance.inv());
//bundle adjustment
Optimizer * ba = Optimizer::create(Optimizer::kTypeG2O);
std::map<int, Transform> poses = keyFramePoses_;
poses.insert(std::make_pair(newS->id(), this->getPose()*output));
if(ba->type() == Optimizer::kTypeG2O)
{
UWARN("Bundle adjustment: fill arguments");
std::multimap<int, Link> links = keyFrameLinks_;
std::map<int, std::vector<CameraModel> > models = keyFrameModels_;
links.insert(std::make_pair(keyFramePoses_.rbegin()->first, newLink));
models.insert(std::make_pair(newS->id(), newModel));
std::map<int, std::map<int, FeatureBA> > wordReferences;
for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
{
const Signature * s = memory_->getSignature(*iter);
for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
{
if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
{
if(wordReferences.find(jter->first)==wordReferences.end())
{
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
}
float depth = 0.0f;
if(keyFrameWords3D_.find(s->id()) != keyFrameWords3D_.end() &&
keyFrameWords3D_.at(s->id()).find(jter->first) != keyFrameWords3D_.at(s->id()).end())
{
depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
}
const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
}
}
}
std::set<int> outliers;
UWARN("Bundle adjustment begin");
poses = ba->optimizeBA(poses.begin()->first, poses, links, models, localMap_, wordReferences, &outliers);
UWARN("Bundle adjustment end");
if(!poses.empty())
{
output = this->getPose().inverse()*poses.at(newS->id());
}
}
//Find the frame with the most similar features
std::set<int> stMem = memory_->getStMem();
stMem.erase(newS->id());
std::map<int, float> likelihood = memory_->computeLikelihood(newS, std::list<int>(stMem.begin(), stMem.end()));
int maxLikelihoodId = -1;
float maxLikelihood = 0;
for(std::map<int, float>::iterator iter=likelihood.begin(); iter!=likelihood.end(); ++iter)
{
if(iter->second > maxLikelihood)
{
maxLikelihood = iter->second;
maxLikelihoodId = iter->first;
}
}
if(maxLikelihoodId == -1)
{
UWARN("Cannot find a keyframe similar enough to generate new 3D points!");
}
else if(poses.size())
{
// Add new points to local map
const Signature* previousS = memory_->getSignature(maxLikelihoodId);
UASSERT(previousS!=0);
Transform cameraTransform = keyFramePoses_.at(previousS->id()).inverse()*this->getPose()*output;
UDEBUG("cameraTransform guess= %s (norm^2=%f)", cameraTransform.prettyPrint().c_str(), cameraTransform.getNormSquared());
UINFO("Inliers= %d/%d (%f)", inliers, (int)imagePoints.size(), float(inliers)/float(imagePoints.size()));
if(cameraTransform.getNorm() < minTranslation_)
{
UINFO("Translation with the nearest frame is too small (%f<%f) to add new points to local map",
cameraTransform.getNorm(), minTranslation_);
}
else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
{
std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
std::map<int, cv::KeyPoint> wordsPrevious;
std::map<int, cv::KeyPoint> wordsNew;
for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
{
wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
{
wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
wordsPrevious,
wordsNew,
cameraModel,
cameraTransform,
fundMatrixReprojError_,
fundMatrixConfidence_);
if((int)inliers3D.size() < minInliers_)
{
UWARN("Epipolar geometry not enough inliers (%d < %d), rejecting the transform (%s)...",
(int)inliers3D.size(), minInliers_, cameraTransform.prettyPrint().c_str());
}
else
{
Transform newPose = keyFramePoses_.at(previousS->id())*cameraTransform;
UDEBUG("cameraTransform= %s", cameraTransform.prettyPrint().c_str());
std::multimap<int, cv::Point3f> wordsToAdd;
for(std::map<int, cv::Point3f>::iterator iter=inliers3D.begin();
iter != inliers3D.end();
++iter)
{
// transform inliers3D in new signature referential
iter->second = util3d::transformPoint(iter->second, cameraTransform.inverse());
if(!uContains(localMap_, iter->first))
{
//UDEBUG("Add new point %d to local map", iter->first);
cv::Point3f newPt = util3d::transformPoint(iter->second, newPose);
wordsToAdd.insert(std::make_pair(iter->first, newPt));
}
}
if((int)wordsToAdd.size())
{
localMap_.insert(wordsToAdd.begin(), wordsToAdd.end());
newPtsAdded = true;
UWARN("Added %d words", (int)wordsToAdd.size());
}
if(newPtsAdded)
{
// if we have depth guess, set it for ba
std::vector<cv::Point3f> newCorners3;
if(!data.depthOrRightRaw().empty())
{
std::vector<cv::KeyPoint> newKeypoints(imagePoints.size());
for(size_t i=0;i<imagePoints.size(); ++i)
{
newKeypoints[i] = cv::KeyPoint(imagePoints[i], 3);
}
newCorners3 = feature2D_->generateKeypoints3D(data, newKeypoints);
for(size_t i=0;i<newCorners3.size(); ++i)
{
if(util3d::isFinite(newCorners3[i]) &&
inliers3D.find(matches[i])!=inliers3D.end())
{
inliers3D.at(matches[i]) = newCorners3[i];
}
else
{
inliers3D.erase(matches[i]);
}
}
}
if(!inliers3D.empty())
{
UDEBUG("Added %d/%d valid 3D features", (int)inliers3D.size(), (int)wordsToAdd.size());
keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
}
keyFramePoses_ = poses;
keyFrameLinks_.insert(std::make_pair(newLink.from(), newLink));
keyFrameModels_.insert(std::make_pair(newS->id(), newModel));
// keep only the two last signatures
while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
{
int nodeId = *memory_->getStMem().begin();
std::list<int> removedPts;
memory_->deleteLocation(nodeId, &removedPts);
keyFrameWords3D_.erase(nodeId);
keyFramePoses_.erase(nodeId);
keyFrameLinks_.erase(nodeId);
keyFrameModels_.erase(nodeId);
for(std::list<int>::iterator iter = removedPts.begin(); iter!=removedPts.end(); ++iter)
{
localMap_.erase(*iter);
}
}
}
}
}
}
}
}
}
if(!newPtsAdded)
{
// remove new words from dictionary
memory_->deleteLocation(newS->id());
}
}
}
else if(!firstFrameGuessCorners_.empty())
{
UDEBUG("INIT PART 2/2");
//flow
if(this->isInfoDataFilled() && info)
{
info->type = 1;
}
const Signature * refS = memory_->getLastWorkingSignature(false);
std::vector<cv::Point2f> refCorners(firstFrameGuessCorners_.size());
std::vector<cv::Point2f> refCornersGuess(firstFrameGuessCorners_.size());
std::vector<int> cornerIds(firstFrameGuessCorners_.size());
int ii=0;
for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
{
std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
refCornersGuess[ii] = iter->second;
cornerIds[ii] = iter->first;
++ii;
}
UDEBUG("flow");
// Find features in the new left image
std::vector<unsigned char> statusFlowInliers;
std::vector<float> err;
UDEBUG("cv::calcOpticalFlowPyrLK() begin");
cv::calcOpticalFlowPyrLK(
refS->sensorData().imageRaw(),
data.imageRaw(),
refCorners,
refCornersGuess,
statusFlowInliers,
err,
cv::Size(flowWinSize_, flowWinSize_), flowMaxLevel_,
cv::TermCriteria(cv::TermCriteria::COUNT+cv::TermCriteria::EPS, flowIterations_, flowEps_),
cv::OPTFLOW_LK_GET_MIN_EIGENVALS | cv::OPTFLOW_USE_INITIAL_FLOW, 1e-4);
UDEBUG("cv::calcOpticalFlowPyrLK() end");
UDEBUG("Filtering optical flow outliers...");
float flow = 0;
if(this->isInfoDataFilled() && info)
{
info->refCorners = refCorners;
info->newCorners = refCornersGuess;
}
int oi = 0;
std::vector<cv::Point2f> tmpRefCorners(statusFlowInliers.size());
std::vector<cv::Point2f> newCorners(statusFlowInliers.size());
std::vector<int> inliersV(statusFlowInliers.size());
std::vector<int> tmpCornersId(statusFlowInliers.size());
UASSERT(refCornersGuess.size() == statusFlowInliers.size());
UASSERT(refCorners.size() == statusFlowInliers.size());
UASSERT(cornerIds.size() == statusFlowInliers.size());
for(unsigned int i=0; i<statusFlowInliers.size(); ++i)
{
if(statusFlowInliers[i])
{
float dx = refCorners[i].x - refCornersGuess[i].x;
float dy = refCorners[i].y - refCornersGuess[i].y;
float tmp = std::sqrt(dx*dx + dy*dy);
flow+=tmp;
tmpRefCorners[oi] = refCorners[i];
newCorners[oi] = refCornersGuess[i];
firstFrameGuessCorners_.at(cornerIds[i]) = refCornersGuess[i];
inliersV[oi] = i;
tmpCornersId[oi] = cornerIds[i];
++oi;
}
else
{
firstFrameGuessCorners_.erase(cornerIds[i]);
}
}
if(oi)
{
flow /=float(oi);
}
tmpRefCorners.resize(oi);
newCorners.resize(oi);
inliersV.resize((oi));
tmpCornersId.resize(oi);
refCorners= tmpRefCorners;
cornerIds = tmpCornersId;
if(this->isInfoDataFilled() && info)
{
// fill flow matches info
info->cornerInliers = inliersV;
inliers = (int)inliersV.size();
}
UDEBUG("Filtering optical flow outliers...done! (inliers=%d/%d)", oi, (int)statusFlowInliers.size());
if(flow > initMinFlow_ && oi > minInliers_)
{
UDEBUG("flow=%f", flow);
std::vector<cv::Point3f> refCorners3;
if(!refS->sensorData().depthOrRightRaw().empty())
{
std::vector<cv::KeyPoint> refKeypoints(refCorners.size());
for(size_t i=0;i<refCorners.size(); ++i)
{
refKeypoints[i] = cv::KeyPoint(refCorners[i], 3);
}
refCorners3 = feature2D_->generateKeypoints3D(refS->sensorData(), refKeypoints);
}
std::map<int, cv::KeyPoint> refWords;
std::map<int, cv::KeyPoint> newWords;
std::map<int, cv::Point3f> refWords3Guess;
for(unsigned int i=0; i<cornerIds.size(); ++i)
{
refWords.insert(std::make_pair(cornerIds[i], cv::KeyPoint(refCorners[i], 3)));
newWords.insert(std::make_pair(cornerIds[i], cv::KeyPoint(newCorners[i], 3)));
if(!refCorners3.empty())
{
refWords3Guess.insert(std::make_pair(cornerIds[i], refCorners3[i]));
}
}
Transform cameraTransform;
std::map<int, cv::Point3f> refWords3 = util3d::generateWords3DMono(
refWords,
newWords,
cameraModel,
cameraTransform,
fundMatrixReprojError_,
fundMatrixConfidence_,
4,
refWords3Guess); // for scale estimation
if(cameraTransform.getNorm() < minTranslation_*5)
{
UWARN("Camera must be moved at least %f m for initialization (current=%f)",
minTranslation_*5, output.getNorm());
}
else
{
localMap_ = refWords3;
// For values that we know the depth, set them for ba
for(std::map<int, cv::Point3f>::iterator iter=refWords3.begin(); iter!=refWords3.end();)
{
std::map<int, cv::Point3f>::iterator jterGuess3D = refWords3Guess.find(iter->first);
if(jterGuess3D != refWords3Guess.end() &&
util3d::isFinite(jterGuess3D->second))
{
iter->second = jterGuess3D->second;
++iter;
}
else
{
refWords3.erase(iter++);
}
}
if(!refWords3.empty())
{
UDEBUG("Added %d/%d valid 3D features", (int)refWords3.size(), (int)localMap_.size());
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), refWords3));
}
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature(false)->id(), newModel));
}
}
else
{
UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
}
}
else
{
UDEBUG("INIT PART 1/2");
//return Identity
output = Transform::getIdentity();
if(info)
{
// a very high variance tells that the new pose is not linked with the previous one
info->reg.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
}
// generate kpts
if(memory_->update(SensorData(data)))
{
const Signature * s = memory_->getLastWorkingSignature(false);
const std::multimap<int, int> & words = s->getWords();
if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
{
for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
}
}
}
else
{
memory_->deleteLocation(memory_->getLastSignatureId());
}
}
else
{
UERROR("Failed creating signature");
}
}
memory_->emptyTrash();
if(this->isInfoDataFilled() && info)
{
//info->variance = variance;
info->reg.inliers = inliers;
info->reg.matches = correspondences;
info->features = nFeatures;
info->localMapSize = (int)localMap_.size();
info->localMap = localMap_;
}
UINFO("Odom update=%fs tf=[%s] inliers=%d/%d, local_map[%d]=%d, accepted=%s",
timer.elapsed(),
output.prettyPrint().c_str(),
inliers,
correspondences,
(int)memory_->getStMem().size(),
(int)localMap_.size(),
!output.isNull()?"true":"false");
return output;
}
} // namespace rtabmap
Reference in New Issue View Git Blame Copy Permalink