Added OdomBow/FixedLocalMapPath parameter

corelib/include/rtabmap/core/Memory.h

@@ -83,7 +83,7 @@ public:
 	std::list<int> forget(const std::set<int> & ignoredIds = std::set<int>());
 	std::set<int> reactivateSignatures(const std::list<int> & ids, unsigned int maxLoaded, double & timeDbAccess);
 
-	std::list<int> cleanup(const std::list<int> & ignoredIds = std::list<int>());
+	int cleanup();
 	void emptyTrash();
 	void joinTrashThread();
 	bool addLink(const Link & link);

corelib/include/rtabmap/core/Odometry.h

@@ -118,6 +118,7 @@ private:
 private:
 	//Parameters
 	int _localHistoryMaxSize;
+	std::string _fixedLocalMapPath;
 
 	Memory * _memory;
 	std::multimap<int, pcl::PointXYZ> localMap_;

corelib/include/rtabmap/core/Parameters.h

@@ -339,6 +339,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(OdomBow, LocalHistorySize,       int, 1000,      "Local history size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
 	RTABMAP_PARAM(OdomBow, NNType,                 int, 3, 	    "kNNFlannNaive=0, kNNFlannKdTree=1, kNNFlannLSH=2, kNNBruteForce=3, kNNBruteForceGPU=4");
 	RTABMAP_PARAM(OdomBow, NNDR,                   float, 0.8,  "NNDR: nearest neighbor distance ratio.");
+	RTABMAP_PARAM_STR(OdomBow, FixedLocalMapPath,   "",         "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.");

corelib/src/Memory.cpp

@@ -1487,10 +1487,10 @@ std::list<int> Memory::forget(const std::set<int> & ignoredIds)
 }
 
 
-std::list<int> Memory::cleanup(const std::list<int> & ignoredIds)
+int Memory::cleanup()
 {
 	UDEBUG("");
-	std::list<int> signaturesRemoved;
+	int signatureRemoved = 0;
 
 	// bad signature
 	if(_lastSignature && ((_lastSignature->isBadSignature() && _badSignaturesIgnored) || !_incrementalMemory))
@@ -1499,11 +1499,11 @@ std::list<int> Memory::cleanup(const std::list<int> & ignoredIds)
 		{
 			UDEBUG("Bad signature! %d", _lastSignature->id());
 		}
-		signaturesRemoved.push_back(_lastSignature->id());
+		signatureRemoved = _lastSignature->id();
 		moveToTrash(_lastSignature, _incrementalMemory);
 	}
 
-	return signaturesRemoved;
+	return signatureRemoved;
 }
 
 void Memory::emptyTrash()

corelib/src/Odometry.cpp

@@ -162,9 +162,10 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 	}
 
 	UASSERT(!data.imageRaw().empty());
