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

OdomF2M: added Odom/KeyFrameThr=0.5 and OdomF2M/MaxNewFeatures=0 parameters. Set Odom/GuessMotion to false by default.

Browse Source
This commit is contained in:
matlabbe
2016-03-01 20:47:46 -05:00
parent b6fb947310
commit 264146d8b3
10 changed files with 180 additions and 100 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/OdometryF2F.h
View File

@@ -50,7 +50,7 @@ private:
private: private:
//Parameters: //Parameters:
int keyFrameThr_; float keyFrameThr_;
Registration * registrationPipeline_; Registration * registrationPipeline_;
Signature refFrame_; Signature refFrame_;

2
corelib/include/rtabmap/core/OdometryF2M.h
View File

@@ -51,6 +51,8 @@ private:
private: private:
//Parameters //Parameters
int maximumMapSize_; int maximumMapSize_;
float keyFrameThr_;
int maxNewFeatures_;
std::string fixedMapPath_; std::string fixedMapPath_;
RegistrationVis * regVis_; RegistrationVis * regVis_;

15
corelib/include/rtabmap/core/Parameters.h
View File

@@ -358,11 +358,13 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw)."); RTABMAP_PARAM(Odom, ParticleLambdaR, float, 100, "Lambda of rotational components (roll,pitch,yaw).");
RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value."); RTABMAP_PARAM(Odom, KalmanProcessNoise, float, 0.001, "Process noise covariance value.");
RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value."); RTABMAP_PARAM(Odom, KalmanMeasurementNoise, float, 0.01, "Process measurement covariance value.");
RTABMAP_PARAM(Odom, GuessMotion, bool, true, "Guess next transformation from the last motion computed."); RTABMAP_PARAM(Odom, GuessMotion, bool, false, "Guess next transformation from the last motion computed.");
RTABMAP_PARAM(Odom, KeyFrameThr, float, 0.5, "Create a new keyframe when the number of inliers drops under this ratio of features in last frame. Setting the value to 0 means that a keyframe is created for each processed frame.");
// Odometry Bag-of-words // Odometry Bag-of-words
RTABMAP_PARAM(OdomF2M, MaxSize, int, 1000, "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words."); RTABMAP_PARAM(OdomF2M, MaxSize, int, 1000, "Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
RTABMAP_PARAM_STR(OdomF2M, FixedMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.") RTABMAP_PARAM(OdomF2M, MaxNewFeatures, int, 0, "Maximum features added to local map (nearest to farthest) from a new key-frame. 0 means no limit.");
RTABMAP_PARAM_STR(OdomF2M, FixedMapPath, "", "Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP estimation is used.")
// Odometry Mono // Odometry Mono
RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step."); RTABMAP_PARAM(OdomMono, InitMinFlow, float, 100, "Minimum optical flow required for the initialization step.");
@@ -370,9 +372,6 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum."); RTABMAP_PARAM(OdomMono, MinTranslation, float, 0.02, "Minimum translation to add new points to local map. On initialization, translation x 5 is used as the minimum.");
RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map."); RTABMAP_PARAM(OdomMono, MaxVariance, float, 0.01, "Maximum variance to add new points to local map.");
// Odometry Optical Flow
RTABMAP_PARAM(OdomF2F, KeyFrameThr, int, 500, "Create a new keyframe when the number of inliers drops under this threshold. Setting the value to 0 means that a keyframe is created for each processed frame.");
// Common registration parameters // Common registration parameters
RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers."); RTABMAP_PARAM(Reg, VarianceFromInliersCount, bool, false, "Set variance as the inverse of the number of inliers. Otherwise, the variance is computed as the average 3D position error of the inliers.");
RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp"); RTABMAP_PARAM(Reg, Strategy, int, 0, "0=Vis, 1=Icp, 2=VisIcp");
@@ -385,11 +384,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, RefineIterations, int, 5, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined."); RTABMAP_PARAM(Vis, RefineIterations, int, 5, "[Vis/EstimationType = 0] Number of iterations used to refine the transformation found by RANSAC. 0 means that the transformation is not refined.");
RTABMAP_PARAM(Vis, PnPReprojError, float, 2.0, "[Vis/EstimationType = 1] PnP reprojection error."); RTABMAP_PARAM(Vis, PnPReprojError, float, 2.0, "[Vis/EstimationType = 1] PnP reprojection error.");
RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P"); RTABMAP_PARAM(Vis, PnPFlags, int, 1, "[Vis/EstimationType = 1] PnP flags: 0=Iterative, 1=EPNP, 2=P3P");
#ifdef RTABMAP_OPENCV3
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 0, "[Vis/EstimationType = 1] Refine iterations.");
#else
RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, "[Vis/EstimationType = 1] Refine iterations."); RTABMAP_PARAM(Vis, PnPRefineIterations, int, 1, "[Vis/EstimationType = 1] Refine iterations.");
#endif
RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation."); RTABMAP_PARAM(Vis, EpipolarGeometryVar, float, 0.02, "[Vis/EstimationType = 2] Epipolar geometry maximum variance to accept the transformation.");
RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation."); RTABMAP_PARAM(Vis, MinInliers, int, 10, "Minimum feature correspondences to compute/accept the transformation.");
RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform."); RTABMAP_PARAM(Vis, Iterations, int, 100, "Maximum iterations to compute the transform.");
@@ -404,7 +399,7 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow"); RTABMAP_PARAM(Vis, CorType, int, 0, "Correspondences computation approach: 0=Features Matching, 1=Optical Flow");
RTABMAP_PARAM(Vis, CorNNType, int, 3, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach."); RTABMAP_PARAM(Vis, CorNNType, int, 3, "[Vis/CorrespondenceType=0] kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach."); RTABMAP_PARAM(Vis, CorNNDR, float, 0.8, "[Vis/CorrespondenceType=0] NNDR: nearest neighbor distance ratio. Used for features matching approach.");
RTABMAP_PARAM(Vis, CorGuessWinSize, int, 0, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled."); RTABMAP_PARAM(Vis, CorGuessWinSize, int, 16, "[Vis/CorrespondenceType=0] Matching window size (pixels) around projected points when a guess transform is provided to find correspondences. 0 means disabled.");
RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowWinSize, int, 16, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowIterations, int, 30, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");
RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach."); RTABMAP_PARAM(Vis, CorFlowEps, float, 0.01, "[Vis/CorrespondenceType=1] See cv::calcOpticalFlowPyrLK(). Used for optical flow approach.");

2
corelib/src/Memory.cpp
View File

@@ -3167,7 +3167,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
UDEBUG("Intermediate node detected, don't extract features!"); UDEBUG("Intermediate node detected, don't extract features!");
} }
} }
else if(!isIntermediateNode) else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{ {
UINFO("Use odometry features"); UINFO("Use odometry features");
keypoints = data.keypoints(); keypoints = data.keypoints();

41
corelib/src/Odometry.cpp
View File

@@ -77,7 +77,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()), _kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_resetCurrentCount(0), _resetCurrentCount(0),
previousStamp_(0), previousStamp_(0),
previousVelocityTransform_(Transform::getIdentity()),
distanceTravelled_(0) distanceTravelled_(0)
{ {
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown); Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -131,7 +130,7 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose) void Odometry::reset(const Transform & initialPose)
{ {
previousVelocityTransform_.setIdentity(); previousVelocityTransform_.setNull();
previousGroundTruthPose_.setNull(); previousGroundTruthPose_.setNull();
_resetCurrentCount = 0; _resetCurrentCount = 0;
previousStamp_ = 0; previousStamp_ = 0;
@@ -196,10 +195,10 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
return Transform(); return Transform();
} }
double dt = data.stamp() - previousStamp_; double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess; Transform guess;
if( !previousVelocityTransform_.isNull() && UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
!previousVelocityTransform_.isIdentity()) if(!previousVelocityTransform_.isNull())
{ {
if(guessFromMotion_) if(guessFromMotion_)
{ {
@@ -222,8 +221,6 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
predictKalmanFilter(dt); predictKalmanFilter(dt);
} }
} }
previousVelocityTransform_.setNull();
previousStamp_ = data.stamp();
UTimer time; UTimer time;
Transform t = this->computeTransform(data, guess, info); Transform t = this->computeTransform(data, guess, info);
@@ -268,16 +265,16 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
{ {
if(_filteringStrategy == 1) if(_filteringStrategy == 1)
{ {
if(_pose.isIdentity()) if(previousVelocityTransform_.isNull())
{ {
// reset Kalman // reset Kalman
if(t.isIdentity()) if(dt)
{ {
initKalmanFilter(); initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
} }
else else
{ {
initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw); initKalmanFilter(t);
} }
} }
else else
@@ -290,7 +287,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
{ {
// Particle filtering // Particle filtering
UASSERT(particleFilters_.size()==6); UASSERT(particleFilters_.size()==6);
if(_pose.isIdentity()) if(previousVelocityTransform_.isNull())
{ {
particleFilters_[0]->init(vx); particleFilters_[0]->init(vx);
particleFilters_[1]->init(vy); particleFilters_[1]->init(vy);
@@ -359,14 +356,27 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
} }
} }
t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt); if(dt)
{
t = Transform(vx*dt, vy*dt, vz*dt, vroll*dt, vpitch*dt, vyaw*dt);
}
else
{
t = Transform(vx, vy, vz, vroll, vpitch, vyaw);
}
if(info) if(info)
{ {
info->transformFiltered = t; info->transformFiltered = t;
} }
} }
previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw); previousStamp_ = data.stamp();
previousVelocityTransform_.setNull();
if(dt)
{
previousVelocityTransform_ = Transform(vx, vy, vz, vroll, vpitch, vyaw);
}
if(info) if(info)
{ {
@@ -388,6 +398,9 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
} }
} }
previousVelocityTransform_.setNull();
previousStamp_ = 0;
return Transform(); return Transform();
} }

