From 2f2a40a737eac17d73eac733ee6eaed660ec6e1e Mon Sep 17 00:00:00 2001 From: Luca Date: Sun, 17 Aug 2014 20:02:46 -0400 Subject: [PATCH] initial implementation of chordal relaxation --- gtsam/slam/InitializePose3.cpp | 237 +++++++++++++++++++++++ gtsam/slam/InitializePose3.h | 60 ++++++ gtsam/slam/tests/testInitializePose3.cpp | 96 +++++++++ 3 files changed, 393 insertions(+) create mode 100644 gtsam/slam/InitializePose3.cpp create mode 100644 gtsam/slam/InitializePose3.h create mode 100644 gtsam/slam/tests/testInitializePose3.cpp diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp new file mode 100644 index 000000000..d4f93293c --- /dev/null +++ b/gtsam/slam/InitializePose3.cpp @@ -0,0 +1,237 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InitializePose3.h + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; + +namespace gtsam { +namespace InitializePose3 { + +//static const Matrix I = eye(1); +static const Matrix I9 = eye(9); +static const Vector zero9 = Vector::Zero(9); +static const Vector zero33= Matrix::Zero(3,3); + +static const Key keyAnchor = symbol('Z', 9999999); + +/* ************************************************************************* */ +GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { + + GaussianFactorGraph linearGraph; + noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); + + BOOST_FOREACH(const boost::shared_ptr& factor, g) { + Matrix3 Rijt; + + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between) + Rijt = pose3Between->measured().rotation().matrix().transpose(); + else + std::cout << "Error in buildLinearOrientationGraph" << std::endl; + + const FastVector& keys = factor->keys(); + Key key1 = keys[0], key2 = keys[1]; + Matrix M9 = (Matrix(9,9) << Rijt, zero33, zero33, + zero33, Rijt, zero33, + zero33, zero33, Rijt); + linearGraph.add(key1, -I9, key2, M9, zero9, model); + } + // prior on the anchor orientation + linearGraph.add(keyAnchor, I9, (Vector(1) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model); + return linearGraph; +} + +/* ************************************************************************* */ +// Transform VectorValues into valid Rot3 +Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { + gttic(InitializePose3_computeOrientations); + + Values validRot3; + + return validRot3; +} + +/* ************************************************************************* */ +// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node +NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { + gttic(InitializePose3_buildPose3graph); + NonlinearFactorGraph pose3Graph; + + BOOST_FOREACH(const boost::shared_ptr& factor, graph) { + + // recast to a between on Pose3 + boost::shared_ptr > pose3Between = + boost::dynamic_pointer_cast >(factor); + if (pose3Between) + pose3Graph.add(pose3Between); + + // recast PriorFactor to BetweenFactor + boost::shared_ptr > pose3Prior = + boost::dynamic_pointer_cast >(factor); + if (pose3Prior) + pose3Graph.add( + BetweenFactor(keyAnchor, pose3Prior->keys()[0], + pose3Prior->prior(), pose3Prior->get_noiseModel())); + } + return pose3Graph; +} + +/* ************************************************************************* */ +// Return the orientations of a graph including only BetweenFactors +Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { + gttic(InitializePose3_computeOrientations); + + // regularize measurements and plug everything in a factor graph + GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); + + // Solve the LFG + VectorValues relaxedRot3 = relaxedGraph.optimize(); + + // normalize and compute Rot3 + Values initializedRot3 = normalizeRelaxedRotations(relaxedRot3); + + return initializedRot3; +} + +/* ************************************************************************* */ +Values initializeOrientations(const NonlinearFactorGraph& graph) { + + // We "extract" the Pose3 subgraph of the original graph: this + // is done to properly model priors and avoiding operating on a larger graph + NonlinearFactorGraph pose3Graph = buildPose3graph(graph); + + // Get orientations from relative orientation measurements + return computeOrientations(pose3Graph); +} + +///* ************************************************************************* */ +//Values computePoses(const NonlinearFactorGraph& pose2graph, +// VectorValues& orientationsLago) { +// gttic(lago_computePoses); +// +// // Linearized graph on full poses +// GaussianFactorGraph linearPose2graph; +// +// // We include the linear version of each between factor +// BOOST_FOREACH(const boost::shared_ptr& factor, pose2graph) { +// +// boost::shared_ptr > pose2Between = +// boost::dynamic_pointer_cast >(factor); +// +// if (pose2Between) { +// Key key1 = pose2Between->keys()[0]; +// double theta1 = orientationsLago.at(key1)(0); +// double s1 = sin(theta1); +// double c1 = cos(theta1); +// +// Key key2 = pose2Between->keys()[1]; +// double theta2 = orientationsLago.at(key2)(0); +// +// double linearDeltaRot = theta2 - theta1 +// - pose2Between->measured().theta(); +// linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize +// +// double dx = pose2Between->measured().x(); +// double dy = pose2Between->measured().y(); +// +// Vector globalDeltaCart = // +// (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy); +// Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs +// Matrix J1 = -I3; +// J1(0, 2) = s1 * dx + c1 * dy; +// J1(1, 2) = -c1 * dx + s1 * dy; +// // Retrieve the noise model for the relative rotation +// boost::shared_ptr diagonalModel = +// boost::dynamic_pointer_cast( +// pose2Between->get_noiseModel()); +// +// linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel); +// } else { +// throw invalid_argument( +// "computeLagoPoses: cannot manage non between factor here!"); +// } +// } +// // add prior +// linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), +// priorPose2Noise); +// +// // optimize +// VectorValues posesLago = linearPose2graph.optimize(); +// +// // put into Values structure +// Values initialGuessLago; +// BOOST_FOREACH(const VectorValues::value_type& it, posesLago) { +// Key key = it.first; +// if (key != keyAnchor) { +// const Vector& poseVector = it.second; +// Pose2 poseLago = Pose2(poseVector(0), poseVector(1), +// orientationsLago.at(key)(0) + poseVector(2)); +// initialGuessLago.insert(key, poseLago); +// } +// } +// return initialGuessLago; +//} + +///* ************************************************************************* */ +//Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) { +// gttic(lago_initialize); +// +// // We "extract" the Pose2 subgraph of the original graph: this +// // is done to properly model priors and avoiding operating on a larger graph +// NonlinearFactorGraph pose2Graph = buildPose2graph(graph); +// +// // Get orientations from relative orientation measurements +// VectorValues orientationsLago = computeOrientations(pose2Graph, +// useOdometricPath); +// +// // Compute the full poses +// return computePoses(pose2Graph, orientationsLago); +//} +// +///* ************************************************************************* */ +//Values initialize(const NonlinearFactorGraph& graph, +// const Values& initialGuess) { +// Values initialGuessLago; +// +// // get the orientation estimates from LAGO +// VectorValues orientations = initializeOrientations(graph); +// +// // for all nodes in the tree +// BOOST_FOREACH(const VectorValues::value_type& it, orientations) { +// Key key = it.first; +// if (key != keyAnchor) { +// const Pose2& pose = initialGuess.at(key); +// const Vector& orientation = it.second; +// Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0)); +// initialGuessLago.insert(key, poseLago); +// } +// } +// return initialGuessLago; +//} + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h new file mode 100644 index 000000000..33efe3f85 --- /dev/null +++ b/gtsam/slam/InitializePose3.h @@ -0,0 +1,60 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file InitializePose3.h + * @brief Initialize Pose3 in a factor graph + * + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { +namespace InitializePose3 { + +GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + +GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + +GTSAM_EXPORT Values computeOrientations(const NonlinearFactorGraph& pose3Graph); + +GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); + +GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); + +///** Linear factor graph with regularized orientation measurements */ +//GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph( +// const std::vector& spanningTreeIds, +// const std::vector& chordsIds, const NonlinearFactorGraph& g, +// const key2doubleMap& orientationsToRoot, const PredecessorMap& tree); +// +///** LAGO: Return the orientations of the Pose2 in a generic factor graph */ +//GTSAM_EXPORT VectorValues initializeOrientations( +// const NonlinearFactorGraph& graph, bool useOdometricPath = true); +// +///** Return the values for the Pose2 in a generic factor graph */ +//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, +// bool useOdometricPath = true); +// +///** Only correct the orientation part in initialGuess */ +//GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, +// const Values& initialGuess); + +} // end of namespace lago +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp new file mode 100644 index 000000000..4cd374b64 --- /dev/null +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -0,0 +1,96 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testInitializePose3.cpp + * @brief Unit tests for 3D SLAM initialization, using rotation relaxation + * + * @author Luca Carlone + * @author Frank Dellaert + * @date August, 2014 + */ + +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace gtsam; +using namespace boost::assign; + +static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3); +static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1)); + +namespace simple { +// We consider a small graph: +// symbolic FG +// x2 0 1 +// / | \ 1 2 +// / | \ 2 3 +// x3 | x1 2 0 +// \ | / 0 3 +// \ | / +// x0 +// +static Point3 p1 = Point3(0,0,0); +static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) ); +static Point3 p2 = Point3(1,2,0); +static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) ); +static Point3 p3 = Point3(0,2,0); +static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) ); +static Point3 p4 = Point3(-1,1,0); +static Rot3 R4 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) ); + +static Pose3 pose0 = Pose3(R1,p1); +static Pose3 pose1 = Pose3(R2,p2); +static Pose3 pose2 = Pose3(R3,p3); +static Pose3 pose3 = Pose3(R4,p4); + +NonlinearFactorGraph graph() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), model)); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), model)); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), model)); + g.add(BetweenFactor(x2, x0, pose2.between(pose0), model)); + g.add(BetweenFactor(x0, x3, pose0.between(pose3), model)); + g.add(PriorFactor(x0, pose0, model)); + return g; +} +} + +/* *************************************************************************** */ +TEST( InitializePose3, buildPose3graph ) { + NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); + // pose3graph.print(""); +} + +/* *************************************************************************** */ +TEST( InitializePose3, orientations ) { + Values initial = InitializePose3::initializeOrientations(simple::graph()); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(initial.at(0), simple::pose0, 1e-6)); +} + + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ +