i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +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

2
CMakeLists.txt
View File

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
####################### #######################
SET(RTABMAP_MAJOR_VERSION 0) SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 21) SET(RTABMAP_MINOR_VERSION 21)
SET(RTABMAP_PATCH_VERSION 12) SET(RTABMAP_PATCH_VERSION 13)
SET(RTABMAP_VERSION SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION}) ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})

2
cmake_modules/android.toolchain.cmake
View File

@@ -184,7 +184,7 @@
# #
# ------------------------------------------------------------------------------ # ------------------------------------------------------------------------------
cmake_minimum_required( VERSION 2.6.3 ) cmake_minimum_required( VERSION 3.14 )
if( DEFINED CMAKE_CROSSCOMPILING ) if( DEFINED CMAKE_CROSSCOMPILING )
# subsequent toolchain loading is not really needed # subsequent toolchain loading is not really needed

64
corelib/include/rtabmap/core/OdometryInfo.h
View File

@@ -1,5 +1,5 @@
/* /*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved. All rights reserved.
Redistribution and use in source and binary forms, with or without Redistribution and use in source and binary forms, with or without
@@ -40,64 +40,9 @@ namespace rtabmap {
class OdometryInfo class OdometryInfo
{ {
public: public:
OdometryInfo() : OdometryInfo();
lost(true), OdometryInfo copyWithoutData() const;
features(0), std::map<std::string, float> statistics(const Transform & pose = Transform());
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 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.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;
}
bool lost; bool lost;
RegistrationInfo reg; RegistrationInfo reg;
@@ -112,6 +57,7 @@ public:
std::map<int, std::vector<CameraModel> > localBundleModels; std::map<int, std::vector<CameraModel> > localBundleModels;
float localBundleAvgInlierDistance; float localBundleAvgInlierDistance;
int localBundleMaxKeyFramesForInlier; int localBundleMaxKeyFramesForInlier;
std::vector<int> localBundleOutliersPerCam;
bool keyFrameAdded; bool keyFrameAdded;
float timeDeskewing; float timeDeskewing;
float timeEstimation; float timeEstimation;

1
corelib/include/rtabmap/core/Parameters.h
View File

@@ -678,7 +678,6 @@ class RTABMAP_CORE_EXPORT Parameters
// Visual registration parameters // Visual registration parameters
RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)"); RTABMAP_PARAM(Vis, EstimationType, int, 1, "Motion estimation approach: 0:3D->3D, 1:3D->2D (PnP), 2:2D->2D (Epipolar Geometry)");
RTABMAP_PARAM(Vis, ForwardEstOnly, bool, true, "Forward estimation only (A->B). If false, a transformation is also computed in backward direction (B->A), then the two resulting transforms are merged (middle interpolation between the transforms).");
RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str())); RTABMAP_PARAM(Vis, InlierDistance, float, 0.1, uFormat("[%s = 0] Maximum distance for feature correspondences. Used by 3D->3D estimation approach.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str())); RTABMAP_PARAM(Vis, RefineIterations, int, 5, uFormat("[%s = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.", kVisEstimationType().c_str()));
RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str())); RTABMAP_PARAM(Vis, PnPReprojError, float, 2, uFormat("[%s = 1] PnP reprojection error.", kVisEstimationType().c_str()));

4
corelib/include/rtabmap/core/RegistrationInfo.h
View File

@@ -59,9 +59,11 @@ public:
output.covariance = covariance.clone(); output.covariance = covariance.clone();
output.rejectedMsg = rejectedMsg; output.rejectedMsg = rejectedMsg;
output.inliers = inliers; output.inliers = inliers;
output.inliersPerCam = inliersPerCam;
output.inliersMeanDistance = inliersMeanDistance; output.inliersMeanDistance = inliersMeanDistance;
output.inliersDistribution = inliersDistribution; output.inliersDistribution = inliersDistribution;
output.matches = matches; output.matches = matches;
output.matchesPerCam = matchesPerCam;
output.icpInliersRatio = icpInliersRatio; output.icpInliersRatio = icpInliersRatio;
output.icpTranslation = icpTranslation; output.icpTranslation = icpTranslation;
output.icpRotation = icpRotation; output.icpRotation = icpRotation;
@@ -85,6 +87,8 @@ public:
int matches; int matches;
std::vector<int> matchesIDs; std::vector<int> matchesIDs;
std::vector<int> projectedIDs; // "From" IDs std::vector<int> projectedIDs; // "From" IDs
std::vector<int> inliersPerCam;
std::vector<int> matchesPerCam;
// RegistrationIcp // RegistrationIcp
float icpInliersRatio; float icpInliersRatio;

1
corelib/include/rtabmap/core/RegistrationVis.h
View File

@@ -78,7 +78,6 @@ private:
int _refineIterations; int _refineIterations;
float _epipolarGeometryVar; float _epipolarGeometryVar;
int _estimationType; int _estimationType;
bool _forwardEstimateOnly;
float _PnPReprojError; float _PnPReprojError;
int _PnPFlags; int _PnPFlags;
int _PnPRefineIterations; int _PnPRefineIterations;

19
corelib/include/rtabmap/core/util3d_motion_estimation.h
View File

@@ -76,6 +76,25 @@ Transform RTABMAP_CORE_EXPORT estimateMotion3DTo2D(
std::vector<int> * inliersOut = 0, std::vector<int> * inliersOut = 0,
bool splitLinearCovarianceComponents = false); bool splitLinearCovarianceComponents = false);
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<std::vector<int> > * matchesOut,
std::vector<std::vector<int> > * inliersOut,
bool splitLinearCovarianceComponents);
Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D( Transform RTABMAP_CORE_EXPORT estimateMotion3DTo3D(
const std::map<int, cv::Point3f> & words3A, const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::Point3f> & words3B, const std::map<int, cv::Point3f> & words3B,

1
corelib/src/CMakeLists.txt
View File

@@ -85,6 +85,7 @@ SET(SRC_FILES
Odometry.cpp Odometry.cpp
OdometryThread.cpp OdometryThread.cpp
OdometryInfo.cpp
odometry/OdometryF2M.cpp odometry/OdometryF2M.cpp
odometry/OdometryMono.cpp odometry/OdometryMono.cpp
odometry/OdometryF2F.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 // removed parameters
// 0.21.13
removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly", std::make_pair(false, "")));
// 0.21.7 // 0.21.7
removedParameters_.insert(std::make_pair("SIFT/NFeatures", std::make_pair(false, ""))); 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()), _refineIterations(Parameters::defaultVisRefineIterations()),
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()), _epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
_estimationType(Parameters::defaultVisEstimationType()), _estimationType(Parameters::defaultVisEstimationType()),
_forwardEstimateOnly(Parameters::defaultVisForwardEstOnly()),
_PnPReprojError(Parameters::defaultVisPnPReprojError()), _PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags()), _PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()), _PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
@@ -132,7 +131,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations); Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations); Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType); Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), _forwardEstimateOnly);
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar); Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError); Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags); Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
@@ -314,7 +312,6 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance); UDEBUG("%s=%f", Parameters::kVisInlierDistance().c_str(), _inlierDistance);
UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations); UDEBUG("%s=%d", Parameters::kVisIterations().c_str(), _iterations);
UDEBUG("%s=%d", Parameters::kVisEstimationType().c_str(), _estimationType); 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::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError); UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags); UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
@@ -721,7 +718,7 @@ Transform RegistrationVis::computeTransformationImpl(
kptsFrom3D = kptsFrom3DKept; kptsFrom3D = kptsFrom3DKept;
std::vector<cv::Point3f> kptsTo3D; std::vector<cv::Point3f> kptsTo3D;
if(_estimationType == 0 || _estimationType == 1 || !_forwardEstimateOnly) if(_estimationType == 0 || _estimationType == 1)
{ {
kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo); kptsTo3D = _detectorTo->generateKeypoints3D(toSignature.sensorData(), kptsTo);
} }
@@ -1576,347 +1573,305 @@ Transform RegistrationVis::computeTransformationImpl(
info.matchesIDs.clear(); info.matchesIDs.clear();
if(toSignature.getWords().size()) if(toSignature.getWords().size())
{ {
Transform transforms[2]; std::vector<int> inliers;
std::vector<int> inliers[2]; std::vector<int> matches;
std::vector<int> matches[2];
cv::Mat covariances[2]; if(_estimationType == 2) // Epipolar Geometry
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)
{ {
// A to B UDEBUG("");
Signature * signatureA; if((toSignature.sensorData().stereoCameraModels().size() != 1 ||
Signature * signatureB; !toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) &&
if(dir == 0) (toSignature.sensorData().cameraModels().size() != 1 ||
!toSignature.sensorData().cameraModels()[0].isValidForProjection()))
{ {
signatureA = &fromSignature; UERROR("Calibrated camera required (multi-cameras not supported).");
signatureB = &toSignature;
} }
else else if((int)fromSignature.getWords().size() >= _minInliers &&
(int)toSignature.getWords().size() >= _minInliers)
{ {
signatureA = &toSignature; UASSERT((fromSignature.sensorData().stereoCameraModels().size() == 1 && fromSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValidForProjection()));
signatureB = &fromSignature; const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModels().size()?fromSignature.sensorData().stereoCameraModels()[0].left():fromSignature.sensorData().cameraModels()[0];
}
if(_estimationType == 2) // Epipolar Geometry // we only need the camera transform, send guess words3 for scale estimation
{ Transform cameraTransform;
UDEBUG(""); double variance = 1.0f;
if((signatureB->sensorData().stereoCameraModels().size() != 1 || std::vector<int> matchesV;
!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) && std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
(signatureB->sensorData().cameraModels().size() != 1 || std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
!signatureB->sensorData().cameraModels()[0].isValidForProjection())) 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 && for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
(int)signatureB->getWords().size() >= _minInliers)
{ {
UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection())); wordsB.insert(std::make_pair(iter->first, toSignature.getWordsKpts()[iter->second]));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0]; }
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 if(!cameraTransform.isNull())
Transform cameraTransform; {
double variance = 1.0f; if((int)inliers3D.size() >= _minInliers)
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)
{ {
wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second])); if(variance <= _epipolarGeometryVar)
if(!signatureA->getWords3().empty())
{ {
words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second])); if(this->force3DoF())
}
}
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()) transform = cameraTransform.to3DoF();
{
transforms[dir] = cameraTransform.to3DoF();
}
else
{
transforms[dir] = cameraTransform;
}
} }
else else
{ {
msg = uFormat("Variance is too high! (Max %s=%f, variance=%f)", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar, variance); transform = cameraTransform;
UINFO(msg.c_str());
} }
} }
else 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()); UINFO(msg.c_str());
} }
} }
else else
{ {
msg = uFormat("No camera transform found"); msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
UINFO(msg.c_str()); 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 else
{ {
msg = uFormat("No camera model"); msg = uFormat("No camera transform found");
UWARN(msg.c_str()); UINFO(msg.c_str());
} }
} }
else if(_estimationType == 1) // PnP else if(fromSignature.getWords().size() == 0)
{ {
UDEBUG(""); msg = uFormat("No enough features (%d)", (int)fromSignature.getWords().size());
if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) && UWARN(msg.c_str());
(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());
}
}
} }
else else
{ {
UDEBUG(""); msg = uFormat("No camera model");
// 3D -> 3D UWARN(msg.c_str());
if((int)signatureA->getWords3().size() >= _minInliers && }
(int)signatureB->getWords3().size() >= _minInliers) }
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> inliersV;
std::vector<int> matchesV; std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords()); std::map<int, int> uniqueWordsA = uMultimapToMapUnique(fromSignature.getWords());
std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords()); std::map<int, int> uniqueWordsB = uMultimapToMapUnique(toSignature.getWords());
std::map<int, cv::Point3f> words3A; std::map<int, cv::Point3f> words3A;
std::map<int, cv::Point3f> words3B; 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) 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) 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()); 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", 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()); UINFO(msg.c_str());
} }
else if(this->force3DoF()) else if(this->force3DoF())
{ {
transforms[dir] = transforms[dir].to3DoF(); transform = transform.to3DoF();
} }
} }
else else
{ {
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)", msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
(int)signatureA->getWords3().size(), (int)signatureB->getWords3().size(), _minInliers); (int)fromSignature.getWords3().size(), (int)toSignature.getWords().size(), _minInliers);
UINFO(msg.c_str()); UINFO(msg.c_str());
} }
} }
}
if(!_forwardEstimateOnly)
{
UDEBUG("from->to=%s", transforms[0].prettyPrint().c_str());
UDEBUG("to->from=%s", transforms[1].prettyPrint().c_str());
} }
else
std::vector<int> allInliers = inliers[0];
if(inliers[1].size())
{ {
std::set<int> allInliersSet(allInliers.begin(), allInliers.end()); UDEBUG("");
unsigned int oi = allInliers.size(); // 3D -> 3D
allInliers.resize(allInliers.size() + inliers[1].size()); if((int)fromSignature.getWords3().size() >= _minInliers &&
for(unsigned int i=0; i<inliers[1].size(); ++i) (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); else
}
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)
{ {
if(allMatchesSet.find(matches[1][i]) == allMatchesSet.end()) msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
{ (int)fromSignature.getWords3().size(), (int)toSignature.getWords3().size(), _minInliers);
allMatches[oi++] = matches[1][i]; UINFO(msg.c_str());
}
} }
allMatches.resize(oi);
} }
if(_bundleAdjustment > 0 && if(_bundleAdjustment > 0 &&
_estimationType < 2 && _estimationType < 2 &&
!transforms[0].isNull() && !transform.isNull() &&
allInliers.size() && inliers.size() &&
fromSignature.getWords3().size() && fromSignature.getWords3().size() &&
toSignature.getWords().size() && toSignature.getWords().size() &&
(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) && (fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
@@ -1930,34 +1885,23 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, cv::Point3f> points3DMap; std::map<int, cv::Point3f> points3DMap;
poses.insert(std::make_pair(1, Transform::getIdentity())); 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(covariance.cols==6 && covariance.rows == 6 && covariance.type() == CV_64FC1);
{ if(covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1); covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON) if(covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON) if(covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON) if(covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON) if(covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON) if(covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform covariance.at<double>(5,5) = 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())));
}
links.insert(std::make_pair(1, Link(1, 2, Link::kNeighbor, transform, covariance.inv())));
std::map<int, Transform> optimizedPoses; std::map<int, Transform> optimizedPoses;
UASSERT((toSignature.sensorData().stereoCameraModels().size() >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) || 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::map<int, std::map<int, FeatureBA> > wordReferences;
std::set<int> sbaOutliers; std::set<int> sbaOutliers;
UDEBUG(""); 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; int indexFrom = fromSignature.getWords().find(wordId)->second;
const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom]; const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
if(!util3d::isFinite(pt3D)) UASSERT_MSG(util3d::isFinite(pt3D), uFormat("3D point %d is not finite!?", wordId).c_str());
{
UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
sbaOutliers.insert(wordId);
continue;
}
points3DMap.insert(std::make_pair(wordId, pt3D)); points3DMap.insert(std::make_pair(wordId, pt3D));
@@ -2093,32 +2032,32 @@ Transform RegistrationVis::computeTransformationImpl(
!optimizedPoses.begin()->second.isNull() && !optimizedPoses.begin()->second.isNull() &&
!optimizedPoses.rbegin()->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()) if(sbaOutliers.size())
{ {
std::vector<int> newInliers(allInliers.size()); std::vector<int> newInliers(inliers.size());
int oi=0; 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); newInliers.resize(oi);
UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(allInliers.size())); UDEBUG("BA outliers ratio %f", float(sbaOutliers.size())/float(inliers.size()));
allInliers = newInliers; 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", 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()); (int)inliers.size(), _minInliers, (int)inliers.size()+sbaOutliers.size(), fromSignature.id(), toSignature.id());
transforms[0].setNull(); transform.setNull();
} }
else else
{ {
transforms[0] = optimizedPoses.rbegin()->second; transform = optimizedPoses.rbegin()->second;
} }
// update 3D points, both from and to signatures // update 3D points, both from and to signatures
/*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3(); /*std::multimap<int, cv::Point3f> cpyWordsFrom3 = fromSignature.getWords3();
@@ -2137,36 +2076,16 @@ Transform RegistrationVis::computeTransformationImpl(
} }
else else
{ {
transforms[0].setNull(); transform.setNull();
} }
transforms[1].setNull();
} }
info.inliersIDs = allInliers; info.inliersIDs = inliers;
info.matchesIDs = allMatches; info.matchesIDs = matches;
inliersCount = (int)allInliers.size(); inliersCount = (int)inliers.size();
matchesCount = (int)allMatches.size(); matchesCount = (int)matches.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];
}
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; cv::Mat pcaData;
std::vector<CameraModel> cameraModelsTo; std::vector<CameraModel> cameraModelsTo;
@@ -2187,7 +2106,7 @@ Transform RegistrationVis::computeTransformationImpl(
{ {
if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0) 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 else
{ {
@@ -2204,11 +2123,11 @@ Transform RegistrationVis::computeTransformationImpl(
std::vector<float> distances; std::vector<float> distances;
if(_maxInliersMeanDistance>0.0f) 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()) if(wordsIter != toSignature.getWords().end() && !toSignature.getWordsKpts().empty())
{ {
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second]; 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))); 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); regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && regPipeline_->isScanRequired()) if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{ {
@@ -497,8 +487,13 @@ Transform OdometryF2M::computeTransform(
{ {
if(!bundlePoses.rbegin()->second.isNull()) if(!bundlePoses.rbegin()->second.isNull())
{ {
if(info)
{
info->localBundleOutliersPerCam = std::vector<int>(lastFrameModels.size(),0);
}
if(sbaOutliers.size()) if(sbaOutliers.size())
{ {
regInfo.inliersPerCam = std::vector<int>(lastFrameModels.size(),0);
std::vector<int> newInliers(regInfo.inliersIDs.size()); std::vector<int> newInliers(regInfo.inliersIDs.size());
int oi=0; int oi=0;
for(unsigned int i=0; i<regInfo.inliersIDs.size(); ++i) 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()) if(sbaOutliers.find(regInfo.inliersIDs[i]) == sbaOutliers.end())
{ {
newInliers[oi++] = regInfo.inliersIDs[i]; 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); newInliers.resize(oi);

74
corelib/src/util3d_motion_estimation.cpp
View File

@@ -288,6 +288,62 @@ Transform estimateMotion3DTo2D(
return transform; 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( Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A, const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B, const std::map<int, cv::KeyPoint> & words2B,
@@ -303,8 +359,8 @@ Transform estimateMotion3DTo2D(
const Transform & guess, const Transform & guess,
const std::map<int, cv::Point3f> & words3B, const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance, cv::Mat * covariance,
std::vector<int> * matchesOut, std::vector<std::vector<int> > * matchesOut,
std::vector<int> * inliersOut, std::vector<std::vector<int> > * inliersOut,
bool splitLinearCovarianceComponents) bool splitLinearCovarianceComponents)
{ {
Transform transform; Transform transform;
@@ -649,14 +705,22 @@ Transform estimateMotion3DTo2D(
if(matchesOut) 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) if(inliersOut)
{ {
inliersOut->resize(inliers.size()); inliersOut->resize(cameraModels.size());
for(unsigned int i=0; i<inliers.size(); ++i) 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 #endif

23
guilib/src/MainWindow.cpp
View File

@@ -1629,7 +1629,8 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose()); _cloudViewer->updateCameraTargetPosition(_odometryCorrection*odom.pose());
UDEBUG("Time Update Pose: %fs", time.ticks()); UDEBUG("Time Update Pose: %fs", time.ticks());
} }
_cloudViewer->refreshView(); // Use update instead of refreshView to avoid high CPU usage and lag
_cloudViewer->update();
if(_ui->graphicsView_graphView->isVisible()) if(_ui->graphicsView_graphView->isVisible())
{ {
@@ -1798,6 +1799,22 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/InliersMeanDistance/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersMeanDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/InliersDistribution/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersDistribution, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/InliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().features<=0?0.0f:float(odom.info().reg.inliers)/float(odom.info().features), _preferencesDialog->isCacheSavedInFigures());
for(size_t i=0; i<odom.info().reg.matchesPerCam.size(); ++i)
{
_ui->statsToolBox->updateStat(QString("Odometry/matchesCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.matchesPerCam[i], _preferencesDialog->isCacheSavedInFigures());
}
for(size_t i=0; i<odom.info().reg.inliersPerCam.size(); ++i)
{
_ui->statsToolBox->updateStat(QString("Odometry/inliersCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.inliersPerCam[i], _preferencesDialog->isCacheSavedInFigures());
}
if(odom.info().reg.matchesPerCam.size() == odom.info().reg.inliersPerCam.size())
{
for(size_t i=0; i<odom.info().reg.matchesPerCam.size(); ++i)
{
_ui->statsToolBox->updateStat(QString("Odometry/inliersRatioCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), odom.info().reg.matchesPerCam[i]>0 ? (float)odom.info().reg.inliersPerCam[i] / (float)odom.info().reg.matchesPerCam[i] : 0.0f, _preferencesDialog->isCacheSavedInFigures());
}
}
_ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPInliersRatio/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpInliersRatio, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPRotation/rad", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpRotation, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ICPTranslation/m", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().reg.icpTranslation, _preferencesDialog->isCacheSavedInFigures());
@@ -1833,6 +1850,10 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
_ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/localBundleTime/ms", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleTime*1000.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/localBundleAvgInlierDistance/pix", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleAvgInlierDistance, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/localBundleMaxKeyFramesForInlier/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleMaxKeyFramesForInlier, _preferencesDialog->isCacheSavedInFigures());
for(size_t i=0; i<odom.info().localBundleOutliersPerCam.size(); ++i)
{
_ui->statsToolBox->updateStat(QString("Odometry/localBundleOutliersCam%1/").arg(i), _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().localBundleOutliersPerCam[i], _preferencesDialog->isCacheSavedInFigures());
}
} }
_ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/KeyFrameAdded/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)odom.info().keyFrameAdded?1.0f:0.0f, _preferencesDialog->isCacheSavedInFigures());
_ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures()); _ui->statsToolBox->updateStat("Odometry/ID/", _preferencesDialog->isTimeUsedInFigures()?data->stamp()-_firstStamp:(float)data->id(), (float)data->id(), _preferencesDialog->isCacheSavedInFigures());

1
guilib/src/PreferencesDialog.cpp
View File

@@ -1272,7 +1272,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str()); _ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int))); connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType()); _ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
_ui->loopClosure_forwardEst->setObjectName(Parameters::kVisForwardEstOnly().c_str());
_ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str()); _ui->loopClosure_bowEpipolarGeometryVar->setObjectName(Parameters::kVisEpipolarGeometryVar().c_str());
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str()); _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str()); _ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());

216
guilib/src/ui/preferencesDialog.ui
View File

@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>18</number> <number>21</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -21712,6 +21712,95 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1,0"> <layout class="QVBoxLayout" name="verticalLayout_41" stretch="0,0,0,1,0">
<item> <item>
<layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1"> <layout class="QGridLayout" name="gridLayout_23" columnstretch="0,1">
<item row="2" column="1">
<widget class="QLabel" name="label_554">
<property name="text">
<string>Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QComboBox" name="loopClosure_bundle">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property>
<item>
<property name="text">
<string>Disabled</string>
</property>
</item>
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_346">
<property name="text">
<string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="visMeanDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Minimum correspondences to accept the estimated transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QDoubleSpinBox" name="visMinDistribution">
<property name="decimals">
<number>4</number>
</property>
<property name="maximum">
<double>0.500000000000000</double>
</property>
<property name="singleStep">
<double>0.010000000000000</double>
</property>
</widget>
</item>
<item row="0" column="0"> <item row="0" column="0">
<widget class="QComboBox" name="loopClosure_estimationType"> <widget class="QComboBox" name="loopClosure_estimationType">
<property name="sizeAdjustPolicy"> <property name="sizeAdjustPolicy">
@@ -21734,22 +21823,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item> </item>
</widget> </widget>
</item> </item>
<item row="4" column="0">
<widget class="QSpinBox" name="loopClosure_bowIterations">
<property name="minimum">
<number>1</number>
</property>
<property name="maximum">
<number>10000</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item row="4" column="1"> <item row="4" column="1">
<widget class="QLabel" name="label_15"> <widget class="QLabel" name="label_15">
<property name="text"> <property name="text">
@@ -21760,26 +21833,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="5" column="1">
<widget class="QLabel" name="label_298">
<property name="text">
<string>Forward estimation only (A-&gt;B). If false, a transformation is also computed in backward direction (B-&gt;A), then the two resulting transforms are merged (middle interpolation between the transforms).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="loopClosure_forwardEst">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0"> <item row="1" column="0">
<widget class="QSpinBox" name="loopClosure_bowMinInliers"> <widget class="QSpinBox" name="loopClosure_bowMinInliers">
<property name="minimum"> <property name="minimum">
@@ -21806,72 +21859,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="1">
<widget class="QLabel" name="label_2">
<property name="text">
<string>Minimum correspondences to accept the estimated transformation.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QComboBox" name="loopClosure_bundle">
<property name="sizeAdjustPolicy">
<enum>QComboBox::AdjustToContentsOnFirstShow</enum>
</property>
<item>
<property name="text">
<string>Disabled</string>
</property>
</item>
<item>
<property name="text">
<string>g2o</string>
</property>
</item>
<item>
<property name="text">
<string>cvsba</string>
</property>
</item>
<item>
<property name="text">
<string>Ceres</string>
</property>
</item>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_346">
<property name="text">
<string>Refine transformation with bundle adjustment. See Optimizer panel.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_554">
<property name="text">
<string>Maximum distance (m) of the mean distance of inliers from the camera to accept the transformation. 0 means disabled.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="1"> <item row="3" column="1">
<widget class="QLabel" name="label_555"> <widget class="QLabel" name="label_555">
<property name="text"> <property name="text">
@@ -21885,26 +21872,19 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="3" column="0"> <item row="4" column="0">
<widget class="QDoubleSpinBox" name="visMinDistribution"> <widget class="QSpinBox" name="loopClosure_bowIterations">
<property name="decimals"> <property name="minimum">
<number>4</number> <number>1</number>
</property> </property>
<property name="maximum"> <property name="maximum">
<double>0.500000000000000</double> <number>10000</number>
</property> </property>
<property name="singleStep"> <property name="singleStep">
<double>0.010000000000000</double> <number>1</number>
</property> </property>
</widget> <property name="value">
</item> <number>100</number>
<item row="2" column="0">
<widget class="QDoubleSpinBox" name="visMeanDistance">
<property name="suffix">
<string> m</string>
</property>
<property name="maximum">
<double>9999.000000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>