24
corelib/src/OdometryF2F.cpp
View File

@@ -29,18 +29,21 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/OdometryInfo.h" #include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/Registration.h" #include "rtabmap/core/Registration.h"
#include "rtabmap/core/EpipolarGeometry.h" #include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h" #include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h" #include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap { namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) : OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters), Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()), keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
motionSinceLastKeyFrame_(Transform::getIdentity()) motionSinceLastKeyFrame_(Transform::getIdentity())
{ {
registrationPipeline_ = Registration::create(parameters); registrationPipeline_ = Registration::create(parameters);
Parameters::parse(parameters, Parameters::kOdomF2FKeyFrameThr(), keyFrameThr_); Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
UASSERT(keyFrameThr_>=0.0f && keyFrameThr_<=1.0f);
} }
OdometryF2F::~OdometryF2F() OdometryF2F::~OdometryF2F()
@@ -80,8 +83,9 @@ Transform OdometryF2F::computeTransform(
Signature newFrame(data); Signature newFrame(data);
if(refFrame_.sensorData().isValid()) if(refFrame_.sensorData().isValid())
{ {
Signature tmpRefFrame = refFrame_;
output = registrationPipeline_->computeTransformationMod( output = registrationPipeline_->computeTransformationMod(
refFrame_, tmpRefFrame,
newFrame, newFrame,
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(), !guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
&regInfo); &regInfo);
@@ -89,7 +93,7 @@ Transform OdometryF2F::computeTransform(
if(info && this->isInfoDataFilled()) if(info && this->isInfoDataFilled())
{ {
std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs; std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
EpipolarGeometry::findPairsUnique(refFrame_.getWords(), newFrame.getWords(), pairs); EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
info->refCorners.resize(pairs.size()); info->refCorners.resize(pairs.size());
info->newCorners.resize(pairs.size()); info->newCorners.resize(pairs.size());
std::map<int, int> idToIndex; std::map<int, int> idToIndex;
@@ -110,6 +114,12 @@ Transform OdometryF2F::computeTransform(
info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]); info->cornerInliers[i] = idToIndex.at(regInfo.inliersIDs[i]);
} }
Transform t = this->getPose()*motionSinceLastKeyFrame_.inverse();
for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
{
info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
}
info->words = newFrame.getWords();
} }
} }
else else
@@ -126,7 +136,7 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output; motionSinceLastKeyFrame_ *= output;
// new key-frame? // new key-frame?
if(keyFrameThr_ <= 0 || (int)regInfo.inliers <= keyFrameThr_) if(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(refFrame_.sensorData().keypoints().size()))
{ {
UDEBUG("Update key frame"); UDEBUG("Update key frame");
int features = newFrame.getWordsDescriptors().size(); int features = newFrame.getWordsDescriptors().size();
@@ -187,14 +197,14 @@ Transform OdometryF2F::computeTransform(
info->inliers = regInfo.inliers; info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio; info->icpInliersRatio = regInfo.icpInliersRatio;
info->matches = regInfo.matches; info->matches = regInfo.matches;
info->features = refFrame_.sensorData().keypoints().size(); info->features = newFrame.sensorData().keypoints().size();
} }
UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s", UINFO("Odom update time = %fs lost=%s inliers=%d, ref frame corners=%d, transform accepted=%s",
timer.elapsed(), timer.elapsed(),
output.isNull()?"true":"false", output.isNull()?"true":"false",
(int)regInfo.inliers, (int)regInfo.inliers,
(int)refFrame_.sensorData().keypoints().size(), (int)newFrame.sensorData().keypoints().size(),
!output.isNull()?"true":"false"); !output.isNull()?"true":"false");
return output; return output;

