Camera refactoring (#315)

* Camera Refactoring part 1 (Mac OS X)

* fixed camera*** -> Camera***

* Fixed build for cameras Zed/RealSense/RealSense2

* increased version to 0.17.7

* fixed build for cameras K4W2 and FlyCapture2

corelib/src/optimizer/OptimizerCVSBA.cpp

@@ -0,0 +1,200 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "rtabmap/core/Graph.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <set>
+
+#include <rtabmap/core/optimizer/OptimizerCVSBA.h>
+
+#ifdef RTABMAP_CVSBA
+#include <cvsba/cvsba.h>
+#include "rtabmap/core/util3d_motion_estimation.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/util3d_correspondences.h"
+#endif
+
+namespace rtabmap {
+
+bool OptimizerCVSBA::available()
+{
+#ifdef RTABMAP_CVSBA
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::map<int, Transform> OptimizerCVSBA::optimizeBA(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & links,
+		const std::map<int, CameraModel> & models,
+		std::map<int, cv::Point3f> & points3DMap,
+		const std::map<int, std::map<int, cv::Point3f> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
+		std::set<int> * outliers)
+{
+#ifdef RTABMAP_CVSBA
+	// run sba optimization
+	cvsba::Sba sba;
+
+	// change params if desired
+	cvsba::Sba::Params params ;
+	params.type = cvsba::Sba::MOTIONSTRUCTURE;
+	params.iterations = this->iterations();
+	params.minError = this->epsilon();
+	params.fixedIntrinsics = 5;
+	params.fixedDistortion = 5; // updated below
+	params.verbose=ULogger::level() <= ULogger::kInfo;
+	sba.setParams(params);
+
+	std::vector<cv::Mat> cameraMatrix(poses.size()); //nframes
+	std::vector<cv::Mat> R(poses.size()); //nframes
+	std::vector<cv::Mat> T(poses.size()); //nframes
+	std::vector<cv::Mat> distCoeffs(poses.size()); //nframes
+	std::map<int, int> frameIdToIndex;
+	int oi=0;
+	for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
+	{
+		// Get camera model
+		std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
+		UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
+
+		frameIdToIndex.insert(std::make_pair(iter->first, oi));
+
+		cameraMatrix[oi] = iterModel->second.K();
+		if(iterModel->second.D().cols != 5)
+		{
+			distCoeffs[oi] = cv::Mat::zeros(1, 5, CV_64FC1);
+			UWARN("Camera model %d: Distortion coefficients are not 5, setting all them to 0 (assuming no distortion)", iter->first);
+		}
+		else
+		{
+			distCoeffs[oi] = iterModel->second.D();
+		}
+
+		Transform t = (iter->second * iterModel->second.localTransform()).inverse();
+
+		R[oi] = (cv::Mat_<double>(3,3) <<
+				(double)t.r11(), (double)t.r12(), (double)t.r13(),
+				(double)t.r21(), (double)t.r22(), (double)t.r23(),
+				(double)t.r31(), (double)t.r32(), (double)t.r33());
+		T[oi] = (cv::Mat_<double>(1,3) << (double)t.x(), (double)t.y(), (double)t.z());
+		++oi;
+
+		UDEBUG("Pose %d = %s", iter->first, t.prettyPrint().c_str());
+	}
+	cameraMatrix.resize(oi);
+	R.resize(oi);
+	T.resize(oi);
+	distCoeffs.resize(oi);
+
+	UDEBUG("points=%d frames=%d", (int)points3DMap.size(), (int)poses.size());
+	std::vector<cv::Point3f> points(points3DMap.size()); //npoints
+	std::vector<std::vector<cv::Point2f> >  imagePoints(poses.size()); //nframes -> npoints
+	std::vector<std::vector<int> > visibility(poses.size()); //nframes -> npoints
+	for(unsigned int i=0; i<poses.size(); ++i)
+	{
+		imagePoints[i].resize(wordReferences.size(), cv::Point2f(std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()));
+		visibility[i].resize(wordReferences.size(), 0);
+	}
+	int i=0;
+	for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
+	{
+		points[i] = kter->second;
+
+		std::map<int, std::map<int, cv::Point3f> >::const_iterator iter = wordReferences.find(kter->first);
+		if(iter != wordReferences.end())
+		{
+			for(std::map<int, cv::Point3f>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
+			{
+				if(frameIdToIndex.find(jter->first) != frameIdToIndex.end())
+				{
+					imagePoints[frameIdToIndex.at(jter->first)][i] = cv::Point2f(jter->second.x, jter->second.y);
+					visibility[frameIdToIndex.at(jter->first)][i] = 1;
+				}
+			}
+		}
+		++i;
+	}
+
+	// SBA
+	try
+	{
+		sba.run( points, imagePoints, visibility, cameraMatrix, R, T, distCoeffs);
+	}
+	catch(cv::Exception & e)
+	{
+		UERROR("Running SBA... error! %s", e.what());
+		return std::map<int, Transform>();
+	}
+
+	//update poses
+	i=0;
+	std::map<int, Transform> newPoses = poses;
+	for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
+	{
+		Transform t(R[i].at<double>(0,0), R[i].at<double>(0,1), R[i].at<double>(0,2), T[i].at<double>(0),
+					R[i].at<double>(1,0), R[i].at<double>(1,1), R[i].at<double>(1,2), T[i].at<double>(1),
+					R[i].at<double>(2,0), R[i].at<double>(2,1), R[i].at<double>(2,2), T[i].at<double>(2));
+
+		UDEBUG("New pose %d = %s", iter->first, t.prettyPrint().c_str());
+
+		if(this->isSlam2d())
+		{
+			t = (models.at(iter->first).localTransform() * t).inverse();
+			t = iter->second.inverse() * t;
+			iter->second *= t.to3DoF();
+		}
+		else
+		{
+			iter->second = (models.at(iter->first).localTransform() * t).inverse();
+		}
+
+		++i;
+	}
+
+	//update 3D points
+	i=0;
+	for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
+	{
+		kter->second = points[i++];
+	}
+
+	return newPoses;
+
+#else
+	UERROR("RTAB-Map is not built with cvsba!");
+	return std::map<int, Transform>();
+#endif
+}
+
+} /* namespace rtabmap */

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -0,0 +1,454 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "rtabmap/core/Graph.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <set>
+
+#include <rtabmap/core/optimizer/OptimizerGTSAM.h>
+
+#ifdef RTABMAP_GTSAM
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/inference/Key.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/nonlinear/NonlinearOptimizer.h>
+#include <gtsam/nonlinear/Marginals.h>
+#include <gtsam/nonlinear/Values.h>
+
+#ifdef RTABMAP_VERTIGO
+#include "vertigo/gtsam/betweenFactorMaxMix.h"
+#include "vertigo/gtsam/betweenFactorSwitchable.h"
+#include "vertigo/gtsam/switchVariableLinear.h"
+#include "vertigo/gtsam/switchVariableSigmoid.h"
+#endif
+#endif // end RTABMAP_GTSAM
+
+namespace rtabmap {
+
+bool OptimizerGTSAM::available()
+{
+#ifdef RTABMAP_GTSAM
+	return true;
+#else
+	return false;
+#endif
+}
+
+void OptimizerGTSAM::parseParameters(const ParametersMap & parameters)
+{
+	Optimizer::parseParameters(parameters);
+	Parameters::parse(parameters, Parameters::kGTSAMOptimizer(), optimizer_);
+}
+
+std::map<int, Transform> OptimizerGTSAM::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
+		std::list<std::map<int, Transform> > * intermediateGraphes,
+		double * finalError,
+		int * iterationsDone)
+{
+	outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
+	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_GTSAM
+
+#ifndef RTABMAP_VERTIGO
+	if(this->isRobust())
+	{
+		UWARN("Vertigo robust optimization is not available! Robust optimization is now disabled.");
+		setRobust(false);
+	}
+#endif
+
+	UDEBUG("Optimizing graph...");
+	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
+	{
+		gtsam::NonlinearFactorGraph graph;
+
+		// detect if there is a global pose prior set, if so remove rootId
+		if(!priorsIgnored())
+		{
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+			{
+				if(iter->second.from() == iter->second.to())
+				{
+					rootId = 0;
+					break;
+				}
+			}
+		}
+
+		//prior first pose
+		if(rootId != 0)
+		{
+			UASSERT(uContains(poses, rootId));
+			const Transform & initialPose = poses.at(rootId);
+			if(isSlam2d())
+			{
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(gtsam::Vector3(0.01, 0.01, 0.01));
+				graph.add(gtsam::PriorFactor<gtsam::Pose2>(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise));
+			}
+			else
+			{
+				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
+				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
+			}
+		}
+
+		UDEBUG("fill poses to gtsam...");
+		gtsam::Values initialEstimate;
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			UASSERT(!iter->second.isNull());
+			if(isSlam2d())
+			{
+				initialEstimate.insert(iter->first, gtsam::Pose2(iter->second.x(), iter->second.y(), iter->second.theta()));
+			}
+			else
+			{
+				initialEstimate.insert(iter->first, gtsam::Pose3(iter->second.toEigen4d()));
+			}
+		}
+
+		UDEBUG("fill edges to gtsam...");
+		int switchCounter = poses.rbegin()->first+1;
+		for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			int id1 = iter->second.from();
+			int id2 = iter->second.to();
+			UASSERT(!iter->second.transform().isNull());
+			if(id1 == id2)
+			{
+				if(!priorsIgnored())
+				{
+					if(isSlam2d())
+					{
+						Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+							information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+							information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+							information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+							information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+							information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+							information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+							information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+							information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
+						}
+
+						gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+						graph.add(gtsam::PriorFactor<gtsam::Pose2>(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
+					else
+					{
+						Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+						if(!isCovarianceIgnored())
+						{
+							memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+						}
+
+						Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
+						mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
+						mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
+						mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
+						mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+						gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
+
+						graph.add(gtsam::PriorFactor<gtsam::Pose3>(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
+				}
+			}
+			else
+			{
+#ifdef RTABMAP_VERTIGO
+				if(this->isRobust() &&
+				   iter->second.type() != Link::kNeighbor &&
+				   iter->second.type() != Link::kNeighborMerged &&
+				   iter->second.type() != Link::kPosePrior)
+				{
+					// create new switch variable
+					// Sunderhauf IROS 2012:
+					// "Since it is reasonable to initially accept all loop closure constraints,
+					//  a proper and convenient initial value for all switch variables would be
+					//  sij = 1 when using the linear switch function"
+					double prior = 1.0;
+					initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior));
+
+					// create switch prior factor
+					// "If the front-end is not able to assign sound individual values
+					//  for Ξij , it is save to set all Ξij = 1, since this value is close
+					//  to the individual optimal choice of Ξij for a large range of
+					//  outliers."
+					gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0));
+					graph.add(gtsam::PriorFactor<vertigo::SwitchVariableLinear> (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel));
+				}
+#endif
+
+				if(isSlam2d())
+				{
+					Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
+						information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
+						information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
+						information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
+						information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
+						information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
+						information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
+						information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
+						information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
+					}
+					gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information);
+
+#ifdef RTABMAP_VERTIGO
+					if(this->isRobust() &&
+					   iter->second.type()!=Link::kNeighbor &&
+					   iter->second.type() != Link::kNeighborMerged)
+					{
+						// create switchable edge factor
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose2>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
+					else
+#endif
+					{
+						graph.add(gtsam::BetweenFactor<gtsam::Pose2>(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model));
+					}
+				}
+				else
+				{
+					Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
+					if(!isCovarianceIgnored())
+					{
+						memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+					}
+
+					Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
+					mgtsam.block(0,0,3,3) = information.block(3,3,3,3); // cov rotation
+					mgtsam.block(3,3,3,3) = information.block(0,0,3,3); // cov translation
+					mgtsam.block(0,3,3,3) = information.block(0,3,3,3); // off diagonal
+					mgtsam.block(3,0,3,3) = information.block(3,0,3,3); // off diagonal
+					gtsam::SharedNoiseModel model = gtsam::noiseModel::Gaussian::Information(mgtsam);
+
+#ifdef RTABMAP_VERTIGO
+					if(this->isRobust() &&
+					   iter->second.type() != Link::kNeighbor &&
+					   iter->second.type() != Link::kNeighborMerged &&
+					   iter->second.type() != Link::kPosePrior)
+					{
+						// create switchable edge factor
+						graph.add(vertigo::BetweenFactorSwitchableLinear<gtsam::Pose3>(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
+					else
+#endif
+					{
+						graph.add(gtsam::BetweenFactor<gtsam::Pose3>(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model));
+					}
+				}
+			}
+		}
+
+		UDEBUG("create optimizer");
+		gtsam::NonlinearOptimizer * optimizer;
+
+		if(optimizer_ == 2)
+		{
+			gtsam::DoglegParams parameters;
+			parameters.relativeErrorTol = epsilon();
+			parameters.maxIterations = iterations();
+			optimizer = new gtsam::DoglegOptimizer(graph, initialEstimate, parameters);
+		}
+		else if(optimizer_ == 1)
+		{
+			gtsam::GaussNewtonParams parameters;
+			parameters.relativeErrorTol = epsilon();
+			parameters.maxIterations = iterations();
+			optimizer = new gtsam::GaussNewtonOptimizer(graph, initialEstimate, parameters);
+		}
+		else
+		{
+			gtsam::LevenbergMarquardtParams parameters;
+			parameters.relativeErrorTol = epsilon();
+			parameters.maxIterations = iterations();
+			optimizer = new gtsam::LevenbergMarquardtOptimizer(graph, initialEstimate, parameters);
+		}
+
+		UINFO("GTSAM optimizing begin (max iterations=%d, robust=%d)", iterations(), isRobust()?1:0);
+		UTimer timer;
+		int it = 0;
+		double lastError = optimizer->error();
+		for(int i=0; i<iterations(); ++i)
+		{
+			if(intermediateGraphes && i > 0)
+			{
+				std::map<int, Transform> tmpPoses;
+				for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+				{
+					if(iter->value.dim() > 1)
+					{
+						if(isSlam2d())
+						{
+							gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
+							tmpPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
+						}
+						else
+						{
+							gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
+							tmpPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
+						}
+					}
+				}
+				intermediateGraphes->push_back(tmpPoses);
+			}
+			try
+			{
+				optimizer->iterate();
+				++it;
+			}
+			catch(gtsam::IndeterminantLinearSystemException & e)
+			{
+				UERROR("GTSAM exception caught: %s", e.what());
+				delete optimizer;
+				return optimizedPoses;
+			}
+
+			// early stop condition
+			double error = optimizer->error();
+			UDEBUG("iteration %d error =%f", i+1, error);
+			double errorDelta = lastError - error;
+			if(i>0 && errorDelta < this->epsilon())
+			{
+				if(errorDelta < 0)
+				{
+					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+				}
+				else
+				{
+					UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+					break;
+				}
+			}
+			else if(i==0 && error < this->epsilon())
+			{
+				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+				break;
+			}
+			lastError = error;
+		}
+		if(finalError)
+		{
+			*finalError = lastError;
+		}
+		if(iterationsDone)
+		{
+			*iterationsDone = it;
+		}
+		UINFO("GTSAM optimizing end (%d iterations done, error=%f (initial=%f final=%f), time=%f s)",
+				optimizer->iterations(), optimizer->error(), graph.error(initialEstimate), graph.error(optimizer->values()), timer.ticks());
+
+		gtsam::Marginals marginals(graph, optimizer->values());
+		for(gtsam::Values::const_iterator iter=optimizer->values().begin(); iter!=optimizer->values().end(); ++iter)
+		{
+			if(iter->value.dim() > 1)
+			{
+				if(isSlam2d())
+				{
+					gtsam::Pose2 p = iter->value.cast<gtsam::Pose2>();
+					optimizedPoses.insert(std::make_pair((int)iter->key, Transform(p.x(), p.y(), p.theta())));
+				}
+				else
+				{
+					gtsam::Pose3 p = iter->value.cast<gtsam::Pose3>();
+					optimizedPoses.insert(std::make_pair((int)iter->key, Transform::fromEigen4d(p.matrix())));
+				}
+			}
+		}
+
+		// compute marginals
+		try {
+			UTimer t;
+			gtsam::Marginals marginals(graph, optimizer->values());
+			gtsam::Matrix info = marginals.marginalCovariance(optimizer->values().rbegin()->key);
+			UINFO("Computed marginals = %fs (key=%d)", t.ticks(), optimizer->values().rbegin()->key);
+			if(isSlam2d())
+			{
+				UASSERT(info.cols() == 3 && info.cols() == 3);
+				outputCovariance.at<double>(0,0) = info(0,0); // x-x
+				outputCovariance.at<double>(0,1) = info(0,1); // x-y
+				outputCovariance.at<double>(0,5) = info(0,2); // x-theta
+				outputCovariance.at<double>(1,0) = info(1,0); // y-x
+				outputCovariance.at<double>(1,1) = info(1,1); // y-y
+				outputCovariance.at<double>(1,5) = info(1,2); // y-theta
+				outputCovariance.at<double>(5,0) = info(2,0); // theta-x
+				outputCovariance.at<double>(5,1) = info(2,1); // theta-y
+				outputCovariance.at<double>(5,5) = info(2,2); // theta-theta
+			}
+			else
+			{
+				UASSERT(info.cols() == 6 && info.cols() == 6);
+				Eigen::Matrix<double, 6, 6> mgtsam = Eigen::Matrix<double, 6, 6>::Identity();
+				mgtsam.block(3,3,3,3) = info.block(0,0,3,3); // cov rotation
+				mgtsam.block(0,0,3,3) = info.block(3,3,3,3); // cov translation
+				mgtsam.block(0,3,3,3) = info.block(0,3,3,3); // off diagonal
+				mgtsam.block(3,0,3,3) = info.block(3,0,3,3); // off diagonal
+				memcpy(outputCovariance.data, mgtsam.data(), outputCovariance.total()*sizeof(double));
+			}
+		} catch(std::exception& e) {
+			cout << e.what() << endl;
+		}
+
+		delete optimizer;
+	}
+	else if(poses.size() == 1 || iterations() <= 0)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UWARN("This method should be called at least with 1 pose!");
+	}
+	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with GTSAM support!");
+#endif
+	return optimizedPoses;
+}
+
+} /* namespace rtabmap */

corelib/src/optimizer/OptimizerTORO.cpp

@@ -0,0 +1,499 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#include "rtabmap/core/Graph.h"
+
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UMath.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <set>
+
+#include <rtabmap/core/optimizer/OptimizerTORO.h>
+
+#ifdef RTABMAP_TORO
+#include "toro3d/treeoptimizer3.hh"
+#include "toro3d/treeoptimizer2.hh"
+#endif
+
+namespace rtabmap {
+
+bool OptimizerTORO::available()
+{
+#ifdef RTABMAP_TORO
+	return true;
+#else
+	return false;
+#endif
+}
+
+std::map<int, Transform> OptimizerTORO::optimize(
+		int rootId,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints,
+		cv::Mat & outputCovariance,
+		std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
+		double * finalError,
+		int * iterationsDone)
+{
+	outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
+	std::map<int, Transform> optimizedPoses;
+#ifdef RTABMAP_TORO
+	UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
+	if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
+	{
+		// Apply TORO optimization
+		AISNavigation::TreeOptimizer2 pg2;
+		AISNavigation::TreeOptimizer3 pg3;
+		pg2.verboseLevel = 0;
+		pg3.verboseLevel = 0;
+
+		UDEBUG("fill poses to TORO...");
+		if(isSlam2d())
+		{
+			for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+			{
+				UASSERT(!iter->second.isNull());
+				AISNavigation::TreePoseGraph2::Pose p(iter->second.x(), iter->second.y(), iter->second.theta());
+				AISNavigation::TreePoseGraph2::Vertex* v = pg2.addVertex(iter->first, p);
+				UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
+			}
+		}
+		else
+		{
+			for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+			{
+				UASSERT(!iter->second.isNull());
+				float x,y,z, roll,pitch,yaw;
+				iter->second.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+				AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
+				AISNavigation::TreePoseGraph3::Vertex* v = pg3.addVertex(iter->first, p);
+				UASSERT_MSG(v != 0, uFormat("cannot insert vertex %d!?", iter->first).c_str());
+				v->transformation=AISNavigation::TreePoseGraph3::Transformation(p);
+			}
+
+		}
+
+		UDEBUG("fill edges to TORO...");
+		if(isSlam2d())
+		{
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+			{
+				UASSERT(!iter->second.transform().isNull());
+				AISNavigation::TreePoseGraph2::Pose p(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta());
+				AISNavigation::TreePoseGraph2::InformationMatrix inf;
+				//Identity:
+				if(isCovarianceIgnored())
+				{
+					inf.values[0][0] = 1.0; inf.values[0][1] = 0.0; inf.values[0][2] = 0.0; // x
+					inf.values[1][0] = 0.0; inf.values[1][1] = 1.0; inf.values[1][2] = 0.0; // y
+					inf.values[2][0] = 0.0; inf.values[2][1] = 0.0; inf.values[2][2] = 1.0; // theta/yaw
+				}
+				else
+				{
+					inf.values[0][0] = iter->second.infMatrix().at<double>(0,0); // x-x
+					inf.values[0][1] = iter->second.infMatrix().at<double>(0,1); // x-y
+					inf.values[0][2] = iter->second.infMatrix().at<double>(0,5); // x-theta
+					inf.values[1][0] = iter->second.infMatrix().at<double>(1,0); // y-x
+					inf.values[1][1] = iter->second.infMatrix().at<double>(1,1); // y-y
+					inf.values[1][2] = iter->second.infMatrix().at<double>(1,5); // y-theta
+					inf.values[2][0] = iter->second.infMatrix().at<double>(5,0); // theta-x
+					inf.values[2][1] = iter->second.infMatrix().at<double>(5,1); // theta-y
+					inf.values[2][2] = iter->second.infMatrix().at<double>(5,5); // theta-theta
+				}
+
+				int id1 = iter->second.from();
+				int id2 = iter->second.to();
+				if(id1 != id2)
+				{
+					AISNavigation::TreePoseGraph2::Vertex* v1=pg2.vertex(id1);
+					AISNavigation::TreePoseGraph2::Vertex* v2=pg2.vertex(id2);
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					AISNavigation::TreePoseGraph2::Transformation t(p);
+					if (!pg2.addEdge(v1, v2, t, inf))
+					{
+						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
+					}
+				}
+				//else // not supporting pose prior
+			}
+		}
+		else
+		{
+			for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+			{
+				UASSERT(!iter->second.transform().isNull());
+				float x,y,z, roll,pitch,yaw;
+				iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+
+				AISNavigation::TreePoseGraph3::Pose p(x, y, z, roll, pitch, yaw);
+				AISNavigation::TreePoseGraph3::InformationMatrix inf = DMatrix<double>::I(6);
+				if(!isCovarianceIgnored())
+				{
+					memcpy(inf[0], iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
+				}
+
+				int id1 = iter->second.from();
+				int id2 = iter->second.to();
+				if(id1 != id2)
+				{
+					AISNavigation::TreePoseGraph3::Vertex* v1=pg3.vertex(id1);
+					AISNavigation::TreePoseGraph3::Vertex* v2=pg3.vertex(id2);
+					UASSERT(v1 != 0);
+					UASSERT(v2 != 0);
+					AISNavigation::TreePoseGraph3::Transformation t(p);
+					if (!pg3.addEdge(v1, v2, t, inf))
+					{
+						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
+					}
+				}
+				//else // not supporting pose prior
+			}
+		}
+		UDEBUG("buildMST... root=%d", rootId);
+		UASSERT(uContains(poses, rootId));
+		if(isSlam2d())
+		{
+			pg2.buildMST(rootId); // pg.buildSimpleTree();
+			//UDEBUG("initializeOnTree()");
+			//pg2.initializeOnTree();
+			UDEBUG("initializeTreeParameters()");
+			pg2.initializeTreeParameters();
+			UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
+				   "TORO is not able to find the root of the graph!)");
+			pg2.initializeOptimization();
+		}
+		else
+		{
+			pg3.buildMST(rootId); // pg.buildSimpleTree();
+			//UDEBUG("initializeOnTree()");
+			//pg3.initializeOnTree();
+			UDEBUG("initializeTreeParameters()");
+			pg3.initializeTreeParameters();
+			UDEBUG("Building TORO tree... (if a crash happens just after this msg, "
+				   "TORO is not able to find the root of the graph!)");
+			pg3.initializeOptimization();
+		}
+
+		UINFO("Initial error = %f", pg2.error());
+		UINFO("TORO optimizing begin (iterations=%d)", iterations());
+		double lastError = 0;
+		int i=0;
+		UTimer timer;
+		for (; i<iterations(); i++)
+		{
+			if(intermediateGraphes && i>0)
+			{
+				std::map<int, Transform> tmpPoses;
+				if(isSlam2d())
+				{
+					for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+					{
+						AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
+						float roll, pitch, yaw;
+						iter->second.getEulerAngles(roll, pitch, yaw);
+						Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
+
+						UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+						tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+					}
+				}
+				else
+				{
+					for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+					{
+						AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
+						AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
+						Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
+
+						UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+						tmpPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+					}
+				}
+				intermediateGraphes->push_back(tmpPoses);
+			}
+
+			double error = 0;
+			if(isSlam2d())
+			{
+				pg2.iterate();
+
+				// compute the error and dump it
+				error=pg2.error();
+				UDEBUG("iteration %d global error=%f error/constraint=%f", i, error, error/pg2.edges.size());
+			}
+			else
+			{
+				pg3.iterate();
+
+				// compute the error and dump it
+				double mte, mre, are, ate;
+				error=pg3.error(&mre, &mte, &are, &ate);
+				UDEBUG("i %d RotGain=%f global error=%f error/constraint=%f",
+						i, pg3.getRotGain(), error, error/pg3.edges.size());
+			}
+
+			// early stop condition
+			double errorDelta = lastError - error;
+			if(i>0 && errorDelta < this->epsilon())
+			{
+				if(errorDelta < 0)
+				{
+					UDEBUG("Negative improvement?! Ignore and continue optimizing... (%f < %f)", errorDelta, this->epsilon());
+				}
+				else
+				{
+					UINFO("Stop optimizing, not enough improvement (%f < %f)", errorDelta, this->epsilon());
+					break;
+				}
+			}
+			else if(i==0 && error < this->epsilon())
+			{
+				UINFO("Stop optimizing, error is already under epsilon (%f < %f)", error, this->epsilon());
+				break;
+			}
+			lastError = error;
+		}
+		if(finalError)
+		{
+			*finalError = lastError;
+		}
+		if(iterationsDone)
+		{
+			*iterationsDone = i;
+		}
+		UINFO("TORO optimizing end (%d iterations done, error=%f, time = %f s)", i, lastError, timer.ticks());
+
+		if(isSlam2d())
+		{
+			for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+			{
+				AISNavigation::TreePoseGraph2::Vertex* v=pg2.vertex(iter->first);
+				float roll, pitch, yaw;
+				iter->second.getEulerAngles(roll, pitch, yaw);
+				Transform newPose(v->pose.x(), v->pose.y(), iter->second.z(), roll, pitch, v->pose.theta());
+
+				UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+				optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+			}
+		}
+		else
+		{
+			for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+			{
+				AISNavigation::TreePoseGraph3::Vertex* v=pg3.vertex(iter->first);
+				AISNavigation::TreePoseGraph3::Pose pose=v->transformation.toPoseType();
+				Transform newPose(pose.x(), pose.y(), pose.z(), pose.roll(), pose.pitch(), pose.yaw());
+
+				UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
+				optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
+			}
+		}
+
+		// TORO doesn't compute marginals...
+	}
+	else if(poses.size() == 1 || iterations() <= 0)
+	{
+		optimizedPoses = poses;
+	}
+	else
+	{
+		UWARN("This method should be called at least with 1 pose!");
+	}
+	UDEBUG("Optimizing graph...end!");
+#else
+	UERROR("Not built with TORO support!");
+#endif
+	return optimizedPoses;
+}
+
+bool OptimizerTORO::saveGraph(
+		const std::string & fileName,
+		const std::map<int, Transform> & poses,
+		const std::multimap<int, Link> & edgeConstraints)
+{
+	FILE * file = 0;
+
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "w");
+#else
+	file = fopen(fileName.c_str(), "w");
+#endif
+
+	if(file)
+	{
+		// VERTEX3 id x y z phi theta psi
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		{
+			float x,y,z, yaw,pitch,roll;
+			iter->second.getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
+			fprintf(file, "VERTEX3 %d %f %f %f %f %f %f\n",
+					iter->first,
+					x,
+					y,
+					z,
+					roll,
+					pitch,
+					yaw);
+		}
+
+		//EDGE3 observed_vertex_id observing_vertex_id x y z roll pitch yaw inf_11 inf_12 .. inf_16 inf_22 .. inf_66
+		for(std::multimap<int, Link>::const_iterator iter = edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
+		{
+			float x,y,z, yaw,pitch,roll;
+			iter->second.transform().getTranslationAndEulerAngles(x,y,z, roll, pitch, yaw);
+			fprintf(file, "EDGE3 %d %d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n",
+					iter->second.from(),
+					iter->second.to(),
+					x,
+					y,
+					z,
+					roll,
+					pitch,
+					yaw,
+					iter->second.infMatrix().at<double>(0,0),
+					iter->second.infMatrix().at<double>(0,1),
+					iter->second.infMatrix().at<double>(0,2),
+					iter->second.infMatrix().at<double>(0,3),
+					iter->second.infMatrix().at<double>(0,4),
+					iter->second.infMatrix().at<double>(0,5),
+					iter->second.infMatrix().at<double>(1,1),
+					iter->second.infMatrix().at<double>(1,2),
+					iter->second.infMatrix().at<double>(1,3),
+					iter->second.infMatrix().at<double>(1,4),
+					iter->second.infMatrix().at<double>(1,5),
+					iter->second.infMatrix().at<double>(2,2),
+					iter->second.infMatrix().at<double>(2,3),
+					iter->second.infMatrix().at<double>(2,4),
+					iter->second.infMatrix().at<double>(2,5),
+					iter->second.infMatrix().at<double>(3,3),
+					iter->second.infMatrix().at<double>(3,4),
+					iter->second.infMatrix().at<double>(3,5),
+					iter->second.infMatrix().at<double>(4,4),
+					iter->second.infMatrix().at<double>(4,5),
+					iter->second.infMatrix().at<double>(5,5));
+		}
+		UINFO("Graph saved to %s", fileName.c_str());
+		fclose(file);
+	}
+	else
+	{
+		UERROR("Cannot save to file %s", fileName.c_str());
+		return false;
+	}
+	return true;
+}
+
+bool OptimizerTORO::loadGraph(
+		const std::string & fileName,
+		std::map<int, Transform> & poses,
+		std::multimap<int, Link> & edgeConstraints)
+{
+	FILE * file = 0;
+#ifdef _MSC_VER
+	fopen_s(&file, fileName.c_str(), "r");
+#else
+	file = fopen(fileName.c_str(), "r");
+#endif
+
+	if(file)
+	{
+		char line[400];
+		while ( fgets (line , 400 , file) != NULL )
+		{
+			std::vector<std::string> strList = uListToVector(uSplit(uReplaceChar(line, '\n', ' '), ' '));
+			if(strList.size() == 8)
+			{
+				//VERTEX3
+				int id = atoi(strList[1].c_str());
+				float x = uStr2Float(strList[2]);
+				float y = uStr2Float(strList[3]);
+				float z = uStr2Float(strList[4]);
+				float roll = uStr2Float(strList[5]);
+				float pitch = uStr2Float(strList[6]);
+				float yaw = uStr2Float(strList[7]);
+				Transform pose(x, y, z, roll, pitch, yaw);
+				if(poses.find(id) == poses.end())
+				{
+					poses.insert(std::make_pair(id, pose));
+				}
+				else
+				{
+					UFATAL("Pose %d already added", id);
+				}
+			}
+			else if(strList.size() == 30)
+			{
+				//EDGE3
+				int idFrom = atoi(strList[1].c_str());
+				int idTo = atoi(strList[2].c_str());
+				float x = uStr2Float(strList[3]);
+				float y = uStr2Float(strList[4]);
+				float z = uStr2Float(strList[5]);
+				float roll = uStr2Float(strList[6]);
+				float pitch = uStr2Float(strList[7]);
+				float yaw = uStr2Float(strList[8]);
+				cv::Mat informationMatrix(6,6,CV_64FC1);
+				informationMatrix.at<double>(3,3) = uStr2Float(strList[9]);
+				informationMatrix.at<double>(4,4) = uStr2Float(strList[15]);
+				informationMatrix.at<double>(5,5) = uStr2Float(strList[20]);
+				UASSERT_MSG(informationMatrix.at<double>(3,3) > 0.0 && informationMatrix.at<double>(4,4) > 0.0 && informationMatrix.at<double>(5,5) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+				informationMatrix.at<double>(0,0) = uStr2Float(strList[24]);
+				informationMatrix.at<double>(1,1) = uStr2Float(strList[27]);
+				informationMatrix.at<double>(2,2) = uStr2Float(strList[29]);
+				UASSERT_MSG(informationMatrix.at<double>(0,0) > 0.0 && informationMatrix.at<double>(1,1) > 0.0 && informationMatrix.at<double>(2,2) > 0.0, uFormat("Information matrix should not be null! line=\"%s\"", line).c_str());
+				Transform transform(x, y, z, roll, pitch, yaw);
+				if(poses.find(idFrom) != poses.end() && poses.find(idTo) != poses.end())
+				{
+					//Link type is unknown
+					Link link(idFrom, idTo, Link::kUndef, transform, informationMatrix);
+					edgeConstraints.insert(std::pair<int, Link>(idFrom, link));
+				}
+				else
+				{
+					UFATAL("Referred poses from the link not exist!");
+				}
+			}
+			else if(strList.size())
+			{
+				UFATAL("Error parsing graph file %s on line \"%s\" (strList.size()=%d)", fileName.c_str(), line, (int)strList.size());
+			}
+		}
+
+		UINFO("Graph loaded from %s", fileName.c_str());
+		fclose(file);
+	}
+	else
+	{
+		UERROR("Cannot open file %s", fileName.c_str());
+		return false;
+	}
+	return true;
+}
+
+} /* namespace rtabmap */

corelib/src/optimizer/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/optimizer/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/optimizer/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/optimizer/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 0;
+  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 0;
+
+  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::endl;
+	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/optimizer/toro3d/posegraph2.cpp

@@ -0,0 +1,441 @@
+/**********************************************************************
+ *
+ * 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 posegraph2.cpp
+ * 
+ * \brief Defines the graph of 2D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#include "posegraph2.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+using namespace std;
+
+namespace AISNavigation {
+
+
+typedef unsigned int uint;
+#define LINESIZE 81920
+
+
+//#define DEBUG(i) if (verboseLevel>i) cerr
+
+
+bool TreePoseGraph2::load(const char* filename, bool overrideCovariances){
+  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 (tag=="VERTEX" || tag=="VERTEX2"){
+      int id;
+      Pose p;
+      ls >> id >> p.x() >> p.y() >> p.theta(); 
+      addVertex(id,p);
+ 	//DEBUG(2) << "V " << id << endl;
+      
+    }
+
+    if (tag=="EDGE" || tag=="EDGE2"){
+      int id1, id2;
+      Pose p;
+      InformationMatrix m;
+      ls >> id1 >> id2 >> p.x() >> p.y() >> p.theta();
+      if (overrideCovariances){
+	m.values[0][0]=1;  m.values[1][1]=1; m.values[2][2]=1;
+	m.values[0][1]=0;  m.values[0][2]=0; m.values[1][2]=0;
+      } else {
+	ls >> m.values[0][0] >> m.values[0][1] >> m.values [1][1]
+	   >> m.values[2][2] >> m.values[0][2] >> m.values [1][2];
+      }
+      m.values[1][0]=m.values[0][1];
+      m.values[2][0]=m.values[0][2];
+      m.values[2][1]=m.values[1][2];
+      TreePoseGraph2::Vertex* v1=vertex(id1);
+      TreePoseGraph2::Vertex* v2=vertex(id2);
+      Transformation t(p);
+      addEdge(v1, v2,t ,m);
+	 //DEBUG(2) << "E " << id1 << " " << id2 <<  endl;
+    }
+  }
+  return true;
+}
+
+bool TreePoseGraph2::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);
+  }
+  for (TreePoseGraph2::VertexMap::iterator it=vertices.begin(); it!=vertices.end(); it++){
+    Vertex* v=it->second;
+    v->edges.clear();
+  }
+  for (TreePoseGraph2::EdgeMap::iterator it=edges.begin(); it!=edges.end(); it++){
+    TreePoseGraph2::Edge * e=it->second;
+    e->v1->edges.push_back(e);
+    e->v2->edges.push_back(e);
+  }
+  return true;
+}
+
+bool TreePoseGraph2::saveGnuplot(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    const Vertex* v1=e->v1;
+    const Vertex* v2=e->v2;
+    
+    os << v1->pose.x() << " " << v1->pose.y() << " " << v1->pose.theta() << endl;
+    os << v2->pose.x() << " " << v2->pose.y() << " " << v2->pose.theta() << endl;
+    os << endl;
+  }
+  return true;
+
+}
+
+bool TreePoseGraph2::save(const char* filename){
+  ofstream os(filename);
+  if (!os)
+    return false;
+  
+  for (TreePoseGraph2::VertexMap::const_iterator it=vertices.begin(); it!=vertices.end(); it++){
+    const TreePoseGraph2::Vertex* v=it->second;
+    os << "VERTEX " 
+       << v->id << " " 
+       << v->pose.x() << " " 
+       << v->pose.y() << " " 
+       << v->pose.theta()<< endl; 
+  }
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.theta() << " ";
+    os << e->informationMatrix.values[0][0] << " "
+       << e->informationMatrix.values[0][1] << " "
+       << e->informationMatrix.values[1][1] << " "
+       << e->informationMatrix.values[2][2] << " "
+       << e->informationMatrix.values[0][2] << " "
+       << e->informationMatrix.values[1][2] << 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(TreePoseGraph2::Vertex* v){
+    std::cout << "(" << v->id << "," << v->level << ")" << endl;
+  }
+};
+
+void TreePoseGraph2::printDepth( std::ostream& os ){
+  IdPrinter ip(os);
+  treeDepthVisit(ip, root);
+}
+
+void TreePoseGraph2::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(TreePoseGraph2::Vertex* v){
+    if (!v->parent)
+      return;
+    TreePoseGraph2::Transformation tParent(v->parent->pose);
+    TreePoseGraph2::Transformation tNode=tParent*v->parentEdge->transformation;
+
+    //cerr << "EDGE(" << v->parentEdge->v1->id << "," << v->parentEdge->v2->id <<"): " << endl;
+    //Pose pParent=v->parent->pose;
+    //cerr << "     p=" << pParent.x() << "," << pParent.y() << "," << pParent.theta() <<  endl;
+    //Pose pEdge=v->parentEdge->transformation.toPoseType();
+    //cerr << "     m=" << pEdge.x() << "," << pEdge.y() << "," << pEdge.theta() <<  endl;
+    //Pose pNode=tNode.toPoseType();
+    //cerr << "     n=" << pNode.x() << "," << pNode.y() << "," << pNode.theta() <<  endl;
+
+    assert(v->parentEdge->v1==v->parent);
+    assert(v->parentEdge->v2==v);
+    v->pose=tNode.toPoseType();
+  }
+};
+
+void TreePoseGraph2::initializeOnTree(){
+  PosePropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+
+void TreePoseGraph2::printEdgesStat(std::ostream& os){
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    const TreePoseGraph2::Edge * e=it->second;
+    os << "EDGE " << e->v1->id << " " << e->v2->id << " ";
+    Pose p=e->transformation.toPoseType();
+    os << p.x() << " " << p.y() << " " << p.theta() << " ";
+    os << e->informationMatrix.values[0][0] << " "
+       << e->informationMatrix.values[0][1] << " "
+       << e->informationMatrix.values[1][1] << " "
+       << e->informationMatrix.values[2][2] << " "
+       << e->informationMatrix.values[0][2] << " "
+       << e->informationMatrix.values[1][2] << endl;
+    os << "   top=" << e->top->id << " length=" << e->length << endl;
+  }
+}
+
+void TreePoseGraph2::revertEdgeInfo(Edge* e){
+  Transformation it=e->transformation.inv();
+  InformationMatrix R;
+  R.values[0][0]=e->transformation.rotationMatrix[0][0];
+  R.values[0][1]=e->transformation.rotationMatrix[0][1];
+  R.values[0][2]=0;
+
+  R.values[1][0]=e->transformation.rotationMatrix[1][0];
+  R.values[1][1]=e->transformation.rotationMatrix[1][1];
+  R.values[1][2]=0;
+
+  R.values[2][0]=0;
+  R.values[2][1]=0;
+  R.values[2][2]=1;
+
+  InformationMatrix IM=R.transpose()*e->informationMatrix*R;
+
+
+  //Pose np=e->transformation.toPoseType();
+ 
+  //Pose ip=it.toPoseType();
+
+  //Transformation tc=it*e->transformation;
+  //Pose pc=tc.toPoseType();
+
+  e->transformation=it;
+  e->informationMatrix=IM;
+};
+
+void TreePoseGraph2::initializeFromParentEdge(Vertex* v){
+  Transformation tp=Transformation(v->parent->pose)*v->parentEdge->transformation;
+  v->transformation=tp;
+  v->pose=tp.toPoseType();
+  v->parameters=v->pose;
+  v->parameters.x()-=v->parent->pose.x();
+  v->parameters.y()-=v->parent->pose.y();
+  v->parameters.theta()-=v->parent->pose.theta();
+  v->parameters.theta()=atan2(sin(v->parameters.theta()), cos(v->parameters.theta()));
+}
+
+void TreePoseGraph2::collapseEdge(Edge* e){
+  EdgeMap::iterator ie_it=edges.find(e);
+  if (ie_it==edges.end())
+    return;
+  //VertexMap::iterator it1=vertices.find(e->v1->id);
+  //VertexMap::iterator it2=vertices.find(e->v2->id);
+  assert(vertices.find(e->v1->id)!=vertices.end());
+  assert(vertices.find(e->v2->id)!=vertices.end());
+
+  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;
+      
+      //InformationMatrix R;
+      //R.values[0][0]=tr.rotationMatrix[0][0];
+      //R.values[0][1]=tr.rotationMatrix[0][1];
+      //R.values[0][2]=0;
+      
+      //R.values[1][0]=tr.rotationMatrix[1][0];
+      //R.values[1][1]=tr.rotationMatrix[1][1];
+      //R.values[1][2]=0;
+      
+      //R.values[2][0]=0;
+      //R.values[2][1]=0;
+      //R.values[2][2]=1;
+
+      //CovarianceMatrix CM=R.transpose()*C2x*R;
+      
+      
+      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;
+      }
+
+    }
+    if (e1x && e2x){
+      Transformation t1x=e1x->transformation;
+      InformationMatrix I1x=e1x->informationMatrix;
+      Pose p1x=t1x.toPoseType();
+
+      Transformation t2x=e2x->transformation;
+      InformationMatrix I2x=e2x->informationMatrix;;
+      Pose p2x=t2x.toPoseType();
+
+      InformationMatrix IM=I1x+I2x;
+      CovarianceMatrix  CM=IM.inv();
+      InformationMatrix scale1=CM*I1x;
+      InformationMatrix scale2=CM*I2x;
+
+
+      Pose p1=scale1*p1x;
+      Pose p2=scale2*p2x;
+
+
+      //need to recover the angles in a decent way.
+      double s=scale1.values[2][2]*sin(p1x.theta())+ scale2.values[2][2]*sin(p2x.theta());
+      double c=scale1.values[2][2]*cos(p1x.theta())+ scale2.values[2][2]*cos(p2x.theta());
+      
+      //DEBUG(2) << "p1x= " << p1x.x() << " " << p1x.y() << " " << p1x.theta() << endl;
+      //DEBUG(2) << "p1x_pred= " << p2x.x() << " " << p2x.y() << " " << p2x.theta() << endl;
+      
+      Pose pFinal(p1.x()+p2.x(), p1.y()+p2.y(), atan2(s,c));
+      //DEBUG(2) << "p1x_final= " << pFinal.x() << " " << pFinal.y() << " " << pFinal.theta() << endl;
+      
+      e1x->transformation=Transformation(pFinal);
+      e1x->informationMatrix=IM;
+    }
+    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++;
+  }
+}
+
+}; //namespace AISNavigation

corelib/src/optimizer/toro3d/posegraph2.hh

@@ -0,0 +1,110 @@
+/**********************************************************************
+ *
+ * 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 posegraph2.hh
+ *
+ * \brief Defines the graph of 2D poses, with specific functionalities
+ * such as loading, saving, merging constraints, and etc.
+ **/
+
+#ifndef _POSEGRAPH2_HH_
+#define _POSEGRAPH2_HH_
+
+#include "posegraph.hh"
+#include "transformation2.hh"
+#include <iostream>
+#include <vector>
+
+namespace AISNavigation {
+
+
+
+/** \brief The class (struct) that contains 2D graph related functions
+    such as loading, saving, merging, etc. **/
+struct TreePoseGraph2: public TreePoseGraph< Operations2D<double> >{
+
+  typedef Operations2D<double>::PoseType           Pose;
+  typedef Operations2D<double>::RotationType       Rotation;
+  typedef Operations2D<double>::TranslationType    Translation;
+  typedef Operations2D<double>::TransformationType Transformation;
+  typedef Operations2D<double>::CovarianceType     CovarianceMatrix;
+  typedef Operations2D<double>::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); 
+
+  /** 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);
+
+  void initializeOnTree();
+  
+  /** Turn around the edge (<i,j>  => <j,i>)  **/
+  virtual void revertEdgeInfo(Edge* e);
+
+  virtual void initializeFromParentEdge(Vertex* v);
+
+  /** 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;
+};
+
+}; //namespace AISNavigation
+#endif
+
+
+

corelib/src/optimizer/toro3d/posegraph3.cpp

@@ -0,0 +1,404 @@
+/**********************************************************************
+ *
+ * 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/optimizer/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/optimizer/toro3d/readme.txt

@@ -0,0 +1,3 @@
+
+Info: https://www.openslam.org/toro.html
+License: Creative Commons (Attribution-NonCommercial-ShareAlike)

corelib/src/optimizer/toro3d/transformation2.hh

@@ -0,0 +1,412 @@
+/**********************************************************************
+ *
+ * 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 transformation2.hh
+ * \brief Definition of the 2d transformations.
+ *
+ * Definition of the 2d transformations, the symmetrix matrix operations,
+ * handling covariance, etc.
+ **/
+
+#ifndef _TRANSFORMATION2_HXX_
+#define _TRANSFORMATION2_HXX_
+
+#include <cmath>
+
+namespace AISNavigation
+{
+
+/** \brief Template class for representing a 2D point (x and y coordinate) **/
+template <class T>
+struct Vector2{
+  T values[2] ; ///< container for x and y
+
+  /** Constructor **/
+  Vector2(T x, T y)        {values[0]=x; values[1]=y;}
+  /** Default constructor which sets x and y to 0 **/
+  Vector2()                {values[0]=0; values[1]=0;}
+
+  /** @returns Const reference to x **/
+  inline const T& x()      const  {return values[0];}
+  /** @returns Const reference to y **/
+  inline const T& y()      const  {return values[1];}
+
+  /** @returns Reference to x **/
+  inline T& x()            {return values[0];}
+  /** @returns Reference to y **/
+  inline T& y()            {return values[1];}
+
+  /** @returns Norm of the vector **/
+  inline T norm2() const   {
+    return values[0]*values[0]+values[1]*values[1];
+  }
+
+};
+
+/** Operator for scalar multiplication.  **/
+template <class T>
+inline Vector2<T> operator * (const T& d, const Vector2<T>& v) {
+  return Vector2<T>(v.values[0]*d, v.values[1]*d);
+}
+
+/** Operator for scalar multiplication.  **/
+template <class T>
+inline Vector2<T> operator * (const Vector2<T>& v, const T& d) {
+  return Vector2<T>(v.values[0]*d, v.values[1]*d);
+}
+
+/** Operator for dot product.  **/
+template <class T>
+inline T operator * (const Vector2<T>& v1, const Vector2<T>& v2){
+  return v1.values[0]*v2.values[0] 
+    + v1.values[1]*v2.values[1];
+}
+
+/** Operator for vector addition.  **/
+template <class T>
+inline Vector2<T> operator + (const Vector2<T>& v1, const Vector2<T>& v2){
+  return Vector2<T>(v1.values[0]+v2.values[0], 
+		    v1.values[1]+v2.values[1]);
+}
+
+/** Operator for vector subtraction.  **/
+template <class T>
+Vector2<T> operator - (const Vector2<T>& v1, const Vector2<T>& v2){
+  return Vector2<T>(v1.values[0]-v2.values[0], 
+		    v1.values[1]-v2.values[1]);
+}
+
+
+/** \brief 2D Point (x,y) with orientation (theta)
+ *
+ * Tenmplate class for representing a 2D Ooint with x and y
+ * coordinates and an orientation theta in the x-y-plane (theta=0 ->
+ * orientation along the x axis).
+**/
+template <class T>
+struct Pose2{
+  T values[3];///< container for x, y, and theta
+
+  /** @returns Const refernce to x **/
+  inline const T& x()     const  {return values[0];}
+  /** @returns Const refernce to y **/
+  inline const T& y()     const  {return values[1];}
+  /** @returns Const refernce to theta **/
+  inline const T& theta() const  {return values[2];}
+
+  /** @returns Refernce to x **/
+  inline T& x()            {return values[0];}
+  /** @returns Refernce to y **/
+  inline T& y()            {return values[1];}
+  /** @returns Refernce to theta **/
+  inline T& theta()        {return values[2];}
+
+  /** Default constructor which sets x, y, and theta to 0 **/
+  Pose2(){
+    values[0]=0.; values[1]=0.;  values[2]=0.;
+  }
+
+  /** Constructor **/
+  Pose2(const T& x, const T& y, const T& theta){
+    values[0]=x, values[1]=y, values[2]=theta;
+  }
+};
+
+/** Operator for scalar multiplication with a pose **/
+template <class T>
+Pose2<T> operator * (const Pose2<T>& v, const T& d){
+  Pose2<T> r;
+  for (int i=0; i<3; i++){
+    r.values[i]=v.values[i]*d;
+  }
+  return r;
+}
+
+
+/** \brief A class to represent 2D transformations (rotation and translation) **/
+template <class T>
+struct Transformation2{
+  T rotationMatrix[2][2]; ///< the rotation matrix
+  T translationVector[2]; ///< the translation vector
+
+  /** Default constructor 
+   * @param initAsIdentity if true (default) the transormation 
+   *        is the identity, otherwise no initializtion **/
+  Transformation2(bool initAsIdentity = true){
+    if (initAsIdentity) {
+      rotationMatrix[0][0]=1.; rotationMatrix[0][1]=0.;
+      rotationMatrix[1][0]=0.; rotationMatrix[1][1]=1.;
+      translationVector[0]=0.;
+      translationVector[1]=0.;
+    }
+  }
+
+  /** @returns Identity transformation **/
+  inline static Transformation2<T> identity(){
+    Transformation2<T> m(true);
+    return m;
+  }
+
+  /** Constructor that sets the translation and rotation  **/
+  Transformation2 (const T& x, const T& y, const T& theta){
+    setRotation(theta);
+    setTranslation(x,y);
+  }
+
+  /** Constructor that sets the translation and rotation  **/
+  Transformation2 (const T& _theta, const Vector2<T>& trans){
+	  setRotation(_theta);
+	  setTranslation(trans.x(), trans.y());
+  }
+
+
+  /** Copy constructor  **/
+  Transformation2 (const Pose2<T>& v){
+    setRotation(v.theta());
+    setTranslation(v.x(),v.y());
+  }
+
+
+  /** Get the translation  **/
+  inline Vector2<T> translation() const {
+    return Vector2<T>(translationVector[0], 
+		      translationVector[1]);
+  }
+  
+  /** Get the rotation  **/
+  inline T rotation()    const {
+    return atan2(rotationMatrix[1][0],rotationMatrix[0][0]);
+  }
+  
+  /** Computed the Pose based on the translation and rotation  **/
+  inline Pose2<T> toPoseType()   const {
+    Vector2<T> t=translation();
+    T r=rotation();
+    Pose2<T> rv(t.x(), t.y(), r );
+    return rv;
+  }
+  
+  /** Set the translation  **/
+  inline void setTranslation(const Vector2<T>& t){
+    setTranslation(t.x(),t.y());
+  }
+
+  /** Set the rotation  **/
+  inline void setRotation(const T& theta){
+    T s=sin(theta), c=cos(theta);
+    rotationMatrix[0][0]=c, rotationMatrix[0][1]=-s;
+    rotationMatrix[1][0]=s, rotationMatrix[1][1]= c;
+  }
+
+  /** Set the translation  **/
+  inline void setTranslation(const T& x, const T& y){
+    translationVector[0]=x;
+    translationVector[1]=y;
+  }
+
+  /** Computes the inveres of the transformation  **/
+  inline Transformation2<T> inv() const {
+    Transformation2<T> rv(*this);
+    for (int i=0; i<2; i++)
+      for (int j=0; j<2; j++){
+	rv.rotationMatrix[i][j]=rotationMatrix[j][i];
+      }
+
+    for (int i=0; i<2; i++){
+      rv.translationVector[i]=0;
+      for (int j=0; j<2; j++){
+	rv.translationVector[i]-=rv.rotationMatrix[i][j]*translationVector[j];
+      }
+    }
+    return rv;
+  }
+
+};
+
+/** Operator for transforming a Vector2 **/
+template <class T>
+Vector2<T> operator * (const Transformation2<T>& m, const Vector2<T>& v){
+  return Vector2<T>(
+		    m.rotationMatrix[0][0]*v.values[0]+
+		    m.rotationMatrix[0][1]*v.values[1]+
+		    m.translationVector[0],
+		    m.rotationMatrix[1][0]*v.values[0]+
+		    m.rotationMatrix[1][1]*v.values[1]+
+		    m.translationVector[1]);
+}
+
+/** Operator for concatenating two transformations **/
+template <class T>
+Transformation2<T> operator * (const Transformation2<T>& m1, const Transformation2<T>& m2){
+  Transformation2<T> rt;
+  for (int i=0; i<2; i++)
+    for (int j=0; j<2; j++){
+      rt.rotationMatrix[i][j]=0.;
+      for (int k=0; k<2; k++)
+	rt.rotationMatrix[i][j]+=m1.rotationMatrix[i][k]*m2.rotationMatrix[k][j];
+    }
+  for (int i=0; i<2; i++){
+    rt.translationVector[i]=m1.translationVector[i];
+    for (int j=0; j<2; j++)
+      rt.translationVector[i]+=m1.rotationMatrix[i][j]*m2.translationVector[j];
+  }
+  return rt;
+}
+
+
+/** \brief A class to represent symmetric 3x3 matrices **/
+template <class T>
+struct SMatrix3{
+  T values[3][3];
+  T det() const;
+  SMatrix3<T> transpose() const;
+  SMatrix3<T> adj() const;
+  SMatrix3<T> inv() const;
+};
+
+
+/** Operator for symmetric matrix-pose multiplication **/
+template <class T>
+Pose2<T> operator * (const SMatrix3<T>& m, const Pose2<T>& p){
+  Pose2<T> v;
+  for (int i=0; i<3; i++){
+    v.values[i]=0.;
+    for (int j=0; j<3; j++)
+      v.values[i]+=m.values[i][j]*p.values[j];
+  }
+  return v;
+}
+
+/** Operator for symmetric matrix-scalar multiplication **/
+template <class T>
+SMatrix3<T> operator * (const SMatrix3<T>& s, T& d){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++)
+      m.values[i][j]=d*s.values[i][j];
+  return m;
+}
+
+/** Operator forsymmetric  matrix-symmetric matrix multiplication **/
+template <class T>
+SMatrix3<T> operator * (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++){
+      m.values[i][j]=0.;
+      for (int k=0; k<3; k++){
+	m.values[i][j]+=s1.values[i][k]*s2.values[k][j];
+      }
+    }
+  return m;
+}
+
+/** Operator for symmetric matrix-symmetric matrix addition **/
+template <class T>
+SMatrix3<T> operator + (const SMatrix3<T>& s1, const SMatrix3<T>& s2){
+  SMatrix3<T> m;
+  for (int i=0; i<3; i++)
+    for (int j=0; j<3; j++){
+      m.values[i][j]=s1.values[i][j]+s2.values[i][j];
+    }
+  return m;
+}
+
+
+/** Computes the determinat of the symmetric matrix **/
+template <class T>
+T SMatrix3<T>::det() const{
+   T dp= values[0][0]*values[1][1]*values[2][2]
+     +values[0][1]*values[1][2]*values[2][0]
+     +values[0][2]*values[1][0]*values[2][1];
+   T dm=values[2][0]*values[1][1]*values[0][2]
+     +values[2][1]*values[1][2]*values[0][0]
+     +values[2][2]*values[1][0]*values[0][1];
+   return dp-dm;
+}
+
+/** Computes the transposed symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::transpose() const{
+   SMatrix3<T> m;
+   for (int i=0; i<3; i++)
+     for (int j=0; j<3; j++)
+       m.values[j][i]=values[i][j];
+   return m;
+}
+
+/** Computes the complement of the symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::adj() const{
+   SMatrix3<T> m;
+   m.values[0][0]= values[1][1]*values[2][2]-values[2][1]*values[1][2];
+   m.values[0][1]=-values[1][0]*values[2][2]+values[1][2]*values[2][0];
+   m.values[0][2]= values[1][0]*values[2][1]-values[2][0]*values[1][1];
+   m.values[1][0]=-values[0][1]*values[2][2]+values[2][1]*values[0][2];
+   m.values[1][1]= values[0][0]*values[2][2]-values[2][0]*values[0][2];
+   m.values[1][2]=-values[0][0]*values[2][1]+values[2][0]*values[0][1];
+   m.values[2][0]= values[0][1]*values[1][2]-values[1][1]*values[0][2];
+   m.values[2][1]=-values[0][0]*values[1][2]+values[1][0]*values[0][2];
+   m.values[2][2]= values[0][0]*values[1][1]-values[1][0]*values[0][1];
+   return m;
+}
+
+/** Computes the inverse (=transposed) symmetric matrix **/
+template <class T>
+SMatrix3<T>  SMatrix3<T>::inv() const{
+   T id=1./det();
+   SMatrix3<T> i=adj().transpose();
+   return i*id;
+}
+
+
+
+/** \brief Tenmplate class to define the operations in 2D  **/
+template <class T>
+struct Operations2D{
+  typedef T                  BaseType;            /**< base type of the operation typedef **/
+  typedef Pose2<T>           PoseType;            /**< plain representation of the 2d pose as x,y,theta **/
+  typedef Pose2<T>           ParametersType;      /**< plain representation of the 2d pose as x,y,theta **/
+  typedef T                  RotationType;        /**< plain representation of the angle **/
+  typedef Vector2<T>         TranslationType;     /**< plain representation of the 2D translation (x,y) **/
+  typedef Transformation2<T> TransformationType;  /**< homogeneous based representation for a 2d pose, as rotation matrix + vector **/
+  typedef SMatrix3<T>        CovarianceType;      /**< 3 by 3 symmetric covariance matrix for the 2D case **/
+  typedef SMatrix3<T>        InformationType;     /**< 3 by 3 symmetric information matrix for the 2D case **/
+};
+
+} // namespace AISNavigation
+
+#endif

corelib/src/optimizer/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/optimizer/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, m10, 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/optimizer/toro3d/treeoptimizer2.cpp

@@ -0,0 +1,368 @@
+/**********************************************************************
+ *
+ * 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 treeoptimizer2.cpp
+ *
+ * \brief Defines the core optimizer class for 2D graphs which is a
+ * subclass of TreePoseGraph2
+ *
+ **/
+
+#include "treeoptimizer2.hh"
+#include <fstream>
+#include <sstream>
+#include <string>
+
+typedef unsigned int uint;
+using namespace std;
+
+namespace AISNavigation {
+
+//#define DEBUG(i) if (verboseLevel>i) cerr
+
+/** \brief A class (struct) to compute the parameterization of the vertex v **/
+struct ParameterPropagator{
+  void perform(TreePoseGraph2::Vertex* v){
+    if (!v->parent){
+      v->parameters=TreePoseGraph2::Pose(0.,0.,0.);
+      return;
+    }
+    v->parameters=TreePoseGraph2::Pose(v->pose.x()-v->parent->pose.x(), 
+		      v->pose.y()-v->parent->pose.y(),
+		      v->pose.theta()-v->parent->pose.theta());
+  }
+};
+
+TreeOptimizer2::TreeOptimizer2():
+	iteration(1){
+  sortedEdges=0;
+}
+
+TreeOptimizer2::~TreeOptimizer2(){
+}
+
+void TreeOptimizer2::initializeTreeParameters(){
+  ParameterPropagator pp;
+  treeDepthVisit(pp, root);
+}
+
+void TreeOptimizer2::initializeOptimization(){
+  // 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();
+}
+
+void TreeOptimizer2::initializeOnlineOptimization(){
+  // 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;
+}
+
+void TreeOptimizer2::computePreconditioner(){
+  gamma[0] = gamma[1] =  gamma[2] = numeric_limits<double>::max();
+
+  for (uint i=0; i<M.size(); i++)
+    M[i]=Pose(0.,0.,0.);
+
+  int edgeCount=0;
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+    edgeCount++;
+    //if (! (edgeCount%10000))
+    //  DEBUG(1) << "m";
+
+    Edge* e=*it;
+    Transformation t=e->transformation;
+    InformationMatrix S=e->informationMatrix;
+    
+    InformationMatrix R;
+    R.values[0][0]=t.rotationMatrix[0][0];
+    R.values[0][1]=t.rotationMatrix[0][1];
+    R.values[0][2]=0;
+    
+    R.values[1][0]=t.rotationMatrix[1][0];
+    R.values[1][1]=t.rotationMatrix[1][1];
+    R.values[1][2]=0;
+    
+    R.values[2][0]=0;
+    R.values[2][1]=0;
+    R.values[2][2]=1;
+
+    InformationMatrix W =R*S*R.transpose();
+    
+    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;
+	M[i].values[0]+=W.values[0][0];
+	M[i].values[1]+=W.values[1][1];
+	M[i].values[2]+=W.values[2][2];
+	gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
+	gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
+	gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
+	n=n->parent;
+      }
+    }
+  }
+  
+  if (verboseLevel>1){
+    for (uint i=0; i<M.size(); i++){
+      cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
+    }
+  }
+}
+
+
+void TreeOptimizer2::propagateErrors(){
+  iteration++;
+  int edgeCount=0;
+  
+  for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
+      edgeCount++;
+      //if (! (edgeCount%10000)) 	DEBUG(1) << "c";
+      
+      Edge* e=*it;
+      Vertex* top=e->top;
+      
+
+      Vertex* v1=e->v1;
+      Vertex* v2=e->v2;
+
+      double l=e->length;
+      //DEBUG(2) << "Edge: " << v1->id << " " << v2->id << ", top=" << top->id << ", length="<< l <<endl;
+      
+      Pose p1=getPose(v1, top);
+      Pose p2=getPose(v2, top);
+
+      //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+      //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+      
+      Transformation et=e->transformation;
+      Transformation t1(p1);
+      Transformation t2(p2);
+
+      Transformation t12=t1*et;
+    
+      Pose p12=t12.toPoseType();
+      //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+
+      Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
+      double angle=r.theta();
+      angle=atan2(sin(angle),cos(angle));
+      r.theta()=angle;
+      //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+    
+      InformationMatrix S=e->informationMatrix;
+      InformationMatrix R;
+      R.values[0][0]=t1.rotationMatrix[0][0];
+      R.values[0][1]=t1.rotationMatrix[0][1];
+      R.values[0][2]=0;
+      
+      R.values[1][0]=t1.rotationMatrix[1][0];
+      R.values[1][1]=t1.rotationMatrix[1][1];
+      R.values[1][2]=0;
+      
+      R.values[2][0]=0;
+      R.values[2][1]=0;
+      R.values[2][2]=1;
+
+      InformationMatrix W=R*S*R.transpose();
+      Pose d=W*r*2.;
+
+      //DEBUG(2) << " d=" << d.x() << " " << d.y() << " " << d.theta() << endl;
+    
+      assert(l>0);
+
+      double alpha[3] = { 1./(gamma[0]*iteration), 1./(gamma[1]*iteration), 1./(gamma[2]*iteration) };
+
+      double tw[3]={0.,0.,0.};
+      for (int dir=0; dir<2; dir++) {
+	Vertex* n = (dir==0)? v1 : v2;
+	while (n!=top){
+	  uint i=n->id;
+	  tw[0]+=1./M[i].values[0];
+	  tw[1]+=1./M[i].values[1];
+	  tw[2]+=1./M[i].values[2];
+	  n=n->parent;
+	}
+      }
+
+      double beta[3] = {l*alpha[0]*d.values[0], l*alpha[1]*d.values[1], l*alpha[2]*d.values[2]};
+      beta[0]=(fabs(beta[0])>fabs(r.values[0]))?r.values[0]:beta[0];
+      beta[1]=(fabs(beta[1])>fabs(r.values[1]))?r.values[1]:beta[1];
+      beta[2]=(fabs(beta[2])>fabs(r.values[2]))?r.values[2]:beta[2];
+
+      //DEBUG(2) << " alpha=" << alpha[0] << " " << alpha[1] << " " << alpha[2] << endl;
+      //DEBUG(2) << " beta=" << beta[0] << " " << beta[1] << " " << beta[2] << endl;
+      
+      for (int dir=0; dir<2; dir++) {
+	Vertex* n = (dir==0)? v1 : v2;
+	double sign=(dir==0)? -1. : 1.;
+	while (n!=top){
+	  uint i=n->id;
+	  assert(M[i].values[0]>0);
+	  assert(M[i].values[1]>0);
+	  assert(M[i].values[2]>0);
+
+	  Pose delta( beta[0]/(M[i].values[0]*tw[0]), beta[1]/(M[i].values[1]*tw[1]), beta[2]/(M[i].values[2]*tw[2]));
+	  delta=delta*sign;
+	  //DEBUG(2) << "   " << dir << ":"  << i <<"," << n->parent->id << ":"
+	  //     << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta() << " -> ";
+
+	  n->parameters.x()+=delta.x();
+	  n->parameters.y()+=delta.y();
+	  n->parameters.theta()+=delta.theta();
+	  //DEBUG(2) << n->parameters.x() << " " << n->parameters.y() << " " << n->parameters.theta()<< endl;
+	  n=n->parent;
+	}
+      }
+      updatePoseChain(v1,top);
+      updatePoseChain(v2,top);
+
+      //Pose pf1=v1->pose;
+      //Pose pf2=v2->pose;
+      
+      //DEBUG(2) << " pf1=" << pf1.x() << " " << pf1.y() << " " << pf1.theta() << endl;
+      //DEBUG(2) << " pf2=" << pf2.x() << " " << pf2.y() << " " << pf2.theta() << endl;
+      //DEBUG(2) << " en=" << p12.x()-pf2.x() << " " << p12.y()-pf2.y() << " " << p12.theta()-pf2.theta() << endl;
+    }
+  
+}
+
+void TreeOptimizer2::iterate(TreePoseGraph2::EdgeSet* eset){
+  TreePoseGraph2::EdgeSet* temp=sortedEdges;
+  if (eset){
+    sortedEdges=eset;
+  }
+  if (iteration==1)
+	  computePreconditioner();
+  propagateErrors();
+  sortedEdges=temp;
+}
+
+void TreeOptimizer2::updatePoseChain(Vertex* v, Vertex* top){
+  if (v!=top){
+    updatePoseChain(v->parent, top);
+    v->pose.x()=v->parent->pose.x()+v->parameters.x();
+    v->pose.y()=v->parent->pose.y()+v->parameters.y();
+    v->pose.theta()=v->parent->pose.theta()+v->parameters.theta();
+    return;
+  } 
+}
+
+TreeOptimizer2::Pose TreeOptimizer2::getPose(Vertex*v, Vertex* top){
+  Pose p(0,0,0);
+  Vertex* aux=v;
+  while (aux!=top){
+    p.x()+=aux->parameters.x();
+    p.y()+=aux->parameters.y();
+    p.theta()+=aux->parameters.theta();
+    aux=aux->parent;
+  }
+  p.x()+=aux->pose.x();
+  p.y()+=aux->pose.y();
+  p.theta()+=aux->pose.theta();
+  return p;
+}
+
+
+double TreeOptimizer2::error(const Edge* e) const{
+  const Vertex* v1=e->v1;
+  const Vertex* v2=e->v2;
+  
+  Pose p1=v1->pose;
+  Pose p2=v2->pose;
+  
+  //DEBUG(2) << " p1=" << p1.x() << " " << p1.y() << " " << p1.theta() << endl;
+  //DEBUG(2) << " p2=" << p2.x() << " " << p2.y() << " " << p2.theta() << endl;
+  
+  Transformation et=e->transformation;
+  Transformation t1(p1);
+  Transformation t2(p2);
+  
+  Transformation t12=t1*et;
+    
+  Pose p12=t12.toPoseType();
+  //DEBUG(2) << " pt2=" << p12.x() << " " << p12.y() << " " << p12.theta() << endl;
+
+  Pose r(p12.x()-p2.x(), p12.y()-p2.y(), p12.theta()-p2.theta());
+  double angle=r.theta();
+  angle=atan2(sin(angle),cos(angle));
+  r.theta()=angle;
+  //DEBUG(2) << " e=" << r.x() << " " << r.y() << " " << r.theta() << endl;
+    
+  InformationMatrix S=e->informationMatrix;
+  InformationMatrix R;
+  R.values[0][0]=t1.rotationMatrix[0][0];
+  R.values[0][1]=t1.rotationMatrix[0][1];
+  R.values[0][2]=0;
+      
+  R.values[1][0]=t1.rotationMatrix[1][0];
+  R.values[1][1]=t1.rotationMatrix[1][1];
+  R.values[1][2]=0;
+      
+  R.values[2][0]=0;
+  R.values[2][1]=0;
+  R.values[2][2]=1;
+
+  InformationMatrix W=R*S*R.transpose();
+
+  Pose r1=W*r;
+  return r.x()*r1.x()+r.y()*r1.y()+r.theta()*r1.theta();
+}
+
+double TreeOptimizer2::error() const{
+  double globalError=0.;
+  for (TreePoseGraph2::EdgeMap::const_iterator it=edges.begin(); it!=edges.end(); it++){
+    globalError+=error(it->second);
+  }
+  return globalError;
+}
+
+}; //namespace AISNavigation

