Merged pcl_integration branch to trunk

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1014 f169173b-cf89-36c8-b27e-44dbe73f0c83

corelib/src/BayesFilter.cpp

@@ -19,7 +19,7 @@
 
 #include "BayesFilter.h"
 #include "rtabmap/core/Memory.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/Parameters.h"
 #include <iostream>
 
@@ -42,37 +42,14 @@ BayesFilter::~BayesFilter() {
 void BayesFilter::parseParameters(const ParametersMap & parameters)
 {
 	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kBayesVirtualPlacePriorThr())) != parameters.end())
-	{
-		this->setVirtualPlacePrior(std::atof((*iter).second.c_str()));
-	}
 	if((iter=parameters.find(Parameters::kBayesPredictionLC())) != parameters.end())
 	{
 		this->setPredictionLC((*iter).second);
 	}
-	if((iter=parameters.find(Parameters::kBayesFullPredictionUpdate())) != parameters.end())
-	{
-		_fullPredictionUpdate = uStr2Bool((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kBayesVirtualPlacePriorThr(), _virtualPlacePrior);
+	Parameters::parse(parameters, Parameters::kBayesFullPredictionUpdate(), _fullPredictionUpdate);
 
-}
-
-void BayesFilter::setVirtualPlacePrior(float virtualPlacePrior)
-{
-	if(virtualPlacePrior < 0)
-	{
-		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
-		_virtualPlacePrior = 0;
-	}
-	else if(virtualPlacePrior > 1)
-	{
-		ULOGGER_WARN("virtualPlacePrior=%f, must be >=0 and <=1", virtualPlacePrior);
-		_virtualPlacePrior = 1;
-	}
-	else
-	{
-		_virtualPlacePrior = virtualPlacePrior;
-	}
+	UASSERT(_virtualPlacePrior >= 0 && _virtualPlacePrior <= 1.0f);
 }
 
 // format = {Virtual place, Loop closure, level1, level2, l3, l4...}

corelib/src/BayesFilter.h

@@ -43,7 +43,6 @@ public:
 	void reset();
 
 	//setters
-	void setVirtualPlacePrior(float virtualPlacePrior);
 	void setPredictionLC(const std::string & prediction);
 
 	//getters

corelib/src/CMakeLists.txt

@@ -13,6 +13,7 @@ SET(SRC_FILES
 	
     Camera.cpp
     CameraThread.cpp
+    CameraOpenni.cpp
     
     EpipolarGeometry.cpp
 	VisualWord.cpp
@@ -22,6 +23,14 @@ SET(SRC_FILES
     Signature.cpp
 	Features2d.cpp
 	NearestNeighbor.cpp
+	Transform.cpp
+	
+	util3d.cpp
+	Odometry.cpp
+	
+	toro3d/posegraph3.cpp
+	toro3d/treeoptimizer3_iteration.cpp
+	toro3d/treeoptimizer3.cpp
 )
 
 SET(INCLUDE_DIRS
@@ -31,11 +40,9 @@ SET(INCLUDE_DIRS
 	${CMAKE_CURRENT_BINARY_DIR}
     ${OpenCV_INCLUDE_DIRS}
 	${SQLITE3_INCLUDE_DIR}
-)
-
-SET(LIBRARIES
-	${OpenCV_LIBS} 
-	${SQLITE3_LIBRARY}
+	${PCL_INCLUDE_DIRS}
+	${ZLIB_INCLUDE_DIRS}
+	
 )
 
 ####################################
@@ -65,26 +72,26 @@ ADD_CUSTOM_COMMAND(
 # Generate resources files END
 ####################################
 
+add_definitions(${PCL_DEFINITIONS})
+
 
 # Make sure the compiler can find include files from our library.
 INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
 
 # Add binary that is built from the source file "main.cpp".
 # The extension is automatically found.
-ADD_LIBRARY(rtabmap_corelib ${SRC_FILES} ${RESOURCES_HEADERS})
-TARGET_LINK_LIBRARIES(rtabmap_corelib rtabmap_utilite ${LIBRARIES})
+ADD_LIBRARY(rtabmap_core ${SRC_FILES} ${RESOURCES_HEADERS})
+TARGET_LINK_LIBRARIES(rtabmap_core rtabmap_utilite ${OpenCV_LIBS} ${SQLITE3_LIBRARY} ${PCL_LIBRARIES} ${ZLIB_LIBRARIES})
 
-SET_TARGET_PROPERTIES(
-rtabmap_corelib 
-PROPERTIES
- OUTPUT_NAME ${PROJECT_PREFIX}_core
- INSTALL_NAME_DIR ${CMAKE_INSTALL_PREFIX}/lib
-)
-
-INSTALL(TARGETS rtabmap_corelib
-        RUNTIME DESTINATION bin COMPONENT runtime
-        LIBRARY DESTINATION lib COMPONENT devel
-        ARCHIVE DESTINATION lib COMPONENT devel)
+INSTALL(TARGETS rtabmap_core
+	    EXPORT RTABMapTargets
+        RUNTIME DESTINATION "${INSTALL_BIN_DIR}" COMPONENT runtime
+        LIBRARY DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel
+        ARCHIVE DESTINATION "${INSTALL_LIB_DIR}" COMPONENT devel)
 		
-install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ DESTINATION include/ COMPONENT devel FILES_MATCHING PATTERN "*.h" PATTERN ".svn" EXCLUDE)
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/../include/ 
+		DESTINATION "${INSTALL_INCLUDE_DIR}"
+		COMPONENT devel 
+		FILES_MATCHING PATTERN "*.h" 
+		PATTERN ".svn" EXCLUDE)
 

corelib/src/Camera.cpp

@@ -37,10 +37,8 @@ namespace rtabmap
 Camera::Camera(float imageRate,
 		unsigned int imageWidth,
 		unsigned int imageHeight,
-		unsigned int framesDropped,
-		int id) :
+		unsigned int framesDropped) :
 	_imageRate(imageRate),
-	_id(id),
 	_imageWidth(imageWidth),
 	_imageHeight(imageHeight),
 	_framesDropped(framesDropped),
@@ -74,7 +72,6 @@ void Camera::setFeaturesExtracted(bool featuresExtracted, KeypointDetector::Dete
 	{
 		ParametersMap pm;
 		pm.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str((int)detector)));
-		pm.insert(ParametersPair(Parameters::kKpDescriptorStrategy(), uNumber2Str((int)detector)));
 		this->parseParameters(pm);
 	}
 }
@@ -101,12 +98,6 @@ void Camera::parseParameters(const ParametersMap & parameters)
 	{
 		detector = (KeypointDetector::DetectorType)std::atoi((*iter).second.c_str());
 	}
-	//Keypoint descriptor
-	KeypointDescriptor::DescriptorType descriptor = KeypointDescriptor::kDescriptorUndef;
-	if((iter=parameters.find(Parameters::kKpDescriptorStrategy())) != parameters.end())
-	{
-		descriptor = (KeypointDescriptor::DescriptorType)std::atoi((*iter).second.c_str());
-	}
 
 	if(detector!=KeypointDetector::kDetectorUndef)
 	{
@@ -116,44 +107,34 @@ void Camera::parseParameters(const ParametersMap & parameters)
 			delete _keypointDetector;
 			_keypointDetector = 0;
 		}
-		switch(detector)
-		{
-		case KeypointDetector::kDetectorSift:
-			_keypointDetector = new SIFTDetector(parameters);
-			break;
-		case KeypointDetector::kDetectorSurf:
-		default:
-			_keypointDetector = new SURFDetector(parameters);
-			break;
-		}
-	}
-	else if(_keypointDetector)
-	{
-		_keypointDetector->parseParameters(parameters);
-	}
-
-	if(descriptor!=KeypointDescriptor::kDescriptorUndef)
-	{
-		ULOGGER_DEBUG("new descriptor strategy %d", int(descriptor));
 		if(_keypointDescriptor)
 		{
 			delete _keypointDescriptor;
 			_keypointDescriptor = 0;
 		}
-		switch(descriptor)
+		switch(detector)
 		{
-		case KeypointDescriptor::kDescriptorSift:
+		case KeypointDetector::kDetectorSift:
+			_keypointDetector = new SIFTDetector(parameters);
 			_keypointDescriptor = new SIFTDescriptor(parameters);
 			break;
-		case KeypointDescriptor::kDescriptorSurf:
+		case KeypointDetector::kDetectorSurf:
 		default:
+			_keypointDetector = new SURFDetector(parameters);
 			_keypointDescriptor = new SURFDescriptor(parameters);
 			break;
 		}
 	}
-	else if(_keypointDescriptor)
+	else
 	{
-		_keypointDescriptor->parseParameters(parameters);
+		if(_keypointDetector)
+		{
+			_keypointDetector->parseParameters(parameters);
+		}
+		if(_keypointDescriptor)
+		{
+			_keypointDescriptor->parseParameters(parameters);
+		}
 	}
 }
 
@@ -172,7 +153,7 @@ cv::Mat Camera::takeImage(cv::Mat & descriptors, std::vector<cv::KeyPoint> & key
 {
 	descriptors = cv::Mat();
 	keypoints.clear();
-	float imageRate = _imageRate;
+	float imageRate = _imageRate==0.0f?33.0f:_imageRate; // limit to 33Hz if infinity
 	if(imageRate>0)
 	{
 		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
@@ -245,9 +226,8 @@ CameraImages::CameraImages(const std::string & path,
 					 float imageRate,
 					 unsigned int imageWidth,
 					 unsigned int imageHeight,
-					 unsigned int framesDropped,
-					 int id) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
+					 unsigned int framesDropped) :
+	Camera(imageRate, imageWidth, imageHeight, framesDropped),
 	_path(path),
 	_startAt(startAt),
 	_refreshDir(refreshDir),
@@ -382,9 +362,8 @@ CameraVideo::CameraVideo(int usbDevice,
 						 float imageRate,
 						 unsigned int imageWidth,
 						 unsigned int imageHeight,
-						 unsigned int framesDropped,
-						 int id) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
+						 unsigned int framesDropped) :
+	Camera(imageRate, imageWidth, imageHeight, framesDropped),
 	_src(kUsbDevice),
 	_usbDevice(usbDevice)
 {
@@ -395,9 +374,8 @@ CameraVideo::CameraVideo(const std::string & filePath,
 						   float imageRate,
 						   unsigned int imageWidth,
 						   unsigned int imageHeight,
-						   unsigned int framesDropped,
-						   int id) :
-	Camera(imageRate, imageWidth, imageHeight, framesDropped, id),
+						   unsigned int framesDropped) :
+	Camera(imageRate, imageWidth, imageHeight, framesDropped),
 	_filePath(filePath),
 	_src(kVideoFile),
 	_usbDevice(0)
@@ -454,7 +432,13 @@ cv::Mat CameraVideo::captureImage()
 	cv::Mat img;  // Null image
 	if(_capture.isOpened())
 	{
-		_capture.read(img);
+		if(!_capture.read(img))
+		{
+			if(_usbDevice)
+			{
+				UERROR("Camera has been disconnected!");
+			}
+		}
 	}
 	else
 	{

corelib/src/CameraOpenni.cpp

@@ -0,0 +1,150 @@
+/*
+ * CameraOpenni.cpp
+ *
+ *  Created on: 2013-08-22
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/core/CameraOpenni.h"
+#include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/utilite/ULogger.h"
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UEventsManager.h>
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <pcl/io/openni_grabber.h>
+
+namespace rtabmap {
+
+CameraOpenni::CameraOpenni(const std::string & deviceId, float inputRate, const Transform & localTransform) :
+		interface_(0),
+		deviceId_(deviceId),
+		rate_(inputRate),
+		frameRateTimer_(new UTimer()),
+		localTransform_(localTransform),
+		seq_(0)
+{
+}
+
+CameraOpenni::~CameraOpenni()
+{
+	UDEBUG("");
+	kill();
+	delete frameRateTimer_;
+	if(interface_)
+	{
+		uSleep(100); // make sure it is stopped
+		delete interface_;
+		interface_ = 0;
+	}
+}
+
+void CameraOpenni::image_cb (
+		const boost::shared_ptr<openni_wrapper::Image>& rgb,
+		const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
+		float constant)
+{
+	if(rate_>0.0f)
+	{
+		if(frameRateTimer_->getElapsedTime() < 1.0f/rate_)
+		{
+			return;
+		}
+	}
+	frameRateTimer_->start();
+
+	UTimer t;
+
+	cv::Mat rgbFrame(rgb->getHeight(), rgb->getWidth(), CV_8UC3);
+	rgb->fillRGB(rgb->getWidth(), rgb->getHeight(), rgbFrame.data);
+	cv::Mat bgrFrame;
+	cv::cvtColor(rgbFrame, bgrFrame, CV_RGB2BGR);
+
+	cv::Mat depthFrame(rgb->getHeight(), rgb->getWidth(), CV_16UC1);
+	depth->fillDepthImageRaw(rgb->getWidth(), rgb->getHeight(), (unsigned short*)depthFrame.data);
+
+	this->post(new CameraEvent(bgrFrame, depthFrame, constant, localTransform_, ++seq_));
+}
+
+bool CameraOpenni::init()
+{
+	if(interface_ && interface_->isRunning())
+	{
+		UERROR("Already started!!!\n");
+		return false;
+	}
+	else if(interface_)
+	{
+		delete interface_;
+		interface_ = 0;
+	}
+
+	seq_ = 0;
+	try
+	{
+		interface_ = new pcl::OpenNIGrabber(deviceId_);
+	}
+	catch(const pcl::IOException& ex)
+	{
+		UERROR("OpenNI exception: %s", ex.what());
+		if(interface_)
+		{
+			delete interface_;
+			interface_ = 0;
+		}
+		return false;
+	}
+
+	frameRateTimer_->start();
+	return true;
+}
+
+void CameraOpenni::start()
+{
+	if(interface_)
+	{
+		if(!connection_.connected())
+		{
+			boost::function<void (
+					const boost::shared_ptr<openni_wrapper::Image>&,
+					const boost::shared_ptr<openni_wrapper::DepthImage>&,
+					float)> f = boost::bind (&CameraOpenni::image_cb, this, _1, _2, _3);
+			connection_ = interface_->registerCallback (f);
+		}
+
+		if(!interface_->isRunning())
+		{
+			interface_->start ();
+		}
+	}
+}
+
+void CameraOpenni::pause()
+{
+	if(connection_.connected())
+	{
+		connection_.disconnect();
+	}
+}
+
+void CameraOpenni::kill()
+{
+	UDEBUG("");
+	if(interface_)
+	{
+		interface_->stop();
+	}
+}
+
+bool CameraOpenni::isRunning()
+{
+	return (interface_ && interface_->isRunning());
+}
+
+void CameraOpenni::setFrameRate(float rate)
+{
+	rate_ = rate;
+}
+
+} /* namespace rtabmap */

corelib/src/CameraThread.cpp

@@ -31,7 +31,8 @@ namespace rtabmap
 // ownership transferred
 CameraThread::CameraThread(Camera * camera, bool autoRestart) :
 		_camera(camera),
-		_autoRestart(autoRestart)
+		_autoRestart(autoRestart),
+		_seq(0)
 {
 	UASSERT(_camera != 0);
 }
@@ -43,6 +44,27 @@ CameraThread::~CameraThread()
 	delete _camera;
 }
 
+bool CameraThread::init()
+{
+	if(!this->isRunning())
+	{
+		if(_camera)
+		{
+			_seq = 0;
+			return _camera->init();
+		}
+		else
+		{
+			UERROR("Cannot initialize the camera because the camera object is null...");
+		}
+	}
+	else
+	{
+		UERROR("Cannot initialize the camera because it is already running...");
+	}
+	return false;
+}
+
 void CameraThread::mainLoop()
 {
 	State state = kStateCapturing;
@@ -82,11 +104,6 @@ void CameraThread::pushNewState(State newState, const ParametersMap & parameters
 	_stateMutex.unlock();
 }
 
-void CameraThread::setImageRate(float imageRate)
-{
-	_camera->setImageRate(imageRate);
-}
-
 void CameraThread::handleEvent(UEvent* anEvent)
 {
 	if(anEvent->getClassName().compare("ParamEvent") == 0)
@@ -116,11 +133,11 @@ void CameraThread::process()
 	{
 		if(_camera->isFeaturesExtracted())
 		{
-			this->post(new CameraEvent(descriptors, keypoints, img, _camera->id()));
+			this->post(new CameraEvent(descriptors, keypoints, img, ++_seq));
 		}
 		else
 		{
-			this->post(new CameraEvent(img, _camera->id()));
+			this->post(new CameraEvent(img, ++_seq));
 		}
 	}
 	else if(!this->isKilled())
@@ -133,7 +150,7 @@ void CameraThread::process()
 		{
 			ULOGGER_DEBUG("Camera::process() : no more images...");
 			this->kill();
-			this->post(new CameraEvent(_camera->id()));
+			this->post(new CameraEvent());
 		}
 	}
 }

corelib/src/DBDriver.cpp

@@ -19,7 +19,7 @@
 
 #include "rtabmap/core/DBDriver.h"
 
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "VisualWord.h"
 #include "rtabmap/utilite/UConversion.h"
 #include "rtabmap/utilite/UMath.h"
@@ -30,7 +30,6 @@
 namespace rtabmap {
 
 DBDriver::DBDriver(const ParametersMap & parameters) :
-	_imagesCompressed(Parameters::defaultDbImagesCompressed()),
 	_emptyTrashesTime(0)
 {
 	this->parseParameters(parameters);
@@ -44,11 +43,6 @@ DBDriver::~DBDriver()
 
 void DBDriver::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kDbImagesCompressed())) != parameters.end())
-	{
-		_imagesCompressed = uStr2Bool((*iter).second.c_str());
-	}
 }
 
 void DBDriver::closeConnection()
@@ -286,6 +280,13 @@ void DBDriver::load(VWDictionary * dictionary) const
 	_dbSafeAccessMutex.unlock();
 }
 
+void DBDriver::load(std::map<int, std::map<int, Transform> > & mapTransforms) const
+{
+	_dbSafeAccessMutex.lock();
+	this->loadQuery(mapTransforms);
+	_dbSafeAccessMutex.unlock();
+}
+
 void DBDriver::loadLastNodes(std::list<Signature *> & signatures) const
 {
 	_dbSafeAccessMutex.lock();
@@ -386,23 +387,45 @@ void DBDriver::loadWords(const std::set<int> & wordIds, std::list<VisualWord *>
 }
 
 //TODO Check also in the trash ?
-void DBDriver::getImage(int signatureId, cv::Mat & rawData) const
+void DBDriver::loadNodeData(std::list<Signature *> & signatures, bool loadMetricData) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getImageQuery(signatureId, rawData);
+	this->loadNodeDataQuery(signatures, loadMetricData);
 	_dbSafeAccessMutex.unlock();
 }
 
 //TODO Check also in the trash ?
-void DBDriver::getNeighborIds(int signatureId, std::set<int> & neighbors, bool onlyWithActions) const
+void DBDriver::getNodeData(
+		int signatureId,
+		std::vector<unsigned char> & image,
+		std::vector<unsigned char> & depth,
+		std::vector<unsigned char> & depth2d,
+		float & depthConstant,
+		Transform & localTransform) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getNeighborIdsQuery(signatureId, neighbors, onlyWithActions);
+	this->getNodeDataQuery(signatureId, image, depth, depth2d, depthConstant, localTransform);
 	_dbSafeAccessMutex.unlock();
 }
 
 //TODO Check also in the trash ?
-void DBDriver::loadNeighbors(int signatureId, std::set<int> & neighbors) const
+void DBDriver::getNodeData(int signatureId, std::vector<unsigned char> & image) const
+{
+	_dbSafeAccessMutex.lock();
+	this->getNodeDataQuery(signatureId, image);
+	_dbSafeAccessMutex.unlock();
+}
+
+//TODO Check also in the trash ?
+void DBDriver::getPose(int signatureId, Transform & pose, int & mapId) const
+{
+	_dbSafeAccessMutex.lock();
+	this->getPoseQuery(signatureId, pose, mapId);
+	_dbSafeAccessMutex.unlock();
+}
+
+//TODO Check also in the trash ?
+void DBDriver::loadNeighbors(int signatureId, std::map<int, Transform> & neighbors) const
 {
 	_dbSafeAccessMutex.lock();
 	this->loadNeighborsQuery(signatureId, neighbors);
@@ -418,10 +441,10 @@ void DBDriver::getWeight(int signatureId, int & weight) const
 }
 
 //TODO Check also in the trash ?
-void DBDriver::getLoopClosureIds(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const
+void DBDriver::loadLoopClosures(int signatureId, std::map<int, Transform> & loopIds, std::map<int, Transform> & childIds) const
 {
 	_dbSafeAccessMutex.lock();
-	this->getLoopClosureIdsQuery(signatureId, loopIds, childIds);
+	this->loadLoopClosuresQuery(signatureId, loopIds, childIds);
 	_dbSafeAccessMutex.unlock();
 }
 
@@ -457,29 +480,25 @@ void DBDriver::getInvertedIndexNi(int signatureId, int & ni) const
 	_dbSafeAccessMutex.unlock();
 }
 
-void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed) const
+void DBDriver::save(const std::map<int, std::map<int, Transform> > & mapTransforms) const
+{
+	_dbSafeAccessMutex.lock();
+	saveQuery(mapTransforms);
+	_dbSafeAccessMutex.unlock();
+}
+
+void DBDriver::addStatisticsAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize) const
 {
 	ULOGGER_DEBUG("");
 	if(this->isConnected())
 	{
 		std::stringstream query;
-		query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used) values("
+		query << "INSERT INTO Statistics(STM_size,last_sign_added,process_mem_used,database_mem_used,dictionary_size) values("
 			  << stMemSize << ","
 		      << lastSignAdded << ","
 		      << processMemUsed << ","
-			  << databaseMemUsed << ");";
-
-		this->executeNoResultQuery(query.str());
-	}
-}
-
-void DBDriver::addStatisticsAfterRunSurf(int dictionarySize) const
-{
-	ULOGGER_DEBUG("");
-	if(this->isConnected())
-	{
-		std::stringstream query;
-		query << "INSERT INTO StatisticsDictionary(dictionary_size) values(" << dictionarySize << ");";
+		      << databaseMemUsed << ","
+			  << dictionarySize << ");";
 
 		this->executeNoResultQuery(query.str());
 	}

corelib/src/DBDriverSqlite3.h

@@ -24,6 +24,7 @@
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
 #include <sqlite3.h>
+#include <pcl/point_types.h>
 
 namespace rtabmap {
 
@@ -47,40 +48,62 @@ private:
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
-	virtual void getNeighborIdsQuery(int signatureId, std::set<int> & neighbors, bool onlyWithActions = false) const;
 	virtual void getWeightQuery(int signatureId, int & weight) const;
-	virtual void getLoopClosureIdsQuery(int signatureId, std::set<int> & loopIds, std::set<int> & childIds) const;
 
 	virtual void saveQuery(const std::list<Signature *> & signatures) const;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
 	virtual void updateQuery(const std::list<Signature *> & signatures) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words) const;
+	virtual void saveQuery(const std::map<int, std::map<int, Transform> > & mapTransforms) const;
 
 	// Load objects
 	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(std::map<int, std::map<int, Transform> > & mapTransforms) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
-	virtual void loadNeighborsQuery(int signatureId, std::set<int> & neighbors) const;
+	virtual void loadNeighborsQuery(int signatureId, std::map<int, Transform> & neighbors) const;
+	virtual void loadLoopClosuresQuery(
+			int signatureId,
+			std::map<int, Transform> & loopIds,
+			std::map<int, Transform> & childIds) const;
 
-	virtual void getImageQuery(int nodeId, cv::Mat & image) const;
+	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool loadMetricData) const;
+	virtual void getNodeDataQuery(
+			int signatureId,
+			std::vector<unsigned char> & image,
+			std::vector<unsigned char> & depth,
+			std::vector<unsigned char> & depth2d,
+			float & depthConstant,
+			Transform & localTransform) const;
+	virtual void getNodeDataQuery(int signatureId, std::vector<unsigned char> & image) const;
+	virtual void getPoseQuery(int signatureId, Transform & pose, int & mapId) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
 
 private:
 	std::string queryStepNode() const;
-	std::string queryStepNodeToSensor() const;
 	std::string queryStepImage() const;
+	std::string queryStepDepth() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
-	void stepNodeToSensor(sqlite3_stmt * ppStmt, int nodeId, int sensorId, int num) const;
-	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & image) const;
-	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type) const;
+	void stepImage(
+			sqlite3_stmt * ppStmt,
+			int id,
+			const std::vector<unsigned char> & image) const;
+	void stepDepth(
+			sqlite3_stmt * ppStmt,
+			int id,
+			const std::vector<unsigned char> & depth,
+			const std::vector<unsigned char> & depth2d,
+			float depthConstant,
+			const Transform & localTransform) const;
+	void stepLink(sqlite3_stmt * ppStmt, int fromId, int toId, int type, const Transform & transform) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const pcl::PointXYZ & pt) const;
 
 private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;

corelib/src/DBReader.cpp

@@ -10,17 +10,20 @@
 #include "DBDriverSqlite3.h"
 
 #include <rtabmap/utilite/ULogger.h>
-#include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UFile.h>
 
 #include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util3d.h"
 
 namespace rtabmap {
 
 DBReader::DBReader(const std::string & databasePath,
-				   float frameRate) :
+				   float frameRate,
+				   bool odometryIgnored) :
 	_path(databasePath),
 	_frameRate(frameRate),