55
corelib/src/OdometryF2M.cpp
View File

@@ -55,6 +55,8 @@ namespace rtabmap {
OdometryF2M::OdometryF2M(const ParametersMap & parameters) : OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Odometry(parameters), Odometry(parameters),
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()), maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()), fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)), regVis_(new RegistrationVis(parameters)),
map_(new Signature(-1)), map_(new Signature(-1)),
@@ -62,7 +64,12 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
{ {
UDEBUG(""); UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_); Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_); Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
if(!fixedMapPath_.empty()) if(!fixedMapPath_.empty())
{ {
@@ -215,7 +222,8 @@ Transform OdometryF2M::computeTransform(
if(!transform.isNull()) if(!transform.isNull())
{ {
if(fixedMapPath_.empty()) if(fixedMapPath_.empty() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
{ {
output = transform; output = transform;
@@ -229,14 +237,32 @@ Transform OdometryF2M::computeTransform(
Transform t = this->getPose()*output; Transform t = this->getPose()*output;
UASSERT(mapPoints.size() == mapDescriptors.size()); UASSERT(mapPoints.size() == mapDescriptors.size());
UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str()); UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
std::list<int> newIds = uUniqueKeys(lastFrame_->getWordsDescriptors());
for(std::list<int>::iterator iter=newIds.begin(); iter!=newIds.end(); ++iter) // sort by feature response
std::multimap<float, std::pair<int, cv::Point3f> > newIds;
int lastId = 0;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D)
{ {
if(mapPoints.find(*iter) == mapPoints.end() && util3d::isFinite(lastFrame_->getWords3().find(*iter)->second)) if(iter == lastFrame_->getWords3().begin() ||
(iter != lastFrame_->getWords3().begin() && lastId != iter->first))
{ {
mapPoints.insert(std::make_pair(*iter, util3d::transformPoint(lastFrame_->getWords3().find(*iter)->second, t))); newIds.insert(std::make_pair(iter2D->second.response, std::make_pair(iter->first, iter->second)));
mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second)); lastId = iter->first;
++added; }
}
for(std::multimap<float, std::pair<int, cv::Point3f> >::reverse_iterator iter=newIds.rbegin(); iter!=newIds.rend(); ++iter)
{
if(maxNewFeatures_ == 0 || added < maxNewFeatures_)
{
if(mapPoints.find(iter->second.first) == mapPoints.end() && util3d::isFinite(iter->second.second))
{
mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(iter->second.second, t)));
mapDescriptors.insert(std::make_pair(iter->second.first, lastFrame_->getWordsDescriptors().find(iter->second.first)->second));
++added;
}
} }
} }
@@ -274,6 +300,13 @@ Transform OdometryF2M::computeTransform(
output = transform; output = transform;
} }
} }
if(this->isInfoDataFilled())
{
// use tmpMap instead of map_ to make sure that correspondences with the new frame matches
info->localMapSize = (int)tmpMap.getWords3().size();
info->localMap = uMultimapToMap(tmpMap.getWords3());
}
} }
else else
{ {
@@ -312,6 +345,12 @@ Transform OdometryF2M::computeTransform(
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels()); map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel()); map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
} }
if(this->isInfoDataFilled())
{
info->localMapSize = (int)map_->getWords3().size();
info->localMap = uMultimapToMap(map_->getWords3());
}
} }
map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features map_->sensorData().setFeatures(std::vector<cv::KeyPoint>(), cv::Mat()); // clear sensorData features
@@ -329,13 +368,11 @@ Transform OdometryF2M::computeTransform(
info->inliers = regInfo.inliers; info->inliers = regInfo.inliers;
info->matches = regInfo.matches; info->matches = regInfo.matches;
info->features = nFeatures; info->features = nFeatures;
info->localMapSize = (int)map_->getWords3().size();
if(this->isInfoDataFilled()) if(this->isInfoDataFilled())
{ {
info->wordMatches = regInfo.matchesIDs; info->wordMatches = regInfo.matchesIDs;
info->wordInliers = regInfo.inliersIDs; info->wordInliers = regInfo.inliersIDs;
info->localMap = uMultimapToMap(map_->getWords3());
} }
} }

