Added CameraStereoImages class to read stereo images from a directory. Added a particle filter to smooth odometry trajectory. Added parameter RGBD/OptimizeEpsilon to limit TORO iterations when error improvement is small. Added Rtabmap/CreateIntermediateNodes parameter: this can be used to keep all odometry poses 'between' nodes used for loop closure detection. Added PnP approach to loop closure constraint estimation. Fixed decimation of stereo images when image size is odd.

corelib/src/BayesFilter.cpp

@@ -38,6 +38,7 @@ namespace rtabmap {
 BayesFilter::BayesFilter(const ParametersMap & parameters) :
 	_virtualPlacePrior(Parameters::defaultBayesVirtualPlacePriorThr()),
 	_fullPredictionUpdate(Parameters::defaultBayesFullPredictionUpdate()),
+	_badSignaturesIgnored(Parameters::defaultRtabmapCreateIntermediateNodes()),
 	_totalPredictionLCValues(0.0f)
 {
 	this->setPredictionLC(Parameters::defaultBayesPredictionLC());
@@ -56,6 +57,7 @@ void BayesFilter::parseParameters(const ParametersMap & parameters)
 	}
 	Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
 	Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
+	Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _badSignaturesIgnored);
 
 	UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
 }
@@ -161,7 +163,7 @@ const std::map<int, float> & BayesFilter::computePosterior(const Memory * memory
 	// STEP 1 - Prediction : Prior*lastPosterior
 	_prediction = this->generatePrediction(memory, uKeys(likelihood));
 
-	ULOGGER_DEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
+	UDEBUG("STEP1-generate prior=%fs, rows=%d, cols=%d", timer.ticks(), _prediction.rows, _prediction.cols);
 	//std::cout << "Prediction=" << _prediction << std::endl;
 
 	// Adjust the last posterior if some images were
@@ -260,7 +262,7 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 				// Set high values (gaussians curves) to loop closure neighbors
 
 				// ADD prob for each neighbors
-				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0);
+				std::map<int, int> neighbors = memory->getNeighborsId(ids[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored);
 				std::list<int> idsLoopMargin;
 				//filter neighbors in STM
 				for(std::map<int, int>::iterator iter=neighbors.begin(); iter!=neighbors.end();)
@@ -474,7 +476,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 		}
 		if(i<newIds.size() && !uContains(oldIdToIndexMap,newIds[i]))
 		{
-			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0);
+			std::map<int, int> neighbors = memory->getNeighborsId(newIds[i], _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored);
 			float sum = this->addNeighborProb(prediction, i, neighbors, newIdToIndexMap);
 			this->normalize(prediction, i, sum, newIds[0]<0);
 			++added;
@@ -494,7 +496,7 @@ cv::Mat BayesFilter::updatePrediction(const cv::Mat & oldPrediction,
 	int modified = 0;
 	for(std::set<int>::iterator iter = idsToUpdate.begin(); iter!=idsToUpdate.end(); ++iter)
 	{
-		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0);
+		std::map<int, int> neighbors = memory->getNeighborsId(*iter, _predictionLC.size()-1, 0, false, false, _badSignaturesIgnored);
 		int index = newIdToIndexMap.at(*iter);
 		float sum = this->addNeighborProb(prediction, index, neighbors, newIdToIndexMap);
 		this->normalize(prediction, index, sum, newIds[0]<0);

corelib/src/BayesFilter.h

@@ -58,6 +58,7 @@ public:
 	float getVirtualPlacePrior() const {return _virtualPlacePrior;}
 	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
 	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
+	bool isBadSignaturesIgnored() const {return _badSignaturesIgnored;}
 
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
 
@@ -79,6 +80,7 @@ private:
 	float _virtualPlacePrior;
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
+	bool _badSignaturesIgnored;
 	float _totalPredictionLCValues;
 };
 

corelib/src/Camera.cpp

@@ -237,9 +237,22 @@ bool CameraImages::init()
 	{
 		UWARN("Directory is empty \"%s\"", _path.c_str());
 	}
+	else
+	{
+		UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
+	}
 	return _dir->isValid();
 }
 
+unsigned int CameraImages::imagesCount() const
+{
+	if(_dir)
+	{
+		return _dir->getFileNames().size();
+	}
+	return 0;
+}
+
 cv::Mat CameraImages::captureImage()
 {
 	cv::Mat img;

corelib/src/CameraModel.cpp

@@ -125,6 +125,10 @@ bool CameraModel::load(const std::string & filePath)
 
 		return true;
 	}
+	else
+	{
+		UWARN("Could not load calibration file \"%s\".", filePath.c_str());
+	}
 	return false;
 }
 
@@ -241,11 +245,15 @@ cv::Mat CameraModel::rectifyDepth(const cv::Mat & raw) const
 //
 //StereoCameraModel
 //
-bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName)
+bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
 {
 	name_ = cameraName;
 	if(left_.load(directory+"/"+cameraName+"_left.yaml") && right_.load(directory+"/"+cameraName+"_right.yaml"))
 	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
 		//load rotation, translation
 		R_ = cv::Mat();
 		T_ = cv::Mat();
@@ -299,13 +307,21 @@ bool StereoCameraModel::load(const std::string & directory, const std::string &
 
 			return true;
 		}
+		else
+		{
+			UWARN("Could not load stereo calibration file \"%s\".", filePath.c_str());
+		}
 	}
 	return false;
 }
-bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName)
+bool StereoCameraModel::save(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform)
 {
 	if(left_.save(directory+"/"+cameraName+"_left.yaml") && right_.save(directory+"/"+cameraName+"_right.yaml"))
 	{
+		if(ignoreStereoTransform)
+		{
+			return true;
+		}
 		std::string filePath = directory+"/"+cameraName+"_pose.yaml";
 		if(!filePath.empty() && !name_.empty() && !R_.empty() && !T_.empty())
 		{

corelib/src/CameraRGBD.cpp

@@ -70,6 +70,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <fc2triclops.h>
 #endif
 
+#include <rtabmap/core/Camera.h>
+
 namespace rtabmap
 {
 
@@ -90,7 +92,7 @@ CameraRGBD::~CameraRGBD()
 	}
 }
 
-void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 	bool warnFrameRateTooHigh = false;
 	float actualFrameRate = 0;
@@ -119,7 +121,7 @@ void CameraRGBD::takeImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & f
 	}
 
 	UTimer timer;
-	this->captureImage(rgb, depth, fx, fy, cx, cy);
+	this->captureImage(rgb, depth, fx, fy, cx, cy, stamp);
 	if(_colorOnly)
 	{
 		depth = cv::Mat();
@@ -258,7 +260,7 @@ std::string CameraOpenni::getSerial() const
 	return "";
 }
 
-void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 	rgb = cv::Mat();
 	depth = cv::Mat();
@@ -266,6 +268,7 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa
 	fy=0.0f;
 	cx=0.0f;
 	cy=0.0f;
+	stamp = 0.0;
 	if(interface_ && interface_->isRunning())
 	{
 		if(!dataReady_.acquire(1, 2000))
@@ -283,6 +286,7 @@ void CameraOpenni::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, floa
 				fy = 1.0f/depthConstant_;
 				cx = float(depth_.cols/2) - 0.5f;
 				cy = float(depth_.rows/2) - 0.5f;
+				stamp = UTimer::now();
 			}
 
 			depth_ = cv::Mat();
@@ -369,7 +373,7 @@ bool CameraOpenNICV::isCalibrated() const
 	return true;
 }
 
-void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 	if(_capture.isOpened())
 	{
@@ -384,6 +388,7 @@ void CameraOpenNICV::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
 		fy = _depthFocal;
 		cx = float(depth.cols/2) - 0.5f;
 		cy = float(depth.rows/2) - 0.5f;
+		stamp = UTimer::now();
 	}
 	else
 	{
@@ -710,7 +715,7 @@ std::string CameraOpenNI2::getSerial() const
 	return "";
 }
 
-void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 #ifdef WITH_OPENNI2
 	rgb = cv::Mat();
@@ -719,6 +724,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo
 	fy = 0.0f;
 	cx = 0.0f;
 	cy = 0.0f;
+	stamp = 0.0;
 
 	int readyStream = -1;
 	if(_device->isValid() &&
@@ -755,6 +761,7 @@ void CameraOpenNI2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, flo
 			fy = _depthFy;
 			cx = float(depth.cols/2) - 0.5f;
 			cy = float(depth.rows/2) - 0.5f;
+			stamp = UTimer::now();
 		}
 	}
 	else
@@ -1061,7 +1068,7 @@ std::string CameraFreenect::getSerial() const
 	return "";
 }
 
-void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 #ifdef WITH_FREENECT
 	rgb = cv::Mat();
@@ -1070,6 +1077,7 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
 	fy = 0.0f;
 	cx = 0.0f;
 	cy = 0.0f;