+	_odometryIgnored(odometryIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end())
 {
@@ -102,22 +105,46 @@ void DBReader::mainLoopBegin()
 
 void DBReader::mainLoop()
 {
-	cv::Mat image;
-	this->getNextImage(image);
+	cv::Mat image, depth, depth2d;
+	float depthConstant;
+	Transform localTransform, pose;
+	this->getNextImage(image, depth, depth2d, depthConstant, localTransform, pose);
 	if(!image.empty())
 	{
-		UEventsManager::post(new CameraEvent(image));
+		if(depth.empty())
+		{
+			this->post(new CameraEvent(image));
+		}
+		else
+		{
+			if(!_odometryIgnored)
+			{
+				Image data(image, depth, depth2d, depthConstant, pose, localTransform);
+				this->post(new OdometryEvent(data));
+			}
+			else
+			{
+				// without odometry
+				this->post(new CameraEvent(image, depth, depth2d, depthConstant, localTransform));
+			}
+		}
 	}
 	else if(!this->isKilled())
 	{
-		UDEBUG("no more images...");
+		UINFO("no more images...");
 		this->kill();
-		UEventsManager::post(new CameraEvent());
+		this->post(new CameraEvent());
 	}
 
 }
 
-void DBReader::getNextImage(cv::Mat & image)
+void DBReader::getNextImage(
+		cv::Mat & image,
+		cv::Mat & depth,
+		cv::Mat & depth2d,
+		float & depthConstant,
+		Transform & localTransform,
+		Transform & pose)
 {
 	if(_dbDriver)
 	{
@@ -143,13 +170,30 @@ void DBReader::getNextImage(cv::Mat & image)
 
 		if(!this->isKilled() && _currentId != _ids.end())
 		{
-			//sensors
-			_dbDriver->getImage(*_currentId, image);
+			std::vector<unsigned char> imageBytes;
+			std::vector<unsigned char> depthBytes;
+			std::vector<unsigned char> depth2dBytes;
+			int mapId;
+			_dbDriver->getNodeData(*_currentId, imageBytes, depthBytes, depth2dBytes, depthConstant, localTransform);
+			_dbDriver->getPose(*_currentId, pose, mapId);
 			++_currentId;
-			if(image.empty())
+			if(imageBytes.empty())
 			{
-				UWARN("No image loaded from the database!");
+				UWARN("No image loaded from the database for id=%d!", *_currentId);
 			}
+
+			util3d::CompressionThread ctImage(imageBytes, true);
+			util3d::CompressionThread ctDepth(depthBytes, true);
+			util3d::CompressionThread ctDepth2D(depth2dBytes, false);
+			ctImage.start();
+			ctDepth.start();
+			ctDepth2D.start();
+			ctImage.join();
+			ctDepth.join();
+			ctDepth2D.join();
+			image = ctImage.getUncompressedData();
+			depth = ctDepth.getUncompressedData();
+			depth2d = ctDepth2D.getUncompressedData();
 		}
 	}
 	else

corelib/src/EpipolarGeometry.cpp

@@ -18,7 +18,7 @@
  */
 
 #include "rtabmap/core/EpipolarGeometry.h"
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/utilite/UTimer.h"
 #include "rtabmap/utilite/UStl.h"
@@ -427,7 +427,8 @@ int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
  * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
  * realPairsCount = 5
  */
-int EpipolarGeometry::findPairsUnique(const std::multimap<int, cv::KeyPoint> & wordsA,
+int EpipolarGeometry::findPairsUnique(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
 		const std::multimap<int, cv::KeyPoint> & wordsB,
 		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs)
 {

corelib/src/Features2d.cpp

@@ -35,6 +35,55 @@
 
 namespace rtabmap {
 
+void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
+{
+	cv::Mat descriptors;
+	limitKeypoints(keypoints, descriptors, maxKeypoints);
+}
+
+void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+{
+	UASSERT((int)keypoints.size() == descriptors.rows || descriptors.rows == 0);
+	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
+	{
+		UTimer timer;
+		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+		// Remove words under the new hessian threshold
+
+		// Sort words by hessian
+		std::multimap<float, int> hessianMap; // <hessian,id>
+		for(unsigned int i = 0; i <keypoints.size(); ++i)
+		{
+			//Keep track of the data, to be easier to manage the data in the next step
+			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+		}
+
+		// Remove them from the signature
+		int removed = hessianMap.size()-maxKeypoints;
+		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		cv::Mat descriptorsTmp;
+		if(descriptors.rows)
+		{
+			descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
+		}
+		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
+		{
+			kptsTmp[k] = keypoints[iter->second];
+			if(descriptors.rows)
+			{
+				memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+			}
+		}
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
+		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
+		keypoints = kptsTmp;
+		if(descriptors.rows)
+		{
+			descriptors = descriptorsTmp;
+		}
+	}
+}
 
 /////////////////////
 // KeypointDescriptor
@@ -73,35 +122,12 @@ SURFDescriptor::~SURFDescriptor()
 
 void SURFDescriptor::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
-	{
-		_extended = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
-	{
-		_hessianThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
-	{
-		_nOctaveLayers = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
-	{
-		_nOctaves = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
-	{
-		_nOctaves = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
-	{
-		_upright = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
-	{
-		_gpuVersion = uStr2Bool((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
+	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
+	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
+	Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
+	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
 	KeypointDescriptor::parseParameters(parameters);
 }
 
@@ -186,26 +212,11 @@ SIFTDescriptor::~SIFTDescriptor()
 void SIFTDescriptor::parseParameters(const ParametersMap & parameters)
 {
 	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
-	{
-		_contrastThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
-	{
-		_edgeThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
-	{
-		_nfeatures = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
-	{
-		_nOctaveLayers = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
-	{
-		_sigma = std::atof((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
+	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
 	KeypointDescriptor::parseParameters(parameters);
 }
 
@@ -253,90 +264,33 @@ cv::Mat SIFTDescriptor::generateDescriptors(const cv::Mat & image, std::vector<c
 /////////////////////
 // KeypointDetector
 /////////////////////
-KeypointDetector::KeypointDetector(const ParametersMap & parameters) :
-		_wordsPerImageTarget(Parameters::defaultKpWordsPerImage()),
-		_roiRatios(std::vector<float>(4, 0.0f))
+KeypointDetector::KeypointDetector(const ParametersMap & parameters)
 {
-	this->setRoi(Parameters::defaultKpRoiRatios());
 	this->parseParameters(parameters);
 }
 
 void KeypointDetector::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kKpWordsPerImage())) != parameters.end())
-	{
-		_wordsPerImageTarget = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kKpRoiRatios())) != parameters.end())
-	{
-		this->setRoi((*iter).second);
-	}
 }
 
-std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & image)
+std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(
+		const cv::Mat & image,
+		int maxKeypoints,
+		const cv::Rect & roi)
 {
 	ULOGGER_DEBUG("");
 	std::vector<cv::KeyPoint> keypoints;
 	if(!image.empty())
 	{
 		UTimer timer;
-		timer.start();
-
-		cv::Rect roi = computeRoi(image);
 
 		// Get keypoints
-		keypoints = this->_generateKeypoints(image, roi);
+		keypoints = this->_generateKeypoints(image, roi.width && roi.height?roi:cv::Rect(0,0,image.cols, image.rows));
 		ULOGGER_DEBUG("Keypoints extraction time = %f s, keypoints extracted = %d", timer.ticks(), keypoints.size());
 
-		//clip the number of words... to _wordsPerImageTarget
-		// Variable hessian threshold
-		if(_wordsPerImageTarget > 0)
-		{
-			if(keypoints.size() > 0)
-			{
-				// 10% margin...
-				if(keypoints.size() > 1.1 * _wordsPerImageTarget)
-				{
-					ULOGGER_DEBUG("too much words (%d), removing words under the new hessian threshold", keypoints.size());
-					// Remove words under the new hessian threshold
+		limitKeypoints(keypoints, maxKeypoints);
 
-					// Sort words by hessian
-					std::multimap<float, std::vector<cv::KeyPoint>::iterator> hessianMap; // <hessian,id>
-					for(std::vector<cv::KeyPoint>::iterator itKey = keypoints.begin(); itKey != keypoints.end(); ++itKey)
-					{
-						//Keep track of the data, to be easier to manage the data in the next step
-						hessianMap.insert(std::pair<float, std::vector<cv::KeyPoint>::iterator>(fabs(itKey->response), itKey));
-					}
-
-					// Remove them from the signature
-					int removed = hessianMap.size()-_wordsPerImageTarget;
-					std::multimap<float, std::vector<cv::KeyPoint>::iterator>::reverse_iterator iter = hessianMap.rbegin();
-					std::vector<cv::KeyPoint> kptsTmp(_wordsPerImageTarget);
-					for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
-					{
-						kptsTmp[k] = *iter->second;
-						// Adjust keypoint position to raw image
-						kptsTmp[k].pt.x += roi.x;
-						kptsTmp[k].pt.y += roi.y;
-					}
-					keypoints = kptsTmp;
-					ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, keypoints.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
-				}
-				else if(roi.x || roi.y)
-				{
-					// Adjust keypoint position to raw image
-					for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
-					{
-						iter->pt.x += roi.x;
-						iter->pt.y += roi.y;
-					}
-				}
-			}
-
-			ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
-		}
-		else if(roi.x || roi.y)
+		if(roi.x || roi.y)
 		{
 			// Adjust keypoint position to raw image
 			for(std::vector<cv::KeyPoint>::iterator iter=keypoints.begin(); iter!=keypoints.end(); ++iter)
@@ -353,71 +307,40 @@ std::vector<cv::KeyPoint> KeypointDetector::generateKeypoints(const cv::Mat & im
 	return keypoints;
 }
 
-void KeypointDetector::setRoi(const std::string & roi)
+cv::Rect KeypointDetector::computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios)
 {
-	std::list<std::string> strValues = uSplit(roi, ' ');
-	if(strValues.size() != 4)
-	{
-		ULOGGER_ERROR("The number of values must be 4 (roi=\"%s\")", roi.c_str());
-	}
-	else
-	{
-		std::vector<float> tmpValues(4);
-		unsigned int i=0;
-		for(std::list<std::string>::iterator iter = strValues.begin(); iter!=strValues.end(); ++iter)
-		{
-			tmpValues[i] = std::atof((*iter).c_str());
-			++i;
-		}
-
-		if(tmpValues[0] >= 0 && tmpValues[0] < 1 && tmpValues[0] < 1.0f-tmpValues[1] &&
-			tmpValues[1] >= 0 && tmpValues[1] < 1 && tmpValues[1] < 1.0f-tmpValues[0] &&
-			tmpValues[2] >= 0 && tmpValues[2] < 1 && tmpValues[2] < 1.0f-tmpValues[3] &&
-			tmpValues[3] >= 0 && tmpValues[3] < 1 && tmpValues[3] < 1.0f-tmpValues[2])
-		{
-			_roiRatios = tmpValues;
-		}
-		else
-		{
-			ULOGGER_ERROR("The roi ratios are not valid (roi=\"%s\")", roi.c_str());
-		}
-	}
-}
-
-cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
-{
-	if(!image.empty() && _roiRatios.size() == 4)
+	if(!image.empty() && roiRatios.size() == 4)
 	{
 		float width = image.cols;
 		float height = image.rows;
 		cv::Rect roi(0, 0, width, height);
-		UDEBUG("roi ratios = %f, %f, %f, %f", _roiRatios[0],_roiRatios[1],_roiRatios[2],_roiRatios[3]);
+		UDEBUG("roi ratios = %f, %f, %f, %f", roiRatios[0],roiRatios[1],roiRatios[2],roiRatios[3]);
 		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
 
 		//left roi
-		if(_roiRatios[0] > 0 && _roiRatios[0] < 1 - _roiRatios[1])
+		if(roiRatios[0] > 0 && roiRatios[0] < 1 - roiRatios[1])
 		{
-			roi.x = width * _roiRatios[0];
+			roi.x = width * roiRatios[0];
 		}
 
 		//right roi
 		roi.width = width - roi.x;
-		if(_roiRatios[1] > 0 && _roiRatios[1] < 1 - _roiRatios[0])
+		if(roiRatios[1] > 0 && roiRatios[1] < 1 - roiRatios[0])
 		{
-			roi.width -= width * _roiRatios[1];
+			roi.width -= width * roiRatios[1];
 		}
 
 		//top roi
-		if(_roiRatios[2] > 0 && _roiRatios[2] < 1 - _roiRatios[3])
+		if(roiRatios[2] > 0 && roiRatios[2] < 1 - roiRatios[3])
 		{
-			roi.y = height * _roiRatios[2];
+			roi.y = height * roiRatios[2];
 		}
 
 		//bottom roi
 		roi.height = height - roi.y;
-		if(_roiRatios[3] > 0 && _roiRatios[3] < 1 - _roiRatios[2])
+		if(roiRatios[3] > 0 && roiRatios[3] < 1 - roiRatios[2])
 		{
-			roi.height -= height * _roiRatios[3];
+			roi.height -= height * roiRatios[3];
 		}
 		UDEBUG("roi = %d, %d, %d, %d", roi.x, roi.y, roi.width, roi.height);
 
@@ -425,7 +348,7 @@ cv::Rect KeypointDetector::computeRoi(const cv::Mat & image) const
 	}
 	else
 	{
-		UERROR("Image is null or _roiRatios(=%d) != 4", _roiRatios.size());
+		UERROR("Image is null or _roiRatios(=%d) != 4", roiRatios.size());
 		return cv::Rect();
 	}
 }
@@ -452,35 +375,12 @@ SURFDetector::~SURFDetector()
 
 void SURFDetector::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kSURFExtended())) != parameters.end())
-	{
-		_extended = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFHessianThreshold())) != parameters.end())
-	{
-		_hessianThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaveLayers())) != parameters.end())
-	{
-		_nOctaveLayers = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
-	{
-		_nOctaves = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFOctaves())) != parameters.end())
-	{
-		_nOctaves = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFUpright())) != parameters.end())
-	{
-		_upright = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSURFGpuVersion())) != parameters.end())
-	{
-		_gpuVersion = uStr2Bool((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kSURFExtended(), _extended);
+	Parameters::parse(parameters, Parameters::kSURFHessianThreshold(), _hessianThreshold);
+	Parameters::parse(parameters, Parameters::kSURFOctaveLayers(), _nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSURFOctaves(), _nOctaves);
+	Parameters::parse(parameters, Parameters::kSURFUpright(), _upright);
+	Parameters::parse(parameters, Parameters::kSURFGpuVersion(), _gpuVersion);
 	KeypointDetector::parseParameters(parameters);
 }
 
@@ -497,6 +397,7 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image
 	cv::Mat imageGrayScale;
 	if(image.channels() != 1 || image.depth() != CV_8U)
 	{
+		ULOGGER_DEBUG("");
 		cv::cvtColor(image, imageGrayScale, CV_BGR2GRAY);
 	}
 	cv::Mat img;
@@ -525,14 +426,17 @@ std::vector<cv::KeyPoint> SURFDetector::_generateKeypoints(const cv::Mat & image
 		detector.detect(imgRoi, keypoints);
 	}
 #else*/
+	ULOGGER_DEBUG("%f %d %d %d %d", _hessianThreshold, _nOctaves, _nOctaveLayers, _extended?1:0, _upright?1:0);
 	cv::SURF detector(_hessianThreshold, _nOctaves, _nOctaveLayers, _extended, _upright);
+	ULOGGER_DEBUG("");
 #if CV_MAJOR_VERSION >=2 and CV_MINOR_VERSION >=4
+	cv::imwrite("test.png", imgRoi);
 	detector.detect(imgRoi, keypoints);
 #else
 	detector(imgRoi, cv::Mat(), keypoints);
 #endif
 //#endif
-
+	ULOGGER_DEBUG("");
 	return keypoints;
 }
 
@@ -556,27 +460,11 @@ SIFTDetector::~SIFTDetector()
 
 void SIFTDetector::parseParameters(const ParametersMap & parameters)
 {
-	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kSIFTContrastThreshold())) != parameters.end())
-	{
-		_contrastThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTEdgeThreshold())) != parameters.end())
-	{
-		_edgeThreshold = std::atof((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTNFeatures())) != parameters.end())
-	{
-		_nfeatures = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTNOctaveLayers())) != parameters.end())
-	{
-		_nOctaveLayers = std::atoi((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kSIFTSigma())) != parameters.end())
-	{
-		_sigma = std::atof((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kSIFTContrastThreshold(), _contrastThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTEdgeThreshold(), _edgeThreshold);
+	Parameters::parse(parameters, Parameters::kSIFTNFeatures(), _nfeatures);
+	Parameters::parse(parameters, Parameters::kSIFTNOctaveLayers(), _nOctaveLayers);
+	Parameters::parse(parameters, Parameters::kSIFTSigma(), _sigma);
 	KeypointDetector::parseParameters(parameters);
 }
 

corelib/src/Odometry.cpp

@@ -0,0 +1,731 @@
+/*
+ * Odometry.cpp
+ *
+ *  Created on: 2013-08-23
+ *      Author: Mathieu
+ */
+
+#include "rtabmap/core/Odometry.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
+
+#include <rtabmap/core/OdometryEvent.h>
+#include <rtabmap/core/CameraEvent.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Features2d.h>
+
+#include <rtabmap/core/Memory.h>
+#include "rtabmap/core/Signature.h"
+
+#include <pcl/io/pcd_io.h>
+#include <pcl/common/transforms.h>
+
+#if _MSC_VER
+	#define ISFINITE(value) _finite(value)
+#else
+	#define ISFINITE(value) std::isfinite(value)
+#endif
+
+namespace rtabmap {
+
+Odometry::Odometry(
+		float inlierDistance,
+		int maxWords,
+		int minInliers,
+		int iterations,
+		float maxDepth,
+		float linearUpdate,
+		float angularUpdate,
+		int resetCoutdown) :
+			_maxFeatures(maxWords),
+			_minInliers(minInliers),
+			_inlierDistance(inlierDistance),
+			_iterations(iterations),
+			_maxDepth(maxDepth),
+			_linearUpdate(linearUpdate),
+			_angularUpdate(angularUpdate),
+			_resetCountdown(resetCoutdown),
+			_pose(Transform::getIdentity()),
+			_resetCurrentCount(0)
+{
+
+}
+
+Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
+		_maxFeatures(Parameters::defaultOdomMaxWords()),
+		_minInliers(Parameters::defaultOdomMinInliers()),
+		_inlierDistance(Parameters::defaultOdomInlierDistance()),
+		_iterations(Parameters::defaultOdomIterations()),
+		_maxDepth(Parameters::defaultOdomMaxDepth()),
+		_linearUpdate(Parameters::defaultOdomLinearUpdate()),
+		_angularUpdate(Parameters::defaultOdomAngularUpdate()),
+		_resetCountdown(Parameters::defaultOdomResetCountdown()),
+		_pose(Transform::getIdentity()),
+		_resetCurrentCount(0)
+{
+	Parameters::parse(parameters, Parameters::kOdomLinearUpdate(), _linearUpdate);
+	Parameters::parse(parameters, Parameters::kOdomAngularUpdate(), _angularUpdate);
+	Parameters::parse(parameters, Parameters::kOdomResetCountdown(), _resetCountdown);
+	Parameters::parse(parameters, Parameters::kOdomMinInliers(), _minInliers);
+	Parameters::parse(parameters, Parameters::kOdomInlierDistance(), _inlierDistance);
+	Parameters::parse(parameters, Parameters::kOdomIterations(), _iterations);
+	Parameters::parse(parameters, Parameters::kOdomMaxDepth(), _maxDepth);
+	Parameters::parse(parameters, Parameters::kOdomMaxWords(), _maxFeatures);
+}
+
+void Odometry::reset()
+{
+	_resetCurrentCount = 0;
+	_pose = Transform::getIdentity();
+}
+
+bool Odometry::isLargeEnoughTransform(const Transform & transform)
+{
+	return fabs(transform.x()) > _linearUpdate ||
+		   fabs(transform.y()) > _linearUpdate ||
+	       fabs(transform.z()) > _linearUpdate;
+}
+
+Transform Odometry::process(Image & image)
+{
+	Transform t = this->computeTransform(image);
+	if(!t.isNull())
+	{
+		_resetCurrentCount = _resetCountdown;
+
+		_pose *= t;
+		return _pose;
+	}
+	else if(_resetCurrentCount > 0)
+	{
+		UWARN("Odometry lost! Odometry will be reset after next %d consecutive unsuccessful odometry updates...", _resetCurrentCount);
+
+		--_resetCurrentCount;
+		if(_resetCurrentCount == 0)
+		{
+			UWARN("Odometry automatically reset!");
+			this->reset();
+		}
+	}
+	return Transform();
+}
+OdometryBinary::OdometryBinary(
+		float inlierDistance,
+		int maxWords,
+		int minInliers,
+		int iterations,
+		float maxDepth,
+		float linearUpdate,
+		float angularUpdate,
+		int resetCoutdown,
+		int briefBytes,
+		int fastThreshold,
+		bool fastNonmaxSuppression,
+		bool bruteForceMatching) :
+	Odometry(inlierDistance, maxWords, minInliers, iterations, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+ 	_briefBytes(briefBytes),
+	_fastThreshold(fastThreshold),
+	_fastNonmaxSuppression(fastNonmaxSuppression),
+	_bruteForceMatching(bruteForceMatching)
+{
+
+}
+
+OdometryBinary::OdometryBinary(const ParametersMap & parameters) :
+	Odometry(parameters),
+ 	_briefBytes(Parameters::defaultOdomBinBriefBytes()),
+	_fastThreshold(Parameters::defaultOdomBinFastThreshold()),
+	_fastNonmaxSuppression(Parameters::defaultOdomBinFastNonmaxSuppression()),
+	_bruteForceMatching(Parameters::defaultOdomBinBruteForceMatching())
+{
+	Parameters::parse(parameters, Parameters::kOdomBinBriefBytes(), _briefBytes);
+	Parameters::parse(parameters, Parameters::kOdomBinFastThreshold(), _fastThreshold);
+	Parameters::parse(parameters, Parameters::kOdomBinFastNonmaxSuppression(), _fastNonmaxSuppression);
+	Parameters::parse(parameters, Parameters::kOdomBinBruteForceMatching(), _bruteForceMatching);
+}
+
+void OdometryBinary::reset()
+{
+	Odometry::reset();
+	_lastKeypoints.clear();
+	_lastDescriptors = cv::Mat();
+	_lastDepth = cv::Mat();
+}
+
+
+// return true if odometry is correctly computed
+Transform OdometryBinary::computeTransform(Image & image)
+{
+	UTimer timer;
+	cv::Mat imageMono;
+	Transform output;
+	// convert to grayscale
+	if(image.image().channels() > 1)
+	{
+		cv::cvtColor(image.image(), imageMono, cv::COLOR_BGR2GRAY);
+	}
+	else
+	{
+		imageMono = image.image();
+	}
+
+	cv::FastFeatureDetector detector(_fastThreshold, _fastNonmaxSuppression);
+	std::vector<cv::KeyPoint> newKeypoints;
+	detector.detect(imageMono, newKeypoints);
+
+	limitKeypoints(newKeypoints, this->getMaxFeatures());
+
+	cv::BriefDescriptorExtractor extractor(_briefBytes);
+	cv::Mat newDescriptors;
+	extractor.compute(imageMono, newKeypoints, newDescriptors);
+
+	int inliers = 0;
+	int correspondences = 0;
+
+	if(_lastKeypoints.size())
+	{
+		if(newDescriptors.rows)
+		{
+			cv::Mat results;
+			cv::Mat dists;
+			int k=1; // find the 1 nearest neighbor
+			std::vector<std::vector<cv::DMatch> > matches;
+
+			if(_bruteForceMatching)
+			{
+				cv::BFMatcher matcher(cv::NORM_HAMMING);
+				  matcher.knnMatch(newDescriptors, _lastDescriptors, matches, k);
+			}
+			else
+			{
+				 // Create Flann LSH index
+				cv::flann::Index flannIndex(_lastDescriptors, cv::flann::LshIndexParams(12, 20, 2), cvflann::FLANN_DIST_HAMMING);
+				results = cv::Mat(newDescriptors.rows, k, CV_32SC1);
+				dists = cv::Mat(newDescriptors.rows, k, CV_32FC1);
+
+				// search (nearest neighbor)
+				flannIndex.knnSearch(newDescriptors, results, dists, k, cv::flann::SearchParams() );
+			}
+
+			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_1(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZ>::Ptr mpts_2(new pcl::PointCloud<pcl::PointXYZ>);
+			std::vector<int> indexes_1, indexes_2;
+			std::vector<uchar> outlier_mask;
+			// Check if this descriptor matches with those of the objects
+			mpts_1->resize(newDescriptors.rows);
+			mpts_2->resize(newDescriptors.rows);
+			UDEBUG("newDescriptors=%d _lastKeypoints=%d time=%fs", newDescriptors.rows, _lastKeypoints.size(), timer.elapsed());
+			int oi = 0;
+			if(_bruteForceMatching)
+			{
+				for(unsigned int i=0; i<matches.size(); ++i)
+				{
+					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
+							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.x+0.5f),
+							int(newKeypoints.at(matches.at(i).at(0).queryIdx).pt.y+0.5f),
+							(float)imageMono.cols/2,
+							(float)imageMono.rows/2,
+							1.0f/image.depthConstant(),
+							1.0f/image.depthConstant());
+					if(matches.at(i).at(0).trainIdx >=0)
+					{
+						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
+								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.x+0.5f),
+								int(_lastKeypoints.at(matches.at(i).at(0).trainIdx).pt.y+0.5f),
+								(float)imageMono.cols/2,
+								(float)imageMono.rows/2,
+								1.0f/image.depthConstant(),
+								1.0f/image.depthConstant());
+
+						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
+						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
+						{
+							mpts_1->at(oi) = pt1;
+							mpts_2->at(oi) = pt2;
+							++oi;
+						}
+					}
+					else
+					{
+						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
+					}
+				}
+			}
+			else
+			{
+				for(int i=0; i<newDescriptors.rows; ++i)
+				{
+					pcl::PointXYZ pt1 = util3d::getDepth(image.depth(),
+							int(newKeypoints.at(i).pt.x+0.5f),
+							int(newKeypoints.at(i).pt.y+0.5f),
+							(float)imageMono.cols/2,
+							(float)imageMono.rows/2,
+							1.0f/image.depthConstant(),
+							1.0f/image.depthConstant());
+					if(results.at<int>(i,0) >=0)
+					{
+						pcl::PointXYZ pt2 = util3d::getDepth(_lastDepth,
+								int(_lastKeypoints.at(results.at<int>(i,0)).pt.x+0.5f),
+								int(_lastKeypoints.at(results.at<int>(i,0)).pt.y+0.5f),
+								(float)imageMono.cols/2,
+								(float)imageMono.rows/2,
+								1.0f/image.depthConstant(),
+								1.0f/image.depthConstant());
+
+						if(uIsFinite(pt1.z) && uIsFinite(pt2.z) &&
+						   (this->getMaxDepth() <= 0 || (pt1.z < this->getMaxDepth() && pt2.z < this->getMaxDepth())))
+						{
+							mpts_1->at(oi) = pt1;
+							mpts_2->at(oi) = pt2;
+							++oi;
+						}
+					}
+					else
+					{
+						UWARN("Index = %d for i=%d ?!?", results.at<int>(i,0), i);
+					}
+				}
+			}
+			mpts_1->resize(oi);
+			mpts_2->resize(oi);
+
+			UDEBUG("Correspondences = %d", oi);
+
+			if(oi >= this->getMinInliers())
+			{
+				mpts_1 = util3d::transformPointCloud(mpts_1, image.localTransform()); // new
+				mpts_2 = util3d::transformPointCloud(mpts_2, image.localTransform()); // previous
+				correspondences = mpts_2->size();
+				Transform t = util3d::transformFromXYZCorrespondences(
+						mpts_1,
+						mpts_2,
+						this->getInlierDistance(),
+						this->getIterations(),
+						&inliers);
+				float x,y,z, roll,pitch,yaw;
+				pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(t), x,y,z, roll,pitch,yaw);
+
+				// Large transforms may be erroneous computed transforms, so keep under 1 m
+				if(inliers >= this->getMinInliers())
+				{
+					if(isLargeEnoughTransform(t))
+					{
+						_lastKeypoints = newKeypoints;
+						_lastDescriptors = newDescriptors;
+						_lastDepth = image.depth().clone();
+						output = t;
+					}
+					else
+					{
+						output.setIdentity();
+					}
+				}
+				else
+				{
+					UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+				}
+			}
+			else
+			{
+				UWARN("Not enough inliers %d < %d", oi, this->getMinInliers());
+			}
+		}
+		else
+		{
+			UWARN("No feature extracted!");
+		}
+	}
+	else
+	{
+		_lastKeypoints = newKeypoints;
+		_lastDescriptors = newDescriptors;
+		_lastDepth = image.depth().clone();
+		output.setIdentity();
+	}
+
+	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
+			timer.elapsed(),
+			newDescriptors.rows,
+			inliers,
+			correspondences);
+
+	return output;
+}
+
+//OdometryBOW
+
+OdometryBOW::OdometryBOW(
+		int detectorType, // SURF or SIFT
+		float inlierDistance,
+		int maxWords,
+		int minInliers,
+		int iterations,
+		float maxDepth,
+		float linearUpdate,
+		float angularUpdate,
+		int resetCoutdown,
+		float surfHessianThreshold,
+		float nndr) : // nearest neighbor distance ratio
+	Odometry(inlierDistance, maxWords, minInliers, iterations, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+	_memory(new Memory())
+{
+	ParametersMap customParameters;
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(maxWords)));
+	customParameters.insert(ParametersPair(Parameters::kKpDetectorStrategy(), uNumber2Str(detectorType)));
+	customParameters.insert(ParametersPair(Parameters::kSURFHessianThreshold(), uNumber2Str(surfHessianThreshold)));
+	customParameters.insert(ParametersPair(Parameters::kKpNndrRatio(), uNumber2Str(nndr)));
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+	if(!_memory->init("", false, customParameters, false))
+	{
+		UERROR("Error initializing the memory for BOW Odometry.");
+	}
+}
+
+OdometryBOW::OdometryBOW(const ParametersMap & parameters) :
+	Odometry(parameters),
+	_memory(new Memory(parameters))
+{
+	ParametersMap customParameters;
+	customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), uNumber2Str(this->getMaxFeatures()))); // hack
+	customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
+	customParameters.insert(ParametersPair(Parameters::kMemImageKept(), "false"));
+	if(!_memory->init("", false, customParameters, false))
+	{
+		UERROR("Error initializing the memory for BOW Odometry.");
+	}
+}
+
+OdometryBOW::~OdometryBOW()
+{
+	UDEBUG("");
+	delete _memory;
+	UDEBUG("");
+}
+
+
+void OdometryBOW::reset()
+{
+	Odometry::reset();
+	_memory->init("", false, ParametersMap(), false);
+}
+
+
+// return true if odometry is correctly computed
+Transform OdometryBOW::computeTransform(Image & image)
+{
+	UTimer timer;
+	Transform output;
+
+	std::vector<cv::KeyPoint> keypoints;
+	cv::Mat descriptors;
+	_memory->extractKeypointsAndDescriptors(image.image(), keypoints, descriptors);
+
+	image.setDescriptors(descriptors);
+	image.setKeypoints(keypoints);
+
+	int inliers = 0;
+	int correspondences = 0;
+
+	const Signature * previousSignature = _memory->getLastWorkingSignature();
+	if(_memory->update(image))
+	{
+		const Signature * newSignature = _memory->getLastWorkingSignature();
+		if(previousSignature && newSignature)
+		{
+			Transform transform;
+			if(!previousSignature->getWords3().empty() && !newSignature->getWords3().empty())
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers1(new pcl::PointCloud<pcl::PointXYZ>); // previous
+				pcl::PointCloud<pcl::PointXYZ>::Ptr inliers2(new pcl::PointCloud<pcl::PointXYZ>); // new
+
+				util3d::findCorrespondences(
+						previousSignature->getWords3(),
+						newSignature->getWords3(),
+						*inliers1,
+						*inliers2,
+						this->getMaxDepth());
+
+				if((int)inliers1->size() >= this->getMinInliers())
+				{
+					correspondences = inliers1->size();
+
+					transform = util3d::transformFromXYZCorrespondences(
+							inliers2,
+							inliers1,
+							this->getInlierDistance(),
+							this->getIterations(),
+							&inliers);
+
+					if(inliers < this->getMinInliers())
+					{
+						transform.setNull();
+						UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
+					}
+				}
+				else
+				{
+					UWARN("Not enough inliers %d < %d", (int)inliers1->size(), this->getMinInliers());
+				}
+			}
+
+			if(transform.isNull())
+			{
+				_memory->deleteLocation(newSignature->id());
+			}
+			else if(!isLargeEnoughTransform(transform))
+			{
+				output.setIdentity();
+				_memory->deleteLocation(newSignature->id());
+			}
+			else
+			{
+				output = transform;
+				_memory->deleteLocation(previousSignature->id());
+			}
+		}
+		else if(!previousSignature && newSignature)
+		{
+			output.setIdentity();
+		}
+
+		_memory->emptyTrash();
+	}
+
+	UINFO("Odom update time = %fs features=%d inliers=%d/%d",
+			timer.elapsed(),
+			descriptors.rows,
+			inliers,
+			correspondences);
+	return output;
+}
+
+// OdometryICP
+OdometryICP::OdometryICP(
+		int decimation,
+		float voxelSize,
+		float samples,
+		float maxCorrespondenceDistance,
+		int	maxIterations,
+		float maxFitness,
+		float maxDepth,
+		float linearUpdate,
+		float angularUpdate,
+		int resetCoutdown) :
+	Odometry(0, 0, 0, 0, maxDepth, linearUpdate, angularUpdate, resetCoutdown),
+	_decimation(decimation),
+	_voxelSize(voxelSize),
+	_samples(samples),
+	_maxCorrespondenceDistance(maxCorrespondenceDistance),
+	_maxIterations(maxIterations),
+	_maxFitness(maxFitness),
+	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
+{
+
+}
+
+OdometryICP::OdometryICP(const ParametersMap & parameters) :
+	Odometry(parameters),
+	_decimation(Parameters::defaultOdomICPDecimation()),
+	_voxelSize(Parameters::defaultOdomICPVoxelSize()),
+	_samples(Parameters::defaultOdomICPSamples()),
+	_maxCorrespondenceDistance(Parameters::defaultOdomICPCorrespondencesDistance()),
+	_maxIterations(Parameters::defaultOdomICPIterations()),
+	_maxFitness(Parameters::defaultOdomICPMaxFitness()),
+	_previousCloud(new pcl::PointCloud<pcl::PointNormal>)
+{
+	Parameters::parse(parameters, Parameters::kOdomICPDecimation(), _decimation);
+	Parameters::parse(parameters, Parameters::kOdomICPVoxelSize(), _voxelSize);
+	Parameters::parse(parameters, Parameters::kOdomICPSamples(), _samples);
+	Parameters::parse(parameters, Parameters::kOdomICPCorrespondencesDistance(), _maxCorrespondenceDistance);
+	Parameters::parse(parameters, Parameters::kOdomICPIterations(), _maxIterations);
+	Parameters::parse(parameters, Parameters::kOdomICPMaxFitness(), _maxFitness);
+}
+
+void OdometryICP::reset()
+{
+	Odometry::reset();
+	_previousCloud.reset(new pcl::PointCloud<pcl::PointNormal>);
+}
+
+// return not null if odometry is correctly computed
+Transform OdometryICP::computeTransform(Image & image)
+{
+	UTimer timer;
+	Transform output;
+
+	bool hasConverged = false;
+	double fitness = 0;
+	unsigned int minPoints = 100;
+	if(!image.depth().empty())
+	{
+		pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudXYZ = util3d::getICPReadyCloud(
+						image.depth(),
+						image.depthConstant(),
+						_decimation,
+						this->getMaxDepth(),
+						_voxelSize,
+						_samples,
+						image.localTransform());
+
+		pcl::PointCloud<pcl::PointNormal>::Ptr newCloud = util3d::computeNormals(newCloudXYZ);
+
+		std::vector<int> indices;
+		newCloud = util3d::removeNaNNormalsFromPointCloud(newCloud);
+		if(newCloudXYZ->size() != newCloud->size())
+		{
+			UWARN("removed nan normals...");
+		}
+
+		if(_previousCloud->size() > minPoints && newCloud->size() > minPoints)
+		{
+			Transform transform = util3d::icpPointToPlane(newCloud,
+					_previousCloud,
+					_maxCorrespondenceDistance,
+					_maxIterations,
+					hasConverged,
+				    fitness);
+
+			//pcl::io::savePCDFile("old.pcd", *_previousCloud);
+			//pcl::io::savePCDFile("new.pcd", *newCloud);
+			//pcl::PointCloud<pcl::PointXYZ>::Ptr newCloudTransformed = util3d::transformPointCloud(newCloud, transform);
+			//pcl::io::savePCDFile("newicp.pcd", *newCloudTransformed);
+
+			if(hasConverged && (_maxFitness == 0 || fitness < _maxFitness))
+			{
+				output = transform;
+				_previousCloud = newCloud;
+			}
+			else
+			{
+				UWARN("Transform not valid (hasConverged=%s fitness = %f < %f)",
+						hasConverged?"true":"false", fitness, _maxFitness);
+			}
+		}
+		else if(newCloud->size() > minPoints)
+		{
+			output.setIdentity();
+			_previousCloud = newCloud;
+		}
+	}
+	else
+	{
+		UERROR("Depth is empty?!?");
+	}
+
+	UINFO("Odom update time = %fs hasConverged=%s fitness=%f cloud=%d",
+			timer.elapsed(),
+			hasConverged?"true":"false",
+			fitness,
+			(int)_previousCloud->size());
+
+	return output;
+}
+
+// OdometryThread
+OdometryThread::OdometryThread(Odometry * odometry) :
+	_odometry(odometry),
+	_resetOdometry(false)
+{
+	UASSERT(_odometry != 0);
+}
+
+OdometryThread::~OdometryThread()
+{
+	this->unregisterFromEventsManager();
+	this->join(true);
+	if(_odometry)
+	{
+		delete _odometry;
+	}
+}
+
+void OdometryThread::handleEvent(UEvent * event)
+{
+	if(this->isRunning())
+	{
+		if(event->getClassName().compare("CameraEvent") == 0)
+		{
+			CameraEvent * cameraEvent = (CameraEvent*)event;
+			if(cameraEvent->getCode() == CameraEvent::kCodeImageDepth)
+			{
+				this->addImage(cameraEvent->image());
+			}
+			else if(cameraEvent->getCode() == CameraEvent::kCodeNoMoreImages)
+			{
+				this->post(new CameraEvent()); // forward the event
+			}
+		}
+		else if(event->getClassName().compare("OdometryResetEvent") == 0)
+		{
+			_resetOdometry = true;
+		}
+	}
+}
+
+void OdometryThread::mainLoopKill()
+{
+	_imageAdded.release();
+}
+
+//============================================================
+// MAIN LOOP
+//============================================================
+void OdometryThread::mainLoop()
+{
+	if(_resetOdometry)
+	{
+		_odometry->reset();
+		_resetOdometry = false;
+	}
+
+	Image image;
+	getImage(image);
+	if(!image.empty())
+	{
+		Transform pose = _odometry->process(image);
+		image.setPose(pose); // a null pose notify that odometry could not be computed
+		this->post(new OdometryEvent(image));
+	}
+}
+
+void OdometryThread::addImage(const Image & image)
+{
+	if(image.empty() || image.depth().empty() || image.depthConstant() == 0.0f)
+	{
+		ULOGGER_ERROR("image empty !?");
+		return;
+	}
+
+	bool notify = true;
+	_imageMutex.lock();
+	{
+		notify = _imageBuffer.empty();
+		_imageBuffer = image;
+	}
+	_imageMutex.unlock();
+
+	if(notify)
+	{
+		_imageAdded.release();
+	}
+}
+
+void OdometryThread::getImage(Image & image)
+{
+	_imageAdded.acquire();
+	_imageMutex.lock();
+	{
+		if(!_imageBuffer.empty())
+		{
+			image = _imageBuffer;
+			_imageBuffer = cv::Mat();
+		}
+	}
+	_imageMutex.unlock();
+}
+
+} /* namespace rtabmap */

