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

Pnp multicam refactoring (#902)

Browse Source 
* gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time)

* Added Vis/PnPMaxVariance and RGBD/InvertedReg parameters. Implemented inlier distribution computation for multicam.

* On loc/small displacement: don't remove from odom cache if loop is rejected (maybe first loc)

* Loc: don't prune odom cache on small movement if delayed loc is enabled

* loc/small movement: cleanup bidirectional links

* Cov/PnP: fixed objPt transform to estimate depth

Co-authored-by: mathieu86 <mathieu@robust.ai>
This commit is contained in:
matlabbe
2022-09-24 12:29:42 -07:00
committed by GitHub
parent 95a76cb696
commit fa31affea0
14 changed files with 685 additions and 495 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
corelib/src/Memory.cpp
View File

@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
_invertedReg(Parameters::defaultRGBDInvertedReg()),
_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -578,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
if(_invertedReg && _localBundleOnLoopClosure)
{
UWARN("%s and %s cannot be used at the same time, disabling %s...",
Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
Parameters::kRGBDInvertedReg().c_str(),
Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
_localBundleOnLoopClosure = false;
}
Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -2855,8 +2865,21 @@ Transform Memory::computeTransform(
(fromS.getWords().size() && toS.getWords().size()) ||
(!guess.isNull() && !_registrationPipeline->isImageRequired()))
{
Signature tmpFrom = fromS;
Signature tmpTo = toS;
Signature tmpFrom, tmpTo;
if(_invertedReg)
{
tmpFrom = toS;
tmpTo = fromS;
if(!guess.isNull())
{
guess = guess.inverse();
}
}
else
{
tmpFrom = fromS;
tmpTo = toS;
}
if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
{
@@ -2891,6 +2914,7 @@ Transform Memory::computeTransform(
_registrationPipeline->isImageRequired() &&
!_registrationPipeline->isScanRequired() &&
!_registrationPipeline->isUserDataRequired() &&
!_invertedReg &&
!tmpTo.getWordsDescriptors().empty() &&
!tmpTo.getWords().empty() &&
!tmpFrom.getWordsDescriptors().empty() &&
@@ -3115,6 +3139,10 @@ Transform Memory::computeTransform(
{
transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
}
if(_invertedReg && !transform.isNull())
{
transform = transform.inverse();
}
}
return transform;
}

47
corelib/src/RegistrationVis.cpp
View File

@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
_PnPFlags(Parameters::defaultVisPnPFlags()),
_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
_correspondencesApproach(Parameters::defaultVisCorType()),
_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -124,6 +125,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -287,6 +289,7 @@ Transform RegistrationVis::computeTransformationImpl(
UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
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=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -1580,6 +1583,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
@@ -1601,6 +1605,7 @@ Transform RegistrationVis::computeTransformationImpl(
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
_PnPMaxVar,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
@@ -1977,31 +1982,31 @@ Transform RegistrationVis::computeTransformationImpl(
if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
{
cv::Mat pcaData;
float cx=0, cy=0, w=0, h=0;
std::vector<CameraModel> cameraModelsTo;
if(toSignature.sensorData().stereoCameraModels().size())
{
for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
{
cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
}
}
else
{
cameraModelsTo = toSignature.sensorData().cameraModels();
}
if(_minInliersDistributionThr > 0)
{
if((toSignature.sensorData().stereoCameraModels().size() == 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
{
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModels().size()?toSignature.sensorData().stereoCameraModels()[0].left():toSignature.sensorData().cameraModels()[0];
cx = cameraModel.cx();
cy = cameraModel.cy();
w = cameraModel.imageWidth();
h = cameraModel.imageHeight();
if(w>0 && h>0)
if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
{
pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
}
else
{
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
else if(toSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().stereoCameraModels().size() > 1)
{
UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
else
{
UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
@@ -2031,12 +2036,14 @@ Transform RegistrationVis::computeTransformationImpl(
if(!pcaData.empty())
{
std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
float * ptr = pcaData.ptr<float>(i, 0);
const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
ptr[0] = (kpt.pt.x-cx) / w;
ptr[1] = (kpt.pt.y-cy) / h;
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
}
}

68
corelib/src/Rtabmap.cpp
View File

@@ -1434,32 +1434,45 @@ bool Rtabmap::process(
//============================================================
// Minimum displacement required to add to Memory
//============================================================
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
Transform t;
if(_memory->isIncremental())
{
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
const std::multimap<int, Link> & links = signature->getLinks();
if(links.size() && links.begin()->second.type() == Link::kNeighbor)
{
float x,y,z, roll,pitch,yaw;
links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
const Signature * s = _memory->getSignature(links.begin()->second.to());
UASSERT(s!=0);
// don't filter if the new node is not intermediate but previous one is
if(signature->getWeight() < 0 || s->getWeight() >= 0)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
t = links.begin()->second.transform();
}
}
}
else if(!_odomCachePoses.empty())
{
t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
}
if(!t.isNull())
{
float x,y,z, roll,pitch,yaw;
t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
bool isMoving = fabs(x) > _rgbdLinearUpdate ||
fabs(y) > _rgbdLinearUpdate ||
fabs(z) > _rgbdLinearUpdate ||
(_rgbdAngularUpdate>0.0f && (
fabs(roll) > _rgbdAngularUpdate ||
fabs(pitch) > _rgbdAngularUpdate ||
fabs(yaw) > _rgbdAngularUpdate));
if(!isMoving)
{
// This will disable global loop closure detection, only retrieval will be done.
// The location will also be deleted at the end.
smallDisplacement = true;
UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
}
}
}
if(odomVelocity.size() == 6)
{
@@ -2951,6 +2964,7 @@ bool Rtabmap::process(
cv::Mat localizationCovariance;
Transform previousMapCorrection;
bool rejectedLandmark = false;
bool delayedLocalization = false;
UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
UDEBUG("Incremental: %d", _memory->isIncremental());
UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -3441,6 +3455,7 @@ bool Rtabmap::process(
else //delayed localization (wait for more than 1 link)
{
UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
delayedLocalization = true;
rejectLocalization = true;
}
}
@@ -3954,10 +3969,21 @@ bool Rtabmap::process(
(smallDisplacement || tooFastMovement) &&
_loopClosureHypothesis.first == 0 &&
lastProximitySpaceClosureId == 0 &&
!delayedLocalization &&
(rejectedLandmark || landmarksDetected.empty()))
{
_odomCachePoses.erase(signatureRemoved);
_odomCacheConstraints.erase(signatureRemoved);
for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
{
if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
{
_odomCacheConstraints.erase(iter++);
}
else
{
++iter;
}
}
}
// Pass this point signature should not be used, since it could have been transferred...

202
corelib/src/util3d_motion_estimation.cpp
View File

@@ -61,6 +61,7 @@ Transform estimateMotion3DTo2D(
double reprojError,
int flagsPnP,
int refineIterations,
float maxVariance,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
@@ -150,69 +151,59 @@ Transform estimateMotion3DTo2D(
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
Transform transformCameraFrame = transform * cameraModel.localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
Transform localTransformInv = cameraModel.localTransform().inverse();
Transform transformCameraFrameInv = (transform * cameraModel.localTransform()).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
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(words3B.empty() || (iter != words3B.end() && util3d::isFinite(iter->second)))
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt;
if(iter!=words3B.end())
{
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModel.cx(),
cameraModel.cy(),
cameraModel.fx(),
cameraModel.fy());
// transform in camera B frame
newPt = util3d::transformPoint(objPt, transformCameraFrameInv);
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * newPt.z*1.1; // Add 10 % error
// put back in frame of camera A
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
newPt = util3d::transformPoint(iter->second, localTransformInv);
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModel.imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModel.cx(),
cameraModel.cy(),
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
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{
UWARN("Not enough close points to compute covariance!");
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(float(oi) / float(inliers.size()) < 0.2f)
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
*covariance = cv::Mat::eye(6,6,CV_64FC1);
transform.setNull();
}
}
else if(covariance)
@@ -256,6 +247,7 @@ Transform estimateMotion3DTo2D(
double reprojError,
int flagsPnP,
int refineIterations,
float maxVariance,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
@@ -273,7 +265,7 @@ Transform estimateMotion3DTo2D(
UASSERT(cameraModels[i].isValidForProjection());
UASSERT(subImageWidth == cameraModels[i].imageWidth());
}
UASSERT(!guess.isNull());
std::vector<int> matches, inliers;
@@ -391,70 +383,60 @@ Transform estimateMotion3DTo2D(
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
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
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(words3B.empty() || (iter != words3B.end() && util3d::isFinite(iter->second)))
cv::Point3f newPt;
if(iter!=words3B.end() && util3d::isFinite(iter->second))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt;
if(iter!=words3B.end())
{
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
//compute from projection
int cameraIndex = cameraIndexes[inliers[i]];
Transform transformCameraFrame = transform * cameraModels[cameraIndex].localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModels[cameraIndex].cx(),
cameraModels[cameraIndex].cy(),
cameraModels[cameraIndex].fx(),
cameraModels[cameraIndex].fy());
// transform in camera B frame
newPt = util3d::transformPoint(objPt, transformCameraFrameInv);
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * newPt.z*1.1; // Add 10 % error
// put back in frame of camera A
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
newPt = util3d::transformPoint(iter->second, cameraModels[cameraIndex].localTransform().inverse());
}
else
{
//compute from projection
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModels[cameraIndex].cx(),
cameraModels[cameraIndex].cy(),
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
}
errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x, objPt.y, objPt.z, 0);
Eigen::Vector4f v2(newPt.x, newPt.y, newPt.z, 0);
errorSqrdAngles[i] = pcl::getAngle3D(v1, v2);
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{
UWARN("Not enough close points to compute covariance!");
}
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr_lin = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_lin));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr_lin;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
double median_error_sqr_ang = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr_ang));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr_ang;
if(float(oi) / float(inliers.size()) < 0.2f)
if(maxVariance > 0 && median_error_sqr_lin > maxVariance)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
UWARN("Rejected PnP transform, variance is too high! %f > %f!", median_error_sqr_lin, maxVariance);
*covariance = cv::Mat::eye(6,6,CV_64FC1);
transform.setNull();
}
}
}