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

Added parameter "RGBD/ProximityAngle"

Browse Source 
Updated default of GuessMotion parameters
Proximity detection by time: added guess transform based on optimized poses
RegistrationVis: ByPass OpticalFlow if no images are detected (use features matching instead if features are set)
solvePnPRansac() refining: using std dev instead of variance
Statistics panel: creating Camera/GUI/Odometry panels without the need to start detection (issue #51)
Statistics: Adding a GT panel when ground truth poses are detected with RMSE info
This commit is contained in:
matlabbe
2016-03-02 18:11:19 -05:00
parent bcd96a97a3
commit eefd557ab4
11 changed files with 240 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/src/CameraRGB.cpp
View File

@@ -125,7 +125,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
_count = 0;
_countScan = 0;
_captureDelay = 0.0;
_captureTimer.restart();
UDEBUG("");
if(_dir)
@@ -404,6 +403,8 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
}
_captureTimer.restart();
return success;
}

7
corelib/src/Memory.cpp
View File

@@ -2047,6 +2047,7 @@ void Memory::removeRawData(int id)
Transform Memory::computeTransform(
int fromId,
int toId,
Transform guess,
RegistrationInfo * info)
{
const Signature * fromS = this->getSignature(fromId);
@@ -2084,13 +2085,17 @@ Transform Memory::computeTransform(
tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());
}
Transform guess = Transform::getIdentity();
if(!_registrationPipeline->isImageRequired())
{
// no visual in the pipeline, make visual registration for guess
RegistrationVis regVis(parameters_);
guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
}
else if(guess.isNull())
{
guess.setIdentity();
}
if(!guess.isNull())
{
transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info);

9
corelib/src/RegistrationVis.cpp
View File

@@ -288,7 +288,9 @@ Transform RegistrationVis::computeTransformationImpl(
std::multimap<int, cv::Point3f> words3To;
std::multimap<int, cv::Mat> wordsDescFrom;
std::multimap<int, cv::Mat> wordsDescTo;
if(_correspondencesApproach == 1) //Optical Flow
if(_correspondencesApproach == 1 && //Optical Flow
!fromSignature.sensorData().imageRaw().empty() &&
!toSignature.sensorData().imageRaw().empty())
{
UDEBUG("");
// convert to grayscale
@@ -1054,6 +1056,11 @@ Transform RegistrationVis::computeTransformationImpl(
matchesCount = (int)matches[0].size();
}
}
else if(toSignature.sensorData().isValid())
{
UWARN("Missing correspondences for registration. toWords = %d toImageEmpty=%d",
(int)toSignature.getWords().size(), toSignature.sensorData().imageRaw().empty()?1:0);
}
info.inliers = inliersCount;
info.matches = matchesCount;

17
corelib/src/Rtabmap.cpp
View File

@@ -101,6 +101,7 @@ Rtabmap::Rtabmap() :
_proximityFilteringRadius(Parameters::defaultRGBDProximityPathFilteringRadius()),
_proximityRawPosesUsed(Parameters::defaultRGBDProximityPathRawPosesUsed()),
_proximityScansMerged(Parameters::defaultRGBDProximityPathScansMerged()),
_proximityAngle(Parameters::defaultRGBDProximityAngle()*M_PI/180.0f),
_databasePath(""),
_optimizeFromGraphEnd(Parameters::defaultRGBDOptimizeFromGraphEnd()),
_optimizationMaxLinearError(Parameters::defaultRGBDOptimizeMaxError()),
@@ -409,6 +410,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kRGBDProximityPathFilteringRadius(), _proximityFilteringRadius);
Parameters::parse(parameters, Parameters::kRGBDProximityPathRawPosesUsed(), _proximityRawPosesUsed);
Parameters::parse(parameters, Parameters::kRGBDProximityPathScansMerged(), _proximityScansMerged);
Parameters::parse(parameters, Parameters::kRGBDProximityAngle(), _proximityAngle);
_proximityAngle *= M_PI/180.0f;
Parameters::parse(parameters, Parameters::kRGBDOptimizeFromGraphEnd(), _optimizeFromGraphEnd);
Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxLinearError);
Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
@@ -1169,7 +1172,13 @@ bool Rtabmap::process(
std::string rejectedMsg;
UDEBUG("Check local transform between %d and %d", signature->id(), *iter);
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), *iter, &info);
Transform guess;
if(_optimizedPoses.find(*iter) != _optimizedPoses.end())
{
guess = newPose.inverse() * _optimizedPoses.at(*iter);
}
Transform transform = _memory->computeTransform(signature->id(), *iter, guess, &info);
if(!transform.isNull())
{
@@ -1722,7 +1731,7 @@ bool Rtabmap::process(
info.variance = 1.0f;
if(_rgbdSlamMode)
{
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, &info);
transform = _memory->computeTransform(signature->id(), _loopClosureHypothesis.first, Transform(), &info);
loopClosureVisualInliers = info.inliers;
rejectedHypothesis = transform.isNull();
if(rejectedHypothesis)
@@ -1814,7 +1823,7 @@ bool Rtabmap::process(
//find the nearest pose on the path looking in the same direction
path.insert(std::make_pair(signature->id(), _optimizedPoses.at(signature->id())));
path = graph::getPosesInRadius(signature->id(), path, _localRadius, M_PI/4);
path = graph::getPosesInRadius(signature->id(), path, _localRadius, _proximityAngle);
int nearestId = rtabmap::graph::findNearestNode(path, _optimizedPoses.at(signature->id()));
if(nearestId > 0)
{
@@ -1824,7 +1833,7 @@ bool Rtabmap::process(
_optimizedPoses.at(signature->id()).getDistanceSquared(_optimizedPoses.at(nearestId)) < _proximityFilteringRadius*_proximityFilteringRadius))
{
RegistrationInfo info;
Transform transform = _memory->computeTransform(signature->id(), nearestId, &info);
Transform transform = _memory->computeTransform(signature->id(), nearestId, Transform(), &info);
if(!transform.isNull())
{
if(_proximityFilteringRadius <= 0 || transform.getNormSquared() <= _proximityFilteringRadius*_proximityFilteringRadius)

7
corelib/src/util3d_motion_estimation.cpp
View File

@@ -92,6 +92,9 @@ Transform estimateMotion3DTo2D(
imagePoints.resize(oi);
matches.resize(oi);
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size());
if((int)matches.size() >= minInliers)
{
//PnPRansac
@@ -341,9 +344,7 @@ void solvePnPRansac(
float inlierThreshold = reprojectionError;
if(inliers.size() >= 4 && refineIterations>0)
{
float inlier_distance_threshold_sqr = inlierThreshold * inlierThreshold;
float error_threshold = inlierThreshold;
float sigma_sqr = refineSigma * refineSigma;
int refine_iterations = 0;
bool inlier_changed = false, oscillating = false;
std::vector<int> new_inliers, prev_inliers = inliers;
@@ -393,7 +394,7 @@ void solvePnPRansac(
// Estimate the variance and the new threshold
float m = uMean(err.data(), err.size());
float variance = uVariance(err.data(), err.size());
error_threshold = sqrt (std::min (inlier_distance_threshold_sqr, sigma_sqr * variance));
error_threshold = std::min(inlierThreshold, refineSigma * float(sqrt(variance)));
UDEBUG ("RANSAC refineModel: New estimated error threshold: %f (variance=%f mean=%f) on iteration %d out of %d.",
error_threshold, variance, m, refine_iterations, refineIterations);