corelib/src/Parameters.cpp

@@ -20,11 +20,15 @@
 #include "rtabmap/core/Parameters.h"
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <math.h>
+#include <stdlib.h>
 
 namespace rtabmap
 {
 
 ParametersMap Parameters::parameters_;
+ParametersMap Parameters::descriptions_;
 Parameters Parameters::instance_;
 
 Parameters::Parameters()
@@ -37,18 +41,97 @@ Parameters::~Parameters()
 
 std::string Parameters::getDefaultWorkingDirectory()
 {
+#ifdef DEMO_BUILD
+	std::string path = "."; // current directory
+#else
 	std::string path = UDirectory::homeDir();
 	if(!path.empty())
 	{
 		UDirectory::makeDir(path += UDirectory::separator() + "Documents");
 		UDirectory::makeDir(path += UDirectory::separator() + "RTAB-Map");
-		path += UDirectory::separator(); // add trailing separator
+
 	}
 	else
 	{
 		UFATAL("Can't get the HOME variable environment!");
 	}
+#endif
+	path += UDirectory::separator(); // add trailing separator
 	return path;
 }
 
+std::string Parameters::getDefaultDatabasePath()
+{
+	return getDefaultWorkingDirectory() + getDefaultDatabaseName();
+}
+
+std::string Parameters::getDefaultDatabaseName()
+{
+	return "rtabmap.db";
+}
+
+std::string Parameters::getDescription(const std::string & paramKey)
+{
+	std::string description;
+	ParametersMap::iterator iter = descriptions_.find(paramKey);
+	if(iter != descriptions_.end())
+	{
+		description = iter->second;
+	}
+	else
+	{
+		UERROR("Parameters \"%s\" doesn't exist!", paramKey.c_str());
+	}
+	return description;
+}
+
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, bool & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = uStr2Bool(iter->second.c_str());
+	}
+}
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, int & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = atoi(iter->second.c_str());
+	}
+}
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, unsigned int & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = atoi(iter->second.c_str());
+	}
+}
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, float & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = atof(iter->second.c_str());
+	}
+}
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, double & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = atof(iter->second.c_str());
+	}
+}
+void Parameters::parse(const ParametersMap & parameters, const std::string & key, std::string & value)
+{
+	ParametersMap::const_iterator iter = parameters.find(key);
+	if(iter != parameters.end())
+	{
+		value = iter->second;
+	}
+}
+
 }

corelib/src/RtabmapThread.cpp

@@ -23,16 +23,21 @@
 #include "rtabmap/core/Camera.h"
 #include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
+#include "rtabmap/core/OdometryEvent.h"
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UTimer.h>
 
 namespace rtabmap {
 
 RtabmapThread::RtabmapThread() :
 		_imageBufferMaxSize(Parameters::defaultRtabmapImageBufferSize()),
-		_rtabmap(new Rtabmap())
+		_rate(Parameters::defaultRtabmapDetectionRate()),
+		_frameRateTimer(new UTimer()),
+		_rtabmap(new Rtabmap()),
+		_paused(false)
 
 {
 }
@@ -44,6 +49,7 @@ RtabmapThread::~RtabmapThread()
 	// Stop the thread first
 	join(true);
 
+	delete _frameRateTimer;
 	delete _rtabmap;
 }
 
@@ -61,12 +67,7 @@ void RtabmapThread::pushNewState(State newState, const ParametersMap & parameter
 	_imageAdded.release();
 }
 
-void RtabmapThread::setWorkingDirectory(const std::string & path)
-{
-	_rtabmap->setWorkingDirectory(path);
-}
-
-void RtabmapThread::clearBufferedSensors()
+void RtabmapThread::clearBufferedData()
 {
 	_imageMutex.lock();
 	{
@@ -75,9 +76,36 @@ void RtabmapThread::clearBufferedSensors()
 	_imageMutex.unlock();
 }
 
+void RtabmapThread::publishMap() const
+{
+	std::map<int, std::vector<unsigned char> > images;
+	std::map<int, std::vector<unsigned char> > depths;
+	std::map<int, std::vector<unsigned char> > depths2d;
+	std::map<int, float> depthConstants;
+	std::map<int, Transform> localTransforms;
+	std::map<int, Transform> poses;
+	Transform mapCorrection;
+
+	_rtabmap->get3DMap(images,
+			depths,
+			depths2d,
+			depthConstants,
+			localTransforms,
+			poses,
+			mapCorrection);
+
+	this->post(new RtabmapEvent3DMap(images,
+			depths,
+			depths2d,
+			depthConstants,
+			localTransforms,
+			poses,
+			mapCorrection));
+}
+
 void RtabmapThread::mainLoopKill()
 {
-	this->clearBufferedSensors();
+	this->clearBufferedData();
 
 	// this will post the newData semaphore
 	_imageAdded.release();
@@ -100,22 +128,21 @@ void RtabmapThread::mainLoop()
 	}
 	_stateMutex.unlock();
 
-	ParametersMap::iterator iter;
 	switch(state)
 	{
 	case kStateDetecting:
 		this->process();
 		break;
 	case kStateChangingParameters:
-		if((iter=parameters.find(Parameters::kRtabmapImageBufferSize())) != parameters.end())
-		{
-			_imageBufferMaxSize = std::atoi(iter->second.c_str());
-		}
+		Parameters::parse(parameters, Parameters::kRtabmapImageBufferSize(), _imageBufferMaxSize);
+		Parameters::parse(parameters, Parameters::kRtabmapDetectionRate(), _rate);
+		UASSERT(_imageBufferMaxSize >= 0);
+		UASSERT(_rate >= 0.0f);
 		_rtabmap->parseParameters(parameters);
 		break;
 	case kStateReseting:
 		_rtabmap->resetMemory();
-		this->clearBufferedSensors();
+		this->clearBufferedData();
 		break;
 	case kStateDumpingMemory:
 		_rtabmap->dumpData();
@@ -131,10 +158,16 @@ void RtabmapThread::mainLoop()
 		break;
 	case kStateDeletingMemory:
 		_rtabmap->resetMemory(true);
-		this->clearBufferedSensors();
+		this->clearBufferedData();
 		break;
-	case kStateCleanSensorsBuffer:
-		this->clearBufferedSensors();
+	case kStateCleanDataBuffer:
+		this->clearBufferedData();
+		break;
+	case kStatePublishingMap:
+		this->publishMap();
+		break;
+	case kStateTriggeringMap:
+		_rtabmap->triggerNewMap();
 		break;
 	default:
 		UFATAL("Invalid state !?!?");
@@ -147,12 +180,24 @@ void RtabmapThread::handleEvent(UEvent* event)
 {
 	if(this->isRunning() && event->getClassName().compare("CameraEvent") == 0)
 	{
+		UDEBUG("CameraEvent");
 		CameraEvent * e = (CameraEvent*)event;
-		if(e->getCode() == CameraEvent::kCodeImage || e->getCode() == CameraEvent::kCodeFeatures)
+		if(e->getCode() == CameraEvent::kCodeImage ||
+		   e->getCode() == CameraEvent::kCodeFeatures ||
+		   e->getCode() == CameraEvent::kCodeImageDepth)
 		{
 			this->addImage(e->image());
 		}
 	}
+	else if(event->getClassName().compare("OdometryEvent") == 0)
+	{
+		UDEBUG("OdometryEvent");
+		OdometryEvent * e = (OdometryEvent*)event;
+		if(e->isValid())
+		{
+			this->addImage(e->data());
+		}
+	}
 	else if(event->getClassName().compare("RtabmapEventCmd") == 0)
 	{
 		RtabmapEventCmd * rtabmapEvent = (RtabmapEventCmd*)event;
@@ -200,10 +245,29 @@ void RtabmapThread::handleEvent(UEvent* event)
 			ULOGGER_DEBUG("CMD_DELETE_MEMORY");
 			pushNewState(kStateDeletingMemory);
 		}
-		else if(cmd == RtabmapEventCmd::kCmdCleanSensorsBuffer)
+		else if(cmd == RtabmapEventCmd::kCmdCleanDataBuffer)
 		{
-			ULOGGER_DEBUG("CMD_CLEAN_SENSORS_BUFFER");
-			pushNewState(kStateCleanSensorsBuffer);
+			ULOGGER_DEBUG("CMD_CLEAN_DATA_BUFFER");
+			pushNewState(kStateCleanDataBuffer);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPublish3DMap)
+		{
+			ULOGGER_DEBUG("CMD_PUBLISH_MAP");
+			pushNewState(kStatePublishingMap);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdTriggerNewMap)
+		{
+			ULOGGER_DEBUG("CMD_TRIGGER_NEW_MAP");
+			pushNewState(kStateTriggeringMap);
+		}
+		else if(cmd == RtabmapEventCmd::kCmdPause)
+		{
+			ULOGGER_DEBUG("CMD_PAUSE");
+			_paused = !_paused;
+		}
+		else
+		{
+			UWARN("Cmd %d unknown!", cmd);
 		}
 	}
 	else if(event->getClassName().compare("ParamEvent") == 0)
@@ -233,28 +297,40 @@ void RtabmapThread::process()
 
 void RtabmapThread::addImage(const Image & image)
 {
-	if(image.empty())
+	if(!_paused)
 	{
-		ULOGGER_ERROR("image empty !?");
-		return;
-	}
-
-	bool notify = true;
-	_imageMutex.lock();
-	{
-		_imageBuffer.push_back(image);
-		while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
+		if(image.empty())
 		{
-			ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
-			_imageBuffer.pop_front();
-			notify = false;
+			ULOGGER_ERROR("image empty !?");
+			return;
 		}
-	}
-	_imageMutex.unlock();
 
-	if(notify)
-	{
-		_imageAdded.release();
+		if(_rate>0.0f)
+		{
+			if(_frameRateTimer->getElapsedTime() < 1.0f/_rate)
+			{
+				return;
+			}
+		}
+		_frameRateTimer->start();
+
+		bool notify = true;
+		_imageMutex.lock();
+		{
+			_imageBuffer.push_back(image);
+			while(_imageBufferMaxSize > 0 && _imageBuffer.size() > (unsigned int)_imageBufferMaxSize)
+			{
+				ULOGGER_WARN("Data buffer is full, the oldest data is removed to add the new one.");
+				_imageBuffer.pop_front();
+				notify = false;
+			}
+		}
+		_imageMutex.unlock();
+
+		if(notify)
+		{
+			_imageAdded.release();
+		}
 	}
 }
 

corelib/src/Signature.cpp

@@ -17,9 +17,10 @@
  * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/EpipolarGeometry.h"
 #include "rtabmap/core/Memory.h"
+#include "rtabmap/core/util3d.h"
 #include <opencv2/highgui/highgui.hpp>
 
 #include <rtabmap/utilite/UtiLite.h>
@@ -34,31 +35,45 @@ Signature::~Signature()
 
 Signature::Signature(
 		int id,
+		int mapId,
 		const std::multimap<int, cv::KeyPoint> & words,
-		const cv::Mat & image) :
+		const std::multimap<int, pcl::PointXYZ> & words3, // in base_link frame (localTransform applied)
+		const Transform & pose,
+		const std::vector<unsigned char> & depth2D, // in base_link frame
+		const std::vector<unsigned char> & image, // in camera_link frame
+		const std::vector<unsigned char> & depth, // in camera_link frame
+		float depthConstant,
+		const Transform & localTransform) :
 	_id(id),
+	_mapId(mapId),
 	_weight(0),
 	_saved(false),
 	_modified(true),
 	_neighborsModified(true),
 	_words(words),
 	_enabled(false),
-	_image(image)
+	_image(image),
+	_depth(depth),
+	_depth2D(depth2D),
+	_depthConstant(depthConstant),
+	_pose(pose),
+	_localTransform(localTransform),
+	_words3(words3)
 {
 }
 
-void Signature::addNeighbors(const std::set<int> & neighbors)
+void Signature::addNeighbors(const std::map<int, Transform> & neighbors)
 {
-	for(std::set<int>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
+	for(std::map<int, Transform>::const_iterator i=neighbors.begin(); i!=neighbors.end(); ++i)
 	{
-		this->addNeighbor(*i);
+		this->addNeighbor(i->first, i->second);
 	}
 }
 
-void Signature::addNeighbor(int neighbor)
+void Signature::addNeighbor(int neighbor, const Transform & transform)
 {
 	UDEBUG("Add neighbor %d to %d", neighbor, this->id());
-	_neighbors.insert(neighbor);
+	_neighbors.insert(std::pair<int, Transform>(neighbor, transform));
 	_neighborsModified = true;
 }
 
@@ -80,15 +95,47 @@ void Signature::removeNeighbors()
 
 void Signature::changeNeighborIds(int idFrom, int idTo)
 {
-	if(_neighbors.find(idFrom) != _neighbors.end())
+	std::map<int, Transform>::iterator iter = _neighbors.find(idFrom);
+	if(iter != _neighbors.end())
 	{
-		_neighbors.erase(idFrom);
-		_neighbors.insert(idTo);
+		Transform t = iter->second;
+		_neighbors.erase(iter);
+		_neighbors.insert(std::pair<int, Transform>(idTo, t));
 		_neighborsModified = true;
 	}
 	UDEBUG("(%d) neighbor ids changed from %d to %d", _id, idFrom, idTo);
 }
 
+void Signature::addLoopClosureId(int loopClosureId, const Transform & transform)
+{
+	if(loopClosureId && _loopClosureIds.insert(std::pair<int, Transform>(loopClosureId, transform)).second)
+	{
+		_neighborsModified=true;
+	}
+}
+
+void Signature::addChildLoopClosureId(int childLoopClosureId, const Transform & transform)
+{
+	if(childLoopClosureId && _childLoopClosureIds.insert(std::pair<int, Transform>(childLoopClosureId, transform)).second)
+	{
+		_neighborsModified=true;
+	}
+}
+
+void Signature::changeLoopClosureId(int idFrom, int idTo)
+{
+	std::map<int, Transform>::iterator iter = _loopClosureIds.find(idFrom);
+	if(iter != _loopClosureIds.end())
+	{
+		Transform t = iter->second;
+		_loopClosureIds.erase(iter);
+		_loopClosureIds.insert(std::pair<int, Transform>(idTo, t));
+		_neighborsModified = true;
+	}
+	UDEBUG("(%d) loop closure ids changed from %d to %d", _id, idFrom, idTo);
+}
+
+
 float Signature::compareTo(const Signature * s) const
 {
 	float similarity = 0.0f;
@@ -109,12 +156,18 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
 	if(kps.size())
 	{
+		std::list<pcl::PointXYZ> pts = uValues(_words3, oldWordId);
 		_words.erase(oldWordId);
+		_words3.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
 		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
 		{
 			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
 		}
+		for(std::list<pcl::PointXYZ>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
+		{
+			_words3.insert(std::pair<int, pcl::PointXYZ>(activeWordId, (*iter)));
+		}
 	}
 }
 
@@ -126,11 +179,20 @@ bool Signature::isBadSignature() const
 void Signature::removeAllWords()
 {
 	_words.clear();
+	_words3.clear();
 }
 
 void Signature::removeWord(int wordId)
 {
 	_words.erase(wordId);
+	_words3.erase(wordId);
+}
+
+void Signature::setDepth(const std::vector<unsigned char> & depth, float depthConstant)
+{
+	UASSERT_MSG(depth.empty() || (!depth.empty() && depthConstant > 0.0f), uFormat("depthConstant=%f",depthConstant).c_str());
+	_depth = depth;
+	_depthConstant=depthConstant;
 }
 
 } //namespace rtabmap

corelib/src/Signature.h

@@ -1,109 +0,0 @@
-/*
- * Copyright (C) 2010-2011, Mathieu Labbe and IntRoLab - Universite de Sherbrooke
- *
- * This file is part of RTAB-Map.
- *
- * RTAB-Map is free software: you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * (at your option) any later version.
- *
- * RTAB-Map is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with RTAB-Map.  If not, see <http://www.gnu.org/licenses/>.
- */
-
-#pragma once
-
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
-
-#include <opencv2/core/core.hpp>
-#include <opencv2/features2d/features2d.hpp>
-#include <opencv2/imgproc/imgproc.hpp>
-#include <map>
-#include <list>
-#include <vector>
-#include <set>
-
-namespace rtabmap
-{
-
-class Memory;
-
-class RTABMAP_EXP Signature
-{
-public:
-	Signature(int id,
-			const std::multimap<int, cv::KeyPoint> & words,
-			const cv::Mat & image = cv::Mat());
-	virtual ~Signature();
-
-	/**
-	 * Must return a value between >=0 and <=1 (1 means 100% similarity).
-	 */
-	float compareTo(const Signature * signature) const;
-	bool isBadSignature() const;
-
-	int id() const {return _id;}
-
-	void addNeighbors(const std::set<int> & neighbors);
-	void addNeighbor(int neighbor);
-	void removeNeighbor(int neighborId);
-	void removeNeighbors();
-	bool hasNeighbor(int neighborId) const {return _neighbors.find(neighborId) != _neighbors.end();}
-	void setWeight(int weight) {if(_weight!=weight)_modified=true;_weight = weight;}
-	void setLoopClosureIds(const std::set<int> & loopClosureIds) {_loopClosureIds = loopClosureIds;_neighborsModified=true;}
-	void addLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.insert(loopClosureId).second)_neighborsModified=true;}
-	void removeLoopClosureId(int loopClosureId) {if(loopClosureId && _loopClosureIds.erase(loopClosureId))_neighborsModified=true;}
-	void removeChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.erase(childLoopClosureId))_neighborsModified=true;}
-	bool hasLoopClosureId(int loopClosureId) const {return _loopClosureIds.find(loopClosureId) != _loopClosureIds.end();}
-	void setChildLoopClosureIds(std::set<int> & childLoopClosureIds) {_childLoopClosureIds = childLoopClosureIds;_neighborsModified=true;}
-	void addChildLoopClosureId(int childLoopClosureId) {if(childLoopClosureId && _childLoopClosureIds.insert(childLoopClosureId).second)_neighborsModified=true;}
-	void setSaved(bool saved) {_saved = saved;}
-	void setModified(bool modified) {_modified = modified; _neighborsModified = modified;}
-	void changeNeighborIds(int idFrom, int idTo);
-
-	const std::set<int> & getNeighbors() const {return _neighbors;}
-	int getWeight() const {return _weight;}
-	const std::set<int> & getLoopClosureIds() const {return _loopClosureIds;}
-	const std::set<int> & getChildLoopClosureIds() const {return _childLoopClosureIds;}
-	bool isSaved() const {return _saved;}
-	bool isModified() const {return _modified || _neighborsModified;}
-	bool isNeighborsModified() const {return _neighborsModified;}
-
-	//visual words stuff
-	void removeAllWords();
-	void removeWord(int wordId);
-	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, cv::KeyPoint> & words) {_enabled = false;_words = words;}
-	bool isEnabled() const {return _enabled;}
-	void setEnabled(bool enabled) {_enabled = enabled;}
-	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
-	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	void setImage(const cv::Mat & image) {_image = image;}
-	const cv::Mat & getImage() const {return _image;}
-
-private:
-	int _id;
-	std::set<int> _neighbors; // id
-	int _weight;
-	std::set<int> _loopClosureIds;
-	std::set<int> _childLoopClosureIds;
-	bool _saved; // If it's saved to bd
-	bool _modified;
-	bool _neighborsModified; // Optimization when updating signatures in database
-
-	// Contains all words (Some can be duplicates -> if a word appears 2
-	// times in the signature, it will be 2 times in this list)
-	// Words match with the CvSeq keypoints and descriptors
-	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
-	std::map<int, int> _wordsChanged; // <oldId, newId>
-	bool _enabled;
-	cv::Mat _image;
-};
-
-} // namespace rtabmap

