ZED driver: added option to use visual odometry approach from zed sdk. RtabmapThread: fixed thread state change on new map trigger on Odometry init (variance=9999). Odometry: on init, verify that the first frame is ok before sending first pose. Parameters: Mem/SaveDepth16Format is now false by default

corelib/include/rtabmap/core/Camera.h

@@ -56,6 +56,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}

corelib/include/rtabmap/core/CameraStereo.h

@@ -118,6 +118,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	CameraStereoZed(
@@ -125,6 +126,7 @@ public:
 			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
 			int sensingMode = 1,// 0=FULL, 1=RAW
 			int confidenceThr = 100,
+			bool computeOdometry = false,
 			float imageRate=0.0f,
 			const Transform & localTransform = Transform::getIdentity());
 	virtual ~CameraStereoZed();
@@ -132,6 +134,7 @@ public:
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
+	virtual bool odomProvided() const { return computeOdometry_; }
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
@@ -146,6 +149,8 @@ private:
 	int quality_;
 	int sensingMode_;
 	int confidenceThr_;
+	bool computeOdometry_;
+	bool lost_;
 };
 
 /////////////////////////

corelib/include/rtabmap/core/DBReader.h

@@ -67,8 +67,7 @@ public:
 
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-
-	bool isOdometryIgnored() const {return _odometryIgnored;}
+	virtual bool odometryProvided() const {return !_odometryIgnored;}
 
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);

corelib/include/rtabmap/core/Parameters.h

@@ -194,7 +194,7 @@ class RTABMAP_EXP Parameters
 	RTABMAP_PARAM(Mem, BinDataKept, 		    bool, true, 	"Keep binary data in db.");
 	RTABMAP_PARAM(Mem, RawDescriptorsKept, 		bool, true, 	"Raw descriptors kept in memory.");
 	RTABMAP_PARAM(Mem, MapLabelsAdded, 		    bool, true, 	"Create map labels. The first node of a map will be labelled as \"map#\" where # is the map ID.");
-	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, true, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
+	RTABMAP_PARAM(Mem, SaveDepth16Format, 		bool, false, 	"Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).");
 	RTABMAP_PARAM(Mem, NotLinkedNodesKept, 	    bool, true, 	"Keep not linked nodes in db (rehearsed nodes and deleted nodes).");
 	RTABMAP_PARAM(Mem, STMSize, 		        unsigned int, 10, "Short-term memory size.");
 	RTABMAP_PARAM(Mem, IncrementalMemory,       bool, true, 	"SLAM mode, otherwise it is Localization mode.");

corelib/include/rtabmap/core/Rtabmap.h

@@ -70,6 +70,7 @@ public:
 	void close(bool databaseSaved = true);
 
 	const std::string & getWorkingDir() const {return _wDir;}
+	bool isRGBDMode() const { return _rgbdSlamMode; }
 	int getLoopClosureId() const {return _loopClosureHypothesis.first;}
 	float getLoopClosureValue() const {return _loopClosureHypothesis.second;}
 	int getHighestHypothesisId() const {return _highestHypothesis.first;}

corelib/src/CameraStereo.cpp

@@ -753,6 +753,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -763,7 +764,9 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(resolution),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
@@ -778,6 +781,7 @@ CameraStereoZed::CameraStereoZed(
 		int quality,
 		int sensingMode,
 		int confidenceThr,
+		bool computeOdometry,
 		float imageRate,
 		const Transform & localTransform) :
 	Camera(imageRate, localTransform),
