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

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:
//Parameters:
int keyFrameThr_;
float keyFrameThr_;
Registration * registrationPipeline_;
Signature refFrame_;

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

@@ -51,6 +51,8 @@ private:
private:
//Parameters
int maximumMapSize_;
float keyFrameThr_;
int maxNewFeatures_;
std::string fixedMapPath_;
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, KalmanProcessNoise, float, 0.001, "Process noise 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
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
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, 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
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");
@@ -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, 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");
#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.");
#endif
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, 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, 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, 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, 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.");

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!");
}
}
else if(!isIntermediateNode)
else if(_feature2D->getMaxFeatures() >= 0 && !isIntermediateNode)
{
UINFO("Use odometry features");
keypoints = data.keypoints();

41
corelib/src/Odometry.cpp
View File

@@ -77,7 +77,6 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_kalmanMeasurementNoise(Parameters::defaultOdomKalmanMeasurementNoise()),
_resetCurrentCount(0),
previousStamp_(0),
previousVelocityTransform_(Transform::getIdentity()),
distanceTravelled_(0)
{
Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
@@ -131,7 +130,7 @@ Odometry::~Odometry()
void Odometry::reset(const Transform & initialPose)
{
previousVelocityTransform_.setIdentity();
previousVelocityTransform_.setNull();
previousGroundTruthPose_.setNull();
_resetCurrentCount = 0;
previousStamp_ = 0;
@@ -196,10 +195,10 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
return Transform();
}
double dt = data.stamp() - previousStamp_;
double dt = previousStamp_>0.0f?data.stamp() - previousStamp_:0.0;
Transform guess;
if( !previousVelocityTransform_.isNull() &&
!previousVelocityTransform_.isIdentity())
UASSERT(dt>0.0 || (dt == 0.0 && previousVelocityTransform_.isNull()));
if(!previousVelocityTransform_.isNull())
{
if(guessFromMotion_)
{
@@ -222,8 +221,6 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
predictKalmanFilter(dt);
}
}
previousVelocityTransform_.setNull();
previousStamp_ = data.stamp();
UTimer time;
Transform t = this->computeTransform(data, guess, info);
@@ -268,16 +265,16 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
if(_filteringStrategy == 1)
{
if(_pose.isIdentity())
if(previousVelocityTransform_.isNull())
{
// reset Kalman
if(t.isIdentity())
if(dt)
{
initKalmanFilter();
initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
}
else
{
initKalmanFilter(t, vx,vy,vz,vroll,vpitch,vyaw);
initKalmanFilter(t);
}
}
else
@@ -290,7 +287,7 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
{
// Particle filtering
UASSERT(particleFilters_.size()==6);
if(_pose.isIdentity())
if(previousVelocityTransform_.isNull())
{
particleFilters_[0]->init(vx);
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)
{
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)
{
@@ -388,6 +398,9 @@ Transform Odometry::process(SensorData & data, OdometryInfo * info)
}
}
previousVelocityTransform_.setNull();
previousStamp_ = 0;
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/Registration.h"
#include "rtabmap/core/EpipolarGeometry.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
#include "rtabmap/utilite/UTimer.h"
#include "rtabmap/utilite/UStl.h"
namespace rtabmap {
OdometryF2F::OdometryF2F(const ParametersMap & parameters) :
Odometry(parameters),
keyFrameThr_(Parameters::defaultOdomF2FKeyFrameThr()),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
motionSinceLastKeyFrame_(Transform::getIdentity())
{
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()
@@ -80,8 +83,9 @@ Transform OdometryF2F::computeTransform(
Signature newFrame(data);
if(refFrame_.sensorData().isValid())
{
Signature tmpRefFrame = refFrame_;
output = registrationPipeline_->computeTransformationMod(
refFrame_,
tmpRefFrame,
newFrame,
!guess.isNull()?motionSinceLastKeyFrame_*guess:Transform(),
&regInfo);
@@ -89,7 +93,7 @@ Transform OdometryF2F::computeTransform(
if(info && this->isInfoDataFilled())
{
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->newCorners.resize(pairs.size());
std::map<int, int> idToIndex;
@@ -110,6 +114,12 @@ Transform OdometryF2F::computeTransform(
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
@@ -126,7 +136,7 @@ Transform OdometryF2F::computeTransform(
motionSinceLastKeyFrame_ *= output;
// 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");
int features = newFrame.getWordsDescriptors().size();
@@ -187,14 +197,14 @@ Transform OdometryF2F::computeTransform(
info->inliers = regInfo.inliers;
info->icpInliersRatio = regInfo.icpInliersRatio;
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",
timer.elapsed(),
output.isNull()?"true":"false",
(int)regInfo.inliers,
(int)refFrame_.sensorData().keypoints().size(),
(int)newFrame.sensorData().keypoints().size(),
!output.isNull()?"true":"false");
return output;

55
corelib/src/OdometryF2M.cpp
View File

@@ -55,6 +55,8 @@ namespace rtabmap {
OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
Odometry(parameters),
maximumMapSize_(Parameters::defaultOdomF2MMaxSize()),
keyFrameThr_(Parameters::defaultOdomKeyFrameThr()),
maxNewFeatures_(Parameters::defaultOdomF2MMaxNewFeatures()),
fixedMapPath_(Parameters::defaultOdomF2MFixedMapPath()),
regVis_(new RegistrationVis(parameters)),
map_(new Signature(-1)),
@@ -62,7 +64,12 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
{
UDEBUG("");
Parameters::parse(parameters, Parameters::kOdomF2MMaxSize(), maximumMapSize_);
Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
Parameters::parse(parameters, Parameters::kOdomF2MMaxNewFeatures(), maxNewFeatures_);
Parameters::parse(parameters, Parameters::kOdomF2MFixedMapPath(), fixedMapPath_);
UASSERT(maximumMapSize_ >= 0);
UASSERT(keyFrameThr_ >= 0.0f && keyFrameThr_<=1.0f);
UASSERT(maxNewFeatures_ >= 0);
if(!fixedMapPath_.empty())
{
@@ -215,7 +222,8 @@ Transform OdometryF2M::computeTransform(
if(!transform.isNull())
{
if(fixedMapPath_.empty())
if(fixedMapPath_.empty() &&
(keyFrameThr_==0 || float(regInfo.inliers) <= keyFrameThr_*float(lastFrame_->sensorData().keypoints().size())))
{
output = transform;
@@ -229,14 +237,32 @@ Transform OdometryF2M::computeTransform(
Transform t = this->getPose()*output;
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());
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)));
mapDescriptors.insert(std::make_pair(*iter, lastFrame_->getWordsDescriptors().find(*iter)->second));
++added;
newIds.insert(std::make_pair(iter2D->second.response, std::make_pair(iter->first, iter->second)));
lastId = iter->first;
}
}
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;
}
}
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
{
@@ -312,6 +345,12 @@ Transform OdometryF2M::computeTransform(
map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
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
@@ -329,13 +368,11 @@ Transform OdometryF2M::computeTransform(
info->inliers = regInfo.inliers;
info->matches = regInfo.matches;
info->features = nFeatures;
info->localMapSize = (int)map_->getWords3().size();
if(this->isInfoDataFilled())
{
info->wordMatches = regInfo.matchesIDs;
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/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/KeyFrameThr", std::make_pair(false, Parameters::kOdomKeyFrameThr())));
// 0.11.0
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("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("Kp/WordsPerImage", std::make_pair(true, Parameters::kKpMaxFeatures())));