+	stamp = 0.0;
 	if(ctx_ && freenectDevice_)
 	{
 		if(freenectDevice_->isRunning())
@@ -1082,6 +1090,7 @@ void CameraFreenect::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, fl
 				fy = freenectDevice_->getDepthFocal();
 				cx = float(depth.cols/2) - 0.5f;
 				cy = float(depth.rows/2) - 0.5f;
+				stamp = UTimer::now();
 			}
 		}
 		else
@@ -1234,7 +1243,7 @@ bool CameraFreenect2::init(const std::string & calibrationFolder)
 		// look for calibration files
 		if(!calibrationFolder.empty())
 		{
-			if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber()))
+			if(!stereoModel_.load(calibrationFolder, dev_->getSerialNumber(), false))
 			{
 				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, default calibration used.",
 						dev_->getSerialNumber().c_str(), calibrationFolder.c_str());
@@ -1300,7 +1309,7 @@ std::string CameraFreenect2::getSerial() const
 	return "";
 }
 
-void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy)
+void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, float & fy, float & cx, float & cy, double & stamp)
 {
 #ifdef WITH_FREENECT2
 	rgb = cv::Mat();
@@ -1309,11 +1318,13 @@ void CameraFreenect2::captureImage(cv::Mat & rgb, cv::Mat & depth, float & fx, f
 	fy = 0.0f;
 	cx = 0.0f;
 	cy = 0.0f;
+	stamp = 0.0;
 	if(dev_ && listener_)
 	{
 		libfreenect2::FrameMap frames;
 		if(listener_->waitForNewFrame(frames, 1000))
 		{
+			stamp = UTimer::now();
 			libfreenect2::Frame *rgbFrame = 0;
 			libfreenect2::Frame *irFrame = 0;
 			libfreenect2::Frame *depthFrame = 0;
@@ -1757,7 +1768,7 @@ public:
 				return false;
 			}
 			dc1394video_frame_t frame1 = *frame;
-			// deinterlace frame into two images one on top the other
+			// deinterlace frame into two imagesCount one on top the other
 			size_t frame1_size = frame->total_bytes;
 			frame1.image = (unsigned char *) malloc(frame1_size);
 			frame1.allocated_image_bytes = frame1_size;
@@ -1876,7 +1887,7 @@ std::string CameraStereoDC1394::getSerial() const
 	return "";
 }
 
-void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
+void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp)
 {
 #ifdef WITH_DC1394
 	left = cv::Mat();
@@ -1885,6 +1896,7 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f
 	baseline = 0.0f;
 	cx = 0.0f;
 	cy = 0.0f;
+	stamp = 0.0;
 	if(device_)
 	{
 		device_->getImages(left, right);
@@ -1896,6 +1908,7 @@ void CameraStereoDC1394::captureImage(cv::Mat & left, cv::Mat & right, float & f
 		cx = stereoModel_.left().cx();
 		cy = stereoModel_.left().cy();
 		baseline = stereoModel_.baseline();
+		stamp = UTimer::now();
 	}
 #else
 	UERROR("CameraDC1394: RTAB-Map is not built with dc1394 support!");
@@ -2056,7 +2069,7 @@ struct ImageContainer
 } ;
 #endif
 
-void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy)
+void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp)
 {
 #ifdef WITH_FLYCAPTURE2
 	left = cv::Mat();
@@ -2065,14 +2078,17 @@ void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, floa
 	baseline = 0.0f;
 	cx = 0.0f;
 	cy = 0.0f;
+	stamp = 0.0;
 
 	if(camera_ && triclopsCtx_ && camera_->IsConnected())
 	{
 		// grab image from camera.
-		// this image contains both right and left images
+		// this image contains both right and left imagesCount
 		FlyCapture2::Image grabbedImage;
 		if(camera_->RetrieveBuffer(&grabbedImage) == FlyCapture2::PGRERROR_OK)
 		{
+			stamp = UTimer::now();
+
 			// right and left image extracted from grabbed image
 			ImageContainer imageCont;
 
@@ -2174,4 +2190,195 @@ void CameraStereoFlyCapture2::captureImage(cv::Mat & left, cv::Mat & right, floa
 #endif
 }
 
+//
+// CameraStereoImages
+//
+bool CameraStereoImages::available()
+{
+	return true;
+}
+
+CameraStereoImages::CameraStereoImages(
+		const std::string & path,
+		const std::string & cameraName,
+		const std::string & timestampsPath,
+		float imageRate,
+		const Transform & localTransform) :
+		CameraRGBD(imageRate, localTransform),
+		camera_(0),
+		camera2_(0),
+		cameraName_(cameraName),
+		timestampsPath_(timestampsPath)
+{
+	std::vector<std::string> paths = uListToVector(uSplit(path, uStrContains(path, ":")?':':';'));
+	if(paths.size() >= 1)
+	{
+		camera_ = new CameraImages(paths[0]);
+
+		if(paths.size() >= 2)
+		{
+			camera2_ = new CameraImages(paths[1]);
+		}
+	}
+	else
+	{
+		UERROR("The path is empty!");
+	}
+}
+
+CameraStereoImages::~CameraStereoImages()
+{
+	if(camera_)
+	{
+		delete camera_;
+	}
+	if(camera2_)
+	{
+		delete camera2_;
+	}
+}
+
+bool CameraStereoImages::init(const std::string & calibrationFolder)
+{
+	// look for calibration files
+	if(!calibrationFolder.empty() && !cameraName_.empty())
+	{
+		if(!stereoModel_.load(calibrationFolder, cameraName_))
+		{
+			UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+					cameraName_.c_str(), calibrationFolder.c_str());
+		}
+		else
+		{
+			UINFO("Stereo parameters: fx=%f cx=%f cy=%f baseline=%f",
+					stereoModel_.left().fx(),
+					stereoModel_.left().cx(),
+					stereoModel_.left().cy(),
+					stereoModel_.baseline());
+		}
+	}
+	bool success = false;
+	if(camera_ == 0)
+	{
+		UERROR("Cannot initialize the camera.");
+	}
+	else if(camera_->init())
+	{
+		if(camera2_)
+		{
+			if(camera2_->init())
+			{
+				if(camera_->imagesCount() == camera2_->imagesCount())
+				{
+					success = true;
+				}
+				else
+				{
+					UERROR("Cameras don't have the same number of images (%d vs %d)",
+							camera_->imagesCount(), camera2_->imagesCount());
+				}
+			}
+			else
+			{
+				UERROR("Cannot initialize the second camera.");
+			}
+		}
+		else
+		{
+			success = true;
+		}
+	}
+
+	stamps_.clear();
+	if(success && timestampsPath_.size())
+	{
+		FILE * file = 0;
+#ifdef _MSC_VER
+		fopen_s(&file, timestampsPath_.c_str(), "r");
+#else
+		file = fopen(timestampsPath_.c_str(), "r");
+#endif
+		if(file)
+		{
+			char line[16];
+			while ( fgets (line , 16 , file) != NULL )
+			{
+				stamps_.push_back(uStr2Double(uReplaceChar(line, '\n', 0)));
+			}
+			fclose(file);
+		}
+		if(stamps_.size() != camera_->imagesCount())
+		{
+			UERROR("The stamps count is not the same as the images (%d vs %d)! Please remove "
+					"the timestamps file path if you don't want to use them (current file path=%s).",
+					(int)stamps_.size(), camera_->imagesCount(), timestampsPath_.c_str());
+			stamps_.clear();
+			success = false;
+		}
+	}
+
+	return success;
+}
+
+bool CameraStereoImages::isCalibrated() const
+{
+	return stereoModel_.isValid();
+}
+
+std::string CameraStereoImages::getSerial() const
+{
+	return "stereo_images";
+}
+
+void CameraStereoImages::captureImage(cv::Mat & left, cv::Mat & right, float & fx, float & baseline, float & cx, float & cy, double & stamp)
+{
+	left = cv::Mat();
+	right = cv::Mat();
+	fx = 0.0f;
+	baseline = 0.0f;
+	cx = 0.0f;
+	cy = 0.0f;
+	stamp = 0.0;
+
+	if(camera_)
+	{
+		if(stamps_.size())
+		{
+			stamp = stamps_.front();
+			stamps_.pop_front();
+		}
+		else
+		{
+			stamp = UTimer::now();
+		}
+		left = camera_->takeImage();
+		if(!left.empty())
+		{
+			if(camera2_)
+			{
+				right = camera2_->takeImage();
+			}
+			else
+			{
+				right = camera_->takeImage();
+			}
+
+			if(!right.empty())
+			{
+				// Rectification
+				//left = stereoModel_.left().rectifyImage(left);
+				//right = stereoModel_.right().rectifyImage(right);
+				fx = stereoModel_.left().fx();
+				cx = stereoModel_.left().cx();
+				cy = stereoModel_.left().cy();
+				baseline = stereoModel_.baseline();
+			}
+			else
+			{
+				left = cv::Mat();
+			}
+		}
+	}
+}
+
 } // namespace rtabmap

corelib/src/CameraThread.cpp

@@ -112,25 +112,26 @@ void CameraThread::mainLoop()
 	float fy = 0.0f;
 	float cx = 0.0f;
 	float cy = 0.0f;
+	double stamp = UTimer::now();
 	if(_cameraRGBD)
 	{
-		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy);
+		_cameraRGBD->takeImage(rgb, depth, fx, fy, cx, cy, stamp);
 	}
 	else
 	{
 		rgb = _camera->takeImage();
 	}
 