corelib/src/optimizer/toro3d/treeoptimizer2.hh

@@ -0,0 +1,107 @@
+/**********************************************************************
+ *
+ * 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 treeoptimizer2.hh
+ *
+ * \brief Defines the core optimizer class for 2D graphs which is a
+ * subclass of TreePoseGraph2
+ *
+ **/
+
+#ifndef _TREEOPTIMIZER2_HH_
+#define _TREEOPTIMIZER2_HH_
+
+#include "posegraph2.hh"
+
+namespace AISNavigation {
+
+/** \brief Class that contains the core optimization algorithm **/
+struct TreeOptimizer2: public TreePoseGraph2{
+  typedef std::vector<Pose> PoseVector;
+
+  /** Constructor **/
+  TreeOptimizer2();
+
+  /** Destructor **/
+  virtual ~TreeOptimizer2();
+
+  /** Initialization function **/
+  void initializeTreeParameters();
+
+  /** Initialization function **/
+  void initializeOptimization();
+
+  /** Initialization function **/
+  void initializeOnlineOptimization();
+  
+  /** Performs one iteration of the algorithm **/
+  void iterate(TreePoseGraph2::EdgeSet* eset=0);
+
+  /** Conmputes the gloabl error of the network **/
+  double error() const;
+
+protected:
+  /** The first of the two main steps of each iteration **/
+  void computePreconditioner();
+
+  /** The second of the two main steps of each iteration **/
+  void propagateErrors();
+
+  /** Recomputes the poses of all vertices from v to an arbitraty
+      parent (top) of v in the tree **/
+  void updatePoseChain(Vertex* v, Vertex* top);
+
+  /** Recomputes only the pose of the node v wrt. to an arbitraty
+      parent (top) of v in the tree **/
+  Pose getPose(Vertex*v, Vertex* top);
+
+  /** Conmputes the error of the constraint/edge e **/
+  double error(const Edge* e) const;
+
+  /** Iteration counter **/
+  int iteration;
+
+  /** Used to compute the learning rate lambda **/
+  double gamma[3];
+
+  /** The diaginal block elements of the preconditioning matrix (D_k
+      in the paper) **/
+  PoseVector M;
+
+};
+
+}; //namespace AISNavigation
+#endif