corelib/src/Statistics.cpp

@@ -34,7 +34,9 @@ Statistics::Statistics() :
 	_extended(0),
 	_refImageId(0),
 	_loopClosureId(0),
-	_localLoopClosureId(0)
+	_localLoopClosureId(0),
+	_refDepthConstant(0),
+	_loopDepthConstant(0)
 {
 	_defaultDataInitialized = true;
 }
@@ -49,14 +51,4 @@ void Statistics::addStatistic(const std::string & name, float value)
 	uInsert(_data, std::pair<std::string, float>(name, value));
 }
 
-void Statistics::setRefImage(const cv::Mat & image)
-{
-	_refImage = image;
-}
-
-void Statistics::setLoopImage(const cv::Mat & image)
-{
-	_loopImage = image;
-}
-
 }

corelib/src/Transform.cpp

@@ -0,0 +1,190 @@
+/*
+ * Transform.cpp
+ *
+ *  Created on: 2013-08-30
+ *      Author: Mathieu
+ */
+
+#include <rtabmap/core/Transform.h>
+
+#include <pcl/common/eigen.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UMath.h>
+#include <iomanip>
+
+namespace rtabmap {
+
+Transform::Transform() : data_(12)
+{
+	data_[0] = 0.0f;
+	data_[1] = 0.0f;
+	data_[2] = 0.0f;
+	data_[3] = 0.0f;
+	data_[4] = 0.0f;
+	data_[5] = 0.0f;
+	data_[6] = 0.0f;
+	data_[7] = 0.0f;
+	data_[8] = 0.0f;
+	data_[9] = 0.0f;
+	data_[10] = 0.0f;
+	data_[11] = 0.0f;
+}
+
+// rotation matrix r## and origin o##
+Transform::Transform(float r11, float r12, float r13, float o14,
+				     float r21, float r22, float r23, float o24,
+				     float r31, float r32, float r33, float o34) :
+	data_(12)
+{
+	data_[0] = r11;
+	data_[1] = r12;
+	data_[2] = r13;
+	data_[3] = o14;
+	data_[4] = r21;
+	data_[5] = r22;
+	data_[6] = r23;
+	data_[7] = o24;
+	data_[8] = r31;
+	data_[9] = r32;
+	data_[10] = r33;
+	data_[11] = o34;
+}
+
+Transform::Transform(float x, float y, float z, float roll, float pitch, float yaw)
+{
+	Eigen::Affine3f t = pcl::getTransformation (x, y, z, roll, pitch, yaw);
+	*this = util3d::transformFromEigen3f(t);
+}
+
+bool Transform::isNull() const
+{
+	return (data_[0] == 0.0f &&
+			data_[1] == 0.0f &&
+			data_[2] == 0.0f &&
+			data_[3] == 0.0f &&
+			data_[4] == 0.0f &&
+			data_[5] == 0.0f &&
+			data_[6] == 0.0f &&
+			data_[7] == 0.0f &&
+			data_[8] == 0.0f &&
+			data_[9] == 0.0f &&
+			data_[10] == 0.0f &&
+			data_[11] == 0.0f) ||
+			uIsNan(data_[0]) ||
+			uIsNan(data_[1]) ||
+			uIsNan(data_[2]) ||
+			uIsNan(data_[3]) ||
+			uIsNan(data_[4]) ||
+			uIsNan(data_[5]) ||
+			uIsNan(data_[6]) ||
+			uIsNan(data_[7]) ||
+			uIsNan(data_[8]) ||
+			uIsNan(data_[9]) ||
+			uIsNan(data_[10]) ||
+			uIsNan(data_[11]);
+}
+
+bool Transform::isIdentity() const
+{
+	return data_[0] == 1.0f &&
+			data_[1] == 0.0f &&
+			data_[2] == 0.0f &&
+			data_[3] == 0.0f &&
+			data_[4] == 0.0f &&
+			data_[5] == 1.0f &&
+			data_[6] == 0.0f &&
+			data_[7] == 0.0f &&
+			data_[8] == 0.0f &&
+			data_[9] == 0.0f &&
+			data_[10] == 1.0f &&
+			data_[11] == 0.0f;
+}
+
+void Transform::setNull()
+{
+	*this = Transform();
+}
+
+void Transform::setIdentity()
+{
+	*this = getIdentity();
+}
+
+Transform Transform::getIdentity()
+{
+	return Transform(1,0,0,0,
+					 0,1,0,0,
+					 0,0,1,0);
+}
+
+Transform Transform::inverse() const
+{
+	Eigen::Matrix4f m = util3d::transformToEigen4f(*this);
+	return util3d::transformFromEigen4f(m.inverse());
+}
+
+Transform Transform::rotation() const
+{
+	return Transform(data_[0], data_[1], data_[2], 0,
+					 data_[4], data_[5], data_[6], 0,
+					 data_[8], data_[9], data_[10], 0);
+}
+
+Transform Transform::translation() const
+{
+	return Transform(1,0,0, data_[3],
+					 0,1,0, data_[7],
+					 0,0,1, data_[11]);
+}
+
+void Transform::getTranslationAndEulerAngles(float & x, float & y, float & z, float & roll, float & pitch, float & yaw) const
+{
+	pcl::getTranslationAndEulerAngles(util3d::transformToEigen3f(*this), x, y, z, roll, pitch, yaw);
+}
+
+std::string Transform::prettyPrint() const
+{
+	float x,y,z,roll,pitch,yaw;
+	getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+	return uFormat("xyz=%f,%f,%f rpy=%f,%f,%f", x,y,z, roll,pitch,yaw);
+}
+
+Transform Transform::operator*(const Transform & t) const
+{
+	Eigen::Matrix4f m1 = util3d::transformToEigen4f(*this);
+	Eigen::Matrix4f m2 = util3d::transformToEigen4f(t);
+	return util3d::transformFromEigen4f(m1*m2);
+}
+
+Transform & Transform::operator*=(const Transform & t)
+{
+	*this = *this * t;
+	return *this;
+}
+
+bool Transform::operator==(const Transform & t) const
+{
+	return memcmp(data_.data(), t.data_.data(), data_.size() * sizeof(float)) == 0;
+}
+
+bool Transform::operator!=(const Transform & t) const
+{
+	return !(*this == t);
+}
+
+std::ostream& operator<<(std::ostream& os, const Transform& s)
+{
+	for(int i = 0; i < 3; ++i)
+	{
+		for(int j = 0; j < 4; ++j)
+		{
+			std::cout << std::left << std::setw(12) << s.data()[i*4 + j];
+		}
+		std::cout << std::endl;
+	}
+	return os;
+}
+
+
+}

corelib/src/VWDictionary.cpp

@@ -20,7 +20,7 @@
 #include "rtabmap/core/VWDictionary.h"
 #include "VisualWord.h"
 
-#include "Signature.h"
+#include "rtabmap/core/Signature.h"
 #include "rtabmap/core/DBDriver.h"
 #include "NearestNeighbor.h"
 #include "rtabmap/core/Parameters.h"
@@ -37,7 +37,6 @@ const int VWDictionary::ID_START = 1;
 const int VWDictionary::ID_INVALID = 0;
 
 VWDictionary::VWDictionary(const ParametersMap & parameters) :
-	_lastNewWordsAddedCount(0),
 	_totalActiveReferences(0),
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_minDistUsed(Parameters::defaultKpMinDistUsed()),
@@ -66,26 +65,14 @@ VWDictionary::~VWDictionary()
 void VWDictionary::parseParameters(const ParametersMap & parameters)
 {
 	ParametersMap::const_iterator iter;
-	if((iter=parameters.find(Parameters::kKpMinDistUsed())) != parameters.end())
-	{
-		_minDistUsed = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kKpMinDist())) != parameters.end())
-	{
-		this->setMinDist(std::atof((*iter).second.c_str()));
-	}
-	if((iter=parameters.find(Parameters::kKpNndrUsed())) != parameters.end())
-	{
-		_nndrUsed = uStr2Bool((*iter).second.c_str());
-	}
-	if((iter=parameters.find(Parameters::kKpNndrRatio())) != parameters.end())
-	{
-		this->setNndrRatio(std::atof((*iter).second.c_str()));
-	}
-	if((iter=parameters.find(Parameters::kKpMaxLeafs())) != parameters.end())
-	{
-		_maxLeafs = (unsigned int)std::atoi((*iter).second.c_str());
-	}
+	Parameters::parse(parameters, Parameters::kKpMinDistUsed(), _minDistUsed);
+	Parameters::parse(parameters, Parameters::kKpMinDist(), _minDist);
+	Parameters::parse(parameters, Parameters::kKpNndrUsed(), _nndrUsed);
+	Parameters::parse(parameters, Parameters::kKpNndrRatio(), _nndrRatio);
+	Parameters::parse(parameters, Parameters::kKpMaxLeafs(), _maxLeafs);
+
+	UASSERT(_minDist >= 0.0f);
+	UASSERT(_nndrRatio >= 0.0f);
 
 	std::string dictionaryPath = _dictionaryPath;
 	bool incrementalDictionary = _incrementalDictionary;
@@ -183,6 +170,7 @@ void VWDictionary::setIncrementalDictionary(bool incrementalDictionary, const st
 								// laplacian not used
 								VisualWord * vw = new VisualWord(id, &(descriptor[0]), dimension, 0);
 								_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord*>(id, vw));
+								_notIndexedWords.insert(_notIndexedWords.end(), id);
 							}
 							else
 							{
@@ -272,30 +260,6 @@ VWDictionary::NNStrategy VWDictionary::nnStrategy() const
 	return strategy;
 }
 
-void VWDictionary::setMinDist(float d)
-{
-	if(d < 0)
-	{
-		ULOGGER_ERROR("Match threshold must be positive (%f)", d);
-	}
-	else
-	{
-		_minDist = d;
-	}
-
-}
-void VWDictionary::setNndrRatio(float ratio)
-{
-	if(ratio < 0)
-	{
-		ULOGGER_ERROR("Ratio must be positive (%f)", ratio);
-	}
-	else
-	{
-		_nndrRatio = ratio;
-	}
-}
-
 int VWDictionary::getLastIndexedWordId() const
 {
 	if(_mapIndexId.size())
@@ -311,7 +275,7 @@ int VWDictionary::getLastIndexedWordId() const
 void VWDictionary::update()
 {
 	ULOGGER_DEBUG("");
-	if(!_incrementalDictionary && !_dataTree.empty())
+	if(!_incrementalDictionary && !_notIndexedWords.size())
 	{
 		// No need to update the search index if we
 		// use a fixed dictionary and the index is
@@ -319,59 +283,74 @@ void VWDictionary::update()
 		return;
 	}
 
-	_mapIndexId.clear();
-
-	if(_nn && _visualWords.size())
+	if(_notIndexedWords.size() || _visualWords.size() == 0 || _removedIndexedWords.size())
 	{
-		UTimer timer;
-		timer.start();
+		_mapIndexId.clear();
+		_dataTree = cv::Mat();
 
-		if(!_dim)
+		if(_nn && _visualWords.size())
 		{
-			_dim = _visualWords.begin()->second->getDim();
-		}
+			UTimer timer;
+			timer.start();
 
-		// Create the kd-Tree
-		_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
-		std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
-		for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
-		{
-			float * rowFl = _dataTree.ptr<float>(i);
-			if(iter->second->getDim() == _dim)
+			if(!_dim)
 			{
-				memcpy(rowFl, iter->second->getDescriptor(), _dim*sizeof(float));
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+				_dim = _visualWords.begin()->second->getDim();
 			}
-			else
+
+			// Create the kd-Tree
+			_dataTree = cv::Mat(_visualWords.size(), _dim, CV_32F); // SURF descriptors are CV_32F
+			std::map<int, VisualWord*>::const_iterator iter = _visualWords.begin();
+			for(unsigned int i=0; i < _visualWords.size(); ++i, ++iter)
 			{
-				ULOGGER_WARN("A word is not the same size than the dictionary, ignoring that word...");
-				_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, 0)); // set to INVALID
+				float * rowFl = _dataTree.ptr<float>(i);
+				if(iter->second->getDim() == _dim)
+				{
+					memcpy(rowFl, iter->second->getDescriptor(), _dim*sizeof(float));
+					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, iter->second->id()));
+				}
+				else
+				{
+					ULOGGER_WARN("A word is not the same size than the dictionary, ignoring that word...");
+					_mapIndexId.insert(_mapIndexId.end(), std::pair<int, int>(i, 0)); // set to INVALID
+				}
 			}
+
+			ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
+			ULOGGER_DEBUG("copying data = %f s", timer.ticks());
+
+			// Update the nearest neighbor algorithm
+			_nn->setData(_dataTree);
+
+			ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 		}
-
-		ULOGGER_DEBUG("_mapIndexId.size() = %d, words.size()=%d, _dim=%d",_mapIndexId.size(), _visualWords.size(), _dim);
-		ULOGGER_DEBUG("copying data = %f s", timer.ticks());
-
-		// Update the nearest neighbor algorithm
-		_nn->setData(_dataTree);
-
-		ULOGGER_DEBUG("Time to create kd tree = %f s", timer.ticks());
 	}