-	if(!rgb.empty() && !this->isKilled())
+	if(!rgb.empty())
 	{
 		if(_cameraRGBD)
 		{
-			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, UTimer::now());
+			SensorData data(rgb, depth, fx, fy, cx, cy, _cameraRGBD->getLocalTransform(), Transform(), 1, 1, ++_seq, stamp);
 			this->post(new CameraEvent(data, _cameraRGBD->getSerial()));
 		}
 		else
 		{
-			this->post(new CameraEvent(rgb, ++_seq, UTimer::now()));
+			this->post(new CameraEvent(rgb, ++_seq, stamp));
 		}
 	}
 	else if(!this->isKilled())

corelib/src/Graph.cpp

@@ -110,17 +110,19 @@ Optimizer * Optimizer::create(Optimizer::Type & type, const ParametersMap & para
 	return optimizer;
 }
 
-Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored) :
+Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon) :
 		iterations_(iterations),
 		slam2d_(slam2d),
-		covarianceIgnored_(covarianceIgnored)
+		covarianceIgnored_(covarianceIgnored),
+		epsilon_(epsilon)
 {
 }
 
 Optimizer::Optimizer(const ParametersMap & parameters) :
-		iterations_(100),
-		slam2d_(false),
-		covarianceIgnored_(false)
+		iterations_(Parameters::defaultRGBDOptimizeIterations()),
+		slam2d_(Parameters::defaultRGBDOptimizeSlam2D()),
+		covarianceIgnored_(Parameters::defaultRGBDOptimizeVarianceIgnored()),
+		epsilon_(Parameters::defaultRGBDOptimizeEpsilon())
 {
 	parseParameters(parameters);
 }
@@ -130,6 +132,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeIterations(), iterations_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeVarianceIgnored(), covarianceIgnored_);
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeSlam2D(), slam2d_);
+	Parameters::parse(parameters, Parameters::kRGBDOptimizeEpsilon(), epsilon_);
 }
 
 void Optimizer::getConnectedGraph(
@@ -350,6 +353,7 @@ std::map<int, Transform> TOROOptimizer::optimize(
 		}
 
 		UINFO("TORO iterate begin (iterations=%d)", iterations());
+		double lasterror = 0;
 		for (int i=0; i<iterations(); i++)
 		{
 			if(intermediateGraphes && i>0)
@@ -382,12 +386,14 @@ std::map<int, Transform> TOROOptimizer::optimize(
 				}
 				intermediateGraphes->push_back(tmpPoses);
 			}
+
+			double error = 0;
 			if(isSlam2d())
 			{
 				pg2.iterate();
 
 				// compute the error and dump it
-				double error=pg2.error();
+				error=pg2.error();
 				UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
 			}
 			else
@@ -396,10 +402,19 @@ std::map<int, Transform> TOROOptimizer::optimize(
 
 				// compute the error and dump it
 				double mte, mre, are, ate;
-				double error=pg3.error(&mre, &mte, &are, &ate);
+				error=pg3.error(&mre, &mte, &are, &ate);
 				UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
 						i, pg3.getRotGain(), error, error/pg3.edges.size());
 			}
+
+			// early stop condition
+			double errorDelta = lasterror - error;
+			if(i>0 && errorDelta < this->epsilon())
+			{
+				UDEBUG("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+				break;
+			}
+			lasterror = error;
 		}
 		UINFO("TORO iterate end");
 

corelib/src/Memory.cpp

@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_correspondences.h"
 #include "rtabmap/core/util3d_registration.h"
 #include "rtabmap/core/util3d_surface.h"
+#include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/Statistics.h"
 #include "rtabmap/core/Compression.h"
@@ -103,6 +104,9 @@ Memory::Memory(const ParametersMap & parameters) :
 	_bowForce2D(Parameters::defaultLccBowForce2D()),
 	_bowEpipolarGeometry(Parameters::defaultLccBowEpipolarGeometry()),
 	_bowEpipolarGeometryVar(Parameters::defaultLccBowEpipolarGeometryVar()),
+	_bowPnPEstimation(Parameters::defaultLccBowPnPEstimation()),
+	_bowPnPReprojError(Parameters::defaultLccBowPnPReprojError()),
+	_bowPnPFlags(Parameters::defaultLccBowPnPFlags()),
 
 	_icpMaxTranslation(Parameters::defaultLccIcpMaxTranslation()),
 	_icpMaxRotation(Parameters::defaultLccIcpMaxRotation()),
@@ -440,6 +444,9 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kLccBowForce2D(), _bowForce2D);
 	Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometry(), _bowEpipolarGeometry);
 	Parameters::parse(parameters, Parameters::kLccBowEpipolarGeometryVar(), _bowEpipolarGeometryVar);
+	Parameters::parse(parameters, Parameters::kLccBowPnPEstimation(), _bowPnPEstimation);
+	Parameters::parse(parameters, Parameters::kLccBowPnPReprojError(), _bowPnPReprojError);
+	Parameters::parse(parameters, Parameters::kLccBowPnPFlags(), _bowPnPFlags);
 	Parameters::parse(parameters, Parameters::kLccIcpMaxTranslation(), _icpMaxTranslation);
 	Parameters::parse(parameters, Parameters::kLccIcpMaxRotation(), _icpMaxRotation);
 	Parameters::parse(parameters, Parameters::kLccIcp3Decimation(), _icpDecimation);
@@ -604,13 +611,23 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 	//============================================================
 	// Transfer the oldest signature of the short-term memory to the working memory
 	//============================================================
-	while(_stMem.size() && _maxStMemSize>0 && (int)_stMem.size() > _maxStMemSize)
+	int validSignaturesCount = 0;
+	for(std::set<int>::iterator iter=_stMem.begin(); iter!=_stMem.end(); ++iter)
+	{
+		const Signature * s = this->getSignature(*iter);
+		UASSERT(s != 0);
+		if(!s->isBadSignature())
+		{
+			++validSignaturesCount;
+		}
+	}
+	while(_stMem.size() && _maxStMemSize>0 && validSignaturesCount > _maxStMemSize)
 	{
 		UDEBUG("Inserting node %d from STM in WM...", *_stMem.begin());
+		Signature * s = this->_getSignature(*_stMem.begin());
 		if(!_localSpaceLinksKeptInWM)
 		{
 			// remove local space links outside STM
-			Signature * s = this->_getSignature(*_stMem.begin());
 			UASSERT(s!=0);
 			std::map<int, Link> links = s->getLinks(); // get a copy because we will remove some links in "s"
 			for(std::map<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
@@ -630,6 +647,10 @@ bool Memory::update(const SensorData & data, Statistics * stats)
 				}
 			}
 		}
+		if(!s->isBadSignature())
+		{
+			--validSignaturesCount;
+		}
 		_workingMem.insert(_workingMem.end(), std::make_pair(*_stMem.begin(), UTimer::now()));
 		_stMem.erase(*_stMem.begin());
 		++_signaturesAdded;
@@ -851,6 +872,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 		int maxCheckedInDatabase, // default -1 (no limit)
 		bool incrementMarginOnLoop, // default false
 		bool ignoreLoopIds, // default false