corelib/src/optimizer/toro3d/treeoptimizer3.cpp

@@ -0,0 +1,360 @@
+/**********************************************************************
+ *
+ * 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=(uint)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/optimizer/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/optimizer/toro3d/treeoptimizer3_iteration.cpp

@@ -0,0 +1,342 @@
+/**********************************************************************
+ *
+ * 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

corelib/src/optimizer/vertigo/g2o/edge_se2MaxMixture.cpp

@@ -0,0 +1,122 @@
+/*
+ * edge_se2MaxMixture.cpp
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+
+#include "edge_se2MaxMixture.h"
+
+using namespace std;
+using namespace Eigen;
+
+// ================================================
+EdgeSE2MaxMixture::EdgeSE2MaxMixture()
+{
+  nullHypothesisMoreLikely = false;
+}
+
+// ================================================
+bool EdgeSE2MaxMixture::read(std::istream& is)
+  {
+    Vector3d p;
+    is >>  weight >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+    //measurement().fromVector(p);
+    //inverseMeasurement() = measurement().inverse();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+
+    information_constraint = _information;
+    nu_constraint = 1.0/sqrt(information_constraint.inverse().determinant());
+    information_nullHypothesis = information_constraint*weight;
+    nu_nullHypothesis = 1.0/sqrt(information_nullHypothesis.inverse().determinant());
+
+    return true;
+  }
+// ================================================
+bool EdgeSE2MaxMixture::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2MaxMixture::linearizeOplus()
+{
+  g2o::EdgeSE2::linearizeOplus();
+  if (nullHypothesisMoreLikely) {
+    _jacobianOplusXi *= weight;
+    _jacobianOplusXj *= weight;
+  }
+}
+
+// ================================================
+void EdgeSE2MaxMixture::computeError()
+{
+  // calculate the error for this constraint
+  g2o::EdgeSE2::computeError();
+
+  // determine the likelihood for constraint and null hypothesis
+  double mahal_constraint = _error.transpose() * information_constraint * _error;
+  double likelihood_constraint = nu_constraint * exp(-mahal_constraint);
+
+  double mahal_nullHypothesis  = _error.transpose() * (information_nullHypothesis) * _error;
+  double likelihood_nullHypothesis = nu_nullHypothesis * exp(-mahal_nullHypothesis);
+
+  // if the nullHypothesis is more likely ...
+  if (likelihood_nullHypothesis > likelihood_constraint) {
+    _information = information_nullHypothesis;
+    nullHypothesisMoreLikely = true;
+  }
+  else {
+    _information = information_constraint;
+    nullHypothesisMoreLikely = false;
+  }
+
+
+}
+
+/*
+#include <GL/gl.h>
+// ================================================
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2MaxMixtureDrawAction::EdgeSE2MaxMixtureDrawAction(): DrawAction(typeid(EdgeSE2MaxMixture).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2MaxMixtureDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* ){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2MaxMixture* e =  static_cast<EdgeSE2MaxMixture*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+
+
+    if (e->nullHypothesisMoreLikely) glColor3f(0.0,0.0,0.0);
+    else glColor3f(1.0,0.5,0.2);
+
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+*/
+
+