@@ -788,7 +792,9 @@ CameraStereoZed::CameraStereoZed(
 	resolution_(2),
 	quality_(quality),
 	sensingMode_(sensingMode),
-	confidenceThr_(confidenceThr)
+	confidenceThr_(confidenceThr),
+	computeOdometry_(computeOdometry),
+	lost_(true)
 {
 #ifdef RTABMAP_ZED
 	UASSERT(resolution_ >= sl::zed::HD2K && resolution_ <sl::zed::LAST_RESOLUTION);
@@ -817,7 +823,7 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 		zed_ = 0;
 	}
 	
-
+	lost_ = true;
 	if(src_ == CameraVideo::kVideoFile)
 	{
 		zed_ = new sl::zed::Camera(svoFilePath_); // Use in SVO playback mode
@@ -858,6 +864,13 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
 
 	zed_->setConfidenceThreshold(confidenceThr_);
 
+	if (computeOdometry_)
+	{
+		Eigen::Matrix4f initPose;
+		initPose.setIdentity(4, 4);
+		zed_->enableTracking(initPose, false);
+	}
+
 	sl::zed::StereoParameters * stereoParams = zed_->getParameters();
 	sl::zed::resolution res = zed_->getImageSize();
 				
@@ -931,6 +944,40 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
 			
 				data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
 			}
+
+			if (computeOdometry_ && info)
+			{
+				Eigen::Matrix4f path;
+				int trackingConfidence = zed_->getTrackingConfidence();
+				if (trackingConfidence)
+				{
+					sl::zed::TRACKING_STATE track_state = zed_->getPosition(path);
+					info->odomPose = Transform::fromEigen4f(path);
+					if (!info->odomPose.isNull())
+					{
+						//transform x->forward, y->left, z->up
+						Transform opticalTransform(0, 0, 1, 0, -1, 0, 0, 0, 0, -1, 0, 0);
+						info->odomPose = opticalTransform * info->odomPose * opticalTransform.inverse();
+					}
+					if (lost_)
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // don't know transform with previous pose
+						lost_ = false;
+						UDEBUG("Init %s (var=%f)", info->odomPose.prettyPrint().c_str(), 9999.0f);
+					}
+					else
+					{
+						info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 1.0f / float(trackingConfidence);
+						UDEBUG("Run %s (var=%f)", info->odomPose.prettyPrint().c_str(), 1.0f / float(trackingConfidence));
+					}
+				}
+				else
+				{
+					info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 9999.0f; // lost
+					lost_ = true;
+					UWARN("ZED lost!");
+				}
+			}
 		}
 		else if(src_ == CameraVideo::kUsbDevice)
 		{

corelib/src/OdometryF2F.cpp

@@ -176,6 +176,12 @@ Transform OdometryF2F::computeTransform(
 			}
 			else
 			{
+				if (!refFrame_.sensorData().isValid())
+				{
+					// Don't send odometry if we don't have a keyframe yet
+					output.setNull();
+				}
+
 				if(features < registrationPipeline_->getMinVisualCorrespondences())
 				{
 					UWARN("Too low 2D features (%d), keeping last key frame...", features);

corelib/src/OdometryF2M.cpp

@@ -419,51 +419,75 @@ Transform OdometryF2M::computeTransform(
 
 			data.setFeatures(lastFrame_->sensorData().keypoints(), lastFrame_->sensorData().descriptors());
 
-			if(fixedMapPath_.empty())
+			// a very high variance tells that the new pose is not linked with the previous one
+			regInfo.variance = 9999;
+
+			bool frameValid = false;
+			Transform newFramePose = this->getPose(); // initial pose may be not identity...
+			if(regPipeline_->isImageRequired())
 			{
-				output.setIdentity();
-				// a very high variance tells that the new pose is not linked with the previous one
-				regInfo.variance = 9999;
-
-				Transform newFramePose = this->getPose(); // initial pose may be not identity...
-				if(regPipeline_->isImageRequired() &&
-				   (int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
+				if ((int)lastFrame_->getWords3().size() >= regPipeline_->getMinVisualCorrespondences())
 				{
-					// update local map
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
-
-					std::multimap<int, cv::KeyPoint> words;
-					std::multimap<int, cv::Point3f> transformedPoints;
-					std::multimap<int, cv::Mat> descriptors;
-					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
-					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
-					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
-					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
-							iter!=lastFrame_->getWords3().end();
-							++iter,++descIter,++wordsIter)
+					frameValid = true;
+					if (fixedMapPath_.empty())
 					{
-						if(util3d::isFinite(iter->second))
-						{
-							words.insert(*wordsIter);
-							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
-							descriptors.insert(*descIter);
-						}
-					}
-					map_->setWords(words);
-					map_->setWords3(transformedPoints);
-					map_->setWordsDescriptors(descriptors);
+						// update local map
+						UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 
-					map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
-					map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+						std::multimap<int, cv::KeyPoint> words;
+						std::multimap<int, cv::Point3f> transformedPoints;
+						std::multimap<int, cv::Mat> descriptors;
+						UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+						std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
+						std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
+						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+							iter != lastFrame_->getWords3().end();
+							++iter, ++descIter, ++wordsIter)
+						{
+							if (util3d::isFinite(iter->second))
+							{
+								words.insert(*wordsIter);
+								transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+								descriptors.insert(*descIter);
+							}
+						}
+						map_->setWords(words);
+						map_->setWords3(transformedPoints);
+						map_->setWordsDescriptors(descriptors);
+
+						map_->sensorData().setCameraModels(lastFrame_->sensorData().cameraModels());
+						map_->sensorData().setStereoCameraModel(lastFrame_->sensorData().stereoCameraModel());
+					}
 				}
-				if(regPipeline_->isScanRequired())
+				else
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
-					scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
-					map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0,0);
+					UWARN("%d visual features required to initialize the odometry (only %d extracted).", regPipeline_->getMinVisualCorrespondences(), (int)lastFrame_->getWords3().size());
 				}
 			}
+			if(regPipeline_->isScanRequired())
+			{
+				if (lastFrame_->sensorData().laserScanRaw().cols)
+				{
+					frameValid = true;
+					if (fixedMapPath_.empty())
+					{
+						pcl::PointCloud<pcl::PointNormal>::Ptr mapCloudNormals = util3d::laserScanToPointCloudNormal(lastFrame_->sensorData().laserScanRaw(), newFramePose);
+						scansBuffer_.insert(std::make_pair(lastFrame_->id(), mapCloudNormals));
+						map_->sensorData().setLaserScanRaw(util3d::laserScanFromPointCloud(*mapCloudNormals), 0, 0);
+					}
+				}
+				else
+				{
+					UWARN("Mising scan to initialize odometry.");
+				}
+			}
+
+			if (frameValid)
+			{
+				// We initialized the local map
+				output.setIdentity();
+			}
 
 			if(info)
 			{

corelib/src/Rtabmap.cpp

@@ -1009,7 +1009,7 @@ bool Rtabmap::process(
 				}
 			}
 			else
-			{
+			{
 				//============================================================
 				// Refine neighbor links
 				//============================================================
@@ -1062,7 +1062,7 @@ bool Rtabmap::process(
 				}
 			}
 			timeNeighborLinkRefining = timer.ticks();
-			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
+			ULOGGER_INFO("timeOdometryRefining=%fs", timeNeighborLinkRefining);
 
 			UASSERT(oldS->hasLink(signature->id()));
 			UASSERT(uContains(_optimizedPoses, oldId));

corelib/src/RtabmapThread.cpp

@@ -333,7 +333,22 @@ void RtabmapThread::handleEvent(UEvent* event)
 			CameraEvent * e = (CameraEvent*)event;
 			if(e->getCode() == CameraEvent::kCodeData)
 			{
-				this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+				if (_rtabmap->isRGBDMode())
+				{
+					if (!e->info().odomPose.isNull())
+					{
+						this->addData(OdometryEvent(e->data(), e->info().odomPose, e->info().odomCovariance));
+					}
+					else
+					{
+						lastPose_.setNull();
+					}
+				}
+				else
+				{ 
+					this->addData(OdometryEvent(e->data(), Transform(), 1, 1));
+				}
+				
 			}
 		}
 		else if(event->getClassName().compare("OdometryEvent") == 0)
@@ -538,21 +553,22 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 {
 	if(!_paused)
 	{
+		UScopeMutex scopeMutex(_dataMutex);
+
 		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
 			if((_previousStamp>0.0 && odomEvent.data().stamp()>_previousStamp && odomEvent.data().stamp() - _previousStamp < 1.0f/_rate) ||
-			   ((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
+				((_previousStamp<=0.0 || odomEvent.data().stamp()<=_previousStamp) && _frameRateTimer->getElapsedTime() < 1.0f/_rate))
 			{
 				ignoreFrame = true;
 			}
 		}
-		if(_dataBufferMaxSize > 0 &&
-				(!lastPose_.isIdentity() &&
-					   (odomEvent.pose().isIdentity() ||
+		if(!lastPose_.isIdentity() &&
+						(odomEvent.pose().isIdentity() ||
 						odomEvent.info().variance>=9999 ||
 						odomEvent.rotVariance()>=9999 ||
-						odomEvent.transVariance()>=9999)))
+						odomEvent.transVariance()>=9999))
 		{
 			UWARN("Odometry is reset (identity pose or high variance >=9999 detected). Increment map id!");
 			pushNewState(kStateTriggeringMap);
@@ -585,40 +601,37 @@ void RtabmapThread::addData(const OdometryEvent & odomEvent)
 		lastPose_ = odomEvent.pose();
 
 		bool notify = true;
-		_dataMutex.lock();
+		
+		if(_rotVariance <= 0)
 		{
-			if(_rotVariance <= 0)
-			{
-				_rotVariance = 1.0;
-			}
-			if(_transVariance <= 0)
-			{
-				_transVariance = 1.0;
-			}
-			if(ignoreFrame)
-			{
-				// set negative id so rtabmap will detect it as an intermediate node
-				SensorData tmp = odomEvent.data();
-				tmp.setId(-1);
-				tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
-				_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
-			}
-			else
-			{
-				_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
-			}
-			UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
-
-			_rotVariance = 0;
-			_transVariance = 0;
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
-			{
-				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
-				_dataBuffer.pop_front();
-				notify = false;
-			}
+			_rotVariance = 1.0;
+		}
+		if(_transVariance <= 0)
+		{
+			_transVariance = 1.0;
+		}
+		if(ignoreFrame)
+		{
+			// set negative id so rtabmap will detect it as an intermediate node
+			SensorData tmp = odomEvent.data();
+			tmp.setId(-1);
+			tmp.setFeatures(std::vector<cv::KeyPoint>(), cv::Mat());// remove features
+			_dataBuffer.push_back(OdometryEvent(tmp, odomEvent.pose(), _rotVariance, _transVariance));
+		}
+		else
+		{
+			_dataBuffer.push_back(OdometryEvent(odomEvent.data(), odomEvent.pose(), _rotVariance, _transVariance));
+		}
+		UINFO("Added data %d (variance=%f)", odomEvent.data().id(), _rotVariance);
+
+		_rotVariance = 0;
+		_transVariance = 0;
+		while(_dataBufferMaxSize > 0 && _dataBuffer.size() > _dataBufferMaxSize)
+		{
+			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+			_dataBuffer.pop_front();
+			notify = false;
 		}
-		_dataMutex.unlock();
 
 		if(notify)
 		{
@@ -636,9 +649,10 @@ bool RtabmapThread::getData(OdometryEvent & data)
 	ULOGGER_INFO("wake-up");
 
 	bool dataFilled = false;
+
 	_dataMutex.lock();
 	{
-		if(!_dataBuffer.empty())
+		if(_state.empty() && !_dataBuffer.empty())
 		{
 			data = _dataBuffer.front();
 			_dataBuffer.pop_front();