+		bool ignoreBadSignatures, // default false
 		double * dbAccessTime
 		) const
 {
@@ -871,6 +893,7 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 	std::set<int> nextMargin;
 	nextMargin.insert(signatureId);
 	int m = 0;
+	std::set<int> ignoredIds;
 	while((maxGraphDepth == 0 || m < maxGraphDepth) && nextMargin.size())
 	{
 		// insert more recent first (priority to be loaded first from the database below if set)
@@ -888,7 +911,14 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 				const std::map<int, Link> * links = &tmpLinks;
 				if(s)
 				{
-					ids.insert(std::pair<int, int>(*jter, m));
+					if(!ignoreBadSignatures || !s->isBadSignature())
+					{
+						ids.insert(std::pair<int, int>(*jter, m));
+					}
+					else
+					{
+						ignoredIds.insert(*jter);
+					}
 
 					links = &s->getLinks();
 				}
@@ -908,12 +938,23 @@ std::map<int, int> Memory::getNeighborsId(int signatureId,
 				// links
 				for(std::map<int, Link>::const_iterator iter=links->begin(); iter!=links->end(); ++iter)
 				{
-					if( !uContains(ids, iter->first))
+					if( !uContains(ids, iter->first) && ignoredIds.find(iter->first) == ignoredIds.end())
 					{
 						UASSERT(iter->second.type() != Link::kUndef);
 						if(iter->second.type() == Link::kNeighbor)
 						{
-							nextMargin.insert(iter->first);
+							if(ignoreBadSignatures && s->isBadSignature())
+							{
+								// stay on the same margin
+								if(currentMargin.insert(iter->first).second)
+								{
+									curentMarginList.push_back(iter->first);
+								}
+							}
+							else
+							{
+								nextMargin.insert(iter->first);
+							}
 						}
 						else if(!ignoreLoopIds)
 						{
@@ -1915,7 +1956,134 @@ Transform Memory::computeVisualTransform(
 	std::string msg;
 	// Guess transform from visual words
 
-	if(_bowEpipolarGeometry)
+	if(_bowPnPEstimation)
+	{
+		if(_bowEpipolarGeometry)
+		{
+			UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
+		}
+
+		// 2D -> 3D
+		if(!oldS.getWords3().empty() && !newS.getWords().empty())
+		{
+			// find correspondences
+			std::vector<int> ids = uListToVector(uUniqueKeys(newS.getWords()));
+			std::vector<cv::Point3f> objectPoints(ids.size());
+			std::vector<cv::Point2f> imagePoints(ids.size());
+			int oi=0;
+			std::vector<int> matches(ids.size());
+			for(unsigned int i=0; i<ids.size(); ++i)
+			{
+				if(oldS.getWords3().count(ids[i]) == 1)
+				{
+					pcl::PointXYZ pt = oldS.getWords3().find(ids[i])->second;
+					if(pcl::isFinite(pt))
+					{
+						objectPoints[oi].x = pt.x;
+						objectPoints[oi].y = pt.y;
+						objectPoints[oi].z = pt.z;
+						imagePoints[oi] = newS.getWords().find(ids[i])->second.pt;
+						matches[oi++] = ids[i];
+					}
+				}
+			}
+
+			objectPoints.resize(oi);
+			imagePoints.resize(oi);
+			matches.resize(oi);
+
+			if((int)matches.size() >= _bowMinInliers)
+			{
+				//PnPRansac
+				cv::Mat K = (cv::Mat_<double>(3,3) <<
+					newS.getFx(), 0, newS.getCx(),
+					0, newS.getFy()>1?newS.getFy():newS.getFx(), newS.getCy(),
+					0, 0, 1);
+
+				Transform guess = (newS.getLocalTransform()).inverse();
+				cv::Mat R = (cv::Mat_<double>(3,3) <<
+						(double)guess.r11(), (double)guess.r12(), (double)guess.r13(),
+						(double)guess.r21(), (double)guess.r22(), (double)guess.r23(),
+						(double)guess.r31(), (double)guess.r32(), (double)guess.r33());
+				cv::Mat rvec(1,3, CV_64FC1);
+				cv::Rodrigues(R, rvec);
+				cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guess.x(), (double)guess.y(), (double)guess.z());
+				std::vector<int> inliersV;
+				cv::solvePnPRansac(objectPoints,
+						imagePoints,
+						K,
+						cv::Mat(),
+						rvec,
+						tvec,
+						true,
+						_bowIterations,
+						_bowPnPReprojError,
+						0,
+						inliersV,
+						_bowPnPFlags);
+
+				if(inliers)
+				{
+					*inliers = (int)inliersV.size();
+				}
+				if((int)inliersV.size() >= _bowMinInliers)
+				{
+					cv::Rodrigues(rvec, R);
+					Transform pnp(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), tvec.at<double>(0),
+								   R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), tvec.at<double>(1),
+								   R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvec.at<double>(2));
+
+					transform = newS.getLocalTransform() * pnp;
+
+					UDEBUG("Odom transform = %s", transform.prettyPrint().c_str());
+
+					// compute variance (like in PCL computeVariance() method of sac_model.h)
+					if(varianceOut)
+					{
+						std::vector<float> errorSqrdDists(inliersV.size());
+						oi = 0;
+						for(unsigned int i=0; i<inliersV.size(); ++i)
+						{
+							std::multimap<int, pcl::PointXYZ>::const_iterator iter = newS.getWords3().find(matches[inliersV[i]]);
+							if(iter != newS.getWords3().end() && pcl::isFinite(iter->second))
+							{
+								const cv::Point3f & objPt = objectPoints[inliersV[i]];
+								pcl::PointXYZ newPt = util3d::transformPoint(iter->second, transform);
+								errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+							}
+						}
+						errorSqrdDists.resize(oi);
+						*varianceOut= 0;
+						if(errorSqrdDists.size())
+						{
+							std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+							double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+							*varianceOut = 2.1981 * median_error_sqr;
+						}
+					}
+				}
+				else
+				{
+					msg = uFormat("PnP not enough inliers (%d[%d] < %d), rejecting the transform...",
+							(int)inliersV.size(), (int)matches.size(), _bowMinInliers);
+					UINFO(msg.c_str());
+				}
+			}
+			else
+			{
+				msg = uFormat("Not enough inliers %d < %d", (int)matches.size(), _bowMinInliers);
+				UINFO(msg.c_str());
+			}
+		}
+		else
+		{
+			msg = uFormat("Not enough features in the new image (old=%d new=%d min=%d)",
+					(int)oldS.getWords3().size(), (int)newS.getWords().size(), _bowMinInliers);
+			UINFO(msg.c_str());
+		}
+
+	}
+	else if(_bowEpipolarGeometry)
 	{
 		// we only need the camera transform, send guess words3 for scale estimation
 		if(oldS.getWords3().size())
@@ -1988,6 +2156,7 @@ Transform Memory::computeVisualTransform(
 	}
 	else
 	{
+		// 3D -> 3D
 		if(!oldS.getWords3().empty() && !newS.getWords3().empty())
 		{
 			pcl::PointCloud<pcl::PointXYZ>::Ptr inliersOld(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2887,72 +3056,97 @@ void Memory::rehearsal(Signature * signature, Statistics * stats)
 	}
 
 	//============================================================
-	// Compare with the last
+	// Compare with the last (not null)
 	//============================================================
-	int id = signature->getLinks().begin()->first;
-	UDEBUG("Comparing with last signature (%d)...", id);
-	Signature * sB = this->_getSignature(id);
-	if(!sB)
+	Signature * sB = 0;
+	for(std::set<int>::reverse_iterator iter=_stMem.rbegin(); iter!=_stMem.rend(); ++iter)
 	{
-		UFATAL("Signature %d null?!?", id);
-	}
-	float sim = signature->compareTo(*sB);
-
-	int merged = 0;
-	if(sim >= _similarityThreshold)
-	{
-		if(_incrementalMemory)
+		Signature * s = this->_getSignature(*iter);
+		UASSERT(s!=0);
+		if(!s->isBadSignature() && s->id() != signature->id())
 		{
-			if(signature->getLinks().begin()->second.transform().isNull())
+			sB = s;
+			break;
+		}
+	}
+	if(sB)
+	{
+		int id = sB->id();
+		UDEBUG("Comparing with signature (%d)...", id);
+
+		float sim = signature->compareTo(*sB);
+
+		int merged = 0;
+		if(sim >= _similarityThreshold)
+		{
+			if(_incrementalMemory)
 			{
-				if(this->rehearsalMerge(id, signature->id()))
+				if(signature->hasLink(id))
 				{
-					merged = id;
+					if(signature->getLinks().begin()->second.transform().isNull())
+					{
+						if(this->rehearsalMerge(id, signature->id()))
+						{
+							merged = id;
+						}
+					}
+					else
+					{
+						float x,y,z, roll,pitch,yaw;
+						signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+						if((_rehearsalMaxDistance>0.0f && (
+								fabs(x) > _rehearsalMaxDistance ||
+								fabs(y) > _rehearsalMaxDistance ||
+								fabs(z) > _rehearsalMaxDistance)) ||
+							(_rehearsalMaxAngle>0.0f && (
+								fabs(roll) > _rehearsalMaxAngle ||
+								fabs(pitch) > _rehearsalMaxAngle ||
+								fabs(yaw) > _rehearsalMaxAngle)))
+						{
+							if(_rehearsalWeightIgnoredWhileMoving)
+							{
+								UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad",
+										_rehearsalMaxDistance, _rehearsalMaxAngle);
+							}
+							else
+							{
+								// if the robot has moved, increase only weight of the new one
+								signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
+								sB->setWeight(0);
+								UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
+										signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
+							}
+						}
+						else if(this->rehearsalMerge(id, signature->id()))
+						{
+							merged = id;
+						}
+					}
+				}
+				else
+				{
+					// cannot merge not neighbor signatures, just update weight
+					signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
+					sB->setWeight(0);
+					UINFO("Only updated weight to %d of %d (old=%d) because the signatures are not neighbors.",
+							signature->getWeight(), signature->id(), sB->id());
 				}
 			}
 			else
 			{
-				float x,y,z, roll,pitch,yaw;
-				signature->getLinks().begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
-				if((_rehearsalMaxDistance>0.0f && (
-						fabs(x) > _rehearsalMaxDistance ||
-						fabs(y) > _rehearsalMaxDistance ||
-						fabs(z) > _rehearsalMaxDistance)) ||
-					(_rehearsalMaxAngle>0.0f && (
-						fabs(roll) > _rehearsalMaxAngle ||
-						fabs(pitch) > _rehearsalMaxAngle ||
-						fabs(yaw) > _rehearsalMaxAngle)))
-				{
-					if(_rehearsalWeightIgnoredWhileMoving)
-					{
-						UINFO("Rehearsal ignored because the robot has moved more than %f m or %f rad",
-								_rehearsalMaxDistance, _rehearsalMaxAngle);
-					}
-					else
-					{
-						// if the robot has moved, increase only weight of the new one
-						signature->setWeight(sB->getWeight() + signature->getWeight() + 1);
-						sB->setWeight(0);
-						UINFO("Only updated weight to %d of %d (old=%d) because the robot has moved. (d=%f a=%f)",
-								signature->getWeight(), signature->id(), sB->id(), _rehearsalMaxDistance, _rehearsalMaxAngle);
-					}
-				}
-				else if(this->rehearsalMerge(id, signature->id()))
-				{
-					merged = id;
-				}
+				signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
 			}
 		}
-		else
-		{
-			signature->setWeight(signature->getWeight() + 1 + sB->getWeight());
-		}
+
+		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
+		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
+		UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
+	}
+	else
+	{
+		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), 0);
+		if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), 0);
 	}
-
-	if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_merged(), merged);
-	if(stats) stats->addStatistic(Statistics::kMemoryRehearsal_sim(), sim);
-
-	UDEBUG("merged=%d, sim=%f t=%fs", merged, sim, timer.ticks());
 }
 
 bool Memory::rehearsalMerge(int oldId, int newId)