corelib/src/optimizer/vertigo/g2o/edge_se2MaxMixture.h

@@ -0,0 +1,46 @@
+/*
+ * edge_se2MaxMixture.h
+ *
+ *  Created on: 12.06.2012
+ *      Author: niko
+ */
+
+#ifndef EDGE_SE2MAXMIXTURE_H_
+#define EDGE_SE2MAXMIXTURE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/types/slam2d/edge_se2.h"
+
+
+class EdgeSE2MaxMixture : public g2o::EdgeSE2
+{
+  public:
+    EdgeSE2MaxMixture();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    double weight;
+    
+    bool nullHypothesisMoreLikely;
+    
+    InformationType information_nullHypothesis;
+    double nu_nullHypothesis;
+    InformationType information_constraint;
+    double nu_constraint ;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2MaxMixtureDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2MaxMixtureDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE2MAXMIXTURE_H_ */

corelib/src/optimizer/vertigo/g2o/edge_se2Switchable.cpp

@@ -0,0 +1,132 @@
+/*
+ * edge_se2Switchable.cpp
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "edge_se2Switchable.h"
+#include "vertex_switchLinear.h"
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE2Switchable::EdgeSE2Switchable() : g2o::BaseMultiEdge<3, g2o::SE2>()
+{
+  resize(3);
+  _jacobianOplus.clear();
+  _jacobianOplus.push_back(JacobianType(0, 3, 3));
+  _jacobianOplus.push_back(JacobianType(0, 3, 3));
+  _jacobianOplus.push_back(JacobianType(0, 3, 1));
+
+}
+// ================================================
+bool EdgeSE2Switchable::read(std::istream& is)
+  {
+    Vector3d p;
+    is >> p[0] >> p[1] >> p[2];
+    setMeasurement(g2o::SE2(p));
+    _inverseMeasurement = measurement().inverse();
+
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j) {
+        is >> information()(i, j);
+        if (i != j)
+          information()(j, i) = information()(i, j);
+      }
+    return true;
+  }
+// ================================================
+bool EdgeSE2Switchable::write(std::ostream& os) const
+{
+    Vector3d p = measurement().toVector();
+    os << p.x() << " " << p.y() << " " << p.z();
+    for (int i = 0; i < 3; ++i)
+      for (int j = i; j < 3; ++j)
+        os << " " << information()(i, j);
+    return os.good();
+}
+
+// ================================================
+void EdgeSE2Switchable::linearizeOplus()
+{
+   
+    const g2o::VertexSE2* vi = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* vj = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    double thetai = vi->estimate().rotation().angle();
+
+    Vector2d dt = vj->estimate().translation() - vi->estimate().translation();
+    double si=sin(thetai), ci=cos(thetai);
+
+    _jacobianOplus[0](0, 0) = -ci; _jacobianOplus[0](0, 1) = -si; _jacobianOplus[0](0, 2) = -si*dt.x()+ci*dt.y();
+    _jacobianOplus[0](1, 0) =  si; _jacobianOplus[0](1, 1) = -ci; _jacobianOplus[0](1, 2) = -ci*dt.x()-si*dt.y();
+    _jacobianOplus[0](2, 0) =  0;  _jacobianOplus[0](2, 1) = 0;   _jacobianOplus[0](2, 2) = -1;
+
+    _jacobianOplus[1](0, 0) = ci; _jacobianOplus[1](0, 1)= si; _jacobianOplus[1](0, 2)= 0;
+    _jacobianOplus[1](1, 0) =-si; _jacobianOplus[1](1, 1)= ci; _jacobianOplus[1](1, 2)= 0;
+    _jacobianOplus[1](2, 0) = 0;  _jacobianOplus[1](2, 1)= 0;  _jacobianOplus[1](2, 2)= 1;
+
+    const g2o::SE2& rmean = _inverseMeasurement;
+    Matrix3d z = Matrix3d::Zero();
+    z.block<2, 2>(0, 0) = rmean.rotation().toRotationMatrix();
+    z(2, 2) = 1.;
+    _jacobianOplus[0] = z * _jacobianOplus[0];
+    _jacobianOplus[1] = z * _jacobianOplus[1];
+
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    g2o::SE2 delta = _inverseMeasurement * (vi->estimate().inverse()*vj->estimate());
+    _jacobianOplus[2] = delta.toVector() * vSwitch->gradient();  
+}
+
+// ================================================
+void EdgeSE2Switchable::computeError()
+{
+    const g2o::VertexSE2* v1 = static_cast<const g2o::VertexSE2*>(_vertices[0]);
+    const g2o::VertexSE2* v2 = static_cast<const g2o::VertexSE2*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+  
+    g2o::SE2 delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = delta.toVector() * v3->estimate();
+}
+
+/*
+#include <GL/gl.h>
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE2SwitchableDrawAction::EdgeSE2SwitchableDrawAction(): DrawAction(typeid(EdgeSE2Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE2SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* ){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE2Switchable* e =  static_cast<EdgeSE2Switchable*>(element);
+
+
+    g2o::VertexSE2* fromEdge = static_cast<g2o::VertexSE2*>(e->vertices()[0]);
+    g2o::VertexSE2* toEdge   = static_cast<g2o::VertexSE2*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+*/

corelib/src/optimizer/vertigo/g2o/edge_se2Switchable.h

@@ -0,0 +1,50 @@
+/*
+ * edge_se2Switchable.h
+ *
+ *  Created on: 13.07.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE2SWITCHABLE_H_
+#define EDGE_SE2SWITCHABLE_H_
+
+#include "g2o/types/slam2d/vertex_se2.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE2Switchable : public g2o::BaseMultiEdge<3, g2o::SE2>
+{
+  public:
+    EdgeSE2Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+
+    virtual void setMeasurement(const g2o::SE2& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+    protected:
+      g2o::SE2 _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE2SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE2SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+
+#endif /* EDGE_SE2SWITCHABLE_H_ */

corelib/src/optimizer/vertigo/g2o/edge_se3Switchable.cpp

@@ -0,0 +1,124 @@
+/*
+ * edge_se3Switchable.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "edge_se3Switchable.h"
+#include "vertex_switchLinear.h"
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/types/slam3d/isometry3d_gradients.h"
+
+
+using namespace std;
+using namespace Eigen;
+
+
+// ================================================
+EdgeSE3Switchable::EdgeSE3Switchable() : g2o::BaseMultiEdge<6, Eigen::Isometry3d>()
+{
+  resize(3);
+  _jacobianOplus.clear();
+  _jacobianOplus.push_back(JacobianType(0, 6, 6));
+  _jacobianOplus.push_back(JacobianType(0, 6, 6));
+  _jacobianOplus.push_back(JacobianType(0, 6, 1));
+
+}
+// ================================================
+bool EdgeSE3Switchable::read(std::istream& is)
+  {    
+ /*   g2o::Vector7d meas;
+    for (int i=0; i<7; i++) 
+      is >> meas[i];
+    // normalize the quaternion to recover numerical precision lost by storing as human readable text
+    Vector4d::MapType(meas.data()+3).normalize();
+    setMeasurement(g2o::internal::fromVectorQT(meas));
+
+    for (int i=0; i<6; i++)
+      for (int j=i; j<6; j++) {
+        is >> information()(i,j);
+        if (i!=j)
+          information()(j,i) = information()(i,j);
+      }
+    return true;*/
+	return false;
+
+  }
+// ================================================
+bool EdgeSE3Switchable::write(std::ostream& os) const
+{
+   /* g2o::Vector7d meas = g2o::internal::toVectorQT(measurement());
+    for (int i=0; i<7; i++) os  << meas[i] << " ";
+    for (int i = 0; i < 6; ++i)
+      for (int j = i; j < 6; ++j)
+        os << " " << information()(i, j);
+    return os.good();*/
+	return false;
+}
+
+// ================================================
+void EdgeSE3Switchable::linearizeOplus()
+{
+
+    g2o::VertexSE3* from = static_cast<g2o::VertexSE3*>(_vertices[0]);
+    g2o::VertexSE3* to = static_cast<g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* vSwitch = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d E;
+    const Eigen::Isometry3d& Xi=from->estimate();
+    const Eigen::Isometry3d& Xj=to->estimate();
+    const Eigen::Isometry3d& Z=_measurement;
+    g2o::internal::computeEdgeSE3Gradient(E, _jacobianOplus[0], _jacobianOplus[1], Z, Xi, Xj);
+
+    _jacobianOplus[0]*=vSwitch->estimate();
+    _jacobianOplus[1]*=vSwitch->estimate();
+
+    // derivative w.r.t switch vertex
+    _jacobianOplus[2].setZero();
+    _jacobianOplus[2] = g2o::internal::toVectorMQT(E) * vSwitch->gradient();
+}
+
+
+// ================================================
+void EdgeSE3Switchable::computeError()
+{
+    const g2o::VertexSE3* v1 = dynamic_cast<const g2o::VertexSE3*>(_vertices[0]);
+    const g2o::VertexSE3* v2 = dynamic_cast<const g2o::VertexSE3*>(_vertices[1]);
+    const VertexSwitchLinear* v3 = static_cast<const VertexSwitchLinear*>(_vertices[2]);
+
+    Eigen::Isometry3d delta = _inverseMeasurement * (v1->estimate().inverse()*v2->estimate());
+    _error = g2o::internal::toVectorMQT(delta) * v3->estimate();
+}
+
+/*
+#include <GL/gl.h>
+#ifdef G2O_HAVE_OPENGL
+  EdgeSE3SwitchableDrawAction::EdgeSE3SwitchableDrawAction(): DrawAction(typeid(EdgeSE3Switchable).name()){}
+
+  g2o::HyperGraphElementAction* EdgeSE3SwitchableDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
+               g2o::HyperGraphElementAction::Parameters* ){
+    if (typeid(*element).name()!=_typeName)
+      return 0;
+    EdgeSE3Switchable* e =  static_cast<EdgeSE3Switchable*>(element);
+
+
+    g2o::VertexSE3* fromEdge = static_cast<g2o::VertexSE3*>(e->vertices()[0]);
+    g2o::VertexSE3* toEdge   = static_cast<g2o::VertexSE3*>(e->vertices()[1]);
+    VertexSwitchLinear* s   = static_cast<VertexSwitchLinear*>(e->vertices()[2]);
+
+    glColor3f(s->estimate()*1.0,s->estimate()*0.1,s->estimate()*0.1);
+    glPushAttrib(GL_ENABLE_BIT);
+    glDisable(GL_LIGHTING);
+    glBegin(GL_LINES);
+    glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),fromEdge->estimate().translation().z());
+    glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),toEdge->estimate().translation().z());
+    glEnd();
+    glPopAttrib();
+    return this;
+  }
+#endif
+*/

