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

2026-09-02 01:20:25 +08:00 · 2016-03-01 20:47:46 -05:00
parent b6fb947310
commit 264146d8b3
10 changed files with 180 additions and 100 deletions
--- a/corelib/src/Memory.cpp
+++ b/corelib/src/Memory.cpp
@@ -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();
--- a/corelib/src/Odometry.cpp
+++ b/corelib/src/Odometry.cpp
@@ -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();
 }

--- a/corelib/src/OdometryF2F.cpp
+++ b/corelib/src/OdometryF2F.cpp
@@ -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;
--- a/corelib/src/OdometryF2M.cpp
+++ b/corelib/src/OdometryF2M.cpp
@@ -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());
 		}
 	}

--- a/corelib/src/Parameters.cpp
+++ b/corelib/src/Parameters.cpp
@@ -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())));