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
Files
cdddb1209e53e52ea2cf076fac794907e4d6150e
rtabmap/corelib/src/OdometryMono.cpp

1036 lines
36 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2014, 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/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/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())
{
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::kVhEpRansacParam1(), fundMatrixReprojError_);
Parameters::parse(parameters, Parameters::kVhEpRansacParam2(), fundMatrixConfidence_);
// Setup memory
ParametersMap customParameters;
float minDepth = Parameters::defaultVisMinDepth();
float maxDepth = Parameters::defaultVisMaxDepth();
std::string roi = Parameters::defaultVisRoiRatios();
Parameters::parse(parameters, Parameters::kVisMinDepth(), minDepth);
Parameters::parse(parameters, Parameters::kVisMaxDepth(), maxDepth);
Parameters::parse(parameters, Parameters::kVisRoiRatios(), roi);
customParameters.insert(ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(minDepth)));
customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(maxDepth)));
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.");
}
bool stereoOpticalFlow = Parameters::defaultStereoOpticalFlow();
Parameters::parse(parameters, Parameters::kStereoOpticalFlow(), stereoOpticalFlow);
if(stereoOpticalFlow)
{
stereo_ = new StereoOpticalFlow(parameters);
}
else
{
stereo_ = new Stereo(parameters);
}
}
OdometryMono::~OdometryMono()
{
delete memory_;
delete stereo_;
}
void OdometryMono::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
memory_->init("", false, ParametersMap());
localMap_.clear();
refDepthOrRight_ = cv::Mat();
cornersMap_.clear();
keyFrameWords3D_.clear();
keyFramePoses_.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.stereoCameraModel().isValidForProjection())))
{
UERROR("Odometry cannot be done without calibration or on multi-camera!");
return output;
}
const CameraModel & cameraModel = data.stereoCameraModel().isValidForProjection()?data.stereoCameraModel().left():data.cameraModels()[0];
UTimer timer;
int inliers = 0;
int correspondences = 0;
int nFeatures = 0;
cv::Mat newFrame;
// convert to grayscale
if(data.imageRaw().channels() > 1)
{
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
}
else
{
newFrame = data.imageRaw().clone();
}
if(memory_->getStMem().size() >= 1)
{
if(localMap_.size())
{
//PnP
UDEBUG("PnP");
if(this->isInfoDataFilled() && info)
{
info->type = 0;
}
// generate kpts
if(memory_->update(SensorData(newFrame)))
{
UDEBUG("");
bool newPtsAdded = false;
const Signature * newS = memory_->getLastWorkingSignature();
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 previous 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, newFrame.cols) &&
uIsInBounds(int(imagePoints[i].y), 0, newFrame.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)
{
// set guess if unique
refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
}
if(newS->getWords().count(ids[i]) == 1)
{
// set guess if unique
newCorners[oi] = 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(),
newFrame,
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])
{
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)
{
kpt = 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->wordMatches.insert(info->wordMatches.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->wordInliers.resize(inliersV.size());
for(unsigned int i=0; i<inliersV.size(); ++i)
{
info->wordInliers[i] = matches[inliersV[i]]; // index and ID should match (index starts at 0, ID starts at 1)
}
}
//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;
}
}
UASSERT(maxLikelihoodId != -1);
// 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());
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
{
double variance = 0;
const std::map<int, cv::Point3f> & previousGuess = keyFrameWords3D_.find(previousS->id())->second;
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
uMultimapToMapUnique(previousS->getWords()),
uMultimapToMapUnique(newS->getWords()),
cameraModel,
cameraTransform,
iterations_,
pnpReprojError_,
pnpFlags_,
pnpRefineIterations_,
fundMatrixReprojError_,
fundMatrixConfidence_,
previousGuess,
&variance);
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 if(variance == 0 || variance > maxVariance_)
{
UWARN("Variance too high %f (max = %f)", variance, maxVariance_);
}
else
{
UDEBUG("inliers3D=%d/%d variance= %f", inliers3D.size(), newS->getWords().size(), variance);
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;
UDEBUG("Added %d words", (int)wordsToAdd.size());
}
if(newPtsAdded)
{
keyFrameWords3D_.insert(std::make_pair(newS->id(), inliers3D));
keyFramePoses_.insert(std::make_pair(newS->id(), newPose));
// 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);
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(cornersMap_.size())
{
//flow
if(this->isInfoDataFilled() && info)
{
info->type = 1;
}
const Signature * refS = memory_->getLastWorkingSignature();
std::vector<cv::Point2f> refCorners(cornersMap_.size());
std::vector<cv::Point2f> refCornersGuess(cornersMap_.size());
std::vector<int> cornerIds(cornersMap_.size());
int ii=0;
for(std::map<int, cv::Point2f>::iterator iter=cornersMap_.begin(); iter!=cornersMap_.end(); ++iter)
{
std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
UASSERT(jter != refS->getWords().end());
refCorners[ii] = 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(),
newFrame,
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];
inliersV[oi] = i;
cornersMap_.at(cornerIds[i]) = refCornersGuess[i];
tmpCornersId[oi] = cornerIds[i];
++oi;
}
else
{
cornersMap_.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);
// compute fundamental matrix
UDEBUG("Find fundamental matrix");
std::vector<unsigned char> statusFInliers;
cv::Mat F = cv::findFundamentalMat(
refCorners,
newCorners,
statusFInliers,
cv::RANSAC,
fundMatrixReprojError_,
fundMatrixConfidence_);
//std::cout << "F=" << F << std::endl;
if(!F.empty())
{
UDEBUG("Filtering fundamental matrix outliers...");
std::vector<cv::Point2f> tmpNewCorners(statusFInliers.size());
std::vector<cv::Point2f> tmpRefCorners(statusFInliers.size());
tmpCornersId.resize(statusFInliers.size());
oi = 0;
UASSERT(newCorners.size() == statusFInliers.size());
UASSERT(refCorners.size() == statusFInliers.size());
UASSERT(cornerIds.size() == statusFInliers.size());
std::vector<int> tmpInliers(statusFInliers.size());
for(unsigned int i=0; i<statusFInliers.size(); ++i)
{
if(statusFInliers[i])
{
tmpNewCorners[oi] = newCorners[i];
tmpRefCorners[oi] = refCorners[i];
tmpInliers[oi] = inliersV[i];
tmpCornersId[oi] = cornerIds[i];
++oi;
}
}
tmpInliers.resize(oi);
tmpNewCorners.resize(oi);
tmpRefCorners.resize(oi);
tmpCornersId.resize(oi);
newCorners = tmpNewCorners;
refCorners = tmpRefCorners;
inliersV = tmpInliers;
cornerIds = tmpCornersId;
if(this->isInfoDataFilled() && info)
{
// update inliers
info->cornerInliers = inliersV;
inliers = (int)inliersV.size();
}
UDEBUG("Filtering fundamental matrix outliers...done! (inliers=%d/%d)", oi, (int)statusFInliers.size());
if((int)refCorners.size() > minInliers_)
{
std::vector<cv::Point2f> refCornersRefined;
std::vector<cv::Point2f> newCornersRefined;
//UDEBUG("Correcting matches...");
cv::correctMatches(F, refCorners, newCorners, refCornersRefined, newCornersRefined);
UASSERT(refCorners.size() == refCornersRefined.size());
UASSERT(newCorners.size() == newCornersRefined.size());
refCorners = refCornersRefined;
newCorners = newCornersRefined;
//UDEBUG("Correcting matches...done!");
UDEBUG("Computing P...");
cv::Mat K = cameraModel.K();
cv::Mat Kinv = K.inv();
cv::Mat E = K.t()*F*K;
//normalize coordinates
cv::Mat x(3, (int)refCorners.size(), CV_64FC1);
cv::Mat xp(3, (int)refCorners.size(), CV_64FC1);
for(unsigned int i=0; i<refCorners.size(); ++i)
{
x.at<double>(0, i) = refCorners[i].x;
x.at<double>(1, i) = refCorners[i].y;
x.at<double>(2, i) = 1;
xp.at<double>(0, i) = newCorners[i].x;
xp.at<double>(1, i) = newCorners[i].y;
xp.at<double>(2, i) = 1;
}
cv::Mat x_norm = Kinv * x;
cv::Mat xp_norm = Kinv * xp;
x_norm = x_norm.rowRange(0,2);
xp_norm = xp_norm.rowRange(0,2);
cv::Mat P = EpipolarGeometry::findPFromE(E, x_norm, xp_norm);
if(!P.empty())
{
cv::Mat P0 = cv::Mat::zeros(3, 4, CV_64FC1);
P0.at<double>(0,0) = 1;
P0.at<double>(1,1) = 1;
P0.at<double>(2,2) = 1;
UDEBUG("Computing P...done!");
//std::cout << "P=" << P << std::endl;
cv::Mat R, T;
EpipolarGeometry::findRTFromP(P, R, T);
UDEBUG("");
std::vector<double> reprojErrors;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
EpipolarGeometry::triangulatePoints(x_norm, xp_norm, P0, P, cloud, reprojErrors);
std::vector<cv::Point3f> inliersRef;
std::vector<cv::Point3f> inliersRefGuess;
std::vector<cv::Point2f> imagePoints(cloud->size());
inliersRef.resize(cloud->size());
inliersRefGuess.resize(cloud->size());
tmpCornersId.resize(cloud->size());
oi = 0;
UASSERT(newCorners.size() == cloud->size());
std::vector<cv::Point3f> newCorners3D;
if(!refDepthOrRight_.empty())
{
if(refDepthOrRight_.type() == CV_8UC1)
{
StereoCameraModel m = data.stereoCameraModel();
m.setLocalTransform(Transform::getIdentity());
std::vector<unsigned char> stereoStatus;
std::vector<cv::Point2f> rightCorners;
rightCorners = stereo_->computeCorrespondences(
refS->sensorData().imageRaw(),
refDepthOrRight_,
refCorners,
stereoStatus);
newCorners3D = util3d::generateKeypoints3DStereo(
refCorners,
rightCorners,
m,
stereoStatus);
}
else if(refDepthOrRight_.type() == CV_32FC1 || refDepthOrRight_.type() == CV_16UC1)
{
std::vector<cv::KeyPoint> tmpKpts;
cv::KeyPoint::convert(refCorners, tmpKpts);
CameraModel m(cameraModel.fx(), cameraModel.fy(), cameraModel.cx(), cameraModel.cy());
newCorners3D = util3d::generateKeypoints3DDepth(
tmpKpts,
refDepthOrRight_,
m);
}
else
{
UWARN("Depth or right image type not supported: %d", refDepthOrRight_.type());
}
}
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(cloud->at(i).z>0)
{
imagePoints[oi] = newCorners[i];
tmpCornersId[oi] = cornerIds[i];
inliersRef[oi].x = cloud->at(i).x;
inliersRef[oi].y = cloud->at(i).y;
inliersRef[oi].z = cloud->at(i).z;
if(!newCorners3D.empty())
{
inliersRefGuess[oi] = newCorners3D.at(i);
}
++oi;
}
}
imagePoints.resize(oi);
inliersRef.resize(oi);
inliersRefGuess.resize(oi);
tmpCornersId.resize(oi);
cornerIds = tmpCornersId;
bool reject = false;
//estimate scale
float scale = 1;
std::multimap<float, float> scales; // <variance, scale>
if(!newCorners3D.empty()) // scale known
{
UASSERT(inliersRefGuess.size() == inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
if(util3d::isFinite(inliersRefGuess.at(i)))
{
float s = inliersRefGuess.at(i).z/inliersRef.at(i).z;
std::vector<float> errorSqrdDists(inliersRef.size());
oi = 0;
for(unsigned int j=0; j<inliersRef.size(); ++j)
{
if(cloud->at(j).z>0)
{
cv::Point3f refPt = inliersRef.at(j);
refPt.x *= s;
refPt.y *= s;
refPt.z *= s;
const cv::Point3f & guess = inliersRefGuess.at(j);
errorSqrdDists[oi++] = uNormSquared(refPt.x-guess.x, refPt.y-guess.y, refPt.z-guess.z);
}
}
errorSqrdDists.resize(oi);
if(errorSqrdDists.size() > 2)
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
float variance = 2.1981 * median_error_sqr;
//UDEBUG("scale %d = %f variance = %f", i, s, variance);
if(variance > 0)
{
scales.insert(std::make_pair(variance, s));
}
}
}
}
if(scales.size() == 0)
{
UWARN("No scales found!?");
reject = true;
}
else
{
scale = scales.begin()->second;
UWARN("scale used = %f (variance=%f scales=%d)", scale, scales.begin()->first, (int)scales.size());
UDEBUG("Max noise variance = %f current variance=%f", maxVariance_, scales.begin()->first);
if(scales.begin()->first > maxVariance_)
{
UWARN("Too high variance %f (should be < %f)", scales.begin()->first, maxVariance_);
reject = true; // 20 cm for good initialization
}
}
}
else if(inliersRef.size())
{
// find centroid of the cloud and set it to 1 meter
Eigen::Vector4f centroid(0,0,0,0);
pcl::PointCloud<pcl::PointXYZ> inliersRefCloud;
inliersRefCloud.resize(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
inliersRefCloud[i].x = inliersRef[i].x;
inliersRefCloud[i].y = inliersRef[i].y;
inliersRefCloud[i].z = inliersRef[i].z;
}
pcl::compute3DCentroid(inliersRefCloud, centroid);
scale = 1.0f / centroid[2];
}
else
{
reject = true;
}
if(!reject)
{
//PnPRansac
std::vector<cv::Point3f> objectPoints(inliersRef.size());
for(unsigned int i=0; i<inliersRef.size(); ++i)
{
objectPoints[i].x = inliersRef.at(i).x * scale;
objectPoints[i].y = inliersRef.at(i).y * scale;
objectPoints[i].z = inliersRef.at(i).z * scale;
}
cv::Mat rvec;
cv::Mat tvec;
std::vector<int> inliersPnP;
util3d::solvePnPRansac(
objectPoints, // 3D points in ref referential
imagePoints, // 2D points in new referential
K,
cv::Mat(),
rvec,
tvec,
false,
iterations_,
pnpReprojError_,
0, // min inliers
inliersPnP,
pnpFlags_,
pnpRefineIterations_);
UDEBUG("PnP inliers = %d / %d", (int)inliersPnP.size(), (int)objectPoints.size());
cv::Rodrigues(rvec, R);
Transform pnp(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 = cameraModel.localTransform() * pnp.inverse() * cameraModel.localTransform().inverse();
if(output.getNorm() < minTranslation_*5)
{
reject = true;
UWARN("Camera must be moved at least %f m for initialization (current=%f)",
minTranslation_*5, output.getNorm());
}
if(!reject)
{
///
std::vector<int> wordsId = uKeys(memory_->getLastWorkingSignature()->getWords());
UASSERT(wordsId.size());
UASSERT(cornerIds.size() == objectPoints.size());
std::map<int, cv::Point3f> keyFrameWords3D;
Transform t = this->getPose()*cameraModel.localTransform();
for(unsigned int i=0; i<inliersPnP.size(); ++i)
{
int index =inliersPnP.at(i);
int id = cornerIds[index];
UASSERT(id > 0 && id <= *wordsId.rbegin());
cv::Point3f pt = util3d::transformPoint(
objectPoints.at(index),
t);
localMap_.insert(std::make_pair(id, cv::Point3f(pt.x, pt.y, pt.z)));
keyFrameWords3D.insert(std::make_pair(id, pt));
}
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), keyFrameWords3D));
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
}
}
}
else
{
UERROR("No valid camera matrix found!");
}
}
else
{
UWARN("Not enough inliers %d/%d", (int)refCorners.size(), minInliers_);
}
}
else
{
UWARN("Fundamental matrix not found!");
}
}
else
{
UWARN("Flow not enough high! flow=%f ki=%d", flow, oi);
}
}
}
else
{
//return Identity
output = Transform::getIdentity();
if(info)
{
// a very high variance tells that the new pose is not linked with the previous one
info->variance = 9999;
}
// generate kpts
if(memory_->update(SensorData(newFrame)))
{
const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
if((int)words.size() > minInliers_)
{
for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
{
if(words.count(iter->first) == 1)
{
cornersMap_.insert(std::make_pair(iter->first, iter->second.pt));
}
}
refDepthOrRight_ = data.depthOrRightRaw().clone();
keyFramePoses_.insert(std::make_pair(memory_->getLastSignatureId(), Transform::getIdentity()));
}
else
{
UWARN("Too low 2D corners (%d), ignoring new frame...",
(int)words.size());
memory_->deleteLocation(memory_->getLastSignatureId());
}
}
else
{
UERROR("Failed creating signature");
}
}
memory_->emptyTrash();
if(this->isInfoDataFilled() && info)
{
//info->variance = variance;
info->inliers = inliers;
info->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