corelib/src/optimizer/vertigo/g2o/edge_se3Switchable.h

@@ -0,0 +1,48 @@
+/*
+ * edge_se3Switchable.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#ifndef EDGE_SE3SWITCHABLE_H_
+#define EDGE_SE3SWITCHABLE_H_
+
+#include "g2o/types/slam3d/vertex_se3.h"
+#include "g2o/core/base_multi_edge.h"
+#include "g2o/core/hyper_graph_action.h"
+
+class EdgeSE3Switchable : public g2o::BaseMultiEdge<6, Eigen::Isometry3d>
+{
+  public:
+    EdgeSE3Switchable();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+    void computeError();
+    void linearizeOplus();
+
+    virtual void setMeasurement(const Eigen::Isometry3d& m){
+      _measurement = m;
+      _inverseMeasurement = m.inverse();
+    }
+
+  protected:
+    Eigen::Isometry3d _inverseMeasurement;
+};
+
+
+#ifdef G2O_HAVE_OPENGL
+    class EdgeSE3SwitchableDrawAction: public g2o::DrawAction{
+    public:
+      EdgeSE3SwitchableDrawAction();
+      virtual g2o::HyperGraphElementAction* operator()(g2o::HyperGraph::HyperGraphElement* element,
+              g2o::HyperGraphElementAction::Parameters* params_);
+    };
+#endif
+
+
+#endif /* EDGE_SE3SWITCHABLE_H_ */

