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

Added Vis/PnPSplitLinearCovComponents parameter (default false -> same as before)

Browse Source
This commit is contained in:
matlabbe
2024-02-07 14:43:16 -08:00
parent 1dadd50cf2
commit 510aef19e4
11 changed files with 259 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/Memory.cpp
View File

@@ -3372,7 +3372,7 @@ bool Memory::addLink(const Link & link, bool addInDatabase)
{
UASSERT(link.type() > Link::kNeighbor && link.type() != Link::kUndef);
ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance());
ULOGGER_INFO("to=%d, from=%d transform: %s var=%f", link.to(), link.from(), link.transform().prettyPrint().c_str(), link.transVariance(false));
Signature * toS = _getSignature(link.to());
Signature * fromS = _getSignature(link.from());
if(toS && fromS)

9
corelib/src/Odometry.cpp
View File

@@ -335,6 +335,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
}
if(!data.imageRaw().empty())
{
UDEBUG("Processing image data %dx%d: rgbd models=%ld, stereo models=%ld",
data.imageRaw().cols,
data.imageRaw().rows,
data.cameraModels().size(),
data.stereoCameraModels().size());
}
if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{

9
corelib/src/RegistrationVis.cpp
View File

@@ -72,6 +72,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_PnPVarMedianRatio(Parameters::defaultVisPnPVarianceMedianRatio()),
_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
_PnPSplitLinearCovarianceComponents(Parameters::defaultVisPnPSplitLinearCovComponents()),
_multiSamplingPolicy(Parameters::defaultVisPnPSamplingPolicy()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
@@ -130,6 +131,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisPnPVarianceMedianRatio(), _PnPVarMedianRatio);
Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
Parameters::parse(parameters, Parameters::kVisPnPSplitLinearCovComponents(), _PnPSplitLinearCovarianceComponents);
Parameters::parse(parameters, Parameters::kVisPnPSamplingPolicy(), _multiSamplingPolicy);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
@@ -295,6 +297,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
UDEBUG("%s=%f", Parameters::kVisPnPSplitLinearCovComponents().c_str(), _PnPSplitLinearCovarianceComponents);
UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1637,7 +1640,8 @@ Transform RegistrationVis::computeTransformationImpl(
words3B,
&covariances[dir],
&matchesV,
&inliersV);
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
@@ -1660,7 +1664,8 @@ Transform RegistrationVis::computeTransformationImpl(
words3B,
&covariances[dir],
&matchesV,
&inliersV);
&inliersV,
_PnPSplitLinearCovarianceComponents);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}

11
corelib/src/Rtabmap.cpp
View File