@@ -3608,7 +3802,7 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 	pcl::PointCloud<pcl::PointXYZ>::Ptr keypoints3D(new pcl::PointCloud<pcl::PointXYZ>);
 	if(data.keypoints().size() == 0)
 	{
-		if(_feature2D->getMaxFeatures() >= 0)
+		if(_feature2D->getMaxFeatures() >= 0 && !data.image().empty())
 		{
 			// Extract features
 			cv::Mat imageMono;
@@ -3812,6 +4006,10 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 				descriptors = cv::Mat();
 			}
 		}
+		else if(data.image().empty())
+		{
+			UDEBUG("Empty image, cannot extract features...");
+		}
 		else
 		{
 			UDEBUG("_feature2D->getMaxFeatures()(%d<0) so don't extract any features...", _feature2D->getMaxFeatures());
@@ -4061,11 +4259,11 @@ Signature * Memory::createSignature(const SensorData & data, Statistics * stats)
 			rtabmap::compressData2(laserScan),
 			cv::Mat(),
 			cv::Mat(),
-			0,
-			0,
-			0,
-			0,
-			Transform(),
+			fx,
+			fyOrBaseline,
+			cx,
+			cy,
+			data.localTransform(),
 			data.laserScanMaxPts());
 	}
 	if(this->isRawDataKept())
@@ -4309,7 +4507,42 @@ void Memory::getMetricConstraints(
 					uContains(poses, jter->first) &&
 					graph::findLink(links, *iter, jter->first) == links.end())
 				{
-					links.insert(std::make_pair(*iter, jter->second));
+					// Remove bad signatures from the graph (Intermediate nodes)
+					if(!lookInDatabase)
+					{
+						Link link = jter->second;
+						const Signature * s = this->getSignature(jter->first);
+						UASSERT(s!=0);
+						while(s && s->isBadSignature())
+						{
+							// skip to next neighbor, well we assume that bad signatures
+							// are only linked by max 2 neighbor links.
+							std::map<int, Link> n = this->getNeighborLinks(s->id(), false);
+							UASSERT(n.size() <= 2);
+							std::map<int, Link>::iterator uter = n.upper_bound(s->id());
+							if(uter != n.end())
+							{
+								const Signature * s2 = this->getSignature(uter->first);
+								if(s2)
+								{
+									link = link.merge(uter->second);
+									poses.erase(s->id());
+									s = s2;
+								}
+
+							}
+							else
+							{
+								break;
+							}
+						}
+
+						links.insert(std::make_pair(*iter, link));
+					}
+					else
+					{
+						links.insert(std::make_pair(*iter, jter->second));
+					}
 				}
 			}
 