corelib/src/optimizer/vertigo/g2o/edge_switchPrior.cpp

@@ -0,0 +1,44 @@
+#include "edge_switchPrior.h"
+using namespace std;
+
+    EdgeSwitchPrior::EdgeSwitchPrior()
+    {
+    	setMeasurement(1.0);
+    }
+
+    bool EdgeSwitchPrior::read(std::istream &is)
+    {
+      double new_measurement;
+      is >> new_measurement;
+
+      setMeasurement(new_measurement);
+
+      is >> information()(0,0);
+      return true;
+    }
+
+    bool EdgeSwitchPrior::write(std::ostream &os) const
+    {
+      os << measurement() << " " << information()(0,0);
+      return true;
+    }
+
+    void EdgeSwitchPrior::setMeasurement(const double & m)
+    {
+    	g2o::BaseEdge<1, double>::setMeasurement(m);
+    	information()(0,0) = 1.0;
+    }
+
+    void EdgeSwitchPrior::linearizeOplus()
+    {
+      _jacobianOplusXi[0]=-1.0;
+    }
+
+    void EdgeSwitchPrior::computeError()
+    {
+      const VertexSwitchLinear* s = static_cast<const VertexSwitchLinear*>(_vertices[0]);
+
+      _error[0] = measurement() - s->x();
+
+    }
+