-	if(dynamic_cast<OdometryMono*>(this) == 0)
+	if(dynamic_cast<OdometryMono*>(this) == 0 && dynamic_cast<OdometryBOW*>(this) == 0)
 	{
-		UASSERT(!data.depthOrRightRaw().empty());
+		UERROR("Depth or stereo images required with the odometry selected!");
+		return Transform();
 	}
 
 	if(!data.stereoCameraModel().isValid() &&

corelib/src/OdometryBOW.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_correspondences.h"
+#include "rtabmap/core/Graph.h"
 #include "rtabmap/core/VWDictionary.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
@@ -49,9 +50,12 @@ namespace rtabmap {
 OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 	Odometry(parameters),
 	_localHistoryMaxSize(Parameters::defaultOdomBowLocalHistorySize()),
+	_fixedLocalMapPath(Parameters::defaultOdomBowFixedLocalMapPath()),
 	_memory(0)
 {
+	UDEBUG("");
 	Parameters::parse(parameters, Parameters::kOdomBowLocalHistorySize(), _localHistoryMaxSize);
+	Parameters::parse(parameters, Parameters::kOdomBowFixedLocalMapPath(), _fixedLocalMapPath);
 
 	ParametersMap customParameters;
 	customParameters.insert(ParametersPair(Parameters::kKpMaxDepth(), uNumber2Str(this->getMaxDepth())));
@@ -101,10 +105,71 @@ OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
 		}
 	}
 
-	_memory = new Memory(customParameters);
+	if(_fixedLocalMapPath.empty())
-	if(!_memory->init("", false, ParametersMap()))
 	{
-		UERROR("Error initializing the memory for BOW Odometry.");
+		_memory = new Memory(customParameters);
+		if(!_memory->init("", false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+	}
+	else
+	{
+		UINFO("Init odometry from a fixed database: \"%s\"", _fixedLocalMapPath.c_str());
+		// init the local map with a all 3D features contained in the database
+		customParameters.insert(ParametersPair(Parameters::kMemIncrementalMemory(), "false"));
+		customParameters.insert(ParametersPair(Parameters::kMemInitWMWithAllNodes(), "true"));
+		_memory = new Memory(customParameters);
+		if(!_memory->init(_fixedLocalMapPath, false, ParametersMap()))
+		{
+			UERROR("Error initializing the memory for BOW Odometry.");
+		}
+		else
+		{
+			// get the graph
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastSignatureId(), 0, -1);
+			std::map<int, Transform> poses;
+			std::multimap<int, Link> links;
+			_memory->getMetricConstraints(uKeysSet(ids), poses, links, true);
+
+			if(poses.size())
+			{
+				//optimize the graph
+				graph::TOROOptimizer optimizer;
+				std::map<int, Transform> optimizedPoses = optimizer.optimize(poses.begin()->first, poses, links);
+
+				// fill the local map
+				for(std::map<int, Transform>::iterator posesIter=optimizedPoses.begin();
+					posesIter!=optimizedPoses.end();
+					++posesIter)
+				{
+					const Signature * s = _memory->getSignature(posesIter->first);
+					if(s)
+					{
+						// Transform 3D points accordingly to pose and add them to local map
+						const std::multimap<int, pcl::PointXYZ> & words3D = s->getWords3();
+						for(std::multimap<int, pcl::PointXYZ>::const_iterator pointsIter=words3D.begin();
+							pointsIter!=words3D.end();
+							++pointsIter)
+						{
+							if(!uContains(localMap_, pointsIter->first))
+							{
+								localMap_.insert(std::make_pair(pointsIter->first, util3d::transformPoint(pointsIter->second, posesIter->second)));
+							}
+						}
+					}
+				}
+			}
+			else
+			{
+				UERROR("No pose loaded from database \"%s\"", _fixedLocalMapPath.c_str());
+			}
+		}
+		if((int)localMap_.size() < this->getMinInliers() || localMap_.size() == 0)
+		{
+			UERROR("The loaded fixed map from \"%s\" is too small! Only %d unique features loaded. Odometry won't be computed!",
+					_fixedLocalMapPath.c_str(), (int)localMap_.size());
+		}
 	}
 }
 
@@ -117,9 +182,16 @@ OdometryBOW::~OdometryBOW()
 
 void OdometryBOW::reset(const Transform & initialPose)
 {
-	Odometry::reset(initialPose);
+	if(_fixedLocalMapPath.empty())
-	_memory->init("", false, ParametersMap());
+	{
-	localMap_.clear();
+		Odometry::reset(initialPose);
+		_memory->init("", false, ParametersMap());
+		localMap_.clear();
+	}
+	else
+	{
+		UWARN("Odometry cannot be reset when a fixed local map is set.");
+	}
 }
 
 // return not null transform if odometry is correctly computed
@@ -140,7 +212,6 @@ Transform OdometryBOW::computeTransform(
 	int correspondences = 0;
 	int nFeatures = 0;
 
-	const Signature * previousSignature = _memory->getLastWorkingSignature();
 	if(_memory->update(data))
 	{
 		const Signature * newSignature = _memory->getLastWorkingSignature();
@@ -153,7 +224,7 @@ Transform OdometryBOW::computeTransform(
 			}
 		}
 
-		if(previousSignature && newSignature)
+		if(localMap_.size() && newSignature)
 		{
 			Transform transform;
 			if((int)localMap_.size() >= this->getMinInliers())
@@ -257,6 +328,10 @@ Transform OdometryBOW::computeTransform(
 									double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
 									variance = 2.1981 * median_error_sqr;
 								}
+								else
+								{
+									variance = 1;
+								}
 							}
 							else
 							{
@@ -364,9 +439,10 @@ Transform OdometryBOW::computeTransform(
 			{
 				_memory->deleteLocation(newSignature->id());
 			}
-			else
+			else if(_fixedLocalMapPath.empty())
 			{
 				output = transform;
+
 				// remove words if history max size is reached
 				while(localMap_.size() && (int)localMap_.size() > _localHistoryMaxSize && _memory->getStMem().size()>1)
 				{
@@ -410,14 +486,18 @@ Transform OdometryBOW::computeTransform(
 					}
 				}
 			}
+			else
+			{
+				// fixed local map, just delete the new signature
+				output = transform;
+				_memory->deleteLocation(newSignature->id());
+			}
 		}
-		else if(!previousSignature && newSignature)
+		else if(newSignature)
 		{
-			localMap_.clear();
-
 			int count = 0;
 			std::list<int> uniques = uUniqueKeys(newSignature->getWords3());
-			if((int)uniques.size() >= this->getMinInliers())
+			if(_fixedLocalMapPath.empty() && (int)uniques.size() >= this->getMinInliers())
 			{
 				output.setIdentity();
 

corelib/src/OdometryThread.cpp

@@ -105,7 +105,7 @@ void OdometryThread::mainLoop()
 
 void OdometryThread::addData(const SensorData & data)
 {
-	if(dynamic_cast<OdometryMono*>(_odometry) == 0)
+	if(dynamic_cast<OdometryMono*>(_odometry) == 0 && dynamic_cast<OdometryBOW*>(_odometry) == 0)
 	{
 		if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
@@ -115,6 +115,7 @@ void OdometryThread::addData(const SensorData & data)
 	}
 	else
 	{
+		// Mono and BOW can accept RGB only
 		if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValid()))
 		{
 			ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");

corelib/src/Rtabmap.cpp

@@ -2141,32 +2141,39 @@ bool Rtabmap::process(
 		lastSignatureData = *signature;
 	}
 
-	//By default, remove all signatures with a loop closure link if they are not in reactivateIds
+	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
-	//This will also remove rehearsed signatures
+	std::list<int> signaturesRemoved;
-	std::list<int> signaturesRemoved = _memory->cleanup();
+	int signatureRemoved = _memory->cleanup();
+	if(signatureRemoved)
+	{
+		signaturesRemoved.push_back(signatureRemoved);
+	}
 
 	// If this option activated, add new nodes only if there are linked with a previous map.
 	// Used when rtabmap is first started, it will wait a
 	// global loop closure detection before starting the new map,
 	// otherwise it deletes the current node.
-	if(_startNewMapOnLoopClosure &&
+	if(signatureRemoved != lastSignatureData.id())
-		_memory->isIncremental() &&              // only in mapping mode
-		signature->getLinks().size() == 0 &&     // alone in the current map
-		_memory->getWorkingMem().size()>1)       // The working memory should not be empty
 	{
-		UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
+		if(_startNewMapOnLoopClosure &&
-				signature->id());
+			_memory->isIncremental() &&              // only in mapping mode
-		signaturesRemoved.push_back(signature->id());
+			signature->getLinks().size() == 0 &&     // alone in the current map
-		_memory->deleteLocation(signature->id());
+			_memory->getWorkingMem().size()>1)       // The working memory should not be empty
-	}
+		{
-	else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
+			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
-	{
+					signature->id());
-		// Don't delete the location if a loop closure is detected
+			signaturesRemoved.push_back(signature->id());
-		UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+			_memory->deleteLocation(signature->id());
-			  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
+		}
-		// If there is a too small displacement, remove the node
+		else if(smallDisplacement && _loopClosureHypothesis.first == 0 && lastLocalSpaceClosureId == 0)
-		signaturesRemoved.push_back(signature->id());
+		{
-		_memory->deleteLocation(signature->id());
+			// Don't delete the location if a loop closure is detected
+			UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
+				  signature->id(), _rgbdLinearUpdate, _rgbdAngularUpdate);
+			// If there is a too small displacement, remove the node
+			signaturesRemoved.push_back(signature->id());
+			_memory->deleteLocation(signature->id());
+		}
 	}
 
 	// Pass this point signature should not be used, since it could have been transferred...

guilib/include/rtabmap/gui/PreferencesDialog.h

@@ -246,6 +246,7 @@ private slots:
 	void updateKpROI();
 	void changeWorkingDirectory();
 	void changeDictionaryPath();
+	void changeOdomBowFixedLocalMapPath();
 	void readSettingsEnd();
 	void setupTreeView();
 	void updateBasicParameter();

guilib/src/MainWindow.cpp

@@ -1738,14 +1738,36 @@ void MainWindow::createAndAddCloudToMap(int nodeId, const Transform & pose, int
 		pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
 		cloud->resize(iter->getWords3().size());
 		int oi=0;
-		for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->getWords3().begin(); jter!=iter->getWords3().end(); ++jter)
+		UASSERT(iter->getWords().size() == iter->getWords3().size());
+		std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
+		for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=iter->getWords3().begin();
+				jter!=iter->getWords3().end(); ++jter, ++kter, ++oi)
 		{
 			(*cloud)[oi].x = jter->second.x;
 			(*cloud)[oi].y = jter->second.y;
 			(*cloud)[oi].z = jter->second.z;
-			(*cloud)[oi].r = 255;
+			int u = kter->second.pt.x+0.5;
-			(*cloud)[oi].g = 255;
+			int v = kter->second.pt.x+0.5;
-			(*cloud)[oi++].b = 255;
+			if(!iter->sensorData().imageRaw().empty() &&
+				uIsInBounds(u, 0, iter->sensorData().imageRaw().cols-1) &&
+				uIsInBounds(v, 0, iter->sensorData().imageRaw().rows-1))
+			{
+				if(iter->sensorData().imageRaw().channels() == 1)
+				{
+					(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = iter->sensorData().imageRaw().at<unsigned char>(u, v);
+				}
+				else
+				{
+					cv::Vec3b bgr = iter->sensorData().imageRaw().at<cv::Vec3b>(u, v);
+					(*cloud)[oi].r = bgr.val[0];
+					(*cloud)[oi].g = bgr.val[1];
+					(*cloud)[oi].b = bgr.val[2];
+				}
+			}
+			else
+			{
+				(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
+			}
 		}
 		if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, pose, color))
 		{

guilib/src/PreferencesDialog.cpp

@@ -597,6 +597,8 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->odom_localHistory->setObjectName(Parameters::kOdomBowLocalHistorySize().c_str());
 	_ui->odom_bin_nn->setObjectName(Parameters::kOdomBowNNType().c_str());
 	_ui->odom_bin_nndrRatio->setObjectName(Parameters::kOdomBowNNDR().c_str());
+	_ui->odom_fixedLocalMapPath->setObjectName(Parameters::kOdomBowFixedLocalMapPath().c_str());
+	connect(_ui->toolButton_odomBowFixedLocalMap, SIGNAL(clicked()), this, SLOT(changeOdomBowFixedLocalMapPath()));
 
 	//Odometry Optical Flow
 	_ui->odom_flow_winSize->setObjectName(Parameters::kOdomFlowWinSize().c_str());
@@ -2927,6 +2929,23 @@ void PreferencesDialog::changeDictionaryPath()
 	}
 }
 
+void PreferencesDialog::changeOdomBowFixedLocalMapPath()
+{
+	QString path;
+	if(_ui->odom_fixedLocalMapPath->text().isEmpty())
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Database"), this->getWorkingDirectory(), tr("RTAB-Map database files (*.db)"));
+	}
+	else
+	{
+		path = QFileDialog::getOpenFileName(this, tr("Database"), _ui->odom_fixedLocalMapPath->text(), tr("RTAB-Map database files (*.db)"));
+	}
+	if(!path.isEmpty())
+	{
+		_ui->odom_fixedLocalMapPath->setText(path);
+	}
+}
+
 void PreferencesDialog::updateRGBDCameraGroupBoxVisibility()
 {
 	_ui->groupBox_openni2->setVisible(_ui->comboBox_cameraRGBD->currentIndex() == kSrcOpenNI2-kSrcOpenNI_PCL);

guilib/src/ui/preferencesDialog.ui

@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-837</y>
+            <y>0</y>
-            <width>755</width>
+            <width>760</width>
-            <height>1591</height>
+            <height>1570</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>3</number>
+                <number>24</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -7387,8 +7387,8 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                    <property name="title">
                     <string>BOW</string>
                    </property>
-                   <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,1">
+                   <layout class="QGridLayout" name="gridLayout_29" columnstretch="0,1,0,0">
-                    <item row="0" column="0">
+                    <item row="0" column="1">
                      <widget class="QSpinBox" name="odom_localHistory">
                       <property name="minimum">
                        <number>0</number>
@@ -7404,7 +7404,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
-                    <item row="0" column="1">
+                    <item row="0" column="3">
                      <widget class="QLabel" name="label_190">
                       <property name="text">
                        <string>Local history size: If &gt; 0 (example 5000), the odometry will maintain a local map of X maximum words. This will decrease odometry drifting when the camera is not moving.</string>
@@ -7414,7 +7414,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
-                    <item row="1" column="0">
+                    <item row="1" column="1">
                      <widget class="QComboBox" name="odom_bin_nn">
                       <property name="sizeAdjustPolicy">
                        <enum>QComboBox::AdjustToContents</enum>
@@ -7446,7 +7446,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </item>
                      </widget>
                     </item>
-                    <item row="1" column="1">
+                    <item row="1" column="3">
                      <widget class="QLabel" name="label_201">
                       <property name="text">
                        <string>Nearest neighbor strategy. FLANN KdTree must be used only with SURF/SIFT. FLANN LSH must be used only with binary feature detector.</string>
@@ -7456,7 +7456,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
-                    <item row="2" column="0">
+                    <item row="2" column="1">
                      <widget class="QDoubleSpinBox" name="odom_bin_nndrRatio">
                       <property name="decimals">
                        <number>1</number>
@@ -7475,7 +7475,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
-                    <item row="2" column="1">
+                    <item row="2" column="3">
                      <widget class="QLabel" name="label_202">
                       <property name="text">
                        <string>NNDR ratio
@@ -7487,6 +7487,26 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
                       </property>
                      </widget>
                     </item>
+                    <item row="3" column="1">
+                     <widget class="QLineEdit" name="odom_fixedLocalMapPath"/>
+                    </item>
+                    <item row="3" column="3">
+                     <widget class="QLabel" name="label_239">
+                      <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>
+                      </property>
+                      <property name="wordWrap">
+                       <bool>true</bool>
+                      </property>
+                     </widget>
+                    </item>
+                    <item row="3" column="2">
+                     <widget class="QToolButton" name="toolButton_odomBowFixedLocalMap">
+                      <property name="text">
+                       <string>...</string>
+                      </property>
+                     </widget>
+                    </item>
                    </layout>
                   </widget>
                  </item>