mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-11 05:50:21 +08:00
Refactoring of some parameters (LocalLoopDetection* to Proximity*) and variables in code
This commit is contained in:
matlabbe
@@ -39,22 +39,22 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
namespace rtabmap {
|
||||
|
||||
RegistrationVis::RegistrationVis(const ParametersMap & parameters) :
|
||||
_bowMinInliers(Parameters::defaultVisMinInliers()),
|
||||
_bowInlierDistance(Parameters::defaultVisInlierDistance()),
|
||||
_bowIterations(Parameters::defaultVisIterations()),
|
||||
_bowRefineIterations(Parameters::defaultVisRefineIterations()),
|
||||
_bowForce2D(Parameters::defaultVisForce2D()),
|
||||
_bowEpipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
|
||||
_bowEstimationType(Parameters::defaultVisEstimationType()),
|
||||
_bowPnPReprojError(Parameters::defaultVisPnPReprojError()),
|
||||
_bowPnPFlags(Parameters::defaultVisPnPFlags()),
|
||||
_reextractNNType(Parameters::defaultVisNNType()),
|
||||
_reextractNNDR(Parameters::defaultVisNNDR()),
|
||||
_reextractFeatureType(Parameters::defaultVisFeatureType()),
|
||||
_reextractMaxWords(Parameters::defaultVisMaxFeatures()),
|
||||
_reextractMaxDepth(Parameters::defaultVisMaxDepth()),
|
||||
_reextractMinDepth(Parameters::defaultVisMinDepth()),
|
||||
_reextractRoiRatios(Parameters::defaultVisRoiRatios()),
|
||||
_minInliers(Parameters::defaultVisMinInliers()),
|
||||
_inlierDistance(Parameters::defaultVisInlierDistance()),
|
||||
_iterations(Parameters::defaultVisIterations()),
|
||||
_refineIterations(Parameters::defaultVisRefineIterations()),
|
||||
_force2D(Parameters::defaultVisForce2D()),
|
||||
_epipolarGeometryVar(Parameters::defaultVisEpipolarGeometryVar()),
|
||||
_estimationType(Parameters::defaultVisEstimationType()),
|
||||
_PnPReprojError(Parameters::defaultVisPnPReprojError()),
|
||||
_PnPFlags(Parameters::defaultVisPnPFlags()),
|
||||
_NNType(Parameters::defaultVisNNType()),
|
||||
_NNDR(Parameters::defaultVisNNDR()),
|
||||
_featureType(Parameters::defaultVisFeatureType()),
|
||||
_maxWords(Parameters::defaultVisMaxFeatures()),
|
||||
_maxDepth(Parameters::defaultVisMaxDepth()),
|
||||
_minDepth(Parameters::defaultVisMinDepth()),
|
||||
_roiRatios(Parameters::defaultVisRoiRatios()),
|
||||
_subPixWinSize(Parameters::defaultKpSubPixWinSize()),
|
||||
_subPixIterations(Parameters::defaultKpSubPixIterations()),
|
||||
_subPixEps(Parameters::defaultKpSubPixEps())
|
||||
@@ -66,29 +66,29 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
|
||||
{
|
||||
Registration::parseParameters(parameters);
|
||||
|
||||
Parameters::parse(parameters, Parameters::kVisMinInliers(), _bowMinInliers);
|
||||
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _bowInlierDistance);
|
||||
Parameters::parse(parameters, Parameters::kVisIterations(), _bowIterations);
|
||||
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _bowRefineIterations);
|
||||
Parameters::parse(parameters, Parameters::kVisForce2D(), _bowForce2D);
|
||||
Parameters::parse(parameters, Parameters::kVisEstimationType(), _bowEstimationType);
|
||||
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _bowEpipolarGeometryVar);
|
||||
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _bowPnPReprojError);
|
||||
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _bowPnPFlags);
|
||||
Parameters::parse(parameters, Parameters::kVisMinInliers(), _minInliers);
|
||||
Parameters::parse(parameters, Parameters::kVisInlierDistance(), _inlierDistance);
|
||||
Parameters::parse(parameters, Parameters::kVisIterations(), _iterations);
|
||||
Parameters::parse(parameters, Parameters::kVisRefineIterations(), _refineIterations);
|
||||
Parameters::parse(parameters, Parameters::kVisForce2D(), _force2D);
|
||||
Parameters::parse(parameters, Parameters::kVisEstimationType(), _estimationType);
|
||||
Parameters::parse(parameters, Parameters::kVisEpipolarGeometryVar(), _epipolarGeometryVar);
|
||||
Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
|
||||
Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
|
||||
|
||||
Parameters::parse(parameters, Parameters::kVisNNType(), _reextractNNType);
|
||||
Parameters::parse(parameters, Parameters::kVisNNDR(), _reextractNNDR);
|
||||
Parameters::parse(parameters, Parameters::kVisFeatureType(), _reextractFeatureType);
|
||||
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _reextractMaxWords);
|
||||
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _reextractMaxDepth);
|
||||
Parameters::parse(parameters, Parameters::kVisNNType(), _NNType);
|
||||
Parameters::parse(parameters, Parameters::kVisNNDR(), _NNDR);
|
||||
Parameters::parse(parameters, Parameters::kVisFeatureType(), _featureType);
|
||||
Parameters::parse(parameters, Parameters::kVisMaxFeatures(), _maxWords);
|
||||
Parameters::parse(parameters, Parameters::kVisMaxDepth(), _maxDepth);
|
||||
|
||||
Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
|
||||
Parameters::parse(parameters, Parameters::kKpSubPixIterations(), _subPixIterations);
|
||||
Parameters::parse(parameters, Parameters::kKpSubPixEps(), _subPixEps);
|
||||
|
||||
UASSERT_MSG(_bowMinInliers >= 1, uFormat("value=%d", _bowMinInliers).c_str());
|
||||
UASSERT_MSG(_bowInlierDistance > 0.0f, uFormat("value=%f", _bowInlierDistance).c_str());
|
||||
UASSERT_MSG(_bowIterations > 0, uFormat("value=%d", _bowIterations).c_str());
|
||||
UASSERT_MSG(_minInliers >= 1, uFormat("value=%d", _minInliers).c_str());
|
||||
UASSERT_MSG(_inlierDistance > 0.0f, uFormat("value=%f", _inlierDistance).c_str());
|
||||
UASSERT_MSG(_iterations > 0, uFormat("value=%d", _iterations).c_str());
|
||||
}
|
||||
|
||||
Transform RegistrationVis::computeTransformation(
|
||||
@@ -125,17 +125,17 @@ Transform RegistrationVis::computeTransformation(
|
||||
uInsert(customParameters, ParametersPair(Parameters::kMemSTMSize(), "0"));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpIncrementalDictionary(), "true")); // make sure it is incremental
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpNewWordsComparedTogether(), "false"));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_reextractNNType))); // bruteforce
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_reextractNNDR)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_reextractFeatureType))); // FAST/BRIEF
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_reextractMaxWords)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_reextractMaxDepth)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_reextractMinDepth)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpNNStrategy(), uNumber2Str(_NNType))); // bruteforce
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(_NNDR)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(_featureType))); // FAST/BRIEF
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(_maxWords)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(_maxDepth)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpMinDepth(), uNumber2Str(_minDepth)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixEps(), uNumber2Str(_subPixEps)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixIterations(), uNumber2Str(_subPixIterations)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpSubPixWinSize(), uNumber2Str(_subPixWinSize)));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpBadSignRatio(), "0"));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _reextractRoiRatios));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kKpRoiRatios(), _roiRatios));
|
||||
uInsert(customParameters, ParametersPair(Parameters::kMemGenerateIds(), "true"));
|
||||
|
||||
Memory memory(customParameters);
|
||||
@@ -186,7 +186,7 @@ Transform RegistrationVis::computeTransformation(
|
||||
words3To = &toSignature.getWords3();
|
||||
}
|
||||
|
||||
if(_bowEstimationType == 2) // Epipolar Geometry
|
||||
if(_estimationType == 2) // Epipolar Geometry
|
||||
{
|
||||
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
|
||||
(toSignature.sensorData().cameraModels().size() != 1 ||
|
||||
@@ -194,8 +194,8 @@ Transform RegistrationVis::computeTransformation(
|
||||
{
|
||||
UERROR("Calibrated camera required (multi-cameras not supported).");
|
||||
}
|
||||
else if((int)wordsFrom->size() >= _bowMinInliers &&
|
||||
(int)wordsTo->size() >= _bowMinInliers)
|
||||
else if((int)wordsFrom->size() >= _minInliers &&
|
||||
(int)wordsTo->size() >= _minInliers)
|
||||
{
|
||||
UASSERT(fromSignature.sensorData().stereoCameraModel().isValid() || (fromSignature.sensorData().cameraModels().size() == 1 && fromSignature.sensorData().cameraModels()[0].isValid()));
|
||||
const CameraModel & cameraModel = fromSignature.sensorData().stereoCameraModel().isValid()?fromSignature.sensorData().stereoCameraModel().left():fromSignature.sensorData().cameraModels()[0];
|
||||
@@ -207,9 +207,9 @@ Transform RegistrationVis::computeTransformation(
|
||||
*wordsTo,
|
||||
cameraModel,
|
||||
cameraTransform,
|
||||
_bowIterations,
|
||||
_bowPnPReprojError,
|
||||
_bowPnPFlags, // cv::SOLVEPNP_ITERATIVE
|
||||
_iterations,
|
||||
_PnPReprojError,
|
||||
_PnPFlags, // cv::SOLVEPNP_ITERATIVE
|
||||
1.0f,
|
||||
0.99f,
|
||||
*words3From, // for scale estimation
|
||||
@@ -219,21 +219,21 @@ Transform RegistrationVis::computeTransformation(
|
||||
|
||||
if(!cameraTransform.isNull())
|
||||
{
|
||||
if((int)inliers3D.size() >= _bowMinInliers)
|
||||
if((int)inliers3D.size() >= _minInliers)
|
||||
{
|
||||
if(variance <= _bowEpipolarGeometryVar)
|
||||
if(variance <= _epipolarGeometryVar)
|
||||
{
|
||||
transform = cameraTransform;
|
||||
}
|
||||
else
|
||||
{
|
||||
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _bowEpipolarGeometryVar, variance);
|
||||
msg = uFormat("Variance is too high! (max inlier distance=%f, variance=%f)", _epipolarGeometryVar, variance);
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _bowMinInliers);
|
||||
msg = uFormat("Not enough inliers %d < %d", (int)inliers3D.size(), _minInliers);
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
@@ -254,7 +254,7 @@ Transform RegistrationVis::computeTransformation(
|
||||
UWARN(msg.c_str());
|
||||
}
|
||||
}
|
||||
else if(_bowEstimationType == 1) // PnP
|
||||
else if(_estimationType == 1) // PnP
|
||||
{
|
||||
if(!toSignature.sensorData().stereoCameraModel().isValid() &&
|
||||
(toSignature.sensorData().cameraModels().size() != 1 ||
|
||||
@@ -269,8 +269,8 @@ Transform RegistrationVis::computeTransformation(
|
||||
else
|
||||
{
|
||||
// 3D to 2D
|
||||
if((int)words3From->size() >= _bowMinInliers &&
|
||||
(int)wordsTo->size() >= _bowMinInliers)
|
||||
if((int)words3From->size() >= _minInliers &&
|
||||
(int)wordsTo->size() >= _minInliers)
|
||||
{
|
||||
UASSERT(toSignature.sensorData().stereoCameraModel().isValid() || (toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValid()));
|
||||
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValid()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
|
||||
@@ -280,10 +280,10 @@ Transform RegistrationVis::computeTransformation(
|
||||
uMultimapToMap(*words3From),
|
||||
uMultimapToMap(*wordsTo),
|
||||
cameraModel,
|
||||
_bowMinInliers,
|
||||
_bowIterations,
|
||||
_bowPnPReprojError,
|
||||
_bowPnPFlags,
|
||||
_minInliers,
|
||||
_iterations,
|
||||
_PnPReprojError,
|
||||
_PnPFlags,
|
||||
Transform::getIdentity(),
|
||||
uMultimapToMap(*words3To),
|
||||
&variance,
|
||||
@@ -293,14 +293,14 @@ Transform RegistrationVis::computeTransformation(
|
||||
if(transform.isNull())
|
||||
{
|
||||
msg = uFormat("Not enough inliers %d/%d between %d and %d",
|
||||
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
|
||||
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
msg = uFormat("Not enough features in images (old=%d, new=%d, min=%d)",
|
||||
(int)words3From->size(), (int)wordsTo->size(), _bowMinInliers);
|
||||
(int)words3From->size(), (int)wordsTo->size(), _minInliers);
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
@@ -309,17 +309,17 @@ Transform RegistrationVis::computeTransformation(
|
||||
else
|
||||
{
|
||||
// 3D -> 3D
|
||||
if((int)words3From->size() >= _bowMinInliers &&
|
||||
(int)words3To->size() >= _bowMinInliers)
|
||||
if((int)words3From->size() >= _minInliers &&
|
||||
(int)words3To->size() >= _minInliers)
|
||||
{
|
||||
std::vector<int> inliersV;
|
||||
transform = util3d::estimateMotion3DTo3D(
|
||||
uMultimapToMap(*words3From),
|
||||
uMultimapToMap(*words3To),
|
||||
_bowMinInliers,
|
||||
_bowInlierDistance,
|
||||
_bowIterations,
|
||||
_bowRefineIterations,
|
||||
_minInliers,
|
||||
_inlierDistance,
|
||||
_iterations,
|
||||
_refineIterations,
|
||||
&variance,
|
||||
0,
|
||||
&inliersV);
|
||||
@@ -327,14 +327,14 @@ Transform RegistrationVis::computeTransformation(
|
||||
if(transform.isNull())
|
||||
{
|
||||
msg = uFormat("Not enough inliers %d/%d between %d and %d",
|
||||
inliersCount, _bowMinInliers, fromSignature.id(), toSignature.id());
|
||||
inliersCount, _minInliers, fromSignature.id(), toSignature.id());
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
msg = uFormat("Not enough 3D features in images (old=%d, new=%d, min=%d)",
|
||||
(int)words3From->size(), (int)words3To->size(), _bowMinInliers);
|
||||
(int)words3From->size(), (int)words3To->size(), _minInliers);
|
||||
UINFO(msg.c_str());
|
||||
}
|
||||
}
|
||||
@@ -353,14 +353,14 @@ Transform RegistrationVis::computeTransformation(
|
||||
roll, pitch, yaw);
|
||||
UWARN(msg.c_str());
|
||||
}
|
||||
else if(_bowForce2D)
|
||||
else if(_force2D)
|
||||
{
|
||||
UDEBUG("Forcing 2D...");
|
||||
transform = Transform(x,y,0, 0, 0, yaw);
|
||||
}
|
||||
}
|
||||
|
||||
if(_bowVarianceFromInliersCount)
|
||||
if(_varianceFromInliersCount)
|
||||
{
|
||||
variance = inliersCount > 0?1.0/double(inliersCount):1.0;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user