-	_lastNewWordsAddedCount = 0;
+	else
+	{
+		UINFO("Dictionary has not changed, so no need to update it!");
+	}
+	_notIndexedWords.clear();
+	_removedIndexedWords.clear();
 }
 
 void VWDictionary::clear()
 {
+	ULOGGER_DEBUG("");
 	if(_visualWords.size() && _incrementalDictionary)
 	{
-		UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", _visualWords.size());
+		UWARN("Visual dictionary would be already empty here (%d words still in dictionary).", (int)_visualWords.size());
+	}
+	if(_notIndexedWords.size())
+	{
+		UWARN("Not indexed words should be empty here (%d words still not indexed)", (int)_notIndexedWords.size());
 	}
 	for(std::map<int, VisualWord *>::iterator i=_visualWords.begin(); i!=_visualWords.end(); ++i)
 	{
 		delete (*i).second;
 	}
 	_visualWords.clear();
-	_lastNewWordsAddedCount = 0;
+	_notIndexedWords.clear();
+	_removedIndexedWords.clear();
 	_totalActiveReferences = 0;
 	_lastWordId = 0;
 	_dataTree = cv::Mat();
@@ -390,19 +369,12 @@ void VWDictionary::addWordRef(int wordId, int signatureId)
 	{
 		VisualWord * vw = 0;
 		vw = uValue(_visualWords, wordId, vw);
-		if(!vw)
-		{
-			vw = uValue(_unusedWords, wordId, vw);
-			if(vw)
-			{
-				_visualWords.insert(std::pair<int, VisualWord*>(vw->id(), vw));
-				_unusedWords.erase(vw->id());
-			}
-		}
 		if(vw)
 		{
 			vw->addRef(signatureId);
 			_totalActiveReferences += 1;
+
+			_unusedWords.erase(vw->id());
 		}
 		else
 		{
@@ -447,7 +419,12 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		return wordIds;
 	}
 
-	int newWordsCount= 0;
+	if(!_incrementalDictionary && _dataTree.empty())
+	{
+		UERROR("Dictionary mode is set to fixed but no words are in it!");
+		return wordIds;
+	}
+
 	int dupWordsCount= 0;
 
 	unsigned int k=1; // k nearest neighbors
@@ -537,10 +514,10 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 				{
 					VisualWord * vw = new VisualWord(getNextId(), newPts.ptr<float>(i), _dim, signatureId);
 					_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
+					_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
 					newWords.push_back(vw);
 					wordIds.push_back(vw->id());
 					UASSERT(vw->id()>0);
-					++newWordsCount;
 				}
 				else
 				{
@@ -602,9 +579,9 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 
 				if(badDist)
 				{
-					++newWordsCount;
 					VisualWord * vw = new VisualWord(getNextId(), d, _dim, signatureId);
 					_visualWords.insert(_visualWords.end(), std::pair<int, VisualWord *>(vw->id(), vw));
+					_notIndexedWords.insert(_notIndexedWords.end(), vw->id());
 					wordIds.push_back(vw->id());
 					UASSERT(vw->id()>0);
 				}
@@ -628,12 +605,11 @@ std::list<int> VWDictionary::addNewWords(const cv::Mat & descriptors,
 		ULOGGER_DEBUG("Naive search time = %fs", timer.ticks());
 	}
 
-	ULOGGER_DEBUG("%d new words added...", newWordsCount);
+	ULOGGER_DEBUG("%d new words added...", _notIndexedWords.size());
 	ULOGGER_DEBUG("%d duplicated words added...", dupWordsCount);
 	UDEBUG("total time %fs", timer.ticks());
 
-	_lastNewWordsAddedCount = newWordsCount;
-	_totalActiveReferences += newWordsCount;
+	_totalActiveReferences += _notIndexedWords.size();
 	return wordIds;
 }
 
@@ -835,9 +811,10 @@ void VWDictionary::addWord(VisualWord * vw)
 {
 	if(vw)
 	{
+		_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
+		_notIndexedWords.insert(vw->id());
 		if(vw->getReferences().size())
 		{
-			_visualWords.insert(std::pair<int, VisualWord *>(vw->id(), vw));
 			_totalActiveReferences += uSum(uValues(vw->getReferences()));
 		}
 		else
@@ -1037,6 +1014,10 @@ void VWDictionary::removeWords(const std::vector<VisualWord*> & words)
 	{
 		_visualWords.erase(words[i]->id());
 		_unusedWords.erase(words[i]->id());
+		if(_notIndexedWords.erase(words[i]->id()) == 0)
+		{
+			_removedIndexedWords.insert(words[i]->id());
+		}
 	}
 }
 

corelib/src/random_sample.h

@@ -0,0 +1,255 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2009, Willow Garage, Inc.
+ *  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 copyright holder(s) 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 OWNER 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.
+ *
+ * $Id: extract_indices.h 1370 2011-06-19 01:06:01Z jspricke $
+ *
+ */
+
+#ifndef PCL_FILTERS_RANDOM_SUBSAMPLE_H_
+#define PCL_FILTERS_RANDOM_SUBSAMPLE_H_
+
+#include <pcl/filters/filter_indices.h>
+#include <time.h>
+#include <limits.h>
+
+  /** \brief @b RandomSample applies a random sampling with uniform probability.
+    * Based off Algorithm A from the paper "Faster Methods for Random Sampling"
+    * by Jeffrey Scott Vitter. The algorithm runs in O(N) and results in sorted
+    * indices
+    * http://www.ittc.ku.edu/~jsv/Papers/Vit84.sampling.pdf
+    * \author Justin Rosen
+    * \ingroup filters
+    */
+  template<typename PointT>
+  class RandomSample : public pcl::FilterIndices<PointT>
+  {
+    using pcl::FilterIndices<PointT>::filter_name_;
+    using pcl::FilterIndices<PointT>::getClassName;
+    using pcl::FilterIndices<PointT>::indices_;
+    using pcl::FilterIndices<PointT>::input_;
+    using pcl::FilterIndices<PointT>::negative_;
+    using pcl::FilterIndices<PointT>::keep_organized_;
+    using pcl::FilterIndices<PointT>::user_filter_value_;
+    using pcl::FilterIndices<PointT>::extract_removed_indices_;
+    using pcl::FilterIndices<PointT>::removed_indices_;
+
+    typedef typename pcl::FilterIndices<PointT>::PointCloud PointCloud;
+    typedef typename PointCloud::Ptr PointCloudPtr;
+    typedef typename PointCloud::ConstPtr PointCloudConstPtr;
+
+    public:
+
+      typedef boost::shared_ptr< RandomSample<PointT> > Ptr;
+      typedef boost::shared_ptr< const RandomSample<PointT> > ConstPtr;
+
+      /** \brief Empty constructor. */
+      RandomSample (bool extract_removed_indices = false) :
+    	  pcl::FilterIndices<PointT> (extract_removed_indices),
+        sample_ (UINT_MAX),
+        seed_ (static_cast<unsigned int> (time (NULL)))
+      {
+        filter_name_ = "RandomSample";
+      }
+
+      /** \brief Set number of indices to be sampled.
+        * \param sample
+        */
+      inline void
+      setSample (unsigned int sample)
+      {
+        sample_ = sample;
+      }
+
+      /** \brief Get the value of the internal \a sample parameter.
+        */
+      inline unsigned int
+      getSample ()
+      {
+        return (sample_);
+      }
+
+      /** \brief Set seed of random function.
+        * \param seed
+        */
+      inline void
+      setSeed (unsigned int seed)
+      {
+        seed_ = seed;
+      }
+
+      /** \brief Get the value of the internal \a seed parameter.
+        */
+      inline unsigned int
+      getSeed ()
+      {
+        return (seed_);
+      }
+
+    protected:
+
+      /** \brief Number of indices that will be returned. */
+      unsigned int sample_;
+      /** \brief Random number seed. */
+      unsigned int seed_;
+
+      /** \brief Sample of point indices into a separate PointCloud
+        * \param output the resultant point cloud
+        */
+      void
+      applyFilter (PointCloud &output)
+      {
+        std::vector<int> indices;
+        if (keep_organized_)
+        {
+          bool temp = extract_removed_indices_;
+          extract_removed_indices_ = true;
+          applyFilter (indices);
+          extract_removed_indices_ = temp;
+          copyPointCloud (*input_, output);
+          // Get X, Y, Z fields
+          std::vector<sensor_msgs::PointField> fields;
+          pcl::getFields (*input_, fields);
+          std::vector<size_t> offsets;
+          for (size_t i = 0; i < fields.size (); ++i)
+          {
+            if (fields[i].name == "x" ||
+                fields[i].name == "y" ||
+                fields[i].name == "z")
+              offsets.push_back (fields[i].offset);
+          }
+          // For every "removed" point, set the x,y,z fields to user_filter_value_
+          const static float user_filter_value = user_filter_value_;
+          for (size_t rii = 0; rii < removed_indices_->size (); ++rii)
+          {
+            uint8_t* pt_data = reinterpret_cast<uint8_t*> (&output[(*removed_indices_)[rii]]);
+            for (size_t i = 0; i < offsets.size (); ++i)
+            {
+              memcpy (pt_data + offsets[i], &user_filter_value, sizeof (float));
+            }
+            if (!pcl_isfinite (user_filter_value_))
+              output.is_dense = false;
+          }
+        }
+        else
+        {
+          output.is_dense = true;
+          applyFilter (indices);
+          copyPointCloud (*input_, indices, output);
+        }
+      }
+
+      /** \brief Sample of point indices
+        * \param indices the resultant point cloud indices
+        */
+      void
+      applyFilter (std::vector<int> &indices)
+      {
+        unsigned N = static_cast<unsigned> (indices_->size ());
+
+        unsigned int sample_size = negative_ ? N - sample_ : sample_;
+        // If sample size is 0 or if the sample size is greater then input cloud size
+        //   then return all indices
+        if (sample_size >= N)
+        {
+          indices = *indices_;
+          removed_indices_->clear ();
+        }
+        else
+        {
+          // Resize output indices to sample size
+          indices.resize (static_cast<size_t> (sample_size));
+          if (extract_removed_indices_)
+            removed_indices_->resize (static_cast<size_t> (N - sample_size));
+
+          // Set random seed so derived indices are the same each time the filter runs
+          std::srand (seed_);
+
+          // Algorithm A
+          unsigned top = N - sample_size;
+          unsigned i = 0;
+          unsigned index = 0;
+          std::vector<bool> added;
+          if (extract_removed_indices_)
+            added.resize (indices_->size (), false);
+          for (size_t n = sample_size; n >= 2; n--)
+          {
+            float V = unifRand ();
+            unsigned S = 0;
+            float quot = static_cast<float> (top) / static_cast<float> (N);
+            while (quot > V)
+            {
+              S++;
+              top--;
+              N--;
+              quot = quot * static_cast<float> (top) / static_cast<float> (N);
+            }
+            index += S;
+            if (extract_removed_indices_)
+              added[index] = true;
+            indices[i++] = (*indices_)[index++];
+            N--;
+          }
+
+          index += N * static_cast<unsigned> (unifRand ());
+          if (extract_removed_indices_)
+            added[index] = true;
+          indices[i++] = (*indices_)[index++];
+
+          // Now populate removed_indices_ appropriately
+          if (extract_removed_indices_)
+          {
+            unsigned ri = 0;
+            for (size_t i = 0; i < added.size (); i++)
+            {
+              if (!added[i])
+              {
+                (*removed_indices_)[ri++] = (*indices_)[i];
+              }
+            }
+          }
+        }
+      }
+
+      /** \brief Return a random number fast using a LCG (Linear Congruential Generator) algorithm.
+        * See http://software.intel.com/en-us/articles/fast-random-number-generator-on-the-intel-pentiumr-4-processor/ for more information.
+        */
+      inline float
+      unifRand ()
+      {
+        return (static_cast<float>(rand () / double (RAND_MAX)));
+        //return (((214013 * seed_ + 2531011) >> 16) & 0x7FFF);
+      }
+  };
+
+
+#endif  //#ifndef PCL_FILTERS_RANDOM_SUBSAMPLE_H_

corelib/src/resources/DatabaseSchema.sql

@@ -15,18 +15,32 @@
 -- *******************************************************************
 CREATE TABLE Node (
 	id INTEGER NOT NULL,
+	map_id INTEGER NOT NULL,
 	weight INTEGER,
+	pose BLOB,
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
 
+CREATE TABLE MapLink (
+	source_map_id INTEGER NOT NULL,
+	target_map_id INTEGER NOT NULL,
+	transform BLOB
+);
+
 CREATE TABLE Image (
 	id INTEGER NOT NULL,
-	raw_width INTEGER NOT NULL,
-	raw_height INTEGER NOT NULL,
-	raw_data_type INTEGER NOT NULL,
-	raw_compressed CHAR NOT NULL,
-	raw_data BLOB,
+	data BLOB,
+	time_enter DATE,
+	PRIMARY KEY (id)
+);
+
+CREATE TABLE Depth (
+	id INTEGER NOT NULL,
+	data BLOB,   -- CV_32FC1, width = Image/raw_width, height=Image/raw_height
+	constant FLOAT,
+	local_transform BLOB,
+	data2d BLOB,   -- CV_32FC2, Example: Laser scan
 	time_enter DATE,
 	PRIMARY KEY (id)
 );
@@ -35,6 +49,7 @@ CREATE TABLE Link (
 	from_id INTEGER NOT NULL,
 	to_id INTEGER NOT NULL,
 	type INTEGER NOT NULL, -- neighbor=0, loop=1, child=2
+	transform BLOB,
 	FOREIGN KEY (from_id) REFERENCES Node(id),
 	FOREIGN KEY (to_id) REFERENCES Node(id)
 );
@@ -56,6 +71,9 @@ CREATE TABLE Map_Node_Word (
 	size INTEGER NOT NULL,
 	dir FLOAT NOT NULL,
 	response FLOAT NOT NULL,
+	depth_x FLOAT,
+	depth_y FLOAT,
+	depth_z FLOAT,
 	FOREIGN KEY (node_id) REFERENCES Node(id),
 	FOREIGN KEY (word_id) REFERENCES Word(id)
 );
@@ -65,14 +83,10 @@ CREATE TABLE Statistics (
 	last_sign_added INTEGER,
 	process_mem_used INTEGER,
 	database_mem_used INTEGER,
+	dictionary_size INTEGER,
 	time_enter DATE
 );
 
-CREATE TABLE StatisticsDictionary (
-        dictionary_size INTEGER,
-        time_enter DATE
-);
-
 -- *******************************************************************
 -- TRIGGERS
 -- *******************************************************************

corelib/src/toro3d/dmatrix.hh

@@ -0,0 +1,94 @@
+#ifndef DMATRIX_HXX 
+#define DMATRIX_HXX
+
+#include <iostream>
+#include <exception>
+
+class DNotInvertibleMatrixException: public std::exception {};
+class DIncompatibleMatrixException: public std::exception {};
+class DNotSquareMatrixException: public std::exception {};
+
+
+template <class X> struct DVector{
+public:
+  DVector(int n=0);
+  ~DVector();
+  
+  DVector(const DVector&);
+  DVector& operator=(const DVector&);
+  
+  X&  operator[](int i) {
+    if ((*shares)>1) detach();
+    return elems[i];
+  }
+  
+  const X&  operator[](int i) const { return elems[i]; }
+  
+  X operator*(const DVector&) const;
+  DVector operator+(const DVector&) const;
+  DVector operator-(const DVector&) const;
+  DVector operator*(const X&) const;
+  
+  int dim() const { return size; }
+  
+  void detach();
+  
+  static DVector<X> I(int);
+  
+protected:
+  X * elems;
+  int size;
+  int * shares;
+};
+
+
+
+template <class X> class DMatrix {
+public:
+  DMatrix(int n=0,int m=0);
+  ~DMatrix();
+  
+  DMatrix(const DMatrix&);
+  DMatrix& operator=(const DMatrix&);
+  
+  X * operator[](int i) {
+    if ((*shares)>1) detach();
+    return mrows[i];
+  }
+  
+  const X * operator[](int i) const { return mrows[i]; }
+  
+  const X det() const;
+  DMatrix inv() const;
+  DMatrix transpose() const;
+  DMatrix operator*(const DMatrix&) const;
+  DMatrix operator+(const DMatrix&) const;
+  DMatrix operator-(const DMatrix&) const;
+  DMatrix operator*(const X&) const;
+  
+  int rows() const { return nrows; }
+  int columns() const { return ncols; }
+  
+  void detach();
+  
+  static DMatrix I(int);
+  
+protected:
+  X * elems;
+  int nrows,ncols;
+  X ** mrows;
+  
+  int * shares;
+};
+
+
+template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v);
+
+template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m);
+
+
+/*************** IMPLEMENTATION ***************/
+
+#include "dmatrix.hxx"
+
+#endif

corelib/src/toro3d/dmatrix.hxx

@@ -0,0 +1,289 @@
+
+template <class X> DVector<X>::DVector(int n) {
+  if (n<1) n=1;
+  size=n;
+  elems=new X[size];
+  for (int i=0;i<size; i++) 
+    elems[i]=X(0);
+  shares=new int;
+  (*shares)=1;
+}
+
+template <class X> DVector<X>::~DVector() {
+  if (--(*shares)) return;
+  delete [] elems;
+  delete shares;
+}
+
+template <class X> DVector<X>::DVector(const DVector<X>& m) {
+  shares=m.shares;
+  elems=m.elems;
+  size=m.size;
+  (*shares)++;
+}
+
+template <class X> DVector<X>& DVector<X>::operator=(const DVector<X>& m) {
+  if (shares==m.shares)
+    return *this;
+  if (!--(*shares)) {
+    delete [] elems;
+    delete shares;
+  }
+  shares=m.shares;
+  elems=m.elems;
+  size=m.size;
+  (*shares)++;
+  return *this;
+}
+
+template <class X> X DVector<X>::operator*(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  X p=X(0);
+  for (int i=0; i<size; i++)
+    p+=elems[i]*v.elems[i];
+  return p;
+}
+
+template <class X> void DVector<X>::detach() {
+  DVector<X> aux(size);
+  for (int i=0;i<size;i++) aux.elems[i]=elems[i];
+  operator=(aux);
+}
+
+template <class X> DVector<X> DVector<X>::operator+(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]+v.elems[i];
+  }
+  return r;
+}
+
+template <class X> DVector<X> DVector<X>::operator-(const DVector<X>& v) const{
+  if (size!=v.size) throw DIncompatibleMatrixException();
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]-v.elems[i];
+  }
+  return r;
+}
+
+template <class X> DVector<X> DVector<X>::operator*(const X& d) const{
+  DVector<X> r(size);
+  for (int i=0; i<size; i++){
+    r.elems[i]=elems[i]*d;
+  }
+  return r;
+}
+
+
+
+template <class X> DMatrix<X>::DMatrix(int n,int m) {
+  if (n<1) n=1;
+  if (m<1) m=1;
+  nrows=n;
+  ncols=m;
+  elems=new X[nrows*ncols];
+  mrows=new X* [nrows];
+  for (int i=0;i<nrows;i++) mrows[i]=elems+ncols*i;
+  for (int i=0;i<nrows*ncols;i++) elems[i]=X(0);
+  shares=new int;
+  (*shares)=1;
+}
+
+template <class X> DMatrix<X>::~DMatrix() {
+  if (--(*shares)) return;
+  delete [] elems;
+  delete [] mrows;
+  delete shares;
+}
+
+template <class X> DMatrix<X>::DMatrix(const DMatrix& m) {
+  shares=m.shares;
+  elems=m.elems;
+  nrows=m.nrows;
+  ncols=m.ncols;
+  mrows=m.mrows;
+  (*shares)++;
+}
+
+template <class X> DMatrix<X>& DMatrix<X>::operator=(const DMatrix& m) {
+  if (shares==m.shares)
+    return *this;
+  if (!--(*shares)) {
+    delete [] elems;
+    delete [] mrows;
+    delete shares;
+  }
+  shares=m.shares;
+  elems=m.elems;
+  nrows=m.nrows;
+  ncols=m.ncols;
+  mrows=m.mrows;
+  (*shares)++;
+  return *this;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::inv() const {
+  if (nrows!=ncols) throw DNotInvertibleMatrixException();
+  DMatrix<X> aux1(*this),aux2(I(nrows));
+  aux1.detach();
+  for (int i=0;i<nrows;i++) {
+    int k=i;
+    for (;k<nrows&&aux1.mrows[k][i]==X(0);k++){};
+    if (k>=nrows) throw DNotInvertibleMatrixException();
+    X val=aux1.mrows[k][i];
+    for (int j=0;j<nrows;j++) {
+      aux1.mrows[k][j]=aux1.mrows[k][j]/val;
+      aux2.mrows[k][j]=aux2.mrows[k][j]/val;
+    }
+    if (k!=i) {
+      for (int j=0;j<nrows;j++) {
+	X tmp=aux1.mrows[k][j];
+	aux1.mrows[k][j]=aux1.mrows[i][j];
+	aux1.mrows[i][j]=tmp;
+	tmp=aux2.mrows[k][j];
+	aux2.mrows[k][j]=aux2.mrows[i][j];
+	aux2.mrows[i][j]=tmp;
+      }
+    }
+    for (int j=0;j<nrows;j++)
+      if (j!=i) {
+	X tmp=aux1.mrows[j][i];
+	for (int l=0;l<nrows;l++) {
+	  aux1.mrows[j][l]=aux1.mrows[j][l]-tmp*aux1.mrows[i][l];
+	  aux2.mrows[j][l]=aux2.mrows[j][l]-tmp*aux2.mrows[i][l];
+	}
+      }
+  }
+  return aux2;
+}
+
+template <class X> const X DMatrix<X>::det() const {
+  if (nrows!=ncols) throw DNotSquareMatrixException();
+  DMatrix<X> aux(*this);
+  X d=X(1);
+  aux.detach();
+  for (int i=0;i<nrows;i++) {
+    int k=i;
+    for (;k<nrows&&aux.mrows[k][i]==X(0);k++){};
+    if (k>=nrows) return X(0);
+    X val=aux.mrows[k][i];
+    for (int j=0;j<nrows;j++) {
+      aux.mrows[k][j]/=val;
+    }
+    d=d*val;
+    if (k!=i) {
+      for (int j=0;j<nrows;j++) {
+	X tmp=aux.mrows[k][j];
+	aux.mrows[k][j]=aux.mrows[i][j];
+	aux.mrows[i][j]=tmp;
+      }
+      d=-d;	
+    }
+    for (int j=i+1;j<nrows;j++){
+      X tmp=aux.mrows[j][i];
+      if (!(tmp==X(0)) ){
+	for (int l=0;l<nrows;l++) {
+	  aux.mrows[j][l]=aux.mrows[j][l]-tmp*aux.mrows[i][l];
+	}
+	//d=d*tmp;
+      }		
+		}
+  }
+  return d;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::transpose() const {
+  DMatrix<X> aux(ncols, nrows);
+  for (int i=0; i<nrows; i++)
+    for (int j=0; j<ncols; j++)
+      aux[j][i]=mrows[i][j];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator*(const DMatrix<X>& m) const {
+  if (ncols!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,m.ncols);
+  for (int i=0;i<nrows;i++)
+    for (int j=0;j<m.ncols;j++){
+      X a=0;
+      for (int k=0;k<ncols;k++)
+	a+=mrows[i][k]*m.mrows[k][j];
+      aux.mrows[i][j]=a;
+    }
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator+(const DMatrix<X>& m) const {
+  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]+m.elems[i];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator-(const DMatrix<X>& m) const {
+  if (ncols!=m.ncols||nrows!=m.nrows) throw DIncompatibleMatrixException();
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]-m.elems[i];
+  return aux;
+}
+
+template <class X> DMatrix<X> DMatrix<X>::operator*(const X& e) const {
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i]*e;
+  return aux;
+}
+
+template <class X> void DMatrix<X>::detach() {
+  DMatrix<X> aux(nrows,ncols);
+  for (int i=0;i<nrows*ncols;i++) aux.elems[i]=elems[i];
+  operator=(aux);
+}
+
+template <class X> DMatrix<X> DMatrix<X>::I(int n) {
+  DMatrix<X> aux(n,n);
+  for (int i=0;i<n;i++) aux[i][i]=X(1);
+  return aux;
+}
+
+template <class X> std::ostream& operator<<(std::ostream& os, const DMatrix<X> &m) {
+  os << "{";
+  for (int i=0;i<m.rows();i++) {
+    if (i>0) os << ",";
+    os << "{";
+    for (int j=0;j<m.columns();j++) {
+      if (j>0) os << ",";
+      os << m[i][j];
+    }
+    os << "}";
+  }
+  return os << "}";
+}
+
+
+template <class X> DVector<X> operator * (const DMatrix<X> m, const DVector<X> v){
+    if (v.dim()!=m.columns()) throw DIncompatibleMatrixException();
+    DVector<X> r(m.rows());
+    for (int i=0; i<m.rows(); i++){
+      X a=X(0);
+      for (int j=0; j<m.columns(); j++){
+	a+=m[i][j]*v[j];
+      }
+      r[i]=a;
+    }
+    return r;
+}
+
+template <class X> DVector<X> operator * (const DVector<X> v, const DMatrix<X> m){
+    if (v.dim()!=m.rows()) throw DIncompatibleMatrixException();
+    DVector<X> r(m.columns());
+    for (int i=0; i<m.columns(); i++){
+      X a=X(0);
+      for (int j=0; j<m.rows(); j++){
+	a+=m[j][i]*v[j];
+      }
+      r[i]=a;
+    }
+    return r;
+}

corelib/src/toro3d/posegraph.hh

@@ -0,0 +1,274 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph.hh 
+ * 
+ * \brief The template class for the node parameters. The graph of
+ * poses with support to tree construction functionalities.
+ **/
+
+#ifndef _TREEPOSEGRAPH_HXX_
+#define _TREEPOSEGRAPH_HXX_
+
+#include <iostream>
+#include <assert.h>
+
+#include <set>
+#include <list>
+#include <map>
+#include <deque>
+#include <vector>
+#include <limits>
+#include <algorithm>
+
+namespace AISNavigation{
+
+/** \brief A comparator class (struct) that compares the level
+    of two vertices if edges **/
+template <class E> 
+struct EVComparator{
+  /** Comparison operator for the level **/
+  enum CompareMode {CompareLevel, CompareLength};
+  CompareMode mode;
+
+  EVComparator(){
+    mode=CompareLevel;
+  }
+  inline bool operator() (const E& e1, const E& e2){
+    int o1=0, o2=0;
+    switch (mode){
+    case CompareLevel:
+      o1=e1->top->level;
+      o2=e2->top->level;
+      break;
+    case CompareLength:
+      o1=e1->length;
+      o2=e2->length;
+      break;
+    }
+    return o1<o2;
+  }
+};
+
+/** \brief The template class for representing an abstract tree
+    without specifing the dimensionality of the exact parameterization
+    of the nodes. This definition is passed in via the Operation (Ops)
+    template class **/
+template <class Ops>
+struct TreePoseGraph{
+  typedef typename Ops::BaseType BaseType;
+  typedef typename Ops::PoseType Pose;
+  typedef typename Ops::RotationType Rotation;
+  typedef typename Ops::TranslationType Translation;
+  typedef typename Ops::TransformationType Transformation;
+  typedef typename Ops::CovarianceType Covariance;
+  typedef typename Ops::InformationType Information;
+  typedef typename Ops::ParametersType  Parameters;
+  
+  struct Vertex;
+
+  /** \brief Definition of an edge in the graph based on the template
+      input from Ops **/
+  struct Edge{
+    Vertex* v1;   /**< The constraint is defined between v1 and v2 **/
+    Vertex* v2;   /**< The constraint is defined between v1 and v2 **/
+    Vertex* top;  /**< The node with the smallest level in the path **/
+    int length;   /**< Length of the path on the tree (number of vertieces involved) **/
+    Transformation transformation;    /**< Transformation describing the constraint (relative mapping) **/
+    Information    informationMatrix; /**< Uncertainty encoded in the information matrix **/
+
+    bool mark;
+    
+    double learningRate;
+  };
+
+  typedef typename EVComparator<Edge*>::CompareMode EdgeCompareMode;
+  typedef typename std::list< Edge* > EdgeList;
+  typedef typename std::map< int, Vertex* > VertexMap;
+  typedef typename std::set< Vertex* > VertexSet;
+  typedef typename std::map< Edge*, Edge* > EdgeMap;
+  typedef typename std::multiset< Edge*, EVComparator<Edge*> > EdgeSet;
+
+  /** \brief Definition of a vertex in the graph based on the
+      template input from Ops **/
+  struct Vertex {
+
+    // Graph-related elements
+    int id;         /**< Id of the vertex in the graph **/
+    EdgeList edges; /**< The edges related to this vertex **/
+
+    // Tree-related elements
+    int level;         /**< level in the tree. It is the distance on the tree to the root **/
+    Vertex* parent;    /**< Parent vertex **/
+    Edge* parentEdge;  /**< Constraint between the parent and the current vertex in the tree **/
+    EdgeList children;   /**< All constraints involving the children of this vertex **/
+
+    // Parameterization-related elements
+    Transformation transformation; /**< redundant representation of the vertex, without gymbal locks **/
+    Pose pose;              /**< The pose of the vertex **/
+    Parameters parameters;  /**< The parameter representation **/
+
+    bool mark;
+  };
+
+  /** Returns the vertex with the given id **/
+  Vertex* vertex(int id);
+
+  /** Returns a const pointer to the vertex with the given id **/
+  const Vertex* vertex (int id) const;
+
+  /** Returns the edge between the two vertices **/
+  Edge* edge(int id1, int id2);
+
+  /** Returns a const pointer tothe edge between the two vertices **/
+  const Edge* edge(int id1, int id2) const;
+
+  /** Add a vertex to the graph **/
+  Vertex* addVertex(int id, const Pose& pose);
+  /** Remove a vertex from the graph **/
+  Vertex* removeVertex (int id);
+
+  /** Add an edge/constraint to the graph **/
+  Edge* addEdge(Vertex* v1, Vertex* v2, const Transformation& t, const Information& i);
+
+  /** Remove an edge/constraint from the graph **/
+  Edge* removeEdge(Edge* eq);
+
+   /** Adds en edge incrementally to the tree. 
+      It builds a simple tree and initializes the structures for the optimization.
+
+      This function is for online processing.
+      It requires that at least one vertex is already present in the graph.
+      The vertices are represented by their ids.
+     
+      Once the edge is introduced in the structure:
+      - the parent of the node with the higher ID is computed.
+      - the top node is assigned
+      - the edge is inserted in the
+
+      @returns A pointer to the added edge, if the insertion was succesfull. 0 otherwise.
+  **/
+  Edge* addIncrementalEdge(int id1, int id2,  const Transformation& t, const Information& i);
+
+  /** Returns a set of edges which are accected by the mofification of the vertex v.
+      The set is ordered according to the level of their top node.
+   **/
+  EdgeSet* affectedEdges(Vertex* v);
+
+  EdgeSet* affectedEdges(VertexSet& vl);
+
+  /** Function to perform a breadth-first visit of the nodes in the tree to carry out a specific action act**/
+  template <class Action>
+  void treeBreadthVisit(Action& act);
+
+  /** Function to perform a depth-first visit of the nodes in the tree to carry out a specific action act **/
+  template <class Action>
+  void treeDepthVisit(Action& act, Vertex *v);
+
+  /** Constructs the tree be computing a minimal spanning tree **/
+  bool buildMST(int id);
+
+  /** Constructs the incremental tree according to the input trajectory **/
+  bool buildSimpleTree();
+
+  /** Trun around an edge (used to ensure a certain oder on the vertexes) **/
+  void revertEdge(Edge* e);
+
+  /** Revert edge info. This function needs to be implemented by a subclass **/
+  virtual void revertEdgeInfo(Edge* e) = 0;
+
+  /** Revert edge info. This function needs to be implemented by a subclass **/
+  virtual void initializeFromParentEdge(Vertex* v) = 0;
+
+  /** Delete all edges and vertices **/
+  void clear();
+
+  /**constructor*/
+  TreePoseGraph(){
+    sortedEdges=0; 
+    edgeCompareMode=EVComparator<Edge*>::CompareLevel;
+  }
+
+  /** Destructor **/
+  virtual ~TreePoseGraph();
+
+  /** Sort constraints for correct processing order **/
+  EdgeSet* sortEdges();
+
+  /** Determines the length of the longest path in the tree **/
+  int maxPathLength();
+
+  /** Determines the path length of all pathes in the tree **/
+  int totalPathLength();
+
+  /** remove gaps in the indices of the vertex ids **/
+  void compressIndices();
+
+  /** compute the highest index of an vertex **/
+  int maxIndex();
+
+  /** performs a consistency check on the tree and the graph structure.
+      @returns false on failure.*/
+  bool sanityCheck();
+
+  /** The root node of the tree **/
+  Vertex* root;
+
+  /** All vertices **/
+  VertexMap vertices;
+
+  /** All edges **/
+  EdgeMap edges;
+
+  /** The constraints/edges sorted according to the level in the tree
+      in order to allow us the efficient update (pose computation) of
+      the nodes in the tree (see the RSS07 paper for further
+      details) **/
+  EdgeSet* sortedEdges;
+
+protected:
+  void fillEdgeInfo(Edge* e);
+  void fillEdgesInfo();
+  EdgeCompareMode edgeCompareMode;
+};
+
+//include the template implementation part
+
+#include "posegraph.hxx"
+
+}; //namespace AISNavigation
+
+#endif

corelib/src/toro3d/posegraph.hxx

@@ -0,0 +1,693 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
+ *                         Slawomir Grzonka and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph.hxx
+ * 
+ * \brief The implementation of the template class for the node
+ * parameters. 
+ **/
+
+
+
+/*********************** IMPLEMENTATION PART ***********************/
+template <typename Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::vertex(int id){
+  typename VertexMap::iterator it=vertices.find(id);
+  if (it==vertices.end())
+    return 0;
+  return it->second;
+}
+
+template <typename Ops>
+const typename TreePoseGraph<Ops>::Vertex * TreePoseGraph<Ops>::vertex (int id) const{
+  typename VertexMap::const_iterator it=vertices.find(id);
+  if (it==edges.end())
+    return 0;
+  return it->second;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::edge(int id1, int id2){
+  Vertex* v1=vertex(id1);
+  if (!v1)
+    return false;
+  typename EdgeList::iterator it=v1->edges.begin();
+  while(it!=v1->edges.end()){
+    if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
+      return *it;
+    it++;
+  }
+  return 0;
+}
+
+template <class Ops>
+const typename TreePoseGraph<Ops>::Edge * TreePoseGraph<Ops>::edge(int id1, int id2) const{
+  const Vertex* v1=vertex(id1);
+  if (!v1)
+    return false;
+  typename EdgeList::const_iterator it=v1->edges.begin();
+  while(it!=v1->edges.end()){
+    if ((*it)->v1->id==id1 && (*it)->v2->id==id2)
+      return *it;
+    it++;
+  }
+  return 0;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::revertEdge(typename TreePoseGraph<Ops>::Edge * e){
+  revertEdgeInfo(e);
+  Vertex* ap=e->v2;
+  e->v2=e->v1;
+  e->v1=ap;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::addVertex(int id, const typename TreePoseGraph<Ops>::Pose& pose){
+  Vertex* v=vertex(id);
+  if (v)
+    return 0;
+  v=new Vertex;
+  v->id=id;
+  v->pose=pose;
+  v->parent=0;
+  v->mark=false;
+  vertices.insert(std::make_pair(id,v));
+  return v;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Vertex* TreePoseGraph<Ops>::removeVertex (int id){
+  typename VertexMap::iterator it=vertices.find(id);
+  if (it==vertices.end())
+    return 0;
+  Vertex* v=it->second;
+
+  if (v==0)
+    return false;
+
+  typename TreePoseGraph<Ops>::EdgeList el=v->edges;
+  for(typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+    removeEdge(*it);
+  }
+  delete v;
+  vertices.erase(it);
+  return v;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addEdge(typename TreePoseGraph<Ops>::Vertex* v1, typename TreePoseGraph<Ops>::Vertex* v2,
+								 const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
+  if (v1==v2)
+    return 0;
+  Edge* e=edge(v1->id, v2->id);
+  if (e)
+    return 0;
+
+  e=new Edge;
+  e->mark=false;
+  e->v1=v1;
+  e->v2=v2;
+  e->top=0;
+  e->transformation=t;
+  e->informationMatrix=i;
+  v1->edges.push_back(e);
+  v2->edges.push_back(e);
+  edges.insert(std::make_pair(e,e));
+  return e;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::addIncrementalEdge(int id1, int id2,
+							       const typename TreePoseGraph<Ops>::Transformation& t, const typename TreePoseGraph<Ops>::Information& i){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  if (! sortedEdges)
+    sortedEdges=new EdgeSet(comp);
+
+
+  typename VertexMap::iterator it1=vertices.find(id1);
+  typename VertexMap::iterator it2=vertices.find(id2);
+  Vertex* v1, *v2, *addedVertex=0;
+  if (it1==vertices.end() && it2==vertices.end()){
+    return 0;
+  }
+
+  if (it1==vertices.end()){
+    typename TreePoseGraph<Ops>::Pose p;
+    v1=addedVertex=addVertex(id1,p);
+  } else {
+    v1=it1->second;
+  }
+  if (it2==vertices.end()){
+    typename TreePoseGraph<Ops>::Pose p;
+    v2=addedVertex=addVertex(id2,p);
+  } else {
+    v2=it2->second;
+  }
+
+  if (v1->id==v2->id){
+    assert(0);
+  }
+
+  Edge* e=addEdge(v1,v2,t,i);
+  if (!e){
+    return 0;
+  }
+  if (v1->id>v2->id)
+    revertEdge(e);
+
+  if (addedVertex){
+    Vertex* otherVertex= (addedVertex==v1)? v2:v1;
+    addedVertex->parent=otherVertex;
+    addedVertex->parentEdge=e;
+    addedVertex->level=otherVertex->level+1;
+    otherVertex->children.push_back(e);
+  }
+
+  fillEdgeInfo(e);
+  sortedEdges->insert(e);
+
+  if (addedVertex){
+    initializeFromParentEdge(addedVertex);
+  }
+  return e;
+}
+
+
+template <class Ops>
+typename TreePoseGraph<Ops>::Edge* TreePoseGraph<Ops>::removeEdge(typename TreePoseGraph<Ops>::Edge* e){
+  {
+    typename EdgeMap::iterator it=edges.find(e);
+    if (it==edges.end()){
+      return 0;
+    }
+    edges.erase(it);
+  }
+
+
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+
+  {
+    typename EdgeList::iterator it=v1->edges.begin();
+    while(it!=v1->edges.end()){
+      if (*it==e){
+	v1->edges.erase(it);
+	break;
+      }
+      it++;
+    }
+  }
+
+  {
+    typename EdgeList::iterator it=v2->edges.begin();
+    while(it!=v2->edges.end()){
+      if ((*it)==e){
+	delete *it;
+	v2->edges.erase(it);
+	break;
+      }
+      it++;
+    }
+  }
+
+  return e;
+}
+
+template <class Ops>
+template <class Action>
+void TreePoseGraph<Ops>::treeBreadthVisit(Action& act){
+  typedef std::deque<Vertex*> VertexDeque;
+  static VertexDeque q;
+  q.push_back(root);
+  while (!q.empty()){
+    Vertex* current=q.front();
+    act.perform(current);
+    q.pop_front();
+    typename EdgeList::iterator it=current->children.begin();
+    while(it!=current->children.end()){
+      typename TreePoseGraph::Edge* e=(*it);
+      q.push_back(e->v2);
+      if(e->v2==current){
+	std::cerr << "error in the link direction v=" << current->id << std::endl;
+	std::cerr << " v1=" << e->v1->id << " v2=" << e->v2->id <<  std::endl;
+	assert(0);
+      }
+      it++;
+    }
+  }
+  q.clear();
+}
+
+template <class Ops>
+template <class Action>
+void TreePoseGraph<Ops>::treeDepthVisit(Action& act, Vertex* v){
+  act.perform(v);
+  typename EdgeList::iterator it=v->children.begin();
+  while(it!=v->children.end()){
+    treeDepthVisit(act, (*it)->v2);
+    it++;
+  }
+}
+
+template <class Ops>
+bool TreePoseGraph<Ops>::buildMST(int id){
+  typedef std::deque<Vertex*> VertexDeque;
+  typename VertexMap::iterator it=vertices.begin();
+  while (it!=vertices.end()){
+    it->second->parent=0;
+    it->second->parentEdge=0;
+    it->second->children.clear();
+    it++;
+  }
+  Vertex* v=vertex(id);
+  if (!v)
+    return false;
+  root=v;
+  root->level=0;
+  VertexDeque q;
+  q.push_back(v);
+  //std::cerr << "v=" << v->id << std::endl;
+  while (!q.empty()){
+    v=q.front();
+    typename EdgeList::iterator it=v->edges.begin();
+    while (it!=v->edges.end()){
+      Edge* e=(*it);
+      bool invertedEdge=false;
+      Vertex* other=e->v2;
+      if (other==v){
+	other=e->v1;
+	invertedEdge=true;
+      }
+      if (other!=root && other->parent==0){
+	if (invertedEdge){
+	  revertEdge(e);
+	}
+	//std::cerr << "INSERT v=" << v->id<< " " << "e=(" << e->v1->id << "," << e->v2->id << ")" << std::endl;
+	other->parent=v;
+	other->parentEdge=e;
+	other->level=v->level+1;
+	q.push_back(other);
+	v->children.push_back(e);
+	//std::cerr << "v=" << other->id << std::endl;
+      }
+      it++;
+    }
+    q.pop_front();
+  }
+  fillEdgesInfo();
+  return true;
+}
+
+/** \brief A class (struct) to dermine the level of a vertex in the tree **/
+template <class TPG>
+struct LevelAssigner{
+  /** Dermines the level of the vertex v in the tree **/
+  void perform(typename TPG::Vertex* v){
+    if (v->parent)
+      v->level=v->parent->level+1;
+    else
+      v->level=0;
+  }
+};
+
+
+
+
+template <class Ops>
+bool TreePoseGraph<Ops>::buildSimpleTree(){
+  root=0;
+  //rectify all the constraints, so that the v1<v2
+  for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    Edge* e=it->second;
+    if (e->v1->id > e->v2->id)
+      revertEdge(e);
+  }
+
+  //clear the tree data
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->parent=0;
+    v->parentEdge=0;
+    v->children.clear();
+  }
+
+  //fill the structure
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    if (v->edges.empty()){
+      assert(0);
+      continue;
+    }
+    Edge* bestEdge=v->edges.front();
+    int bestId=std::numeric_limits<int>::max();
+    bool found=false;
+    typename EdgeList::iterator li=v->edges.begin();
+    while(li!=v->edges.end()){
+      Edge* e =*li;
+      if (e->v2==v && e->v1->id<bestId){ //consider only the entering edges
+	bestId=e->v1->id;
+	bestEdge=e;
+	found=true;
+      }
+      li++;
+    }
+    if (found){
+      v->parentEdge=bestEdge;
+      v->parent=bestEdge->v1;
+      v->parent->children.push_back(bestEdge);
+    } else {
+      assert(! root);
+      root=v;
+    }
+  }
+//   std::cerr << "root="  << root << std::endl;
+  assert(root);
+
+  //assign the level
+  LevelAssigner< TreePoseGraph<Ops> > oa;
+  treeDepthVisit(oa, root);
+
+  fillEdgesInfo();
+  return true;
+}
+
+template <class Ops>
+TreePoseGraph<Ops>::~TreePoseGraph(){
+  clear();
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::clear(){
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    delete it->second;
+    it->second=0;
+  }
+  for (typename EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    delete it->second;
+    it->second=0;
+  }
+  vertices.clear();
+  edges.clear();
+  if ( sortedEdges )
+    delete sortedEdges;
+  sortedEdges=0;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::fillEdgeInfo(Edge* e){
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+  int length=0;
+  while (v1!=v2) {
+    if (v1->level > v2->level){
+      v1=v1->parent;
+      length++;
+    } else if (v2->level > v1->level){
+      v2=v2->parent;
+      length++;
+    } else if (v1->level==v2->level){
+      v1=v1->parent;
+      v2=v2->parent;
+      length+=2;
+    }
+  }
+  e->length=length;
+  e->top=v1;
+}
+
+template <class Ops>
+void TreePoseGraph<Ops>::fillEdgesInfo(){
+  typename TreePoseGraph<Ops>::EdgeMap em=edges;
+  for(typename EdgeMap::iterator it=em.begin(); it!=em.end(); it++){
+    fillEdgeInfo(it->second);
+  }
+}
+
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::sortEdges(){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * el=new EdgeSet(comp);
+  typename EdgeMap::iterator it=edges.begin();
+  while(it!=edges.end()){
+    el->insert(it->second);
+    it++;
+  }
+  return el;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(Vertex* v){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * es=new EdgeSet(comp);
+  std::deque<Vertex*> frontier;
+  std::list<Vertex*> markedVertices;
+
+  //frontier.push_back(v);
+  //v->mark=true;
+
+  for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
+    Edge* e=*it;
+    Vertex* other=(e->v1==v)?e->v2:e->v1;
+    frontier.push_back(other);
+    other->mark=true;
+    markedVertices.push_back(other);
+    e->mark=true;
+    es->insert(e);
+   }
+
+  while (! frontier.empty()){
+    Vertex* c=frontier.front();
+    frontier.pop_front();
+    markedVertices.push_back(c);
+    EdgeList& el=c->edges;
+    for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+      Edge* e=*it;
+      if (e->mark)
+	continue;
+      Vertex* other= (e->v1==c)?e->v2:e->v1;
+      if (other==c->parent)
+	continue;
+      if (other!=e->top && ! e->top->mark){
+	e->top->mark=true;
+	frontier.push_back(e->top);
+      }
+      e->mark=true;
+      es->insert(e);
+      if (!other->mark){
+	other->mark=true;
+	frontier.push_back(other);
+      }
+    }
+  }
+  for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
+    (*it)->mark=false;
+  }
+
+  for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
+    (*it)->mark=false;
+  }
+  return es;
+}
+
+template <class Ops>
+typename TreePoseGraph<Ops>::EdgeSet* TreePoseGraph<Ops>::affectedEdges(typename TreePoseGraph<Ops>::VertexSet& vl){
+  EVComparator<Edge*> comp;
+  comp.mode=edgeCompareMode;
+  EdgeSet * es=new EdgeSet(comp);
+  std::deque<Vertex*> frontier;
+  std::list<Vertex*> markedVertices;
+
+//   for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
+//     frontier.push_back(*it);
+//     (*it)->mark=true;
+//   }
+
+  for (typename VertexSet::iterator it=vl.begin(); it!=vl.end(); it++){
+    Vertex* v=*it;
+    for (typename EdgeList::iterator it=v->children.begin(); it!=v->children.end(); it++){
+      Edge* e=*it;
+      Vertex* other=(e->v1==v)?e->v2:e->v1;
+      frontier.push_back(other);
+      other->mark=true;
+      markedVertices.push_back(other);
+      e->mark=true;
+      es->insert(e);
+    }
+  }
+
+
+
+
+  while (! frontier.empty()){
+    Vertex* c=frontier.front();
+    frontier.pop_front();
+    markedVertices.push_back(c);
+    EdgeList& el=c->edges;
+    for (typename EdgeList::iterator it=el.begin(); it!=el.end(); it++){
+      Edge* e=*it;
+      if (e->mark)
+	continue;
+      Vertex* other= (e->v1==c)?e->v2:e->v1;
+      if (other==c->parent)
+	continue;
+      if (other!=e->top && ! e->top->mark){
+	e->top->mark=true;
+	frontier.push_back(e->top);
+      }
+      e->mark=true;
+      es->insert(e);
+      if (!other->mark){
+	other->mark=true;
+	frontier.push_back(other);
+      }
+    }
+  }
+  for (typename std::list<Vertex*>::iterator it=markedVertices.begin(); it!=markedVertices.end(); it++){
+    (*it)->mark=false;
+  }
+
+  for (typename EdgeSet::iterator it=es->begin(); it!=es->end(); it++){
+    (*it)->mark=false;
+  }
+  return es;
+}
+
+
+
+template <class Ops>
+int TreePoseGraph<Ops>::maxPathLength(){
+  int max=0;
+  typename EdgeMap::const_iterator it=edges.begin();
+  while(it!=edges.end()){
+    int l=it->second->length;
+    max=l>max?l:max;
+    it++;
+  }
+  return max;
+}
+
+template <class Ops>
+int TreePoseGraph<Ops>::totalPathLength(){
+  int t=0;
+  typename EdgeMap::const_iterator it=edges.begin();
+  while(it!=edges.end()){
+    t+=it->second->length;
+    it++;
+  }
+  return t;
+}
+
+
+template <class Ops>
+void TreePoseGraph<Ops>::compressIndices(){
+  VertexMap vmap;
+  int i=0;
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->id=i;
+    vmap.insert(std::make_pair(i,v));
+    i++;
+  }
+  vertices=vmap;
+}
+
+template <class Ops>
+int TreePoseGraph<Ops>::maxIndex(){
+  typename VertexMap::reverse_iterator it=vertices.rbegin();
+  if (it!=vertices.rend())
+    return it->second->id;
+  return -1;
+}
+
+
+
+template <class TPG>
+struct LoopChecker{
+  bool noloops;
+  void perform(typename TPG::Vertex* v){
+    if (!noloops)
+      return;
+    if (!v->mark)
+      v->mark=true;
+    else
+      noloops=false;
+  }
+};
+
+
+template <class Ops>
+bool TreePoseGraph<Ops>::sanityCheck(){
+  //check that each node has exactly one parent
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->mark=false;
+    Vertex* vp=v->parent;
+
+    if (! vp){
+      if (v!=root){
+	std::cerr << "root not found in the graph" << std::endl;
+	return false;
+      }
+    }
+
+    const EdgeList& children=it->second->children;
+    for (typename EdgeList::const_iterator lt=children.begin(); lt!=children.end(); lt++){
+      if ((*lt)->v1!=v){
+	std::cerr << "wrong direction of the edges" << std::cerr;
+	return false;
+      }
+    }
+  }
+  //check that there are no loops in the tree
+
+  LoopChecker< TreePoseGraph<Ops> > lc;
+  lc.noloops=true;
+  treeBreadthVisit(lc);
+  if (!lc.noloops){
+    std::cerr << "the tree contains loops" << std::endl;
+    return false;
+  }
+  for (typename VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->mark=false;
+  }
+  return true;
+}

corelib/src/toro3d/posegraph3.cpp

@@ -0,0 +1,405 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss,
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ * * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph3.cpp
+ * 
+ * \brief Defines the graph of 3D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#include "posegraph3.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define LINESIZE 81920
+
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+
+bool TreePoseGraph3::load(const char* filename, bool overrideCovariances, bool twoDimensions){
+  clear();
+  ifstream is(filename);
+  if (!is)
+    return false;
+
+  while(is){
+    char buf[LINESIZE];
+    is.getline(buf,LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    
+    if (twoDimensions){
+      if (tag=="VERTEX"){
+	int id;
+	Pose p(0.,0.,0.,0.,0.,0.);
+	ls >> id >> p.x() >> p.y() >> p.yaw(); 
+	TreePoseGraph3::Vertex* v=addVertex(id,p);
+	if (v){
+	  v->transformation=Transformation(p);
+	}
+      }
+    } else {
+      if (tag=="VERTEX3"){
+	int id;
+	Pose p;
+	ls >> id >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw(); 
+	TreePoseGraph3::Vertex* v=addVertex(id,p);
+	if (v){
+	  v->transformation=Transformation(p);
+	}
+      }
+    }      
+  }
+  is.clear(); /* clears the end-of-file and error flags */
+  is.seekg(0, ios::beg);
+
+  bool edgesOk=true;
+  while(is){
+    char buf[LINESIZE];
+    is.getline(buf,LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    
+    if (twoDimensions){
+      if (tag=="EDGE"){
+	int id1, id2;
+	Pose p(0.,0.,0.,0.,0.,0.);
+	InformationMatrix m;
+	ls >> id1 >> id2 >> p.x() >> p.y() >> p.yaw();
+	m=DMatrix<double>::I(6);
+	if (! overrideCovariances){
+	  ls >> m[0][0] >> m[0][1] >> m[1][1] >> m[2][2] >> m[0][2] >> m[1][2];
+	  m[2][0]=m[0][2]; m[2][1]=m[1][2]; m[1][0]=m[0][1];
+	}
+      
+	TreePoseGraph3::Vertex* v1=vertex(id1);
+	TreePoseGraph3::Vertex* v2=vertex(id2);
+	Transformation t(p);
+	if (!addEdge(v1, v2,t ,m)){
+	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
+	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
+	  edgesOk=false;
+	} 
+      }
+    } else {
+      if (tag=="EDGE3"){
+	int id1, id2;
+	Pose p;
+	InformationMatrix m;
+	ls >> id1 >> id2 >> p.x() >> p.y() >> p.z() >> p.roll() >> p.pitch() >> p.yaw();
+	m=DMatrix<double>::I(6);
+	if (! overrideCovariances){
+	  for (int i=0; i<6; i++)
+	    for (int j=i; j<6; j++)
+	      ls >> m[i][j];
+	}
+	TreePoseGraph3::Vertex* v1=vertex(id1);
+	TreePoseGraph3::Vertex* v2=vertex(id2);
+	Transformation t(p);
+	if (!addEdge(v1, v2,t ,m)){
+	  cerr << "Fatal, attempting to insert an edge between non existing nodes, skipping";
+	  cerr << "edge=" << id1 <<" -> " << id2 << endl;
+	  edgesOk=false;
+	}
+      }
+    }
+  }
+  return true;
+  //return edgesOk;
+}
+
+bool TreePoseGraph3::loadEquivalences(const char* filename){
+  ifstream is(filename);
+  if (!is)
+    return false;
+  EdgeList suppressed;
+  uint equivCount=0;
+  while (is){
+    char buf[LINESIZE];
+    is.getline(buf, LINESIZE);
+    istringstream ls(buf);
+    string tag;
+    ls >> tag;
+    if (tag=="EQUIV"){
+      int id1, id2;
+      ls >> id1 >> id2;
+      Edge* e=edge(id1,id2);
+      if (!e)
+	e=edge(id2,id1);
+      if (e){
+	suppressed.push_back(e);
+	equivCount++;
+      }
+    }
+  }
+  for (EdgeList::iterator it=suppressed.begin(); it!=suppressed.end(); it++){
+    Edge* e=*it;
+    if (e->v1->id > e->v2->id)
+      revertEdge(e);
+    collapseEdge(e);
+  }
+  return true;
+}
+
+bool TreePoseGraph3::saveGnuplot(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    TreePoseGraph3::Vertex* v=it->second;
+    v->pose=v->transformation.toPoseType();
+  }
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    const Vertex* v1=e->v1;
+    const Vertex* v2=e->v2;
+    
+    os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.z() << " "
+       << v1->pose.roll() << " " << v1->pose.pitch() << " " << v1->pose.yaw() <<endl;
+    os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.z() << " "
+       << v2->pose.roll() << " " << v2->pose.pitch() << " " << v2->pose.yaw() <<endl;
+    os << endl << endl;
+  }
+  return true;
+
+}
+
+bool TreePoseGraph3::save(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  
+  for (TreePoseGraph3::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    TreePoseGraph3::Vertex* v=it->second;
+    v->pose=v->transformation.toPoseType();
+
+    os << "VERTEX3 " 
+       << v->id << " " 
+       << v->pose.x() << " " 
+       << v->pose.y() << " " 
+       << v->pose.z() << " "
+       << v->pose.roll() << " " 
+       << v->pose.pitch() << " " 
+       << v->pose.yaw() << endl; 
+  }
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    os << "EDGE3 " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << " ";
+    for (int i=0; i<6; i++)
+      for (int j=i; j<6; j++)
+	os << e->informationMatrix[i][j] << " ";
+    os << endl;
+  }
+  return true;
+}
+
+/** \brief A class (struct) used to print vertex information to a
+    stream. Needed for debugging. **/
+struct IdPrinter{
+  IdPrinter(std::ostream& _os):os(_os){}
+  std::ostream& os;
+  void perform(TreePoseGraph3::Vertex* v){
+    std::cout << "(" << v->id << "," << v->level << ")" << endl;
+  }
+};
+
+void TreePoseGraph3::printDepth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeDepthVisit(ip, root);
+}
+
+void TreePoseGraph3::printWidth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeBreadthVisit(ip);
+}
+
+/** \brief A class (struct) for realizing the pose update of the
+    individual nodes. Assumes the correct order of constraint updates
+    (according to the tree level, see RSS07 paper)**/
+
+struct PosePropagator{
+  void perform(TreePoseGraph3::Vertex* v){
+    if (!v->parent)
+      return;
+    TreePoseGraph3::Transformation tParent(v->parent->transformation);
+    TreePoseGraph3::Transformation tNode=tParent*v->parentEdge->transformation;
+
+    assert(v->parentEdge->v1==v->parent);
+    assert(v->parentEdge->v2==v);
+    v->transformation=tNode;
+  }
+};
+
+void TreePoseGraph3::initializeOnTree(){
+  PosePropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+
+void TreePoseGraph3::printEdgesStat(std::ostream& os){
+  for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph3::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.z() << " " << p.roll() << " " << p.pitch() << " " << p.yaw() << endl;
+    os << "   top=" << e->top->id << " length=" << e->length << endl;
+  }
+}
+
+void TreePoseGraph3::revertEdgeInfo(Edge* e){
+  // here we assume uniform covariances, and we neglect the transofrmation
+  // induced by the Jacobian when reverting the link
+  e->transformation=e->transformation.inv();
+};
+
+void TreePoseGraph3::initializeFromParentEdge(Vertex* v){
+  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
+  v->transformation=tp;
+  v->pose=tp.toPoseType();
+  v->parameters=v->parentEdge->transformation;
+}
+
+
+void TreePoseGraph3::collapseEdge(Edge* e){
+  Vertex* v1=e->v1;
+  Vertex* v2=e->v2;
+  
+  // all the edges of v2 become outgoing
+  for (EdgeList::iterator it=v2->edges.begin(); it!=v2->edges.end(); it++){
+    if ( (*it)->v1!=v2 )
+      revertEdge(*it);
+  }
+
+ // all the edges of v1 become outgoing
+  for (EdgeList::iterator it=v1->edges.begin(); it!=v1->edges.end(); it++){
+    if ( (*it)->v1!=v1 )
+      revertEdge(*it);
+  }
+
+  assert(e->v1==v1);
+
+  InformationMatrix I12=e->informationMatrix;
+  CovarianceMatrix  C12=I12.inv();
+  Transformation    T12=e->transformation;
+  Pose              p12=T12.toPoseType();
+
+  Transformation iT12=T12.inv();
+
+  //compute the marginal information of the nodes in the path v1-v2-v*
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e2=*it2;
+    if (e2->v1==v2){ //edge leaving v2
+      Transformation T2x=e2->transformation;
+      Pose           p2x=T2x.toPoseType();
+      InformationMatrix I2x=e2->informationMatrix;
+      CovarianceMatrix  C2x=I2x.inv();
+      
+      //compute the estimate of the vertex based on the path v1-v2-vx
+      
+      Transformation tr=iT12*T2x;
+      
+      CovarianceMatrix CM=C2x;
+      
+      
+      Transformation T1x_pred=T12*e2->transformation;
+      Covariance C1x_pred=C12+C2x;
+      InformationMatrix I1x_pred=C1x_pred.inv();
+
+      e2->transformation=T1x_pred;
+      e2->informationMatrix=I1x_pred;
+    }
+  }
+
+  //all the edges leaving v1 and leaving v2 and leading to the same point are merged
+  std::list<Transformation> tList;
+  std::list<InformationMatrix> iList;
+  std::list<Vertex*> vList;
+
+  //others are transformed and added to v1
+  for (EdgeList::iterator it2=v2->edges.begin(); it2!=v2->edges.end(); it2++){
+    Edge* e1x=0;
+    Edge* e2x=0;
+    if ( ((*it2)->v1!=v1)){
+      e2x=*it2;
+      for (EdgeList::iterator it1=v1->edges.begin(); it1!=v1->edges.end(); it1++){
+	if ((*it1)->v2==(*it2)->v2)
+	  e1x=*it1;
+      }
+
+    }
+    // FIXME
+    // edges leading to the same node are ignored
+    // should be merged
+    if (e1x && e2x){
+      // here goes something for mergin the constraints, according to the information matrices.
+      // in 3D it is a nightmare, so i postpone this, and i simply ignore the redundant constraints.
+      // the resultng system is overconfident
+    }
+    if (!e1x && e2x){
+      tList.push_back(e2x->transformation);
+      iList.push_back(e2x->informationMatrix);
+      vList.push_back(e2x->v2);
+    }
+  }
+  removeVertex(v2->id);
+
+  std::list<Transformation>::iterator t=tList.begin();
+  std::list<InformationMatrix>::iterator i=iList.begin();
+  std::list<Vertex*>::iterator v=vList.begin();
+  while (i!=iList.end()){
+    addEdge(v1,*v,*t,*i);
+    i++;
+    t++;
+    v++;
+  }
+}
+
+void TreePoseGraph3::recomputeAllTransformations(){
+  TransformationPropagator tp;
+  treeDepthVisit(tp,root);
+}
+
+}; //namespace AISNavigation

