i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-11 05:50:21 +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
co-authored by mathieu86
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
corelib/src/RegistrationVis.cpp
+27 -20
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();
}
}