corelib/src/Odometry.cpp

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/OdometryInfo.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
+#include "ParticleFilter.h"
 
 namespace rtabmap {
 
@@ -41,11 +42,18 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_maxDepth(Parameters::defaultOdomMaxDepth()),
 		_resetCountdown(Parameters::defaultOdomResetCountdown()),
 		_force2D(Parameters::defaultOdomForce2D()),
+		_particleFiltering(Parameters::defaultOdomParticleFiltering()),
+		_particleSize(Parameters::defaultOdomParticleSize()),
+		_particleNoiseT(Parameters::defaultOdomParticleNoiseT()),
+		_particleLambdaT(Parameters::defaultOdomParticleLambdaT()),
+		_particleNoiseR(Parameters::defaultOdomParticleNoiseR()),
+		_particleLambdaR(Parameters::defaultOdomParticleLambdaR()),
 		_fillInfoData(Parameters::defaultOdomFillInfoData()),
 		_pnpEstimation(Parameters::defaultOdomPnPEstimation()),
 		_pnpReprojError(Parameters::defaultOdomPnPReprojError()),
 		_pnpFlags(Parameters::defaultOdomPnPFlags()),
-		_resetCurrentCount(0)
+		_resetCurrentCount(0),
+		previousStamp_(0)
 {
 	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
 	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
@@ -60,21 +68,78 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomPnPReprojError(), _pnpReprojError);
 	Parameters::parse(parameters, Parameters::kOdomPnPFlags(), _pnpFlags);
 	UASSERT(_pnpFlags>=0 && _pnpFlags <=2);
+	Parameters::parse(parameters, Parameters::kOdomParticleFiltering(), _particleFiltering);
+	Parameters::parse(parameters, Parameters::kOdomParticleSize(), _particleSize);
+	Parameters::parse(parameters, Parameters::kOdomParticleNoiseT(), _particleNoiseT);
+	Parameters::parse(parameters, Parameters::kOdomParticleLambdaT(), _particleLambdaT);
+	Parameters::parse(parameters, Parameters::kOdomParticleNoiseR(), _particleNoiseR);
+	Parameters::parse(parameters, Parameters::kOdomParticleLambdaR(), _particleLambdaR);
+	UASSERT(_particleNoiseT>0);
+	UASSERT(_particleLambdaT>0);
+	UASSERT(_particleNoiseR>0);
+	UASSERT(_particleLambdaR>0);
+	if(_particleFiltering)
+	{
+		filters_.resize(6);
+		for(unsigned int i = 0; i<filters_.size(); ++i)
+		{
+			if(i<3)
+			{
+				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseT, _particleLambdaT);
+			}
+			else
+			{
+				filters_[i] = new ParticleFilter(_particleSize, _particleNoiseR, _particleLambdaR);
+			}
+		}
+	}
+}
+
+Odometry::~Odometry()
+{
+	for(unsigned int i=0; i<filters_.size(); ++i)
+	{
+		delete filters_[i];
+	}
+	filters_.clear();
 }
 
 void Odometry::reset(const Transform & initialPose)
 {
 	_resetCurrentCount = 0;
-	if(_force2D)
+	previousStamp_ = 0;
+	if(_force2D || filters_.size())
 	{
 		float x,y,z, roll,pitch,yaw;
 		initialPose.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-		if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
+
+		if(_force2D)
 		{
-			UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
+			if(z != 0.0f || roll != 0.0f || yaw != 0.0f)
+			{
+				UWARN("Force2D=true and the initial pose contains z, roll or pitch values (%s). They are set to null.", initialPose.prettyPrint().c_str());
+			}
+			z = 0;
+			roll = 0;
+			yaw = 0;
+			Transform pose(x, y, z, roll, pitch, yaw);
+			_pose = pose;
+		}
+		else
+		{
+			_pose = initialPose;
+		}
+
+		if(filters_.size())
+		{
+			UASSERT(filters_.size() == 6);
+			filters_[0]->init(x);
+			filters_[1]->init(y);
+			filters_[2]->init(z);
+			filters_[3]->init(roll);
+			filters_[4]->init(pitch);
+			filters_[5]->init(yaw);
 		}
-		Transform pose(x, y, 0, 0, 0, yaw);
-		_pose = pose;
 	}
 	else
 	{
@@ -107,19 +172,48 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
 
 	if(info)
 	{
-		info->time = time.elapsed();
+		info->timeEstimation = time.ticks();
 		info->lost = t.isNull();
+		info->stamp = data.stamp();
+		info->interval = data.stamp() - previousStamp_;
+		info->transform = t;
 	}
+	previousStamp_ = data.stamp();
 
 	if(!t.isNull())
 	{
 		_resetCurrentCount = _resetCountdown;
 
-		if(_force2D)
+		if(_force2D || filters_.size())
 		{
 			float x,y,z, roll,pitch,yaw;
 			t.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
-			t = Transform(x,y,0, 0,0,yaw);
+
+			if(filters_.size())
+			{
+				UASSERT(filters_.size()==6);
+				x = filters_[0]->filter(x);
+				y = filters_[1]->filter(y);
+				yaw = filters_[5]->filter(yaw);
+
+				if(!_force2D)
+				{
+					z = filters_[2]->filter(z);
+					roll = filters_[3]->filter(roll);
+					pitch = filters_[4]->filter(pitch);
+				}
+
+				if(info)
+				{
+					info->timeParticleFiltering = time.ticks();
+				}
+			}
+			t = Transform(x,y,_force2D?0:z, _force2D?0:roll,_force2D?0:pitch,yaw);
+
+			if(info)
+			{
+				info->transformFiltered = t;
+			}
 		}
 
 		return _pose *= t; // updated

corelib/src/OdometryBOW.cpp

@@ -235,17 +235,24 @@ Transform OdometryBOW::computeTransform(
 
 								// compute variance (like in PCL computeVariance() method of sac_model.h)
 								std::vector<float> errorSqrdDists(inliersV.size());
+								oi = 0;
 								for(unsigned int i=0; i<inliersV.size(); ++i)
 								{
 									std::multimap<int, pcl::PointXYZ>::const_iterator iter = newSignature->getWords3().find(matches[inliersV[i]]);
-									UASSERT(iter != newSignature->getWords3().end());
-									const cv::Point3f & objPt = objectPoints[inliersV[i]];
-									pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
-									errorSqrdDists[i] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+									if(iter != newSignature->getWords3().end() && pcl::isFinite(iter->second))
+									{
+										const cv::Point3f & objPt = objectPoints[inliersV[i]];
+										pcl::PointXYZ newPt = util3d::transformPoint(iter->second, this->getPose()*transform);
+										errorSqrdDists[oi++] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
+									}
+								}
+								errorSqrdDists.resize(oi);
+								if(errorSqrdDists.size())
+								{
+									std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
+									double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
+									variance = 2.1981 * median_error_sqr;
 								}
-								std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
-								double median_error_sqr = (double)errorSqrdDists[errorSqrdDists.size () >> 1];
-								variance = 2.1981 * median_error_sqr;
 							}
 							else
 							{

corelib/src/OdometryThread.cpp

@@ -34,8 +34,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-OdometryThread::OdometryThread(Odometry * odometry) :
+OdometryThread::OdometryThread(Odometry * odometry, unsigned int dataBufferMaxSize) :
 	_odometry(odometry),
+	_dataBufferMaxSize(dataBufferMaxSize),
 	_resetOdometry(false)
 {
 	UASSERT(_odometry != 0);
@@ -92,8 +93,7 @@ void OdometryThread::mainLoop()
 	}
 
 	SensorData data;
-	getData(data);
-	if(data.isValid())
+	if(getData(data))
 	{
 		OdometryInfo info;
 		Transform pose = _odometry->process(data, &info);
@@ -124,8 +124,13 @@ void OdometryThread::addData(const SensorData & data)
 	bool notify = true;
 	_dataMutex.lock();
 	{
-		notify = !_dataBuffer.isValid();
-		_dataBuffer = data;
+		_dataBuffer.push_back(data);
+		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();
 
@@ -135,18 +140,21 @@ void OdometryThread::addData(const SensorData & data)
 	}
 }
 
-void OdometryThread::getData(SensorData & data)
+bool OdometryThread::getData(SensorData & data)
 {
+	bool dataFilled = false;
 	_dataAdded.acquire();
 	_dataMutex.lock();
 	{
-		if(_dataBuffer.isValid())
+		if(!_dataBuffer.empty())
 		{
-			data = _dataBuffer;
-			_dataBuffer = SensorData();
+			data = _dataBuffer.front();
+			_dataBuffer.pop_front();
+			dataFilled = true;
 		}
 	}
 	_dataMutex.unlock();
+	return dataFilled;
 }
 
 } // namespace rtabmap

corelib/src/ParticleFilter.h

@@ -0,0 +1,173 @@
+/*
+Copyright (c) 2010-2015, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef PARTICLEFILTER_H_
+#define PARTICLEFILTER_H_
+
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/ULogger.h>
+
+
+namespace rtabmap {
+
+// taken from http://www.developpez.net/forums/d544518/c-cpp/c/equivalent-randn-matlab-c/
+#define TWOPI (6.2831853071795864769252867665590057683943387987502) /* 2 * pi */
+
+/*
+   RAND is a macro which returns a pseudo-random numbers from a uniform
+   distribution on the interval [0 1]
+*/
+#define RAND (rand())/((double) RAND_MAX)
+
+/*
+   RANDN is a macro which returns a pseudo-random numbers from a normal
+   distribution with mean zero and standard deviation one. This macro uses Box
+   Muller's algorithm
+*/
+#define RANDN (sqrt(-2.0*log(RAND))*cos(TWOPI*RAND))
+
+std::vector<double> cumSum(const std::vector<double> & v)
+{
+	std::vector<double> cum(v.size());
+	double sum = 0;
+	for(unsigned int i=0; i<v.size(); ++i)
+	{
+		cum[i] = v[i] + sum;
+		sum += v[i];
+	}
+	return cum;
+}
+
+std::vector<double> resample(const std::vector<double> & p, // particles
+							 const std::vector<double> & w, // weights
+							 bool normalizeWeights = false)
+{
+	std::vector<double> np; //new particles
+	if(p.size() != w.size() || p.size() == 0)
+	{
+		UERROR("particles (%d) and weights (%d) are not the same size", p.size(), w.size());
+		return np;
+	}
+
+	std::vector<double> cs;
+	if(normalizeWeights)
+	{
+		double wSum = uSum(w);
+		std::vector<double> wNorm(w.size());
+		for(unsigned int i=0; i<w.size(); ++i)
+		{
+			wNorm[i] = w[i]/wSum;
+		}
+		cs = cumSum(wNorm); // cumulative sum
+	}
+	else
+	{
+		cs = cumSum(w); // cumulative sum
+	}
+	for(unsigned int j=0; j<cs.size(); ++j)
+	{
+		cs[j]/=cs.back();
+	}
+
+	np.resize(p.size());
+	for(unsigned int i=0; i<np.size(); ++i)
+	{
+		unsigned int index = 0;
+		double randnum = RAND;
+		for(unsigned int j=0; j<cs.size(); ++j)
+		{
+			if(randnum < cs[j])
+			{
+				index = j;
+				break;
+			}
+		}
+		np[i] = p[index];
+	}
+	return np;
+}
+
+
+class ParticleFilter
+{
+public:
+	ParticleFilter(unsigned int nParticles = 200,
+				   double noise = 0.1,
+				   double lambda = 10.0,
+				   double initValue = 0.0) :
+		noise_(noise),
+		lambda_(lambda)
+	{
+		particles_.resize(nParticles, initValue);
+	}
+
+	void init(double initValue = 0.0f)
+	{
+		particles_ = std::vector<double>(particles_.size(), initValue);
+	}
+
+	double filter(double val)
+	{
+		std::vector<double> weights(particles_.size());
+		double sumWeights = 0;
+		for(unsigned int i=0; i<particles_.size(); ++i)
+		{
+			// add noise to particle
+			particles_[i] += noise_ * RANDN;
+
+			// compute weight
+			double dist = fabs(particles_[i] - val);
+			//dist = sqrt(dist*dist);
+			weights[i] = exp(-lambda_*dist);
+			sumWeights += weights[i];
+		}
+
+
+		//normalize and compute estimated value
+		double value =0.0;
+		for(unsigned int i=0; i<weights.size(); ++i)
+		{
+			weights[i] /= sumWeights;
+			value += weights[i] * particles_[i];
+		}
+
+		//resample the particles
+		particles_ = resample(particles_, weights, false);
+
+		return value;
+	}
+
+private:
+	std::vector<double> particles_;
+	double noise_;
+	double lambda_;
+};
+
+}
+
+
+#endif /* PARTICLEFILTER_H_ */

corelib/src/Rtabmap.cpp

@@ -737,6 +737,27 @@ void Rtabmap::generateTOROGraph(const std::string & path, bool optimized, bool g
 	}
 }
 
+void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global)
+{
+	if(_memory && _memory->getLastWorkingSignature())
+	{
+		std::map<int, Transform> poses;
+		std::multimap<int, Link> constraints;
+
+		if(optimized)
+		{
+			this->optimizeCurrentMap(_memory->getLastWorkingSignature()->id(), global, poses, &constraints);
+		}
+		else
+		{
+			std::map<int, int> ids = _memory->getNeighborsId(_memory->getLastWorkingSignature()->id(), 0, global?-1:0, true);
+			_memory->getMetricConstraints(uKeysSet(ids), poses, constraints, global);
+		}
+
+		this->dumpPoses(path, poses);
+	}
+}
+
 void Rtabmap::resetMemory()
 {
 	_highestHypothesis = std::make_pair(0,0.0f);
@@ -829,11 +850,6 @@ bool Rtabmap::process(const SensorData & data)
 	// Wait for an image...
 	//============================================================
 	ULOGGER_INFO("getting data...");
-	if(!data.isValid())
-	{
-		ULOGGER_INFO("image is not valid...");
-		return false;
-	}
 
 	timer.start();
 	timerTotal.start();
@@ -1002,18 +1018,25 @@ bool Rtabmap::process(const SensorData & data)
 		if(signature->getLinks().size() == 1)
 		{
 			// link should be old to new
-			if(signature->id() > signature->getLinks().begin()->second.to())
+			UASSERT_MSG(signature->id() > signature->getLinks().begin()->second.to(),
+					"Only forward links should be added.");
+
+			Link tmp = signature->getLinks().begin()->second.inverse();
+
+			// if the previous signature is a bad signature, remove it from the local graph
+			if(_constraints.size() &&
+			   _constraints.rbegin()->second.to() == signature->getLinks().begin()->second.to())
 			{
-				Link tmp = signature->getLinks().begin()->second;
-				tmp.setFrom(tmp.to());
-				tmp.setTo(signature->id());
-				tmp.setTransform(tmp.transform().inverse());
-				_constraints.insert(std::make_pair(tmp.from(), tmp));
-			}
-			else
-			{
-				_constraints.insert(std::make_pair(signature->id(), signature->getLinks().begin()->second));
+				const Signature * s = _memory->getSignature(signature->getLinks().begin()->second.to());
+				UASSERT(s!=0);
+				if(s->isBadSignature())
+				{
+					tmp = _constraints.rbegin()->second.merge(tmp);
+					_optimizedPoses.erase(s->id());
+					_constraints.erase(--_constraints.end());
+				}
 			}
+			_constraints.insert(std::make_pair(tmp.from(), tmp));
 		}
 
 		//============================================================
@@ -1090,7 +1113,29 @@ bool Rtabmap::process(const SensorData & data)
 			// with all images contained in the working memory + reactivated.
 			//============================================================
 			ULOGGER_INFO("computing likelihood...");
-			std::list<int> signaturesToCompare = uKeysList(_memory->getWorkingMem());
+
+			// select only not empty signatures (may happen often if intermediate nodes are created)
+			std::list<int> signaturesToCompare;
+			for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
+				iter!=_memory->getWorkingMem().end();
+				++iter)
+			{
+				if(iter->first > 0)
+				{
+					const Signature * s = _memory->getSignature(iter->first);
+					UASSERT(s!=0);
+					if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature())
+					{
+						signaturesToCompare.push_back(iter->first);
+					}
+				}
+				else
+				{
+					// virtual signature should be added
+					signaturesToCompare.push_back(iter->first);
+				}
+			}
+
 			rawLikelihood = _memory->computeLikelihood(signature, signaturesToCompare);
 
 			// Adjust the likelihood (with mean and std dev)