corelib/src/toro3d/posegraph3.hh

@@ -0,0 +1,145 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ *   TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                           Slawomir Grzonka and  Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file posegraph3.hh
+ *
+ * \brief Defines the graph of 3D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#ifndef _POSEGRAPH3_HH_
+#define _POSEGRAPH3_HH_
+
+#include "posegraph.hh"
+#include "transformation3.hh"
+#include <iostream>
+#include <vector>
+
+typedef unsigned int uint;
+#ifndef M_PI
+	#define M_PI 3.14159265359
+#endif
+
+namespace AISNavigation {
+
+
+/** \brief The class (struct) that contains 2D graph related functions
+    such as loading, saving, merging, etc. **/
+struct TreePoseGraph3: public TreePoseGraph<Operations3D<double> >{
+  typedef Operations3D<double> Ops;
+  
+  typedef Ops::PoseType           Pose;
+  typedef Ops::RotationType       Rotation;
+  typedef Ops::TranslationType    Translation;
+  typedef Ops::TransformationType Transformation;
+  typedef Ops::CovarianceType     CovarianceMatrix;
+  typedef Ops::InformationType    InformationMatrix;
+
+
+  /** Load a graph from a file ignoring the equivalence constraints
+      @param filename the graph file
+      @param overrideCovariances ignore the covariances from the file, and use identities instead
+   **/
+  bool load( const char* filename, bool overrideCovariances=false, bool twoDimensions=false); 
+
+  /** Load only the equivalence constraints from a  graph file (call load before)  **/
+  bool loadEquivalences( const char* filename); 
+
+  /** Saves the graph in the graph-format**/
+  bool save( const char* filename);
+
+  /** Saved the graph for visualizing it using gnuplot **/
+  bool saveGnuplot( const char* filename);
+
+  /** Debug function  **/
+  void printDepth( std::ostream& os );
+
+  /** Debug function  **/
+  void printWidth( std::ostream& os );
+
+  /** Debug function  **/
+  void printEdgesStat( std::ostream& os);
+
+  /** Initializes the parameters based on the topology of the tree and the actual transformation*/
+  void initializeOnTree();
+  
+  /** Recomputes all the transformations based on the parameters and the tree*/
+  void recomputeAllTransformations();
+
+  virtual void initializeFromParentEdge(Vertex* v);
+
+  /** Turn around the edge (<i,j>  => <j,i>)  **/
+  virtual void revertEdgeInfo(Edge* e);
+
+  /** Function to compress a graph. Needed if, for example, equivalence
+      constraints are used to build a graoh structure with indices
+      without gaps.  **/
+  virtual void collapseEdge(Edge* e);
+
+  /** Specifies the verbose level for debugging **/  
+  int verboseLevel;
+
+protected:
+
+  /** \brief A class (struct) to compute the parameterization of the vertex v **/
+  struct ParameterPropagator{
+    inline void perform(TreePoseGraph3::Vertex* v){
+      if (!v->parent){
+	v->parameters=TreePoseGraph3::Transformation(0.,0.,0.,0.,0.,0.);
+	return;
+      }
+      v->parameters=v->parent->transformation.inv()*v->transformation;
+    }
+  };
+
+  /** \brief A class (struct) to compute the parameterization of the vertex v **/
+  struct TransformationPropagator{
+    inline void perform(TreePoseGraph3::Vertex* v){
+      if (!v->parent){
+	return;
+      }
+      v->transformation=v->parent->transformation*v->parameters;
+    }
+  };
+
+};
+
+
+}; //namespace AISNavigation
+#endif
+
+
+