corelib/src/optimizer/vertigo/g2o/edge_switchPrior.h

@@ -0,0 +1,22 @@
+#pragma once
+
+
+#include "vertex_switchLinear.h"
+#include "g2o/core/base_unary_edge.h"
+
+
+
+class EdgeSwitchPrior : public g2o::BaseUnaryEdge<1, double, VertexSwitchLinear>
+{
+  public:
+    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+    EdgeSwitchPrior();
+
+    virtual bool read(std::istream& is);
+    virtual bool write(std::ostream& os) const;
+
+    virtual void setMeasurement(const double & m);
+
+    virtual void linearizeOplus();
+    void computeError();
+};

corelib/src/optimizer/vertigo/g2o/types_g2o_robust.cpp

@@ -0,0 +1,23 @@
+#include "g2o/core/factory.h"
+#include "g2o/stuff/macros.h"
+
+#include "edge_switchPrior.h"
+#include "edge_se2Switchable.h"
+//#include "edge_se2MaxMixture.h"
+#include "edge_se3Switchable.h"
+#include "vertex_switchLinear.h"
+
+
+G2O_REGISTER_TYPE(EDGE_SWITCH_PRIOR, EdgeSwitchPrior);
+G2O_REGISTER_TYPE(EDGE_SE2_SWITCHABLE, EdgeSE2Switchable);
+//G2O_REGISTER_TYPE(EDGE_SE2_MAXMIX, EdgeSE2MaxMixture);
+G2O_REGISTER_TYPE(EDGE_SE3_SWITCHABLE, EdgeSE3Switchable);
+G2O_REGISTER_TYPE(VERTEX_SWITCH, VertexSwitchLinear);
+
+/*
+#ifdef G2O_HAVE_OPENGL
+  G2O_REGISTER_ACTION(EdgeSE2SwitchableDrawAction);
+  //G2O_REGISTER_ACTION(EdgeSE2MaxMixtureDrawAction);
+  G2O_REGISTER_ACTION(EdgeSE3SwitchableDrawAction);
+#endif
+*/

corelib/src/optimizer/vertigo/g2o/vertex_switchLinear.cpp

@@ -0,0 +1,59 @@
+/*
+ * vertex_switchLinear.cpp
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+#include "vertex_switchLinear.h"
+#include <iostream>
+
+using namespace std;
+
+    VertexSwitchLinear::VertexSwitchLinear() :
+		_x(0)
+    {
+    	setToOrigin();
+    	setEstimate(1.0);
+    }
+
+    bool VertexSwitchLinear:: read(std::istream& is)
+    {
+      is >> _x;
+      _estimate=_x;
+
+      return true;
+    }
+
+    bool  VertexSwitchLinear::write(std::ostream& os) const
+    {
+      os << _x;
+      return os.good();
+    }
+
+    void VertexSwitchLinear::setToOriginImpl()
+    {
+      _x=0;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::setEstimate(const double &et)
+    {
+      _x=et;
+      _estimate=_x;
+    }
+
+
+    void VertexSwitchLinear::oplusImpl(const double* update)
+    {
+      _x += update[0];
+
+      if (_x<0) _x=0;
+      if (_x>1) _x=1;
+
+      _estimate=_x;
+    }

corelib/src/optimizer/vertigo/g2o/vertex_switchLinear.h

@@ -0,0 +1,43 @@
+/*
+ * vertex_switchLinear.h
+ *
+ *  Created on: 17.10.2011
+ *      Author: niko
+ *
+ *  Updated on: 14.01.2013
+ *      Author: Christian Kerl <christian.kerl@in.tum.de>
+ */
+
+
+#pragma once
+
+#include "g2o/core/base_vertex.h"
+#include <math.h>
+
+
+
+    class VertexSwitchLinear : public g2o::BaseVertex<1, double>
+    {
+
+    public:
+      VertexSwitchLinear();
+
+      virtual void setToOriginImpl();
+
+      virtual void oplusImpl(const double* update);
+
+      virtual bool read(std::istream& is);
+      virtual bool write(std::ostream& os) const;
+      virtual void setEstimate(const double &et);
+
+
+      double x() const { return _x; };
+
+
+      //! The gradient at the current estimate is always 1;
+      double gradient() const { return 1; } ;
+
+    private:
+      double _x;
+
+    };