@@ -1228,6 +1273,7 @@ bool Rtabmap::process(const SensorData & data)
 				_maxRetrieved,
 				true,
 				true,
+				false,
 				&timeGetNeighborsTimeDb);
 		ULOGGER_DEBUG("neighbors of %d in time = %d", retrievalId, (int)neighbors.size());
 		//Priority to locations near in time (direct neighbor) then by space (loop closure)
@@ -1281,6 +1327,7 @@ bool Rtabmap::process(const SensorData & data)
 				_maxRetrieved,
 				true,
 				false,
+				false,
 				&timeGetNeighborsSpaceDb);
 		ULOGGER_DEBUG("neighbors of %d in space = %d", retrievalId, (int)neighbors.size());
 		firstPassDone = false;
@@ -2426,6 +2473,37 @@ void Rtabmap::dumpData() const
 	}
 }
 
+void Rtabmap::dumpPoses(
+		const std::string & path,
+		const std::map<int, Transform> & poses) const
+{
+	UDEBUG("");
+	FILE* fout = 0;
+#ifdef _MSC_VER
+	fopen_s(&fout, path.c_str(), "w");
+#else
+	fout = fopen(path.c_str(), "w");
+#endif
+	if(fout)
+	{
+		Transform localTransformInv = Transform(0,0,0, -CV_PI/2, 0, -CV_PI/2).inverse();
+		for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+		{
+			Transform t = localTransformInv * (*iter).second;
+			// in camera frame
+			const float * p = (const float *)t.data();
+
+			fprintf(fout, "%f", p[0]);
+			for(int i=1; i<(*iter).second.size(); i++)
+			{
+				fprintf(fout, " %f", p[i]);
+			}
+			fprintf(fout, "\n");
+		}
+		fclose(fout);
+	}
+}
+
 // fromId must be in _memory and in _optimizedPoses
 // Get poses in front of the robot, return optimized poses
 std::map<int, Transform> Rtabmap::getForwardWMPoses(
@@ -2586,7 +2664,7 @@ void Rtabmap::optimizeCurrentMap(
 	if(_memory && id > 0)
 	{
 		UTimer timer;
-		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true);
+		std::map<int, int> ids = _memory->getNeighborsId(id, 0, lookInDatabase?-1:0, true, false);
 		if(!_optimizeFromGraphEnd && ids.size() > 1)
 		{
 			id = ids.begin()->first;
@@ -2713,7 +2791,27 @@ void Rtabmap::dumpPrediction() const
 {
 	if(_memory && _bayesFilter)
 	{
-		cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uKeys(_memory->getWorkingMem()));
+		std::list<int> signaturesToCompare;
+		for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
+			iter!=_memory->getWorkingMem().end();
+			++iter)
+		{
+			if(iter->first > 0)
+			{
+				const Signature * s = _memory->getSignature(iter->first);
+				UASSERT(s!=0);
+				if(!_bayesFilter->isBadSignaturesIgnored() || !s->isBadSignature())
+				{
+					signaturesToCompare.push_back(iter->first);
+				}
+			}
+			else
+			{
+				// virtual signature should be added
+				signaturesToCompare.push_back(iter->first);
+			}
+		}
+		cv::Mat prediction = _bayesFilter->generatePrediction(_memory, uListToVector(signaturesToCompare));
 
 		FILE* fout = 0;
 		std::string fileName = this->getWorkingDir() + "/DumpPrediction.txt";

corelib/src/RtabmapThread.cpp

@@ -46,6 +46,7 @@ namespace rtabmap {
 RtabmapThread::RtabmapThread(Rtabmap * rtabmap) :
 		_dataBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
 		_rate(Parameters::defaultRtabmapDetectionRate()),
+		_createIntermediateNodes(Parameters::defaultRtabmapCreateIntermediateNodes()),
 		_frameRateTimer(new UTimer()),
 		_rtabmap(rtabmap),
 		_paused(false),
@@ -106,10 +107,14 @@ void RtabmapThread::setDetectorRate(float rate)
 	_rate = rate;
 }
 
-void RtabmapThread::setBufferSize(int bufferSize)
+void RtabmapThread::setDataBufferSize(unsigned int size)
 {
-	UASSERT(bufferSize >= 0);
-	_dataBufferMaxSize = bufferSize;
+	_dataBufferMaxSize = size;
+}
+
+void RtabmapThread::createIntermediateNodes(bool enabled)
+{
+	enabled = _createIntermediateNodes;
 }
 
 void RtabmapThread::publishMap(bool optimized, bool full) const
@@ -206,6 +211,7 @@ void RtabmapThread::mainLoop()
 		UASSERT(!parameters.at("RtabmapThread/DatabasePath").empty());
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
 		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->init(parameters, parameters.at("RtabmapThread/DatabasePath"));
@@ -213,6 +219,7 @@ void RtabmapThread::mainLoop()
 	case kStateChangingParameters:
 		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _dataBufferMaxSize);
 		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		Parameters::parse(parameters, Parameters::kRtabmapCreateIntermediateNodes(), _createIntermediateNodes);
 		UASSERT(_dataBufferMaxSize >= 0);
 		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
@@ -247,6 +254,12 @@ void RtabmapThread::mainLoop()
 	case kStateGeneratingTOROGraphGlobal:
 		_rtabmap->generateTOROGraph(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
 		break;
+	case kStateExportingPosesLocal:
+		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, false);
+		break;
+	case kStateExportingPosesGlobal:
+		_rtabmap->exportPoses(parameters.at("path"), atoi(parameters.at("optimized").c_str())!=0, true);
+		break;
 	case kStateCleanDataBuffer:
 		this->clearBufferedData();
 		break;
@@ -418,6 +431,28 @@ void RtabmapThread::handleEvent(UEvent* event)
 			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
 			pushNewState(kStateGeneratingTOROGraphGlobal, param);
 
+		}
+		else if(cmd == RtabmapEventCmd::kCmdExportPosesLocal)
+		{
+			UASSERT(!rtabmapEvent->getStr().empty());
+
+			ULOGGER_DEBUG("CMD_EXPORT_POSES_LOCAL");
+			ParametersMap param;
+			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateExportingPosesLocal, param);
+
+		}
+		else if(cmd == RtabmapEventCmd::kCmdExportPosesGlobal)
+		{
+			UASSERT(!rtabmapEvent->getStr().empty());
+
+			ULOGGER_DEBUG("CMD_EXPORT_POSES_GLOBAL");
+			ParametersMap param;
+			param.insert(ParametersPair("path", rtabmapEvent->getStr()));
+			param.insert(ParametersPair("optimized", uNumber2Str(rtabmapEvent->getInt())));
+			pushNewState(kStateExportingPosesGlobal, param);
+
 		}
 		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
 		{
@@ -488,8 +523,7 @@ void RtabmapThread::handleEvent(UEvent* event)
 void RtabmapThread::process()
 {
 	SensorData data;
-	getData(data);
-	if(data.isValid() && _state.empty())
+	if(_state.empty() && getData(data))
 	{
 		if(_rtabmap->getMemory())
 		{
@@ -518,20 +552,14 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			return;
 		}
 
+		bool ignoreFrame = false;
 		if(_rate>0.0f)
 		{
 			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
 			{
-				if(!lastPose_.isIdentity() && sensorData.pose().isIdentity())
-				{
-					UWARN("Odometry is reset (identity pose detected). Increment map id!");
-					pushNewState(kStateTriggeringMap);
-					_rotVariance = 0;
-					_transVariance = 0;
-				}
-
-				return;
+				ignoreFrame = true;
 			}
+
 		}
 		if(_dataBufferMaxSize > 0 && !lastPose_.isIdentity() && sensorData.pose().isIdentity())
 		{
@@ -540,7 +568,15 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			_rotVariance = 0;
 			_transVariance = 0;
 		}
-		_frameRateTimer->start();
+
+		if(ignoreFrame && !_createIntermediateNodes)
+		{
+			return;
+		}
+		else if(!ignoreFrame)
+		{
+			_frameRateTimer->start();
+		}
 
 		lastPose_ = sensorData.pose();
 		if(sensorData.poseRotVariance() > _rotVariance)
@@ -555,7 +591,26 @@ void RtabmapThread::addData(const SensorData & sensorData)
 		bool notify = true;
 		_dataMutex.lock();
 		{
-			_dataBuffer.push_back(sensorData);
+			if(ignoreFrame)
+			{
+				// remove data from the frame, keeping only constraints
+				SensorData tmp(
+						cv::Mat(),
+						cv::Mat(),
+						0,0,0,0,
+						sensorData.localTransform(),
+						sensorData.pose(),
+						sensorData.poseRotVariance(),
+						sensorData.poseTransVariance(),
+						sensorData.id(),
+						sensorData.stamp(),
+						sensorData.userData());
+				_dataBuffer.push_back(tmp);
+			}
+			else
+			{
+				_dataBuffer.push_back(sensorData);
+			}
 			if(_rotVariance <= 0)
 			{
 				_rotVariance = 1.0f;
@@ -567,7 +622,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 			_dataBuffer.back().setPose(_dataBuffer.back().pose(), _rotVariance, _transVariance);
 			_rotVariance = 0;
 			_transVariance = 0;
-			while(_dataBufferMaxSize > 0 && _dataBuffer.size() > (unsigned int)_dataBufferMaxSize)
+			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();
@@ -583,7 +638,7 @@ void RtabmapThread::addData(const SensorData & sensorData)
 	}
 }
 
-void RtabmapThread::getData(SensorData & image)
+bool RtabmapThread::getData(SensorData & image)
 {
 	ULOGGER_DEBUG("");
 
@@ -591,28 +646,18 @@ void RtabmapThread::getData(SensorData & image)
 	_dataAdded.acquire();
 	ULOGGER_INFO("wake-up");
 
+	bool dataFilled = false;
 	_dataMutex.lock();
 	{
 		if(!_dataBuffer.empty())
 		{
 			image = _dataBuffer.front();
 			_dataBuffer.pop_front();
+			dataFilled = true;
 		}
 	}
 	_dataMutex.unlock();
-}
-
-void RtabmapThread::setDataBufferSize(int size)
-{
-	if(size < 0)
-	{
-		ULOGGER_WARN("size < 0, then setting it to 0 (inf).");
-		_dataBufferMaxSize = 0;
-	}
-	else
-	{
-		_dataBufferMaxSize = size;
-	}
+	return dataFilled;
 }
 
 } /* namespace rtabmap */

corelib/src/util2d.cpp

@@ -210,7 +210,7 @@ float getDepth(
 
 	if(!(u >=0 && u<depthImage.cols && v >=0 && v<depthImage.rows))
 	{
-		UERROR("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
+		UDEBUG("!(x >=0 && x<depthImage.cols && y >=0 && y<depthImage.rows) cond failed! returning bad point. (x=%f (u=%d), y=%f (v=%d), cols=%d, rows=%d)",
 				x,u,y,v,depthImage.cols, depthImage.rows);
 		return 0;
 	}

corelib/src/util3d.cpp

@@ -351,12 +351,9 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFromDisparity(
 	UASSERT(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1);
 	UASSERT(imageDisparity.rows % decimation == 0);
 	UASSERT(imageDisparity.cols % decimation == 0);
+	UASSERT(decimation >= 1);
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	if(decimation < 1)
-	{
-		return cloud;
-	}
 
 	//cloud.header = cameraInfo.header;
 	cloud->height = imageDisparity.rows/decimation;
@@ -396,30 +393,25 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromDisparityRGB(
 		float fx, float baseline,
 		int decimation)
 {
+	UASSERT(!imageRgb.empty() && !imageDisparity.empty());
 	UASSERT(imageRgb.rows == imageDisparity.rows &&
 			imageRgb.cols == imageDisparity.cols &&
 			(imageDisparity.type() == CV_32FC1 || imageDisparity.type()==CV_16SC1));
-	UASSERT(imageDisparity.rows % decimation == 0);
-	UASSERT(imageDisparity.cols % decimation == 0);
+	UASSERT(imageRgb.channels() == 3 || imageRgb.channels() == 1);
+	UASSERT(decimation >= 1);
+	UASSERT(imageDisparity.rows % decimation == 0 && imageDisparity.cols % decimation == 0);
+
 	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-	if(decimation < 1)
-	{
-		return cloud;
-	}
 
 	bool mono;
 	if(imageRgb.channels() == 3) // BGR
 	{
 		mono = false;
 	}
-	else if(imageRgb.channels() == 1) // Mono
+	else // Mono
 	{
 		mono = true;
 	}
-	else
-	{
-		return cloud;
-	}
 
 	//cloud.header = cameraInfo.header;
 	cloud->height = imageRgb.rows/decimation;
@@ -463,20 +455,40 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFromStereoImages(
 		float fx, float baseline,
 		int decimation)
 {
+	UASSERT(!imageLeft.empty() && !imageRight.empty());
 	UASSERT(imageRight.type() == CV_8UC1);
+	UASSERT(imageLeft.channels() == 3 || imageLeft.channels() == 1);
+	UASSERT(imageLeft.rows == imageRight.rows &&
+			imageLeft.cols == imageRight.cols);
+	UASSERT(decimation >= 1);
+
+	cv::Mat leftColor = imageLeft;
+	cv::Mat rightMono = imageRight;
+
+	if(leftColor.rows % decimation != 0 ||
+	   leftColor.cols % decimation != 0)
+	{
+		leftColor = util2d::decimate(leftColor, decimation);
+		rightMono = util2d::decimate(rightMono, decimation);
+		fx /= float(decimation);
+		cx /= float(decimation);
+		cy /= float(decimation);
+		decimation = 1;
+	}
 
 	cv::Mat leftMono;
-	if(imageLeft.channels() == 3)
+	if(leftColor.channels() == 3)
 	{
-		cv::cvtColor(imageLeft, leftMono, CV_BGR2GRAY);
+		cv::cvtColor(leftColor, leftMono, CV_BGR2GRAY);
 	}
 	else
 	{
-		leftMono = imageLeft;
+		leftMono = leftColor;
 	}
+
 	return cloudFromDisparityRGB(
-			imageLeft,
-			util2d::disparityFromStereoImages(leftMono, imageRight),
+			leftColor,
+			util2d::disparityFromStereoImages(leftMono, rightMono),
 			cx, cy,
 			fx, baseline,
 			decimation);