corelib/src/toro3d/transformation3.hh

@@ -0,0 +1,275 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+#ifndef _TRANSFORMATION3_HXX_
+#define _TRANSFORMATION3_HXX_
+
+#include <assert.h>
+#include <cmath>
+#include "dmatrix.hh"
+
+namespace AISNavigation {
+
+template <class T>
+struct Vector3 {
+  T elems[3] ;
+
+  Vector3(T x, T y, T z) {elems[0]=x; elems[1]=y; elems[2]=z;}
+  Vector3()              {elems[0]=0.; elems[1]=0.; elems[2]=0.;}
+  Vector3(const DVector<T>& t){}
+
+  // translational view
+  inline const T& x()     const  {return elems[0];}
+  inline const T& y()     const  {return elems[1];}
+  inline const T& z()     const  {return elems[2];}
+  inline T& x()            {return elems[0];}
+  inline T& y()            {return elems[1];}
+  inline T& z()            {return elems[2];}
+
+  // rotational view
+  inline const T& roll()   const  {return elems[0];}
+  inline const T& pitch() const  {return elems[1];}
+  inline const T& yaw()   const  {return elems[2];}
+  inline T& roll()          {return elems[0];}
+  inline T& pitch()        {return elems[1];}
+  inline T& yaw()          {return elems[2];}
+
+};
+
+template <class T>
+struct Pose3 : public DVector<T>{
+  Pose3();
+  Pose3(const Vector3<T>& rot, const Vector3<T>& trans);
+  Pose3(const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
+  Pose3(const DVector<T>& v): DVector<T>(v) {assert(v.dim()==6);} 
+    
+  inline operator const DVector<T>&  () {return (const DVector<T>)*this;}
+  inline operator DVector<T>&        () {return *this;}
+
+  inline const T& roll()  const  {return DVector<T>::elems[0];}
+  inline const T& pitch() const  {return DVector<T>::elems[1];}
+  inline const T& yaw()   const  {return DVector<T>::elems[2];}
+  inline const T& x()     const  {return DVector<T>::elems[3];}
+  inline const T& y()     const  {return DVector<T>::elems[4];}
+  inline const T& z()     const  {return DVector<T>::elems[5];}
+
+  inline T& roll()               {return DVector<T>::elems[0];}
+  inline T& pitch()              {return DVector<T>::elems[1];}
+  inline T& yaw()                {return DVector<T>::elems[2];}
+  inline T& x()                  {return DVector<T>::elems[3];}
+  inline T& y()                  {return DVector<T>::elems[4];}
+  inline T& z()                  {return DVector<T>::elems[5];}
+
+};
+
+
+/*!
+ * A Quaternion can be used to either represent a rotational axis
+ * and a Rotation, or, the point which will be rotated
+ */
+
+template <class T>
+struct Quaternion{
+
+  /*!
+   * Default Constructor: w=x=y=z=0;
+   */
+  Quaternion();
+  
+  /*!
+   * The Quaternion representation of the point "pose"
+   */
+  Quaternion(const Vector3<T>& pose);
+  
+  /*!
+   * create a Quaternion by scalar w and the imaginery parts x,y, and z.
+   */
+  Quaternion(const T _w, const T _x, const T _y, const T _z);
+  
+  /*!
+   * create a rotational Quaternion, roll along x-axis, pitch along y-axis and yaw along z-axis
+   */
+  Quaternion(const T _roll_x_phi, const T _pitch_y_theta, const T _yaw_z_psi);
+  
+  /*!
+   * @return the conjugated version of this quaternion
+   */
+  inline Quaternion<T> conjugated() const;
+  
+  /*!
+   * @return this quaternion, but normalized
+   */
+  inline Quaternion<T> normalized() const;
+  
+  /*!
+   * @return the inverse of this Quaternion
+   */
+  inline Quaternion<T> inverse() const;
+
+  /*construct a quaternion on the axis/angle representation*/
+  inline Quaternion(const Vector3<T>& axis, const T& angle);
+
+  /*!
+   * if this Quaternion represents a point, use this function
+   * to rotate the point along <axis> with angle <alpha>
+   * @param axis the rotational axis
+   * @param alpha rotational angle
+   */
+  inline Quaternion<T> rotateThisAlong (const Vector3<T>& axis, const T alpha) const;
+  
+	
+  /*!
+   * if this Quaternion represents a rotational axis + rotation,
+   * use this function to rotate another point represented as a Quaternion p
+   * @param p the point to be rotated by <this>. Point is represented as a Quaternion
+   * @return rotated Point (represented as a Quaternion)
+   */
+  inline Quaternion<T> rotatePoint(const Quaternion& p) const;
+  
+  /*!
+   * if this Quaternion represents a rotational axis + rotation,
+   * use this function to rotate another point
+   * @param p the point to be rotated by <this>. 
+   * @return rotated Point 
+   */
+  inline Vector3<T> rotatePoint(const Vector3<T>& p) const;
+  
+  /*!
+   * if this Quaternion represents a rotational axis, add a rotation of angle <alpha>
+   * along <this> axis to the Quaternion
+   * @param alpha rotational value
+   * @return this Quaternion with included information about the rotation along <this> axis
+   */
+  inline Quaternion withRotation (const T alpha) const;
+	
+  /*!
+   * Given rotational axis x,y,z, get the rotation along these axis encoded in this Quaternion
+   * @return rotation along x,y,z axis encoded in <this> Quaternion
+	*/
+  inline Vector3<T> toAngles() const;
+  
+  inline Vector3<T> axis() const;
+  inline T angle() const;
+
+	
+  /*!
+   * @return the norm of this Quaternion
+   */
+  inline T norm() const;
+  
+  /*!
+   * @return the real part (==w) of this Quaternion
+   */
+  inline T re() const;
+  
+  /*!
+   * @return the imaginery part (== (x,y,z)) of this Quaternion
+   */
+  inline Vector3<T> im() const;
+	
+  T w,x,y,z;
+};
+
+
+
+template <class T> inline Quaternion<T> operator + (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator - (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> operator * (const Quaternion<T> & left, const T scalar);
+template <class T> inline Quaternion<T> operator * (const T scalar, const Quaternion<T>& right);
+template <class T> std::ostream& operator << (std::ostream& os, const Quaternion<T>& q);
+
+template <class T> inline T innerproduct(const Quaternion<T>& left, const Quaternion<T>& right);
+template <class T> inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda);
+
+
+
+template <class T>
+struct Transformation3{
+  Quaternion<T> rotationQuaternion;
+  Vector3<T>    translationVector;
+
+  Transformation3(){}
+
+  inline static Transformation3<T> identity();
+
+  Transformation3 (const Vector3<T>& trans, const Quaternion<T>& rot);
+  Transformation3 (const Pose3<T>& v);
+  Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw);
+
+  inline Vector3<T> translation() const;
+  inline Quaternion <T> rotation() const;
+
+  inline Pose3<T> toPoseType() const;
+  
+  inline void setTranslation(const Vector3<T>& t);
+  inline void setTranslation(const T& x, const T& y, const T& z);
+
+  inline void setRotation(const Vector3<T>& r);
+  inline void setRotation(const T& roll, const T& pitch, const T& yaw);
+  inline void setRotation(const Quaternion<T>& q);
+
+
+  inline Transformation3<T> inv() const;
+  inline bool validRotation(const T& epsilon=0.001) const;
+
+};
+
+template <class T>
+inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v);
+
+template <class T>
+inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2);
+
+template <class T>
+struct Operations3D{
+  typedef T                  BaseType;
+  typedef Pose3<T>           PoseType;
+  typedef Quaternion<T>      RotationType;
+  typedef Vector3<T>         TranslationType;
+  typedef Transformation3<T> TransformationType;
+  typedef DMatrix<T>         CovarianceType;
+  typedef DMatrix<T>         InformationType;
+  typedef Transformation3<T> ParametersType;
+};
+
+} // namespace AISNavigation
+/**************************** IMPLEMENTATION ****************************/
+
+#include "transformation3.hxx"
+
+#endif
+

corelib/src/toro3d/transformation3.hxx