3
corelib/src/Parameters.cpp
View File

@@ -144,11 +144,12 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize()))); removedParameters_.insert(std::make_pair("OdomLocalMap/HistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath()))); removedParameters_.insert(std::make_pair("OdomLocalMap/FixedMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion()))); removedParameters_.insert(std::make_pair("OdomF2F/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
removedParameters_.insert(std::make_pair("OdomF2F/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
// 0.11.0 // 0.11.0
removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize()))); removedParameters_.insert(std::make_pair("OdomBow/LocalHistorySize", std::make_pair(true, Parameters::kOdomF2MMaxSize())));
removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath()))); removedParameters_.insert(std::make_pair("OdomBow/FixedLocalMapPath", std::make_pair(true, Parameters::kOdomF2MFixedMapPath())));
removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr", std::make_pair(true, Parameters::kOdomF2FKeyFrameThr()))); removedParameters_.insert(std::make_pair("OdomFlow/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion()))); removedParameters_.insert(std::make_pair("OdomFlow/GuessMotion", std::make_pair(true, Parameters::kOdomGuessMotion())));
removedParameters_.insert(std::make_pair("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures()))); removedParameters_.insert(std::make_pair("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures())));

30
guilib/src/PreferencesDialog.cpp
View File