corelib/src/optimizer/vertigo/gtsam/betweenFactorMaxMix.h

@@ -0,0 +1,67 @@
+/*
+ * betweenFactorMaxMix.h
+ *
+ *  Created on: 14.08.2012
+ *      Author: niko
+ */
+
+#ifndef BETWEENFACTORMAXMIX_H_
+#define BETWEENFACTORMAXMIX_H_
+
+#include <gtsam/linear/NoiseModel.h>
+#include <Eigen/Eigen>
+
+namespace vertigo {
+
+  template<class VALUE>
+  class BetweenFactorMaxMix : public gtsam::NoiseModelFactor2<VALUE, VALUE>
+  {
+    public:
+      BetweenFactorMaxMix() : weight(0.0) {};
+      BetweenFactorMaxMix(gtsam::Key key1, gtsam::Key key2, const VALUE& measured, const gtsam::SharedNoiseModel& model, const gtsam::SharedNoiseModel& model2, double w)
+      : gtsam::NoiseModelFactor2<VALUE, VALUE>(model, key1, key2), weight(w), nullHypothesisModel(model2),
+        betweenFactor(key1, key2, measured, model)  {   };
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none) const
+        {
+
+          // calculate error
+          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+
+
+          // which hypothesis is more likely
+          double m1 = this->noiseModel_->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g1 = this->noiseModel_;
+          gtsam::Matrix info1(g1->R().transpose()*g1->R());
+          double nu1 = 1.0/sqrt(gtsam::inverse(info1).determinant());
+          double l1 = nu1 * exp(-0.5*m1);
+
+          double m2 = nullHypothesisModel->distance(error);
+          gtsam::noiseModel::Gaussian::shared_ptr g2 = nullHypothesisModel;
+          gtsam::Matrix info2(g2->R().transpose()*g2->R());
+          double nu2 = 1.0/sqrt(gtsam::inverse(info2).determinant());
+          double l2 = nu2 * exp(-0.5*m2);
+
+          // if the null hypothesis is more likely, than proceed by applying the weight ...
+          if (l2>l1) {
+            if (H1) *H1 = *H1 * weight;
+            if (H2) *H2 = *H2 * weight;
+            error *= sqrt(weight);
+          }
+
+          return error;
+        };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+      gtsam::SharedNoiseModel nullHypothesisModel;
+      double weight;
+
+  };
+}
+
+
+#endif /* BETWEENFACTORMAXMIX_H_ */

corelib/src/optimizer/vertigo/gtsam/betweenFactorSwitchable.h

@@ -0,0 +1,97 @@
+/*
+ * betweenFactorSwitchable.h
+ *
+ *  Created on: 02.08.2012
+ *      Author: niko
+ */
+
+#ifndef BETWEENFACTORSWITCHABLE_H_
+#define BETWEENFACTORSWITCHABLE_H_
+
+#include <gtsam/nonlinear/NonlinearFactor.h>
+
+#include <iostream>
+using std::cout;
+using std::endl;
+
+#include "switchVariableLinear.h"
+#include "switchVariableSigmoid.h"
+
+
+namespace vertigo {
+
+  template<class VALUE>
+  class BetweenFactorSwitchableLinear : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>
+  {
+    public:
+      BetweenFactorSwitchableLinear() {};
+      BetweenFactorSwitchableLinear(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
+      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableLinear>(model, key1, key2, key3),
+        betweenFactor(key1, key2, measured, model) {};
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableLinear& s,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+        {
+
+          // calculate error
+          gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+          error *= s.value();
+
+          // handle derivatives
+          if (H1) *H1 = *H1 * s.value();
+          if (H2) *H2 = *H2 * s.value();
+          if (H3) *H3 = error;
+
+          return error;
+        };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+
+  };
+
+
+
+  template<class VALUE>
+  class BetweenFactorSwitchableSigmoid : public gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>
+  {
+    public:
+      BetweenFactorSwitchableSigmoid() {};
+      BetweenFactorSwitchableSigmoid(gtsam::Key key1, gtsam::Key key2, gtsam::Key key3, const VALUE& measured, const gtsam::SharedNoiseModel& model)
+      : gtsam::NoiseModelFactor3<VALUE, VALUE, SwitchVariableSigmoid>(model, key1, key2, key3),
+        betweenFactor(key1, key2, measured, model) {};
+
+      gtsam::Vector evaluateError(const VALUE& p1, const VALUE& p2, const SwitchVariableSigmoid& s,
+          boost::optional<gtsam::Matrix&> H1 = boost::none,
+          boost::optional<gtsam::Matrix&> H2 =  boost::none,
+          boost::optional<gtsam::Matrix&> H3 =  boost::none) const
+      {
+
+        // calculate error
+        gtsam::Vector error = betweenFactor.evaluateError(p1, p2, H1, H2);
+
+
+        double w = sigmoid(s.value());
+        error *= w;
+
+        // handle derivatives
+        if (H1) *H1 = *H1 * w;
+        if (H2) *H2 = *H2 * w;
+        if (H3) *H3 = error /* (w*(1.0-w))*/;  // sig(x)*(1-sig(x)) is the derivative of sig(x) wrt. x
+
+        return error;
+      };
+
+    private:
+      gtsam::BetweenFactor<VALUE> betweenFactor;
+
+      double sigmoid(double x) const {
+        return 1.0/(1.0+exp(-x));
+      }
+  };
+
+}
+
+#endif /* BETWEENFACTORSWITCHABLE_H_ */

corelib/src/optimizer/vertigo/gtsam/switchVariableLinear.h

@@ -0,0 +1,130 @@
+/*
+ * switchVariableLinear.h
+ *
+ *  Created on: 02.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLELINEAR_H_
+#define SWITCHVARIABLELINEAR_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableLinear is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableLinear : public gtsam::DerivedValue<SwitchVariableLinear> {
+
+    /** default constructor */
+    SwitchVariableLinear() : d_(0.0) {};
+
+    /** wrap a double */
+    SwitchVariableLinear(double d) : d_(d) {
+     // if (d_ < 0.0) d_=0.0;
+     // else if(d_>1.0) d_=1.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableLinear& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableLinear with a tangent space update */
+    inline SwitchVariableLinear retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>1.0) x=1.0;
+      else if (x<0.0) x=0.0;
+
+      return SwitchVariableLinear(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableLinear& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableLinear identity() {
+      return SwitchVariableLinear();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableLinear compose(const SwitchVariableLinear& p) const {
+      return SwitchVariableLinear(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableLinear between(const SwitchVariableLinear& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableLinear(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableLinear inverse() const {
+      return SwitchVariableLinear(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableLinear Expmap(const gtsam::Vector& v) { return SwitchVariableLinear(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableLinear& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableLinear> {
+  static void Print(const vertigo::SwitchVariableLinear& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableLinear& key1, const vertigo::SwitchVariableLinear& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableLinear & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableLinear& origin, const vertigo::SwitchVariableLinear& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableLinear Retract(const vertigo::SwitchVariableLinear& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+
+
+#endif /* SWITCHVARIABLELINEAR_H_ */

corelib/src/optimizer/vertigo/gtsam/switchVariableSigmoid.h

@@ -0,0 +1,129 @@
+/*
+ * switchVariableSigmoid.h
+ *
+ *  Created on: 08.08.2012
+ *      Author: niko
+ */
+
+#ifndef SWITCHVARIABLESIGMOID_H_
+#define SWITCHVARIABLESIGMOID_H_
+
+#pragma once
+
+#include <gtsam/base/DerivedValue.h>
+#include <gtsam/base/Lie.h>
+
+namespace vertigo {
+
+  /**
+   * SwitchVariableSigmoid is a wrapper around double to allow it to be a Lie type
+   */
+  struct SwitchVariableSigmoid : public gtsam::DerivedValue<SwitchVariableSigmoid> {
+
+    /** default constructor */
+    SwitchVariableSigmoid() : d_(10.0) {};
+
+    /** wrap a double */
+    SwitchVariableSigmoid(double d) : d_(d) {
+      if (d_ < -10.0) d_=-10.0;
+      else if(d_>10.0) d_=10.0;
+    };
+
+    /** access the underlying value */
+    double value() const { return d_; }
+
+    /** print @param s optional string naming the object */
+    inline void print(const std::string& name="") const {
+      std::cout << name << ": " << d_ << std::endl;
+    }
+
+    /** equality up to tolerance */
+    inline bool equals(const SwitchVariableSigmoid& expected, double tol=1e-5) const {
+      return fabs(expected.d_ - d_) <= tol;
+    }
+
+    // Manifold requirements
+
+    /** Returns dimensionality of the tangent space */
+    inline size_t dim() const { return 1; }
+    inline static size_t Dim() { return 1; }
+
+    /** Update the SwitchVariableSigmoid with a tangent space update */
+    inline SwitchVariableSigmoid retract(const gtsam::Vector& v) const {
+      double x = value() + v(0);
+
+      if (x>10.0) x=10.0;
+      else if (x<-10.0) x=-10.0;
+
+      return SwitchVariableSigmoid(x);
+    }
+
+    /** @return the local coordinates of another object */
+    inline gtsam::Vector localCoordinates(const SwitchVariableSigmoid& t2) const { return gtsam::Vector1(t2.value() - value()); }
+
+    // Group requirements
+
+    /** identity */
+    inline static SwitchVariableSigmoid identity() {
+      return SwitchVariableSigmoid();
+    }
+
+    /** compose with another object */
+    inline SwitchVariableSigmoid compose(const SwitchVariableSigmoid& p) const {
+      return SwitchVariableSigmoid(d_ + p.d_);
+    }
+
+    /** between operation */
+    inline SwitchVariableSigmoid between(const SwitchVariableSigmoid& l2,
+        boost::optional<gtsam::Matrix&> H1=boost::none,
+        boost::optional<gtsam::Matrix&> H2=boost::none) const {
+      if(H1) *H1 = -gtsam::eye(1);
+      if(H2) *H2 = gtsam::eye(1);
+      return SwitchVariableSigmoid(l2.value() - value());
+    }
+
+    /** invert the object and yield a new one */
+    inline SwitchVariableSigmoid inverse() const {
+      return SwitchVariableSigmoid(-1.0 * value());
+    }
+
+    // Lie functions
+
+    /** Expmap around identity */
+    static inline SwitchVariableSigmoid Expmap(const gtsam::Vector& v) { return SwitchVariableSigmoid(v(0)); }
+
+    /** Logmap around identity - just returns with default cast back */
+    static inline gtsam::Vector Logmap(const SwitchVariableSigmoid& p) { return gtsam::Vector1(p.value()); }
+
+  private:
+      double d_;
+  };
+}
+
+
+namespace gtsam {
+// Define Key to be Testable by specializing gtsam::traits
+template<typename T> struct traits;
+template<> struct traits<vertigo::SwitchVariableSigmoid> {
+  static void Print(const vertigo::SwitchVariableSigmoid& key, const std::string& str = "") {
+    key.print(str);
+  }
+  static bool Equals(const vertigo::SwitchVariableSigmoid& key1, const vertigo::SwitchVariableSigmoid& key2, double tol = 1e-8) {
+    return key1.equals(key2, tol);
+  }
+  static int GetDimension(const vertigo::SwitchVariableSigmoid & key) {return key.Dim();}
+
+  typedef OptionalJacobian<3, 3> ChartJacobian;
+  typedef gtsam::Vector TangentVector;
+  static TangentVector Local(const vertigo::SwitchVariableSigmoid& origin, const vertigo::SwitchVariableSigmoid& other,
+  ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
+    return origin.localCoordinates(other);
+  }
+  static vertigo::SwitchVariableSigmoid Retract(const vertigo::SwitchVariableSigmoid& g, const TangentVector& v,
+        ChartJacobian H1 = boost::none, ChartJacobian H2 = boost::none) {
+      return g.retract(v);
+    }
+};
+}
+
+#endif /* SWITCHVARIABLESIGMOID_H_ */

corelib/src/optimizer/vertigo/readme.txt

@@ -0,0 +1,8 @@
+
+Info: http://openslam.org/vertigo.html
+Source: https://github.com/christiankerl/vertigo/tree/master/trunk
+Commit: fbd438488a56cdba805fa2f75aa3e394e3eda7ff
+License: GPL v3
+
+Tested with g2o (ROS Indigo/2014.02.18)
+Tested with GTSAM commit c73b835