@@ -0,0 +1,451 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+#include <limits>
+
+namespace AISNavigation {
+
+template <class T>
+inline Vector3<T> operator * (const T& d, const Vector3<T>& v) {
+  return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
+}
+
+template <class T>
+inline Vector3<T> operator * (const Vector3<T>& v, const T& d) {
+  return Vector3<T>(v.elems[0]*d, v.elems[1]*d, v.elems[2]*d);
+}
+
+template <class T>
+inline T operator * (const Vector3<T>& v1, const Vector3<T>& v2){
+  return v1.elems[0]*v2.elems[0] 
+    + v1.elems[1]*v2.elems[1] 
+    + v1.elems[2]*v2.elems[2];
+}
+
+template <class T>
+inline Vector3<T> operator + (const Vector3<T>& v1, const Vector3<T>& v2){
+  return Vector3<T>(v1.elems[0]+v2.elems[0], 
+		    v1.elems[1]+v2.elems[1], 
+		    v1.elems[2]+v2.elems[2]);
+}
+
+template <class T>
+Vector3<T> operator - (const Vector3<T>& v1, const Vector3<T>& v2){
+  return Vector3<T>(v1.elems[0]-v2.elems[0], 
+		    v1.elems[1]-v2.elems[1],
+		    v1.elems[2]-v2.elems[2]);
+}
+
+
+
+template <class T>
+Pose3<T>::Pose3(): DVector<T>(6){
+}
+
+template <class T>
+Pose3<T>::Pose3(const Vector3<T>& trans, const Vector3<T>& rot): DVector<T>(6){
+  DVector<T>::elems[0]=rot.roll(); 
+  DVector<T>::elems[1]=rot.pitch(); 
+  DVector<T>::elems[2]=rot.yaw(); 
+  DVector<T>::elems[3]=trans.x(); 
+  DVector<T>::elems[4]=trans.y(); 
+  DVector<T>::elems[5]=trans.z();
+}
+
+template <class T>
+Pose3<T>::Pose3(const T& x, const T& y, const T& z, const T& r, const T& p, const T& yw): DVector<T>(6){
+  DVector<T>::elems[0]=r; 
+  DVector<T>::elems[1]=p; 
+  DVector<T>::elems[2]=yw; 
+  DVector<T>::elems[3]=x; 
+  DVector<T>::elems[4]=y; 
+  DVector<T>::elems[5]=z;
+}
+
+
+#define MY_MAX(a,b) (((a)>(b))?(a):(b))
+
+
+template<class T>
+Quaternion<T>::Quaternion(){
+	w = 1;
+	x = 0;
+	y = 0;
+	z = 0;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const Vector3<T>& pose){
+	w = 0;
+	x = pose.x();
+	y = pose.y();
+	z = pose.z();
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const Vector3<T>& axis, const T& angle){
+  T sa=sin(angle/2);
+  T ca=cos(angle/2);
+  w=ca;
+  x=axis.x()*sa;
+  y=axis.y()*sa;
+  z=axis.z()*sa;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const T _w, const T _x, const T _y, const T _z){
+	w = _w;
+	x = _x;
+	y = _y;
+	z = _z;
+}
+
+template<class T>
+Quaternion<T>::Quaternion(const T phi, const T theta, const T psi){
+	T sphi   = sin(phi);
+	T stheta = sin(theta);
+	T spsi   = sin(psi);
+	T cphi   = cos(phi);
+	T ctheta = cos(theta);
+	T cpsi   = cos(psi);
+	
+	T _r[3][3] = { //create rotational Matrix
+		{cpsi*ctheta, cpsi*stheta*sphi - spsi*cphi, cpsi*stheta*cphi + spsi*sphi},
+		{spsi*ctheta, spsi*stheta*sphi + cpsi*cphi, spsi*stheta*cphi - cpsi*sphi},
+		{    -stheta,                  ctheta*sphi,                  ctheta*cphi}
+	};
+	
+	T _w = sqrt(MY_MAX(0, 1 + _r[0][0] + _r[1][1] + _r[2][2]))/2.0;
+	T _x = sqrt(MY_MAX(0, 1 + _r[0][0] - _r[1][1] - _r[2][2]))/2.0;
+	T _y = sqrt(MY_MAX(0, 1 - _r[0][0] + _r[1][1] - _r[2][2]))/2.0;
+	T _z = sqrt(MY_MAX(0, 1 - _r[0][0] - _r[1][1] + _r[2][2]))/2.0;
+	this->w = _w;
+	this->x = (_r[2][1] - _r[1][2])>=0?fabs(_x):-fabs(_x);
+	this->y = (_r[0][2] - _r[2][0])>=0?fabs(_y):-fabs(_y);
+	this->z = (_r[1][0] - _r[0][1])>=0?fabs(_z):-fabs(_z);
+}
+
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::conjugated() const{
+	return Quaternion<T>(w,-x,-y,-z);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::normalized() const{
+	T n = this->norm();
+	if (n > 0)
+		return ((1./n) * (*this));
+	else
+		return Quaternion<T>(0.,0.,0.,0.);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::inverse() const{
+	return ((1./this->norm()) * this->conjugated());
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::rotateThisAlong(const Vector3<T>& axis, const T alpha) const{
+	Quaternion<T> q(axis);
+	q = q.normalized();
+	q = q.withRotation(alpha);
+	return q.rotatePoint(*this);
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::rotatePoint(const Quaternion<T>& p) const{
+	return (*this)*p*(this->conjugated());
+}
+
+template<class T>
+inline Vector3<T> Quaternion<T>::rotatePoint(const Vector3<T>& point) const{
+	Quaternion<T> p(point);
+	Quaternion<T> q = this->rotatePoint(p);
+	return q.im();
+}
+
+template<class T>
+inline Quaternion<T> Quaternion<T>::withRotation(const T alpha) const{
+	Quaternion<T> q = normalized();
+	T salpha = sin(alpha/2.);
+	T calpha = cos(alpha/2.);
+	q.w = calpha;
+	q.x = salpha * q.x;
+	q.y = salpha * q.y;
+	q.z = salpha * q.z;
+	return q;
+}
+
+
+template<class T>
+inline Vector3<T> Quaternion<T>::toAngles() const{
+	T n = this->norm();
+	T s = n > 0?2./(n*n):0.;
+	
+	T m00, m01, m02, m10, m11, m12, m20, m21, m22;
+	T phi,theta,psi;
+	
+	
+	T xs = this->x*s;
+	T ys = this->y*s;
+	T zs = this->z*s;
+	
+	T wx = this->w*xs;
+	T wy = this->w*ys;
+	T wz = this->w*zs;
+	
+	T xx = this->x*xs;
+	T xy = this->x*ys;
+	T xz = this->x*zs;
+	
+	T yy = this->y*ys;
+	T yz = this->y*zs;
+	
+	T zz = this->z*zs;
+	
+	m00 = 1.0 - (yy + zz);
+	m11 = 1.0 - (xx + zz);
+	m22 = 1.0 - (xx + yy);
+	
+	
+	m10 = xy + wz;
+	m01 = xy - wz;
+	
+	m20 = xz - wy;
+	m02 = xz + wy;
+	m21 = yz + wx;
+	m12 = yz - wx;
+	
+	phi   = atan2(m21,m22);
+	theta = atan2(-m20,sqrt(m21*m21 + m22*m22));
+	psi 	= atan2(m10,m00);
+	
+	return Vector3<T>(phi, theta, psi);
+}
+
+
+template<class T>
+inline Vector3<T> Quaternion<T>::axis() const {
+  double imNorm=sqrt(x*x+y*y+z*z);
+  if (imNorm<std::numeric_limits<double>::min()){
+    return Vector3<T>(0.,0.,1.);
+  }
+  return Vector3<T>(x/imNorm, y/imNorm, z/imNorm);
+}
+
+template<class T>
+inline T Quaternion<T>::angle() const{
+  Quaternion<T> q=normalized();
+  double a=2*atan2(sqrt(q.x*q.x + q.y*q.y  + q.z*q.z), q.w);
+  return atan2(sin(a), cos(a));
+}
+
+template<class T>
+inline T Quaternion<T>::norm() const{
+	return sqrt(w*w + x*x + y*y + z*z);
+}
+
+
+template<class T>
+inline T Quaternion<T>::re() const{
+	return w;
+}
+
+template<class T>
+inline Vector3<T> Quaternion<T>::im() const{
+	return Vector3<T>(x, y, z);
+}
+
+
+template<class T>
+inline Quaternion<T> operator + (const Quaternion<T>& left, const Quaternion<T>& right){
+	return Quaternion<T>(left.w + right.w, left.x + right.x, left.y + right.y, left.z + right.z);
+}
+
+template<class T>
+inline Quaternion<T> operator - (const Quaternion<T>& left, const Quaternion<T>& right){
+	return Quaternion<T>(left.w - right.w, left.x - right.x, left.y - right.y, left.z - right.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const Quaternion<T>& q1, const Quaternion<T>& q2){
+  return Quaternion<T> (q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z,
+			q1.y*q2.z - q2.y*q1.z + q1.w*q2.x + q2.w*q1.x,
+			q1.z*q2.x - q2.z*q1.x + q1.w*q2.y + q2.w*q1.y,
+			q1.x*q2.y - q2.x*q1.y + q1.w*q2.z + q2.w*q1.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const Quaternion<T>& q, const T s){
+	return Quaternion<T>(s*q.w, s*q.x, s*q.y, s*q.z);
+}
+
+template<class T>
+inline Quaternion<T> operator * (const T s, const Quaternion<T>& q){
+	return Quaternion<T>(q.w*s, q.x*s, q.y*s, q.z*s);
+}
+
+template<class T>
+std::ostream& operator << (std::ostream& os, const Quaternion<T>& q){
+	os << q.w << " " << q.x << " " << q.y << " " << q.z << " ";
+	return os;
+}
+
+template<class T>
+inline T innerproduct(const Quaternion<T>& q1, const Quaternion<T>& q2){
+	return q1.w*q2.w + q1.x*q2.x + q1.y*q2.y + q1.z*q2.z;
+}
+
+template<class T>
+inline Quaternion<T> slerp(const Quaternion<T>& from, const Quaternion<T>& to, const T lambda){
+	Quaternion<T> _from = from.normalized();
+	Quaternion<T> _to = to.normalized();
+	T _cos_omega = innerproduct(_from,_to);
+	_cos_omega = (_cos_omega>1)?1:_cos_omega;
+	_cos_omega = (_cos_omega<-1)?-1:_cos_omega;
+	T _omega = acos(_cos_omega);
+	assert (!isnan(_cos_omega));
+	if (fabs(_omega) < 1e-6)
+		return to;
+	
+	//determine right direction of slerp:
+	Quaternion<T> _pq = _from - _to;
+	Quaternion<T> _pmq = _from + _to;
+	T _first = _pq.norm();
+	T _alternativ = _pmq.norm();
+	
+	Quaternion<T> q1 = _from;
+	Quaternion<T> q2 = (_first < _alternativ)? (Quaternion<T>) _to: -1.*(Quaternion<T>)_to;
+	//now calculate intermediate quaternion.
+	
+	Quaternion<T> ret = q1*(sin((1-lambda)*_omega)/(sin(_omega))) + q2*(sin(lambda*_omega)/sin(_omega));
+	assert (!(isnan(ret.w) || isnan(ret.x) || isnan(ret.y) || isnan(ret.z)));
+	return ret;
+}
+
+
+template <class T>
+inline Transformation3<T> Transformation3<T>::identity(){
+  Transformation3<T> m;
+  m.rotationQuaternion=Quaternion<T>();
+  m.translationVector(0.,0.,0.);
+  return m;
+}
+
+
+
+template <class T>
+inline Transformation3<T>::Transformation3 (const T& x, const T& y, const T& z, const T& roll, const T& pitch, const T& yaw){
+  rotationQuaternion=Quaternion<T>(roll,pitch,yaw);
+  translationVector=Vector3<T>(x,y,z);
+}
+
+template <class T>
+inline Transformation3<T>::Transformation3 (const Pose3<T>& v){
+  rotationQuaternion=Quaternion<T>(v.roll(),v.pitch(),v.yaw());
+  translationVector=Vector3<T>(v.x(),v.y(),v.z());
+
+}
+
+template <class T>
+inline Vector3<T> Transformation3<T>::translation() const {
+  return translationVector;
+}
+  
+template <class T>
+inline Quaternion<T> Transformation3<T>::rotation()    const {
+  return rotationQuaternion;
+}
+  
+template <class T>
+inline Pose3<T> Transformation3<T>::toPoseType()   const {
+  Vector3<T> t=translation();
+  Vector3<T> r=rotationQuaternion.toAngles();
+  Pose3<T> rv(t.x(), t.y(), t.z(), r.roll(), r.pitch(), r.yaw() );
+  return rv;
+}
+  
+template <class T>
+inline void Transformation3<T>::setTranslation(const Vector3<T>& t){
+  translationVector=t;
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const Quaternion<T>& q){
+  rotationQuaternion=q.normalized();
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const Vector3<T>& r){
+  setRotation(r.roll(),r.pitch(), r.yaw());
+}
+
+template <class T>
+inline void Transformation3<T>::setRotation(const T& roll_phi, const T& pitch_theta, const T& yaw_psi){
+  rotationQuaternion=Quaternion<T>(roll_phi, pitch_theta, yaw_psi);
+}
+
+template <class T>
+inline void Transformation3<T>::setTranslation(const T& x, const T& y, const T& z){
+  translationVector=Vector3<T>(x,y,z);
+}
+
+
+template <class T>
+inline Transformation3<T> Transformation3<T>::inv() const {
+
+  Transformation3<T> rv(*this);
+  rv.rotationQuaternion=rotationQuaternion.inverse().normalized();
+  rv.translationVector=rv.rotationQuaternion.rotatePoint(translationVector*-1.);
+  return rv;
+}
+
+template <class T>
+inline Vector3<T> operator * (const Transformation3<T>& m, const Vector3<T>& v){
+  return m.translationVector+m.rotationQuaternion.rotatePoint(v);
+}
+
+template <class T>
+inline Transformation3<T> operator * (const Transformation3<T>& m1, const Transformation3<T>& m2){
+  Transformation3<T> rv;
+  rv.translationVector=m1.rotationQuaternion.rotatePoint(m2.translationVector)+m1.translationVector;
+  rv.rotationQuaternion=(m1.rotationQuaternion*m2.rotationQuaternion).normalized();
+  return rv;
+}
+
+} // namespace AISNavigation
+

corelib/src/toro3d/treeoptimizer3.cpp

@@ -0,0 +1,361 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file treeoptimizer3.cpp
+ *
+ * \brief Defines the core optimizer class for 3D graphs which is a
+ * subclass of TreePoseGraph3
+ *
+ **/
+
+#include "treeoptimizer3.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+
+TreeOptimizer3::TreeOptimizer3(){
+  restartOnDivergence=false;
+  sortedEdges=0;
+  mpl=-1;
+  edgeCompareMode=EVComparator<Edge*>::CompareLevel;
+}
+
+
+TreeOptimizer3::~TreeOptimizer3(){
+}
+
+void TreeOptimizer3::initializeTreeParameters(){
+  ParameterPropagator pp;
+  treeDepthVisit(pp,root);
+}
+
+
+void TreeOptimizer3::iterate(TreePoseGraph3::EdgeSet* eset, bool noPreconditioner){
+  TreePoseGraph3::EdgeSet* temp=sortedEdges;
+  if (eset){
+    sortedEdges=eset;
+  }
+
+  if (noPreconditioner)
+    propagateErrors(false);
+  else {
+    if (iteration==1)
+      computePreconditioner();  
+    propagateErrors(true);
+  }
+  sortedEdges=temp;
+
+  onRestartBegin();
+  if (restartOnDivergence){
+    double mte, ate;
+    double mre, are;
+    error(&mre, &mte, &are, &ate);
+    maxTranslationalErrors.push_back(mte);
+    maxRotationalErrors.push_back(mre);
+    int interval=3;
+    if ((int)maxRotationalErrors.size()>=interval){
+      uint s=maxRotationalErrors.size();
+      double re0 = maxRotationalErrors[s-interval];
+      double re1 = maxRotationalErrors[s-1];
+
+      if ((re1-re0)>are || sqrt(re1)>0.99*M_PI){
+	double rg=rotGain;
+	if (sqrt(re1)>M_PI/4){
+	  cerr << "RESTART!!!!! : Angular wraparound may be occourring" << endl;
+	  cerr << " err=" << re0 << " -> " << re1 << endl; 
+	  cerr << "Restarting optimization and reducing the rotation factor" << endl;
+	  cerr << rg << " -> ";
+	  initializeOnTree();
+	  initializeTreeParameters();
+	  initializeOptimization();
+	  error(&mre, &mte);
+	  maxTranslationalErrors.push_back(mte);
+	  maxRotationalErrors.push_back(mre);
+	  rg*=0.1;
+	  rotGain=rg;
+	  cerr << rotGain << endl;
+	}
+	else {
+	  cerr << "decreasing angular gain" << rotGain*0.1 << endl;
+	  rotGain*=0.1;
+	}
+      }
+    }
+  }
+  onRestartDone();
+}
+
+
+void TreeOptimizer3::recomputeTransformations(Vertex*v, Vertex* top){
+  if (v==top)
+    return;
+  recomputeTransformations(v->parent, top);
+  v->transformation=v->parent->transformation*v->parameters;
+}
+
+
+void TreeOptimizer3::recomputeParameters(Vertex*v, Vertex* top){
+  while (v!=top){
+    v->parameters=v->parent->transformation.inv()*v->transformation;
+    v=v->parent;
+  }
+}
+
+
+TreeOptimizer3::Transformation TreeOptimizer3::getPose(Vertex*v, Vertex* top){
+  Transformation t(0.,0.,0.,0.,0.,0.);
+  if (v==top)
+    return v->transformation;
+  while (v!=top){
+    t=v->parameters*t;
+    v=v->parent;
+  }
+  return top->transformation*t;
+}
+
+TreeOptimizer3::Rotation TreeOptimizer3::getRotation(Vertex*v, Vertex* top){
+  Rotation r(0.,0.,0.);
+  if (v==top)
+    return v->transformation.rotation();
+  while (v!=top){
+    r=v->parameters.rotation()*r;
+    v=v->parent;
+  }
+  return top->transformation.rotation()*r;
+}
+
+
+
+double TreeOptimizer3::error(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Transformation et=e->transformation;
+  Transformation t1=v1->transformation;
+  Transformation t2=v2->transformation;
+  
+  Transformation t12=(t1*et)*t2.inv();
+  Pose p12=t12.toPoseType();
+
+  Pose ps=e->informationMatrix*p12;
+  double err=p12*ps;
+  DEBUG(100) << "e(" << v1->id << "," << v2->id << ")" << err << endl;
+  return err;
+ 
+}
+
+double TreeOptimizer3::traslationalError(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Transformation et=e->transformation;
+  Transformation t1=v1->transformation;
+  Transformation t2=v2->transformation;
+  
+  Translation t12=(t2.inv()*(t1*et)).translation();
+  return t12*t12;;
+ 
+}
+
+double TreeOptimizer3::rotationalError(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Rotation er=e->transformation.rotation();
+  Rotation r1=v1->transformation.rotation();
+  Rotation r2=v2->transformation.rotation();
+  
+  Rotation r12=r2.inverse()*(r1*er);
+  double r=r12.angle();
+  return r*r;
+}
+
+
+double TreeOptimizer3::loopError(const Edge* e) const{
+  double err=0;
+  const Vertex* v=e->v1;
+  while (v!=e->top){
+    err+=error(v->parentEdge);
+    v=v->parent;
+  }
+  v=e->v2;
+  while (v==e->top){
+    err+=error(v->parentEdge);
+    v=v->parent;
+  }
+  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
+    err+=error(e);
+  return err;
+}
+
+double TreeOptimizer3::loopRotationalError(const Edge* e) const{
+  double err=0;
+  const Vertex* v=e->v1;
+  while (v!=e->top){
+    err+=rotationalError(v->parentEdge);
+    v=v->parent;
+  }
+  v=e->v2;
+  while (v!=e->top){
+    err+=rotationalError(v->parentEdge);
+    v=v->parent;
+  }
+  if (e->v2->parentEdge!=e && e->v1->parentEdge!=e)
+    err+=rotationalError(e);
+  return err;
+}
+
+
+double TreeOptimizer3::error(double* mre, double* mte, double* are, double* ate, TreePoseGraph3::EdgeSet* eset) const{
+  double globalRotError=0.;
+  double maxRotError=0;
+  double globalTrasError=0.;
+  double maxTrasError=0;
+
+  int c=0;
+  if (! eset){
+    for (TreePoseGraph3::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+      double re=rotationalError(it->second);
+      globalRotError+=re;
+      maxRotError=maxRotError>re?maxRotError:re;
+      double te=traslationalError(it->second);
+      globalTrasError+=te;
+      maxTrasError=maxTrasError>te?maxTrasError:te;
+      c++;
+    }
+  } else {
+    for (TreePoseGraph3::EdgeSet::const_iterator it=eset->begin(); it!=eset->end(); it++){
+      const TreePoseGraph3::Edge* edge=*it;
+      double re=rotationalError(edge);
+      globalRotError+=re;
+      maxRotError=maxRotError>re?maxRotError:re;
+      double te=traslationalError(edge);
+      globalTrasError+=te;
+      maxTrasError=maxTrasError>te?maxTrasError:te;
+      c++;
+    }
+  }
+
+  if (mte)
+    *mte=maxTrasError;
+  if (mre)
+    *mre=maxRotError;
+  if (ate)
+    *ate=globalTrasError/c;
+  if (are)
+    *are=globalRotError/c;
+  return globalRotError+globalTrasError;
+}
+
+
+
+void TreeOptimizer3::initializeOptimization(EdgeCompareMode mode){
+  edgeCompareMode=mode;
+  // compute the size of the preconditioning matrix
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+
+  // sorting edges
+  if (sortedEdges!=0){
+    delete sortedEdges;
+    sortedEdges=0;
+  }
+  sortedEdges=sortEdges();
+  mpl=maxPathLength();
+  rotGain=1.;
+  trasGain=1.;
+}
+
+void TreeOptimizer3::initializeOnlineIterations(){
+  int sz=maxIndex()+1;
+  DEBUG(1) << "Size= " << sz << endl;
+  M.resize(sz);
+  DEBUG(1) << "allocating M(" << sz << ")" << endl;
+  iteration=1;
+  maxRotationalErrors.clear();
+  maxTranslationalErrors.clear();
+  rotGain=1.;
+  trasGain=1.;
+}
+
+void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
+  edgeCompareMode=mode;
+  // compute the size of the preconditioning matrix
+  clear();
+  Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
+  root=v0;
+  v0->parameters=Transformation(v0->pose);
+  v0->parentEdge=0;
+  v0->parent=0;
+  v0->level=0;
+  v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
+}
+
+void TreeOptimizer3::onStepStart(Edge* e){
+  DEBUG(5) << "entering edge" << e << endl;
+}  
+void TreeOptimizer3::onStepFinished(Edge* e){
+  DEBUG(5) << "exiting edge" << e << endl;
+}
+
+void TreeOptimizer3::onIterationStart(int iteration){
+  DEBUG(5) << "entering iteration " << iteration << endl;
+}
+
+void TreeOptimizer3::onIterationFinished(int iteration){
+  DEBUG(5) << "exiting iteration " << iteration << endl;
+}
+
+void TreeOptimizer3::onRestartBegin(){}
+void TreeOptimizer3::onRestartDone(){}
+bool TreeOptimizer3::isDone(){
+  return false;
+}
+
+
+}; //namespace AISNavigation

corelib/src/toro3d/treeoptimizer3.hh

@@ -0,0 +1,181 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka, and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+/** \file treeoptimizer3.hh
+ *
+ * \brief Defines the core optimizer class for 3D graphs which is a
+ * subclass of TreePoseGraph3
+ *
+ **/
+
+#ifndef _TREEOPTIMIZER3_HH_
+#define _TREEOPTIMIZER3_HH_
+
+#include "posegraph3.hh"
+
+namespace AISNavigation {
+
+/** \brief Class that contains the core optimization algorithm **/
+struct TreeOptimizer3: public TreePoseGraph3{
+  typedef std::vector<Pose> PoseVector;
+
+  /** Constructor **/
+  TreeOptimizer3();
+
+  /** Destructor **/
+  virtual ~TreeOptimizer3();
+
+  /** Initialization function **/
+  void initializeTreeParameters();
+
+  /** Initialization function **/
+  void initializeOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineOptimization(EdgeCompareMode mode=EVComparator<Edge*>::CompareLevel);
+
+  void initializeOnlineIterations();
+
+  /** Performs one iteration of the algorithm **/
+  void iterate(TreePoseGraph3::EdgeSet* eset=0, bool noPreconditioner=false);
+
+  /** Conmputes the gloabl error of the network **/
+  double error(double* mre=0, double* mte=0, double* are=0, double* ate=0, TreePoseGraph3::EdgeSet* eset=0) const;
+
+  /** Conmputes the gloabl error of the network **/
+  double angularError() const;
+
+  /** Conmputes the gloabl error of the network **/
+  double translationalError() const;
+
+  bool restartOnDivergence;
+
+  inline double getRotGain() const {return rotGain;} 
+
+  /** Iteration counter **/
+  int iteration;
+
+  double rpFraction;
+
+protected:
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Transformation getPose(Vertex*v, Vertex* top);
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Rotation getRotation(Vertex*v, Vertex* top);
+
+  void recomputeTransformations(Vertex*v, Vertex* top);
+
+  void recomputeParameters(Vertex*v, Vertex* top);
+
+  void computePreconditioner();
+
+  void propagateErrors(bool usePreconditioner=false);
+
+  /** Computes the error of the constraint/edge e **/
+  double error(const Edge* e) const;
+
+  /** Computes the error of the constraint/edge e **/
+  double loopError(const Edge* e) const;
+
+  /** Computes the rotational error of the constraint/edge e **/
+  double loopRotationalError(const Edge* e) const;
+  
+  /** Conmputes the error of the constraint/edge e **/
+  double translationalError(const Edge* e) const;
+
+  /** Conmputes the error of the constraint/edge e **/
+  double rotationalError(const Edge* e) const;
+
+  double traslationalError(const Edge* e) const;
+  
+  /** Used to compute the learning rate lambda **/
+  double gamma[2];
+
+  /** The simplified version of the preconditioning matrix **/
+  struct PM_t{
+    double v [2];
+    inline double& operator[](int i){return v[i];}
+  };
+  typedef std::vector< PM_t > PMVector;
+  PMVector M;
+
+  /**cached maximum path length*/
+  int mpl;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxRotationalErrors;
+
+  /**history of rhe maximum rotational errors*, used when adaptiveRestart is enabled */
+  std::vector<double> maxTranslationalErrors;
+  double rotGain, trasGain;
+
+  /**callback invoked before starting the optimization of an individual constraint,
+   @param e: the constraint being optimized*/
+  virtual void onStepStart(Edge* e);
+
+  /**callback invoked after finishing the optimization of an individual constraint,
+   @param e: the constraint optimized*/
+  virtual void onStepFinished(Edge* e);
+
+  /**callback invoked before starting a full iteration,
+   @param i: the current iteration number*/
+  virtual void onIterationStart(int i);
+
+  /**callback invoked after finishing a full iteration,
+   @param i: the current iteration number*/
+
+  virtual void onIterationFinished(int iteration);
+
+  /**callback invoked before a restart of the optimizer
+   when the angular wraparound is detected*/
+  virtual void onRestartBegin();
+
+  /**callback invoked after a restart of the optimizer*/
+  virtual void onRestartDone();
+
+  /**callback for determining a termination condition,
+     it can be used by an external thread for stopping the optimizer while performing an iteration.
+     @returns true when the optimizer has to stop.*/
+  virtual bool isDone();
+  
+};
+
+}; //namespace AISNavigation
+
+#endif

corelib/src/toro3d/treeoptimizer3_iteration.cpp

@@ -0,0 +1,343 @@
+/**********************************************************************
+ *
+ * This source code is part of the Tree-based Network Optimizer (TORO)
+ *
+ * TORO Copyright (c) 2007 Giorgio Grisetti, Cyrill Stachniss, 
+ *                         Slawomir Grzonka and Wolfram Burgard
+ *
+ * TORO is licences under the Common Creative License,
+ * Attribution-NonCommercial-ShareAlike 3.0
+ *
+ * You are free:
+ *   - to Share - to copy, distribute and transmit the work
+ *   - to Remix - to adapt the work
+ *
+ * Under the following conditions:
+ *
+ *   - Attribution. You must attribute the work in the manner specified
+ *     by the author or licensor (but not in any way that suggests that
+ *     they endorse you or your use of the work).
+ *  
+ *   - Noncommercial. You may not use this work for commercial purposes.
+ *  
+ *   - Share Alike. If you alter, transform, or build upon this work,
+ *     you may distribute the resulting work only under the same or
+ *     similar license to this one.
+ *
+ * Any of the above conditions can be waived if you get permission
+ * from the copyright holder.  Nothing in this license impairs or
+ * restricts the author's moral rights.
+ *
+ * TORO is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+ * PURPOSE.  
+ **********************************************************************/
+
+#include "treeoptimizer3.hh"
+#include <fstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+#define DEBUG(i) \
+        if (verboseLevel>i) cerr
+
+//helper functions. Should I explain :-)?
+inline double max3( const double& a, const double& b, const double& c){
+  double m=a>b?a:b;
+  return m>c?m:c;
+}
+inline double min3( const double& a, const double& b, const double& c){
+  double m=a<b?a:b;
+  return m<c?m:c;
+}
+
+
+struct NodeInfo{
+  TreeOptimizer3::Vertex* n;
+  double translationalWeight;
+  double rotationalWeight;
+  int direction;
+  TreeOptimizer3::Transformation transformation;
+  TreeOptimizer3::Transformation parameters;
+  NodeInfo(TreeOptimizer3::Vertex* v=0, double tw=0, double rw=0, int dir=0,
+	   TreeOptimizer3::Transformation t=TreeOptimizer3::Transformation(0,0,0,0,0,0), 
+	   TreeOptimizer3::Parameters p=TreeOptimizer3::Transformation(0,0,0,0,0,0)){
+    n=v;
+    translationalWeight=tw;
+    rotationalWeight=rw;
+    direction=dir;
+    transformation=t;
+    parameters=p;
+  }
+};
+
+typedef std::vector<NodeInfo> NodeInfoVector;
+
+
+/********************************** Preconditioned and unpreconditioned error distribution ************************************/
+
+
+
+
+
+void TreeOptimizer3::computePreconditioner(){
+  for (uint i=0; i<M.size(); i++){
+    M[i][0]=0;
+    M[i][1]=0;
+  }
+  gamma[0] = gamma[1] = numeric_limits<double>::max();
+
+  int edgeCount=0;
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    if (! (edgeCount%1000))
+      DEBUG(1) << "m";
+
+    Edge* e=*it;
+    Transformation t=e->transformation;
+    InformationMatrix W=e->informationMatrix;
+    
+    Vertex* top=e->top;
+    for (int dir=0; dir<2; dir++){
+      Vertex* n = (dir==0)? e->v1 : e->v2;
+      while (n!=top){
+	uint i=n->id;
+	double rW=min3(W[0][0], W[1][1], W[2][2]);
+	double tW=min3(W[3][3], W[4][4], W[5][5]);
+	M[i][0]+=rW;
+	M[i][1]+=tW;
+	gamma[0]=gamma[0]<rW?gamma[0]:rW;
+	gamma[1]=gamma[1]<tW?gamma[1]:tW;
+	n=n->parent;
+      }
+    }
+
+  }
+  
+  if (verboseLevel>1){
+    for (uint i=0; i<M.size(); i++){
+      cerr << "M[" << i << "]=" << M[i][0] << " " << M[i][1] << endl;
+    }
+  }
+}
+
+
+void TreeOptimizer3::propagateErrors(bool usePreconditioner){
+  iteration++;
+  int edgeCount=0;
+  // this is the workspace for computing the paths without 
+  // bothering too much the memory allocation
+  static NodeInfoVector path;
+  path.resize(edges.size()+1);
+  static Rotation zero(0.,0.,0.);
+
+  onIterationStart(iteration);
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    if (! (edgeCount%1000))
+      DEBUG(1) << "c";
+
+    if (isDone())
+      return;
+    
+    Edge* e=*it;
+    Vertex* top=e->top;
+    Vertex* v1=e->v1;
+    Vertex* v2=e->v2;
+    int l=e->length;
+    onStepStart(e);
+    
+    recomputeTransformations(v1,top);
+    recomputeTransformations(v2,top);
+    DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+
+    //BEGIN: Path and weight computation 
+    int pc=0;
+    Vertex* aux=v1;
+    double totTW=0, totRW=0;
+    while(aux!=top){
+      int index=aux->id;
+      double tw=1./(double)l, rw=1./(double)l;
+      if (usePreconditioner){
+	tw=1./M[index][0]; 
+	rw=1./M[index][1];
+      }
+      totTW+=tw;
+      totRW+=rw;
+      path[pc++]=NodeInfo(aux,tw,rw,-1,aux->transformation, aux->parameters);
+      aux=aux->parent;
+    }
+    int topIndex=pc;
+    path[pc++]=NodeInfo(top,0.,0.,0, top->transformation, top->parameters);
+    pc=l;
+    aux=v2;
+    while(aux!=top){
+      int index=aux->id;
+      double tw=1./l, rw=1./l;
+      if (usePreconditioner){
+	tw=1./M[index][0]; 
+	rw=1./M[index][1];
+      }
+      totTW+=tw;
+      totRW+=rw;
+      path[pc--]=NodeInfo(aux,tw,rw,1,aux->transformation, aux->parameters);
+      aux=aux->parent;
+    }
+
+    //store the transformations relative to the top node
+    Transformation topTransformation=top->transformation;
+    Transformation topParameters=top->parameters;
+
+    //END: Path and weight computation
+    
+
+
+    //BEGIN: Rotational Error
+    Rotation r1=getRotation(v1, top);
+    Rotation r2=getRotation(v2, top);
+    Rotation re=e->transformation.rotation();
+    Rotation rR=r2.inverse()*(r1*re);
+
+    double rotationFactor=(usePreconditioner)?
+      sqrt(double(l))* min3(e->informationMatrix[0][0],
+			    e->informationMatrix[1][1], 
+			    e->informationMatrix[2][2])/
+      ( gamma[0]* (double)iteration ):
+      sqrt(double(l))*rotGain/(double)iteration;
+
+//     double rotationFactor=(usePreconditioner)?
+//       sqrt(double(l))*rotGain/
+//       ( gamma[0]* (double)iteration * min3(e->informationMatrix[0][0],
+// 					   e->informationMatrix[1][1], 
+// 					   e->informationMatrix[2][2])):
+//       sqrt(double(l))*rotGain/(double)iteration;
+
+    if (rotationFactor>1)
+      rotationFactor=1;
+
+    Rotation totalRotation = path[l].transformation.rotation() * rR * path[l].transformation.rotation().inverse();
+
+    Translation axis   = totalRotation.axis();
+    double angle=totalRotation.angle();
+
+    double cw=0;
+    for (int i= 1; i<=topIndex; i++){
+      cw+=path[i-1].rotationalWeight/totRW;
+      Rotation R=path[i].transformation.rotation();
+      Rotation B(axis, angle*cw*rotationFactor);
+      R= B*R;
+      path[i].transformation.setRotation(R);
+    }
+    for (int i= topIndex+1; i<=l; i++){
+      cw+=path[i].rotationalWeight/totRW;
+      Rotation R=path[i].transformation.rotation();
+      Rotation B(axis, angle*cw*rotationFactor);
+      R= B*R;
+      path[i].transformation.setRotation(R);
+    }
+
+    //recompute the parameters based on the transformation
+    for (int i=0; i<topIndex; i++){
+      Vertex* n=path[i].n;
+      n->parameters.setRotation(path[i+1].transformation.rotation().inverse()*path[i].transformation.rotation());
+    }
+    for (int i= topIndex+1; i<=l; i++){
+      Vertex* n=path[i].n;
+      n->parameters.setRotation(path[i-1].transformation.rotation().inverse()*path[i].transformation.rotation());
+    }
+
+    //END: Rotational Error
+
+
+    //now spread the parameters
+    recomputeTransformations(v1,top);
+    recomputeTransformations(v2,top);
+
+    //BEGIN: Translational Error
+    Translation topTranslation=top->transformation.translation();
+   
+    Transformation tr12=v1->transformation*e->transformation;
+    Translation tR=tr12.translation()-v2->transformation.translation();
+
+
+//     double translationFactor=(usePreconditioner)?
+//       trasGain*l/( gamma[1]* (double)iteration * min3(e->informationMatrix[3][3],
+// 						      e->informationMatrix[4][4], 
+// 						      e->informationMatrix[5][5])):
+//       trasGain*l/(double)iteration;
+
+    double translationFactor=(usePreconditioner)?
+      trasGain*l*min3(e->informationMatrix[3][3],
+		      e->informationMatrix[4][4], 
+		      e->informationMatrix[5][5])/( gamma[1]* (double)iteration):
+      trasGain*l/(double)iteration;
+
+    if (translationFactor>1)
+      translationFactor=1;
+    Translation dt=tR*translationFactor;
+
+    //left wing
+    double lcum=0;
+    for (int i=topIndex-1; i>=0; i--){
+      Vertex* n=path[i].n;
+      lcum-=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
+      double fraction=lcum;
+      Translation offset= dt*fraction;
+      Translation T=n->transformation.translation()+offset;
+      n->transformation.setTranslation(T);
+    }
+    //right wing
+    
+    double rcum=0;
+    for (int i=topIndex+1; i<=l; i++){
+      Vertex* n=path[i].n;
+      rcum+=(usePreconditioner) ? path[i].translationalWeight/totTW :  1./(double)l;
+      double fraction=rcum;
+      Translation offset= dt*fraction;
+      Translation T=n->transformation.translation()+offset;
+      n->transformation.setTranslation(T);
+    }
+    assert(fabs(lcum+rcum)-1<1e-6);
+
+    recomputeParameters(v1, top);
+    recomputeParameters(v2, top);
+
+    //END: Translational Error
+
+    onStepFinished(e);
+
+    if (verboseLevel>2){
+      Rotation newRotResidual=v2->transformation.rotation().inverse()*(v1->transformation.rotation()*re);
+      Translation newRotResidualAxis=newRotResidual.axis();
+      double      newRotResidualAngle=newRotResidual.angle();
+      Translation rotResidualAxis=rR.axis();
+      double      rotResidualAngle=rR.angle();
+      Translation newTransResidual=(v1->transformation*e->transformation).translation()-v2->transformation.translation();
+
+      cerr << "RotationalFraction:  " << rotationFactor << endl;
+      cerr << "Rotational residual: " 
+	   << " axis " << rotResidualAxis.x()  << "\t" << rotResidualAxis.y()  << "\t" << rotResidualAxis.z() << " --> "
+	   << "   ->  " << newRotResidualAxis.x()  << "\t" << newRotResidualAxis.y()  << "\t" << newRotResidualAxis.z()  << endl;
+      cerr << " angle " << rotResidualAngle  << "\t" << newRotResidualAngle  << endl;
+      
+      cerr << "Translational Fraction:  " << translationFactor << endl;
+      cerr << "Translational Residual" << endl;
+      cerr << "    " << tR.x()  << "\t" << tR.y()  << "\t" << tR.z()  << endl;
+      cerr << "    " << newTransResidual.x()  << "\t" << newTransResidual.y()  << "\t" << newTransResidual.z()  << endl;
+    }
+    
+    if (verboseLevel>101){
+      char filename [1000];
+      sprintf(filename, "po-%02d-%03d-%03d-.dat", iteration, v1->id, v2->id);
+      recomputeAllTransformations();
+      saveGnuplot(filename);
+    }
+  }
+  onIterationFinished(iteration);
+}
+
+};//namespace AISNavigation