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

Removed parameter Vis/ForwardEstOnly. Added odometry statistics (matches,inliers,inliersRatio) per camera. Added OdometryInfo::statistics() function for convenience.

Browse Source
This commit is contained in:
matlabbe
2025-04-14 09:52:41 -07:00
parent b750e94eaa
commit 1d6c70c1db
16 changed files with 720 additions and 546 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
corelib/src/CMakeLists.txt
View File

@@ -85,6 +85,7 @@ SET(SRC_FILES
Odometry.cpp
OdometryThread.cpp
OdometryInfo.cpp
odometry/OdometryF2M.cpp
odometry/OdometryMono.cpp
odometry/OdometryF2F.cpp

220
corelib/src/OdometryInfo.cpp Normal file
View File

@@ -0,0 +1,220 @@
/*
Copyright (c) 2010-2025, 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/OdometryInfo.h"
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap {
OdometryInfo::OdometryInfo() :
lost(true),
features(0),
localMapSize(0),
localScanMapSize(0),
localKeyFrames(0),
localBundleOutliers(0),
localBundleConstraints(0),
localBundleTime(0),
localBundleAvgInlierDistance(0.0f),
localBundleMaxKeyFramesForInlier(0),
keyFrameAdded(false),
timeDeskewing(0.0f),
timeEstimation(0.0f),
timeParticleFiltering(0.0f),
stamp(0),
interval(0),
distanceTravelled(0.0f),
memoryUsage(0),
gravityRollError(0.0),
gravityPitchError(0.0),
type(0)
{}
OdometryInfo OdometryInfo::copyWithoutData() const
{
OdometryInfo output;
output.lost = lost;
output.reg = reg.copyWithoutData();
output.features = features;
output.localMapSize = localMapSize;
output.localScanMapSize = localScanMapSize;
output.localKeyFrames = localKeyFrames;
output.localBundleOutliers = localBundleOutliers;
output.localBundleConstraints = localBundleConstraints;
output.localBundleTime = localBundleTime;
output.localBundlePoses = localBundlePoses;
output.localBundleModels = localBundleModels;
output.localBundleAvgInlierDistance = localBundleAvgInlierDistance;
output.localBundleMaxKeyFramesForInlier = localBundleMaxKeyFramesForInlier;
output.localBundleOutliersPerCam = localBundleOutliersPerCam;
output.keyFrameAdded = keyFrameAdded;
output.timeDeskewing = timeDeskewing;
output.timeEstimation = timeEstimation;
output.timeParticleFiltering = timeParticleFiltering;
output.stamp = stamp;
output.interval = interval;
output.transform = transform;
output.transformFiltered = transformFiltered;
output.transformGroundTruth = transformGroundTruth;
output.guessVelocity = guessVelocity;
output.guess = guess;
output.distanceTravelled = distanceTravelled;
output.memoryUsage = memoryUsage;
output.gravityRollError = gravityRollError;
output.gravityPitchError = gravityPitchError;
output.type = type;
return output;
}
std::map<std::string, float> OdometryInfo::statistics(const Transform & pose)
{
std::map<std::string, float> stats;
stats.insert(std::make_pair("Odometry/TimeRegistration/ms", reg.totalTime*1000.0f));
stats.insert(std::make_pair("Odometry/RAM_usage/MB", memoryUsage));
// Based on rtabmap/MainWindow.cpp
stats.insert(std::make_pair("Odometry/Features/", features));
stats.insert(std::make_pair("Odometry/Matches/", reg.matches));
stats.insert(std::make_pair("Odometry/MatchesRatio/", features<=0?0.0f:float(reg.inliers)/float(features)));
stats.insert(std::make_pair("Odometry/Inliers/", reg.inliers));
stats.insert(std::make_pair("Odometry/InliersMeanDistance/m", reg.inliersMeanDistance));
stats.insert(std::make_pair("Odometry/InliersDistribution/", reg.inliersDistribution));
stats.insert(std::make_pair("Odometry/InliersRatio/", reg.inliers));
for(size_t i=0; i<reg.matchesPerCam.size(); ++i)
{
stats.insert(std::make_pair(uFormat("Odometry/matchesCam%ld/", i), reg.matchesPerCam[i]));
}
for(size_t i=0; i<reg.inliersPerCam.size(); ++i)
{
stats.insert(std::make_pair(uFormat("Odometry/inliersCam%ld/", i), reg.inliersPerCam[i]));
}
if(reg.matchesPerCam.size() == reg.inliersPerCam.size())
{
for(size_t i=0; i<reg.matchesPerCam.size(); ++i)
{
stats.insert(std::make_pair(uFormat("Odometry/inliersRatioCam%ld/", i), reg.matchesPerCam[i]>0 ? (float)reg.inliersPerCam[i] / (float)reg.matchesPerCam[i] : 0.0f));
}
}
stats.insert(std::make_pair("Odometry/ICPInliersRatio/", reg.icpInliersRatio));
stats.insert(std::make_pair("Odometry/ICPRotation/rad", reg.icpRotation));
stats.insert(std::make_pair("Odometry/ICPTranslation/m", reg.icpTranslation));
stats.insert(std::make_pair("Odometry/ICPStructuralComplexity/", reg.icpStructuralComplexity));
stats.insert(std::make_pair("Odometry/ICPStructuralDistribution/", reg.icpStructuralDistribution));
stats.insert(std::make_pair("Odometry/ICPCorrespondences/", reg.icpCorrespondences));
stats.insert(std::make_pair("Odometry/StdDevLin/", sqrt((float)reg.covariance.at<double>(0,0))));
stats.insert(std::make_pair("Odometry/StdDevAng/", sqrt((float)reg.covariance.at<double>(5,5))));
stats.insert(std::make_pair("Odometry/VarianceLin/", (float)reg.covariance.at<double>(0,0)));
stats.insert(std::make_pair("Odometry/VarianceAng/", (float)reg.covariance.at<double>(5,5)));
stats.insert(std::make_pair("Odometry/TimeEstimation/ms", timeEstimation*1000.0f));
stats.insert(std::make_pair("Odometry/TimeFiltering/ms", timeParticleFiltering*1000.0f));
stats.insert(std::make_pair("Odometry/LocalMapSize/", localMapSize));
stats.insert(std::make_pair("Odometry/LocalScanMapSize/", localScanMapSize));
stats.insert(std::make_pair("Odometry/LocalKeyFrames/", localKeyFrames));
stats.insert(std::make_pair("Odometry/LocalBundleOutliers/", localBundleOutliers));
stats.insert(std::make_pair("Odometry/LocalBundleConstraints/", localBundleConstraints));
stats.insert(std::make_pair("Odometry/LocalBundleTime/ms", localBundleTime*1000.0f));
stats.insert(std::make_pair("Odometry/localBundleAvgInlierDistance/pix", localBundleAvgInlierDistance));
stats.insert(std::make_pair("Odometry/localBundleMaxKeyFramesForInlier/", localBundleMaxKeyFramesForInlier));
for(size_t i=0; i<localBundleOutliersPerCam.size(); ++i)
{
stats.insert(std::make_pair(uFormat("Odometry/localBundleOutliersCam%ld/", i), localBundleOutliersPerCam[i]));
}
stats.insert(std::make_pair("Odometry/KeyFrameAdded/", keyFrameAdded?1.0f:0.0f));
stats.insert(std::make_pair("Odometry/Interval/ms", (float)interval));
stats.insert(std::make_pair("Odometry/Distance/m", distanceTravelled));
float x,y,z,roll,pitch,yaw;
if(!pose.isNull())
{
pose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
stats.insert(std::make_pair("Odometry/Px/m", x));
stats.insert(std::make_pair("Odometry/Py/m", y));
stats.insert(std::make_pair("Odometry/Pz/m", z));
stats.insert(std::make_pair("Odometry/Proll/deg", roll*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Ppitch/deg", pitch*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Pyaw/deg", yaw*180.0/CV_PI));
}
float dist = 0.0f, speed=0.0f;
if(!transform.isNull())
{
transform.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = transform.getNorm();
stats.insert(std::make_pair("Odometry/T/m", dist));
stats.insert(std::make_pair("Odometry/Tx/m", x));
stats.insert(std::make_pair("Odometry/Ty/m", y));
stats.insert(std::make_pair("Odometry/Tz/m", z));
stats.insert(std::make_pair("Odometry/Troll/deg", roll*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Tpitch/deg", pitch*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/Tyaw/deg", yaw*180.0/CV_PI));
if(interval>0.0)
{
speed = dist/interval;
stats.insert(std::make_pair("Odometry/Speed/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/Speed/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/Speed/mps", speed));
stats.insert(std::make_pair("Odometry/Vx/mps", x/interval));
stats.insert(std::make_pair("Odometry/Vy/mps", y/interval));
stats.insert(std::make_pair("Odometry/Vz/mps", z/interval));
stats.insert(std::make_pair("Odometry/Vroll/degps", (roll*180.0/CV_PI)/interval));
stats.insert(std::make_pair("Odometry/Vpitch/degps", (pitch*180.0/CV_PI)/interval));
stats.insert(std::make_pair("Odometry/Vyaw/degps", (yaw*180.0/CV_PI)/interval));
}
}
if(!transformGroundTruth.isNull())
{
if(!transform.isNull())
{
rtabmap::Transform diff = transformGroundTruth.inverse()*transform;
stats.insert(std::make_pair("Odometry/TG_error_lin/m", diff.getNorm()));
stats.insert(std::make_pair("Odometry/TG_error_ang/deg", diff.getAngle()*180.0/CV_PI));
}
transformGroundTruth.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
dist = transformGroundTruth.getNorm();
stats.insert(std::make_pair("Odometry/TG/m", dist));
stats.insert(std::make_pair("Odometry/TGx/m", x));
stats.insert(std::make_pair("Odometry/TGy/m", y));
stats.insert(std::make_pair("Odometry/TGz/m", z));
stats.insert(std::make_pair("Odometry/TGroll/deg", roll*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TGpitch/deg", pitch*180.0/CV_PI));
stats.insert(std::make_pair("Odometry/TGyaw/deg", yaw*180.0/CV_PI));
if(interval>0.0)
{
speed = dist/interval;
stats.insert(std::make_pair("Odometry/SpeedG/kph", speed*3.6));
stats.insert(std::make_pair("Odometry/SpeedG/mph", speed*2.237));
stats.insert(std::make_pair("Odometry/SpeedG/mps", speed));
}
}
return stats;
}
} // namespace rtabmap

3
corelib/src/Parameters.cpp
View File

@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.21.13
removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly", std::make_pair(false, "")));
// 0.21.7
removedParameters_.insert(std::make_pair("SIFT/NFeatures", std::make_pair(false, "")));

615
corelib/src/RegistrationVis.cpp
View File

@@ -71,7 +71,6 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_refineIterations(Parameters::defaultVisRefineIterations()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
@@ -132,7 +131,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
@@ -314,7 +312,6 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType);
UDEBUG("%s=%d", Parameters::kVisForwardEstOnly().c_str(), _forwardEstimateOnly);
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
@@ -721,7 +718,7 @@ Transform RegistrationVis::computeTransformationImpl(
kptsFrom3D = kptsFrom3DKept;
std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly)
if(_estimationType == 0 || _estimationType == 1)
{
kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
}
@@ -1576,347 +1573,305 @@ Transform RegistrationVis::computeTransformationImpl(
info.matchesIDs.clear();
if(toSignature.getWords().size())
{
Transform transforms[2];
std::vector<int> inliers[2];
std::vector<int> matches[2];
cv::Mat covariances[2];
covariances[0] = cv::Mat::eye(6,6,CV_64FC1);
covariances[1] = cv::Mat::eye(6,6,CV_64FC1);
for(int dir=0; dir<(!_forwardEstimateOnly?2:1); ++dir)
std::vector<int> inliers;
std::vector<int> matches;
if(_estimationType == 2) // Epipolar Geometry
{
// A to B
Signature * signatureA;
Signature * signatureB;
if(dir == 0)
UDEBUG("");
if((toSignature.sensorData().stereoCameraModels().size() != 1 ||
!toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValidForProjection()))
{
signatureA = &fromSignature;
signatureB = &toSignature;
UERROR("Calibrated camera required (multi-cameras not supported).");
}
else
else if((int)fromSignature.getWords().size() >= _minInliers &&
(int)toSignature.getWords().size() >= _minInliers)
{
signatureA = &toSignature;
signatureB = &fromSignature;
}
if(_estimationType == 2) // Epipolar Geometry
{
UDEBUG("");
if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
UASSERT((fromSignature.sensorData().stereoCameraModels().size() == 1 && fromSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModels().size()?fromSignature.sensorData().stereoCameraModels()[0].left():fromSignature.sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
double variance = 1.0f;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
std::map<int, cv::KeyPoint> wordsA;
std::map<int, cv::Point3f> words3A;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
UERROR("Calibrated camera required (multi-cameras not supported).");
wordsA.insert(std::make_pair(iter->first, fromSignature.getWordsKpts()[iter->second]));
if(!fromSignature.getWords3().empty())
{
words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
}
}
else if((int)signatureA->getWords().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
wordsA,
wordsB,
cameraModel,
cameraTransform,
_PnPReprojError,
0.99f,
words3A, // for scale estimation
&variance,
&matchesV);
covariance *= variance;
inliers = uKeys(inliers3D);
matches = matchesV;
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
double variance = 1.0f;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, cv::KeyPoint> wordsA;
std::map<int, cv::Point3f> words3A;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _minInliers)
{
wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
if(!signatureA->getWords3().empty())
if(variance <= _epipolarGeometryVar)
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
}
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
}
std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
wordsA,
wordsB,
cameraModel,
cameraTransform,
_PnPReprojError,
0.99f,
words3A, // for scale estimation
&variance,
&matchesV);
covariances[dir] *= variance;
inliers[dir] = uKeys(inliers3D);
matches[dir] = matchesV;
if(!cameraTransform.isNull())
{
if((int)inliers3D.size() >= _minInliers)
{
if(variance <= _epipolarGeometryVar)
if(this->force3DoF())
{
if(this->force3DoF())
{
transforms[dir] = cameraTransform.to3DoF();
}
else
{
transforms[dir] = cameraTransform;
}
transform = cameraTransform.to3DoF();
}
else
{
msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
UINFO(msg.c_str());
transform = cameraTransform;
}
}
else
{
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance);
UINFO(msg.c_str());
}
}
else
{
msg = uFormat("No camera transform found");
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str());
}
}
else if(signatureA->getWords().size() == 0)
{
msg = uFormat("No enough features (%d)", (int)signatureA->getWords().size());
UWARN(msg.c_str());
}
else
{
msg = uFormat("No camera model");
UWARN(msg.c_str());
msg = uFormat("No camera transform found");
UINFO(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
else if(fromSignature.getWords().size() == 0)
{
UDEBUG("");
if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
{
UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
signatureB->id(),
(int)signatureB->sensorData().cameraModels().size(),
signatureB->sensorData().stereoCameraModels().size(),
signatureB->getWeight());
}
#ifndef RTABMAP_OPENGV
else if(signatureB->sensorData().cameraModels().size() > 1)
{
UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
}
#endif
else
{
UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
// 3D to 2D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
if(!signatureB->getWords3().empty())
{
words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
}
}
std::vector<CameraModel> models;
if(signatureB->sensorData().stereoCameraModels().size())
{
for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
}
}
else
{
models = signatureB->sensorData().cameraModels();
}
if(models.size()>1)
{
// Multi-Camera
UASSERT(models[0].isValidForProjection());
transforms[dir] = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models,
_multiSamplingPolicy,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPVarMedianRatio,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
&matchesV,
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
else
{
UASSERT(models.size() == 1 && models[0].isValidForProjection());
transforms[dir] = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models[0],
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPVarMedianRatio,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
&matchesV,
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transforms[dir].isNull())
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
UINFO(msg.c_str());
}
else if(this->force3DoF())
{
transforms[dir] = transforms[dir].to3DoF();
}
}
else
{
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords().size(), _minInliers);
UINFO(msg.c_str());
}
}
msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
UWARN(msg.c_str());
}
else
{
UDEBUG("");
// 3D -> 3D
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords3().size() >= _minInliers)
msg = uFormat("No camera model");
UWARN(msg.c_str());
}
}
else if(_estimationType == 1) // PnP
{
UDEBUG("");
if((toSignature.sensorData().stereoCameraModels().empty() || !toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().cameraModels()[0].isValidForProjection()))
{
UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
toSignature.id(),
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModels().size(),
toSignature.getWeight());
}
#ifndef RTABMAP_OPENGV
else if(toSignature.sensorData().cameraModels().size() > 1)
{
UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
}
#endif
else
{
UDEBUG("words from3D=%d to2D=%d", (int)fromSignature.getWords3().size(), (int)toSignature.getWords().size());
// 3D to 2D
if((int)fromSignature.getWords3().size() >= _minInliers &&
(int)toSignature.getWords().size() >= _minInliers)
{
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B;
std::map<int, cv::KeyPoint> wordsB;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
if(!toSignature.getWords3().empty())
{
words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
}
}
std::vector<CameraModel> models;
if(toSignature.sensorData().stereoCameraModels().size())
{
for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
}
}
else
{
models = toSignature.sensorData().cameraModels();
}
if(models.size()>1)
{
// Multi-Camera
UASSERT(models[0].isValidForProjection());
std::vector<std::vector<int> > matchesPerCam;
std::vector<std::vector<int> > inliersPerCam;
transform = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models,
_multiSamplingPolicy,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPVarMedianRatio,
_PnPMaxVar,
!guess.isNull()?guess:Transform::getIdentity(),
words3B,
&covariance,
&matchesPerCam,
&inliersPerCam,
_PnPSplitLinearCovarianceComponents);
info.matchesPerCam.resize(matchesPerCam.size());
for(size_t i=0; i<matchesPerCam.size(); ++i)
{
matches.insert(matches.end(), matchesPerCam[i].begin(), matchesPerCam[i].end());
info.matchesPerCam[i] = matchesPerCam[i].size();
}
info.inliersPerCam.resize(inliersPerCam.size());
for(size_t i=0; i<inliersPerCam.size(); ++i)
{
inliers.insert(inliers.end(), inliersPerCam[i].begin(), inliersPerCam[i].end());
info.inliersPerCam[i] = inliersPerCam[i].size();
}
}
else
{
UASSERT(models.size() == 1 && models[0].isValidForProjection());
transform = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models[0],
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPVarMedianRatio,
_PnPMaxVar,
!guess.isNull()?guess:Transform::getIdentity(),
words3B,
&covariance,
&matchesV,
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers = inliersV;
matches = matchesV;
}
transforms[dir] = util3d::estimateMotion3DTo3D(
words3A,
words3B,
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transforms[dir].isNull())
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers[dir].size(), _minInliers, (int)matches[dir].size(), signatureA->id(), signatureB->id());
(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
else if(this->force3DoF())
{
transforms[dir] = transforms[dir].to3DoF();
transform = transform.to3DoF();
}
}
else
{
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers);
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)fromSignature.getWords3().size(), (int)toSignature.getWords().size(), _minInliers);
UINFO(msg.c_str());
}
}
}
if(!_forwardEstimateOnly)
{
UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
}
std::vector<int> allInliers = inliers[0];
if(inliers[1].size())
else
{
std::set<int> allInliersSet(allInliers.begin(), allInliers.end());
unsigned int oi = allInliers.size();
allInliers.resize(allInliers.size() + inliers[1].size());
for(unsigned int i=0; i<inliers[1].size(); ++i)
UDEBUG("");
// 3D -> 3D
if((int)fromSignature.getWords3().size() >= _minInliers &&
(int)toSignature.getWords3().size() >= _minInliers)
{
if(allInliersSet.find(inliers[1][i]) == allInliersSet.end())
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B;
for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
{
allInliers[oi++] = inliers[1][i];
words3A.insert(std::make_pair(iter->first, fromSignature.getWords3()[iter->second]));
}
for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
{
words3B.insert(std::make_pair(iter->first, toSignature.getWords3()[iter->second]));
}
transform = util3d::estimateMotion3DTo3D(
words3A,
words3B,
_minInliers,
_inlierDistance,
_iterations,
_refineIterations,
&covariance,
&matchesV,
&inliersV);
inliers = inliersV;
matches = matchesV;
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transform.isNull())
{
msg = uFormat("Not enough inliers %d/%d (matches=%d) between %d and %d",
(int)inliers.size(), _minInliers, (int)matches.size(), fromSignature.id(), toSignature.id());
UINFO(msg.c_str());
}
else if(this->force3DoF())
{
transform = transform.to3DoF();
}
}
allInliers.resize(oi);
}
std::vector<int> allMatches = matches[0];
if(matches[1].size())
{
std::set<int> allMatchesSet(allMatches.begin(), allMatches.end());
unsigned int oi = allMatches.size();
allMatches.resize(allMatches.size() + matches[1].size());
for(unsigned int i=0; i<matches[1].size(); ++i)
else
{
if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end())
{
allMatches[oi++] = matches[1][i];
}
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
(int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
UINFO(msg.c_str());
}
allMatches.resize(oi);
}
if(_bundleAdjustment > 0 &&
_estimationType < 2 &&
!transforms[0].isNull() &&
allInliers.size() &&
!transform.isNull() &&
inliers.size() &&
fromSignature.getWords3().size() &&
toSignature.getWords().size() &&
(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
@@ -1930,34 +1885,23 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, cv::Point3f> points3DMap;
poses.insert(std::make_pair(1, Transform::getIdentity()));
poses.insert(std::make_pair(2, transforms[0]));
poses.insert(std::make_pair(2, transform));
for(int i=0;i<2;++i)
{
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
}
cv::Mat cov = covariances[0].clone();
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transforms[0], cov.inv())));
if(!transforms[1].isNull() && inliers[1].size())
{
cov = covariances[1].clone();
links.insert(std::make_pair(2, Link(2, 1, Link::kNeighbor, transforms[1], cov.inv())));
}
UASSERT(covariance.cols==6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
if(covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transform, covariance.inv())));
std::map<int, Transform> optimizedPoses;
UASSERT((toSignature.sensorData().stereoCameraModels().size() >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
@@ -2015,17 +1959,12 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::set<int> sbaOutliers;
UDEBUG("");
for(unsigned int i=0; i<allInliers.size(); ++i)
for(unsigned int i=0; i<inliers.size(); ++i)
{
int wordId = allInliers[i];
int wordId = inliers[i];
int indexFrom = fromSignature.getWords().find(wordId)->second;
const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
if(!util3d::isFinite(pt3D))
{
UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
sbaOutliers.insert(wordId);
continue;
}
UASSERT_MSG(util3d::isFinite(pt3D), uFormat("3D point %d is not finite!?", wordId).c_str());
points3DMap.insert(std::make_pair(wordId, pt3D));
@@ -2093,32 +2032,32 @@ Transform RegistrationVis::computeTransformationImpl(
!optimizedPoses.begin()->second.isNull() &&
!optimizedPoses.rbegin()->second.isNull())
{
UDEBUG("Pose optimization: %s -> %s", transforms[0].prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
UDEBUG("Pose optimization: %s -> %s", transform.prettyPrint().c_str(), optimizedPoses.rbegin()->second.prettyPrint().c_str());
if(sbaOutliers.size())
{
std::vector<int> newInliers(allInliers.size());
std::vector<int> newInliers(inliers.size());
int oi=0;
for(unsigned int i=0; i<allInliers.size(); ++i)
for(unsigned int i=0; i<inliers.size(); ++i)
{
if(sbaOutliers.find(allInliers[i]) == sbaOutliers.end())
if(sbaOutliers.find(inliers[i]) == sbaOutliers.end())
{
newInliers[oi++] = allInliers[i];
newInliers[oi++] = inliers[i];
}
}
newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size()));
allInliers = newInliers;
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
inliers = newInliers;
}
if((int)allInliers.size() < _minInliers)
if((int)inliers.size() < _minInliers)
{
msg = uFormat("Not enough inliers after bundle adjustment %d/%d (matches=%d) between %d and %d",
(int)allInliers.size(), _minInliers, (int)allInliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
transforms[0].setNull();
(int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
transform.setNull();
}
else
{
transforms[0] = optimizedPoses.rbegin()->second;
transform = optimizedPoses.rbegin()->second;
}
// update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
@@ -2137,36 +2076,16 @@ Transform RegistrationVis::computeTransformationImpl(
}
else
{
transforms[0].setNull();
transform.setNull();
}
transforms[1].setNull();
}
info.inliersIDs = allInliers;
info.matchesIDs = allMatches;
inliersCount = (int)allInliers.size();
matchesCount = (int)allMatches.size();
if(!transforms[1].isNull())
{
transforms[1] = transforms[1].inverse();
if(transforms[0].isNull())
{
transform = transforms[1];
covariance = covariances[1];
}
else
{
transform = transforms[0].interpolate(0.5f, transforms[1]);
covariance = (covariances[0]+covariances[1])/2.0f;
}
}
else
{
transform = transforms[0];
covariance = covariances[0];
}
info.inliersIDs = inliers;
info.matchesIDs = matches;
inliersCount = (int)inliers.size();
matchesCount = (int)matches.size();
if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
if(!transform.isNull() && !inliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
{
cv::Mat pcaData;
std::vector<CameraModel> cameraModelsTo;
@@ -2187,7 +2106,7 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
{
pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
pcaData = cv::Mat(inliers.size(), 2, CV_32FC1);
}
else
{
@@ -2204,11 +2123,11 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<float> distances;
if(_maxInliersMeanDistance>0.0f)
{
distances.reserve(allInliers.size());
distances.reserve(inliers.size());
}
for(unsigned int i=0; i<allInliers.size(); ++i)
for(unsigned int i=0; i<inliers.size(); ++i)
{
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(inliers[i]);
if(wordsIter != toSignature.getWords().end() && !toSignature.getWordsKpts().empty())
{
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];

20
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -154,16 +154,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
bool forwardEst = Parameters::defaultVisForwardEstOnly();
Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
if(!forwardEst)
{
UWARN("%s=false is not supported by OdometryF2M, setting to true.",
Parameters::kVisForwardEstOnly().c_str());
forwardEst = true;
}
uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{
@@ -497,8 +487,13 @@ Transform OdometryF2M::computeTransform(
{
if(!bundlePoses.rbegin()->second.isNull())
{
if(info)
{
info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
}
if(sbaOutliers.size())
{
regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i)
@@ -506,6 +501,11 @@ Transform OdometryF2M::computeTransform(
if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{
newInliers[oi++] = regInfo.inliersIDs[i];
regInfo.inliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
}
else if(info)
{
info->localBundleOutliersPerCam[wordReferences.at(regInfo.inliersIDs[i]).at(lastFrame_->id()).cameraIndex] += 1;
}
}
newInliers.resize(oi);

74
corelib/src/util3d_motion_estimation.cpp
View File

@@ -288,6 +288,62 @@ Transform estimateMotion3DTo2D(
return transform;
}
Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const std::vector<CameraModel> & cameraModels,
unsigned int samplingPolicy,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
int refineIterations,
int varianceMedianRatio,
float maxVariance,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut,
bool splitLinearCovarianceComponents)
{
std::vector<std::vector<int> > matchesPerCamera;
std::vector<std::vector<int> > inliersPerCamera;
Transform t = estimateMotion3DTo2D(
words3A,
words2B,
cameraModels,
samplingPolicy,
minInliers,
iterations,
reprojError,
flagsPnP,
refineIterations,
varianceMedianRatio,
maxVariance,
guess,
words3B,
covariance,
matchesOut?&matchesPerCamera:0,
inliersOut?&inliersPerCamera:0,
splitLinearCovarianceComponents);
if(matchesOut)
{
for(size_t i=0; i<matchesPerCamera.size(); ++i)
{
matchesOut->insert(matchesOut->end(), matchesPerCamera[i].begin(), matchesPerCamera[i].end());
}
}
if(inliersOut)
{
for(size_t i=0; i<inliersPerCamera.size(); ++i)
{
inliersOut->insert(inliersOut->end(), inliersPerCamera[i].begin(), inliersPerCamera[i].end());
}
}
return t;
}
Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
@@ -303,8 +359,8 @@ Transform estimateMotion3DTo2D(
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut,
std::vector<std::vector<int> > * matchesOut,
std::vector<std::vector<int> > * inliersOut,
bool splitLinearCovarianceComponents)
{
Transform transform;
@@ -649,14 +705,22 @@ Transform estimateMotion3DTo2D(
if(matchesOut)
{
*matchesOut = matches;
matchesOut->resize(cameraModels.size());
UASSERT(matches.size() == cameraIndexes.size());
for(size_t i=0; i<matches.size(); ++i)
{
UASSERT(cameraIndexes[i]>=0 && cameraIndexes[i] < (int)cameraModels.size());
matchesOut->at(cameraIndexes[i]).push_back(matches[i]);
}
}
if(inliersOut)
{
inliersOut->resize(inliers.size());
inliersOut->resize(cameraModels.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{
inliersOut->at(i) = matches[inliers[i]];
UASSERT(inliers[i]>=0 && inliers[i] < (int)cameraIndexes.size());
UASSERT(cameraIndexes[inliers[i]]>=0 && cameraIndexes[inliers[i]] < (int)cameraModels.size());
inliersOut->at(cameraIndexes[inliers[i]]).push_back(matches[inliers[i]]);
}
}
#endif