@@ -670,12 +670,18 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str()); _ui->checkBox_localSpacePathOdomPosesUsed->setObjectName(Parameters::kRGBDProximityPathRawPosesUsed().c_str());
_ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str()); _ui->checkBox_localSpaceAssembleScans->setObjectName(Parameters::kRGBDProximityPathScansMerged().c_str());
_ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str()); _ui->rgdb_localImmunizationRatio->setObjectName(Parameters::kRGBDLocalImmunizationRatio().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
// Registration
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().c_str());
//RegistrationVis
_ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str()); _ui->loopClosure_bowMinInliers->setObjectName(Parameters::kVisMinInliers().c_str());
_ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str()); _ui->loopClosure_bowInlierDistance->setObjectName(Parameters::kVisInlierDistance().c_str());
_ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str()); _ui->loopClosure_bowIterations->setObjectName(Parameters::kVisIterations().c_str());
_ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str()); _ui->loopClosure_bowRefineIterations->setObjectName(Parameters::kVisRefineIterations().c_str());
_ui->loopClosure_bowForce2D->setObjectName(Parameters::kRegForce3DoF().c_str());
_ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str()); _ui->loopClosure_estimationType->setObjectName(Parameters::kVisEstimationType().c_str());
connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int))); connect(_ui->loopClosure_estimationType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureEstimation, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType()); _ui->stackedWidget_loopClosureEstimation->setCurrentIndex(Parameters::defaultVisEstimationType());
@@ -684,11 +690,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str()); _ui->loopClosure_pnpReprojError->setObjectName(Parameters::kVisPnPReprojError().c_str());
_ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str()); _ui->loopClosure_pnpFlags->setObjectName(Parameters::kVisPnPFlags().c_str());
_ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str()); _ui->loopClosure_pnpRefineIterations->setObjectName(Parameters::kVisPnPRefineIterations().c_str());
_ui->loopClosure_bowVarianceFromInliersCount->setObjectName(Parameters::kRegVarianceFromInliersCount().c_str());
_ui->comboBox_registrationStrategy->setObjectName(Parameters::kRegStrategy().c_str());
_ui->loopClosure_reextract->setObjectName(Parameters::kRGBDLoopClosureReextractFeatures().c_str());
_ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str()); _ui->loopClosure_correspondencesType->setObjectName(Parameters::kVisCorType().c_str());
connect(_ui->loopClosure_correspondencesType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureCorrespondences, SLOT(setCurrentIndex(int))); connect(_ui->loopClosure_correspondencesType, SIGNAL(currentIndexChanged(int)), _ui->stackedWidget_loopClosureCorrespondences, SLOT(setCurrentIndex(int)));
_ui->stackedWidget_loopClosureCorrespondences->setCurrentIndex(Parameters::defaultVisCorType()); _ui->stackedWidget_loopClosureCorrespondences->setCurrentIndex(Parameters::defaultVisCorType());
@@ -703,10 +704,14 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str()); _ui->subpix_winSize->setObjectName(Parameters::kVisSubPixWinSize().c_str());
_ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str()); _ui->subpix_iterations->setObjectName(Parameters::kVisSubPixIterations().c_str());
_ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str()); _ui->subpix_eps->setObjectName(Parameters::kVisSubPixEps().c_str());
_ui->odom_flow_winSize_2->setObjectName(Parameters::kVisCorFlowWinSize().c_str());
_ui->odom_flow_maxLevel_2->setObjectName(Parameters::kVisCorFlowMaxLevel().c_str());
_ui->odom_flow_iterations_2->setObjectName(Parameters::kVisCorFlowIterations().c_str());
_ui->odom_flow_eps_2->setObjectName(Parameters::kVisCorFlowEps().c_str());
//RegistrationIcp
_ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str()); _ui->globalDetection_icpMaxTranslation->setObjectName(Parameters::kIcpMaxTranslation().c_str());
_ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str()); _ui->globalDetection_icpMaxRotation->setObjectName(Parameters::kIcpMaxRotation().c_str());
_ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str()); _ui->loopClosure_icpVoxelSize->setObjectName(Parameters::kIcpVoxelSize().c_str());
_ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str()); _ui->loopClosure_icpDownsamplingStep->setObjectName(Parameters::kIcpDownsamplingStep().c_str());
_ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str()); _ui->loopClosure_icpMaxCorrespondenceDistance->setObjectName(Parameters::kIcpMaxCorrespondenceDistance().c_str());
@@ -725,20 +730,15 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str()); _ui->odom_holonomic->setObjectName(Parameters::kOdomHolonomic().c_str());
_ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str()); _ui->odom_fillInfoData->setObjectName(Parameters::kOdomFillInfoData().c_str());
_ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str()); _ui->odom_dataBufferSize->setObjectName(Parameters::kOdomImageBufferSize().c_str());
_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomKeyFrameThr().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
//Odometry Frame to Map //Odometry Frame to Map
_ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str()); _ui->odom_localHistory->setObjectName(Parameters::kOdomF2MMaxSize().c_str());
_ui->spinBox_odom_f2m_maxNewFeatures->setObjectName(Parameters::kOdomF2MMaxNewFeatures().c_str());
_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str()); _ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomF2MFixedMapPath().c_str());
connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath())); connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
//Odometry Frame to Frame
_ui->odom_flow_keyframeThr->setObjectName(Parameters::kOdomF2FKeyFrameThr().c_str());
_ui->odom_flow_winSize_2->setObjectName(Parameters::kVisCorFlowWinSize().c_str());
_ui->odom_flow_maxLevel_2->setObjectName(Parameters::kVisCorFlowMaxLevel().c_str());
_ui->odom_flow_iterations_2->setObjectName(Parameters::kVisCorFlowIterations().c_str());
_ui->odom_flow_eps_2->setObjectName(Parameters::kVisCorFlowEps().c_str());
_ui->odom_flow_guessMotion->setObjectName(Parameters::kOdomGuessMotion().c_str());
//Odometry Mono //Odometry Mono
_ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str()); _ui->doubleSpinBox_minFlow->setObjectName(Parameters::kOdomMonoInitMinFlow().c_str());
_ui->doubleSpinBox_minInitTranslation->setObjectName(Parameters::kOdomMonoInitMinTranslation().c_str()); _ui->doubleSpinBox_minInitTranslation->setObjectName(Parameters::kOdomMonoInitMinTranslation().c_str());