@@ -1442,8 +1442,10 @@ bool Rtabmap::process(
float angleToClosestNodeInTheGraph = 0;
if(_rgbdSlamMode)
{
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(5,5));
double linVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(0,0), odomCovariance.at<double>(1,1)>=9999?0:odomCovariance.at<double>(1,1), odomCovariance.at<double>(2,2)>=9999?0:odomCovariance.at<double>(2,2));
double angVar = odomCovariance.empty()?1.0f:uMax3(odomCovariance.at<double>(3,3)>=9999?0:odomCovariance.at<double>(3,3), odomCovariance.at<double>(4,4)>=9999?0:odomCovariance.at<double>(4,4), odomCovariance.at<double>(5,5));
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), (float)linVar);
statistics_.addStatistic(Statistics::kMemoryOdometry_variance_ang(), (float)angVar);
//Verify if there was a rehearsal
int rehearsedId = (int)uValue(statistics_.data(), Statistics::kMemoryRehearsal_merged(), 0.0f);
@@ -2959,9 +2961,10 @@ bool Rtabmap::process(
{
// Make the new one the parent of the old one
UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
loopClosureLinearVariance = uMax3(info.covariance.at<double>(0,0), info.covariance.at<double>(1,1)>=9999?0:info.covariance.at<double>(1,1), info.covariance.at<double>(2,2)>=9999?0:info.covariance.at<double>(2,2));
loopClosureAngularVariance = uMax3(info.covariance.at<double>(3,3)>=9999?0:info.covariance.at<double>(3,3), info.covariance.at<double>(4,4)>=9999?0:info.covariance.at<double>(4,4), info.covariance.at<double>(5,5));
cv::Mat information = getInformation(info.covariance);
loopClosureLinearVariance = 1.0/information.at<double>(0,0);
loopClosureAngularVariance = 1.0/information.at<double>(5,5);
rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
if(!rejectedGlobalLoopClosure)
{

131
corelib/src/util3d_motion_estimation.cpp
View File

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UStl.h"
#include "rtabmap/utilite/UMath.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/core/util3d_registration.h"
#include "rtabmap/core/util3d_correspondences.h"
@@ -70,7 +71,8 @@ Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
bool splitLinearCovarianceComponents)
{
UASSERT(cameraModel.isValidForProjection());
UASSERT(!guess.isNull());
@@ -155,25 +157,36 @@ Transform estimateMotion3DTo2D(
if(covariance && (!words3B.empty() || cameraModel.imageSize() != cv::Size()))
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdX;
std::vector<float> errorSqrdY;
std::vector<float> errorSqrdZ;
if(splitLinearCovarianceComponents)
{
errorSqrdX.resize(inliers.size());
errorSqrdY.resize(inliers.size());
errorSqrdZ.resize(inliers.size());
}
std::vector<float> errorSqrdAngles(inliers.size());
Transform localTransformInv = cameraModel.localTransform().inverse();
Transform transformCameraFrameInv = (transform * cameraModel.localTransform()).inverse();
Transform transformCameraFrame = transform * cameraModel.localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
for(unsigned int i=0; i<inliers.size(); ++i)
{
cv::Point3f objPt = objectPoints[inliers[i]];
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
objPt = util3d::transformPoint(objPt, transformCameraFrameInv);
// Get 3D point from target in cameraB frame
// Get 3D point from cameraB base frame in cameraA base frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
newPt = util3d::transformPoint(iter->second, localTransformInv);
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformCameraFrameInv);
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
@@ -184,7 +197,20 @@ Transform estimateMotion3DTo2D(
cameraModel.fx(),
cameraModel.fy());
// transform in camera B frame
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
//transform back into cameraA base frame
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
if(splitLinearCovarianceComponents)
{
double errorX = objPt.x-newPt.x;
double errorY = objPt.y-newPt.y;
double errorZ = objPt.z-newPt.z;
errorSqrdX[i] = errorX * errorX;
errorSqrdY[i] = errorY * errorY;
errorSqrdZ[i] = errorZ * errorZ;
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
@@ -204,6 +230,25 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(splitLinearCovarianceComponents)
{
std::sort(errorSqrdX.begin(), errorSqrdX.end());
double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
std::sort(errorSqrdY.begin(), errorSqrdY.end());
double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
@@ -259,7 +304,8 @@ Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
std::vector<int> * inliersOut,
bool splitLinearCovarianceComponents)
{
Transform transform;
#ifndef RTABMAP_OPENGV
@@ -491,27 +537,44 @@ Transform estimateMotion3DTo2D(
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(covariance)
{
std::vector<float> errorSqrdX;
std::vector<float> errorSqrdY;
std::vector<float> errorSqrdZ;
if(splitLinearCovarianceComponents)
{
errorSqrdX.resize(inliers.size());
errorSqrdY.resize(inliers.size());
errorSqrdZ.resize(inliers.size());
}
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
std::vector<Transform> transformsCameraFrame(cameraModels.size());
std::vector<Transform> transformsCameraFrameInv(cameraModels.size());
for(size_t i=0; i<cameraModels.size(); ++i)
{
transformsCameraFrame[i] = transform * cameraModels[i].localTransform();
transformsCameraFrameInv[i] = transformsCameraFrame[i].inverse();
}
for(unsigned int i=0; i<inliers.size(); ++i)
{
cv::Point3f objPt = objectPoints[inliers[i]];
int cameraIndex = cameraIndexes[inliers[i]];
Transform transformCameraFrameInv = (transform * cameraModels[cameraIndex].localTransform()).inverse();
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
objPt = util3d::transformPoint(objPt, transformCameraFrameInv);
// Get 3D point from target in cameraB frame
// Get 3D point from cameraB base frame in cameraA base frame
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
newPt = util3d::transformPoint(iter->second, cameraModels[cameraIndex].localTransform().inverse());
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
int cameraIndex = cameraIndexes[inliers[i]];
// Project obj point from base frame of cameraA in cameraB frame (z+ in front of the cameraB)
cv::Point3f objPtCamBFrame = util3d::transformPoint(objPt, transformsCameraFrameInv[cameraIndex]);
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
@@ -522,7 +585,20 @@ Transform estimateMotion3DTo2D(
cameraModels[cameraIndex].fx(),
cameraModels[cameraIndex].fy());
// transform in camera B frame
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPt.z*1.1; // Add 10 % error
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * objPtCamBFrame.z*1.1; // Add 10 % error
//transfor back into cameraA base frame
newPt = util3d::transformPoint(newPt, transformsCameraFrame[cameraIndex]);
}
if(splitLinearCovarianceComponents)
{
double errorX = objPt.x-newPt.x;
double errorY = objPt.y-newPt.y;
double errorZ = objPt.z-newPt.z;
errorSqrdX[i] = errorX * errorX;
errorSqrdY[i] = errorY * errorY;
errorSqrdZ[i] = errorZ * errorZ;
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
@@ -542,6 +618,25 @@ Transform estimateMotion3DTo2D(
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(splitLinearCovarianceComponents)
{
std::sort(errorSqrdX.begin(), errorSqrdX.end());
double median_error_sqr_x = 2.1981 * (double)errorSqrdX[errorSqrdX.size () / varianceMedianRatio];
std::sort(errorSqrdY.begin(), errorSqrdY.end());
double median_error_sqr_y = 2.1981 * (double)errorSqrdY[errorSqrdY.size () / varianceMedianRatio];
std::sort(errorSqrdZ.begin(), errorSqrdZ.end());
double median_error_sqr_z = 2.1981 * (double)errorSqrdZ[errorSqrdZ.size () / varianceMedianRatio];
UASSERT(uIsFinite(median_error_sqr_x));
UASSERT(uIsFinite(median_error_sqr_y));
UASSERT(uIsFinite(median_error_sqr_z));
covariance->at<double>(0,0) = median_error_sqr_x;
covariance->at<double>(1,1) = median_error_sqr_y;
covariance->at<double>(2,2) = median_error_sqr_z;
median_error_sqr_lin = uMax3(median_error_sqr_x, median_error_sqr_y, median_error_sqr_z);
}
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);