106
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>-629</y> <y>-215</y>
<width>681</width> <width>681</width>
<height>2010</height> <height>2010</height>
</rect> </rect>
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>1</number> <number>14</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -7282,7 +7282,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="0"> <item row="8" column="0">
<widget class="QSpinBox" name="odom_dataBufferSize"> <widget class="QSpinBox" name="odom_dataBufferSize">
<property name="maximum"> <property name="maximum">
<number>999999</number> <number>999999</number>
@@ -7319,7 +7319,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</item> </item>
</widget> </widget>
</item> </item>
<item row="8" column="0"> <item row="9" column="0">
<widget class="QPushButton" name="pushButton_testOdometry"> <widget class="QPushButton" name="pushButton_testOdometry">
<property name="text"> <property name="text">
<string>Test selected odometry</string> <string>Test selected odometry</string>
@@ -7339,7 +7339,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="1"> <item row="8" column="1">
<widget class="QLabel" name="label_232"> <widget class="QLabel" name="label_232">
<property name="text"> <property name="text">
<string>Data buffer size (0 means inf).</string> <string>Data buffer size (0 means inf).</string>
@@ -7414,6 +7414,32 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="7" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QDoubleSpinBox" name="odom_flow_keyframeThr">
<property name="maximum">
<double>1.000000000000000</double>
</property>
<property name="singleStep">
<double>0.100000000000000</double>
</property>
<property name="value">
<double>0.500000000000000</double>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
<item> <item>
@@ -7457,7 +7483,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="0"> <item row="2" column="0">
<widget class="QLineEdit" name="odom_fixedLocalMapPath"/> <widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
</item> </item>
<item row="0" column="0"> <item row="0" column="0">
@@ -7476,14 +7502,14 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="1"> <item row="2" column="1">
<widget class="QToolButton" name="toolButton_odomBowFixedLocalMap"> <widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
<property name="text"> <property name="text">
<string>...</string> <string>...</string>
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="2"> <item row="2" column="2">
<widget class="QLabel" name="label_239"> <widget class="QLabel" name="label_239">
<property name="text"> <property name="text">
<string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string> <string>Path to a fixed map (RTAB-Map's database) to be used for odometry. Odometry will be constraint to this map. RGB-only images can be used if odometry PnP pose estimation is activated.</string>
@@ -7496,6 +7522,35 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item row="1" column="2">
<widget class="QLabel" name="label_194">
<property name="text">
<string>Maximum features added to local map (nearest to farthest) from a new key-frame. 0 means no limit.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QSpinBox" name="spinBox_odom_f2m_maxNewFeatures">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
</layout> </layout>
</item> </item>
<item> <item>
@@ -7537,39 +7592,6 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property> </property>
</widget> </widget>
</item> </item>
<item>
<layout class="QGridLayout" name="gridLayout_37" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_196">
<property name="text">
<string>Create a new keyframe when the number of inliers drops under this threshold. Setting value to 0 means that a keyframe is created for each processed frame.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QSpinBox" name="odom_flow_keyframeThr">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>999999</number>
</property>
<property name="singleStep">
<number>1</number>
</property>
<property name="value">
<number>0</number>
</property>
</widget>
</item>
</layout>
</item>
<item> <item>
<spacer name="verticalSpacer_43"> <spacer name="verticalSpacer_43">
<property name="orientation"> <property name="orientation">
@@ -7578,7 +7600,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
<property name="sizeHint" stdset="0"> <property name="sizeHint" stdset="0">
<size> <size>
<width>20</width> <width>20</width>
<height>0</height> <height>40</height>
</size> </size>
</property> </property>
</spacer> </spacer>