diff --git a/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py new file mode 100644 index 000000000..02c696905 --- /dev/null +++ b/cython/gtsam/examples/Pose3SLAMExample_initializePose3Chordal.py @@ -0,0 +1,35 @@ +""" +GTSAM Copyright 2010-2018, 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 + +Initialize PoseSLAM with Chordal init +Author: Luca Carlone, Frank Dellaert (python port) +""" +# pylint: disable=invalid-name, E1101 + +from __future__ import print_function + +import numpy as np + +import gtsam + +# Read graph from file +g2oFile = gtsam.findExampleDataFile("pose3example.txt") + +is3D = True +graph, initial = gtsam.readG2o(g2oFile, is3D) + +# Add prior on the first key. TODO: assumes first key ios z +priorModel = gtsam.noiseModel_Diagonal.Variances( + np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4])) +firstKey = initial.keys().at(0) +graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel)) + +# Initializing Pose3 - chordal relaxation" +initialization = gtsam.InitializePose3.initialize(graph) + +print(initialization) diff --git a/cython/gtsam/tests/test_initialize_pose3.py b/cython/gtsam/tests/test_initialize_pose3.py new file mode 100644 index 000000000..ee9195c48 --- /dev/null +++ b/cython/gtsam/tests/test_initialize_pose3.py @@ -0,0 +1,88 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Unit tests for 3D SLAM initialization, using rotation relaxation. +Author: Luca Carlone and Frank Dellaert (Python) +""" +# pylint: disable=invalid-name, E1101, E0611 +import unittest + +import numpy as np + +import gtsam +from gtsam import NonlinearFactorGraph, Point3, Pose3, Rot3, Values + +x0, x1, x2, x3 = 0, 1, 2, 3 + + +class TestValues(unittest.TestCase): + + def setUp(self): + + model = gtsam.noiseModel_Isotropic.Sigma(6, 0.1) + + # We consider a small graph: + # symbolic FG + # x2 0 1 + # / | \ 1 2 + # / | \ 2 3 + # x3 | x1 2 0 + # \ | / 0 3 + # \ | / + # x0 + # + p0 = Point3(0, 0, 0) + self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0])) + p1 = Point3(1, 2, 0) + self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796])) + p2 = Point3(0, 2, 0) + self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593])) + p3 = Point3(-1, 1, 0) + self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389])) + + pose0 = Pose3(self.R0, p0) + pose1 = Pose3(self.R1, p1) + pose2 = Pose3(self.R2, p2) + pose3 = Pose3(self.R3, p3) + + g = NonlinearFactorGraph() + g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model)) + g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model)) + g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model)) + g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model)) + g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model)) + g.add(gtsam.PriorFactorPose3(x0, pose0, model)) + self.graph = g + + def test_buildPose3graph(self): + pose3graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + def test_orientations(self): + pose3Graph = gtsam.InitializePose3.buildPose3graph(self.graph) + + initial = gtsam.InitializePose3.computeOrientationsChordal(pose3Graph) + + # comparison is up to M_PI, that's why we add some multiples of 2*M_PI + self.assertTrue(initial.atRot3(x0).equals(self.R0, 1e-6)) + self.assertTrue(initial.atRot3(x1).equals(self.R1, 1e-6)) + self.assertTrue(initial.atRot3(x2).equals(self.R2, 1e-6)) + self.assertTrue(initial.atRot3(x3).equals(self.R3, 1e-6)) + + def test_initializePoses(self): + g2oFile = gtsam.findExampleDataFile("pose3example-grid") + is3D = True + inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D) + priorModel = gtsam.noiseModel_Unit.Create(6) + inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel)) + + initial = gtsam.InitializePose3.initialize(inputGraph) + # TODO(frank): very loose !! + self.assertTrue(initial.equals(expectedValues, 0.1)) + + +if __name__ == "__main__": + unittest.main() diff --git a/gtsam.h b/gtsam.h index 9a06b7bd5..014907037 100644 --- a/gtsam.h +++ b/gtsam.h @@ -2523,6 +2523,26 @@ class BetweenFactorPose3s gtsam::BetweenFactorPose3* at(size_t i) const; }; +#include +class InitializePose3 { + static gtsam::Values computeOrientationsChordal( + const gtsam::NonlinearFactorGraph& pose3Graph); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess, size_t maxIter, const bool setRefFrame); + static gtsam::Values computeOrientationsGradient( + const gtsam::NonlinearFactorGraph& pose3Graph, + const gtsam::Values& givenGuess); + static gtsam::NonlinearFactorGraph buildPose3graph( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initializeOrientations( + const gtsam::NonlinearFactorGraph& graph); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph, + const gtsam::Values& givenGuess, + bool useGradient); + static gtsam::Values initialize(const gtsam::NonlinearFactorGraph& graph); +}; + gtsam::BetweenFactorPose3s parse3DFactors(string filename); pair load3D(string filename); diff --git a/gtsam/slam/InitializePose3.cpp b/gtsam/slam/InitializePose3.cpp index 3979996da..2f247811d 100644 --- a/gtsam/slam/InitializePose3.cpp +++ b/gtsam/slam/InitializePose3.cpp @@ -16,7 +16,7 @@ * @date August, 2014 */ -#include +#include #include #include #include @@ -29,69 +29,65 @@ using namespace std; namespace gtsam { -namespace InitializePose3 { -static const Matrix I9 = I_9x9; -static const Vector zero9 = Vector::Zero(9); -static const Matrix zero33 = Z_3x3; - -static const Key keyAnchor = symbol('Z', 9999999); +static const Key kAnchorKey = symbol('Z', 9999999); /* ************************************************************************* */ -GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { +GaussianFactorGraph InitializePose3::buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph linearGraph; - noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); - for(const boost::shared_ptr& factor: g) { + for(const auto& factor: g) { Matrix3 Rij; + double rotationPrecision = 1.0; - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); - if (pose3Between) + auto pose3Between = boost::dynamic_pointer_cast >(factor); + if (pose3Between){ Rij = pose3Between->measured().rotation().matrix(); - else - std::cout << "Error in buildLinearOrientationGraph" << std::endl; + Vector precisions = Vector::Zero(6); + precisions[0] = 1.0; // vector of all zeros except first entry equal to 1 + pose3Between->noiseModel()->whitenInPlace(precisions); // gets marginal precision of first variable + rotationPrecision = precisions[0]; // rotations first + }else{ + cout << "Error in buildLinearOrientationGraph" << endl; + } - const KeyVector& keys = factor->keys(); + const auto& keys = factor->keys(); Key key1 = keys[0], key2 = keys[1]; Matrix M9 = Z_9x9; M9.block(0,0,3,3) = Rij; M9.block(3,3,3,3) = Rij; M9.block(6,6,3,3) = Rij; - linearGraph.add(key1, -I9, key2, M9, zero9, model); + linearGraph.add(key1, -I_9x9, key2, M9, Z_9x1, noiseModel::Isotropic::Precision(9, rotationPrecision)); } // prior on the anchor orientation - linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0).finished(), model); + linearGraph.add( + kAnchorKey, I_9x9, + (Vector(9) << 1.0, 0.0, 0.0, /* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0) + .finished(), + noiseModel::Isotropic::Precision(9, 1)); return linearGraph; } /* ************************************************************************* */ // Transform VectorValues into valid Rot3 -Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { +Values InitializePose3::normalizeRelaxedRotations( + const VectorValues& relaxedRot3) { gttic(InitializePose3_computeOrientationsChordal); Matrix ppm = Z_3x3; // plus plus minus ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; Values validRot3; - for(const VectorValues::value_type& it: relaxedRot3) { + for(const auto& it: relaxedRot3) { Key key = it.first; - if (key != keyAnchor) { - const Vector& rotVector = it.second; - Matrix3 rotMat; - rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2); - rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5); - rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8); + if (key != kAnchorKey) { + Matrix3 R; R << it.second; Matrix U, V; Vector s; - svd(rotMat, U, s, V); + svd(R.transpose(), U, s, V); Matrix3 normalizedRotMat = U * V.transpose(); - // std::cout << "rotMat \n" << rotMat << std::endl; - // std::cout << "U V' \n" << U * V.transpose() << std::endl; - // std::cout << "V \n" << V << std::endl; - if(normalizedRotMat.determinant() < 0) normalizedRotMat = U * ppm * V.transpose(); @@ -103,32 +99,29 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { } /* ************************************************************************* */ -// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node -NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) { +NonlinearFactorGraph InitializePose3::buildPose3graph(const NonlinearFactorGraph& graph) { gttic(InitializePose3_buildPose3graph); NonlinearFactorGraph pose3Graph; - for(const boost::shared_ptr& factor: graph) { + for(const auto& factor: graph) { // recast to a between on Pose3 - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + const auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between) pose3Graph.add(pose3Between); // recast PriorFactor to BetweenFactor - boost::shared_ptr > pose3Prior = - boost::dynamic_pointer_cast >(factor); + const auto pose3Prior = boost::dynamic_pointer_cast >(factor); if (pose3Prior) - pose3Graph.emplace_shared >(keyAnchor, pose3Prior->keys()[0], + pose3Graph.emplace_shared >(kAnchorKey, pose3Prior->keys()[0], pose3Prior->prior(), pose3Prior->noiseModel()); } return pose3Graph; } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { +Values InitializePose3::computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph) { gttic(InitializePose3_computeOrientationsChordal); // regularize measurements and plug everything in a factor graph @@ -142,14 +135,15 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { } /* ************************************************************************* */ -// Return the orientations of a graph including only BetweenFactors -Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { +Values InitializePose3::computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + const size_t maxIter, const bool setRefFrame) { gttic(InitializePose3_computeOrientationsGradient); // this works on the inverse rotations, according to Tron&Vidal,2011 Values inverseRot; - inverseRot.insert(keyAnchor, Rot3()); - for(const Values::ConstKeyValuePair& key_value: givenGuess) { + inverseRot.insert(kAnchorKey, Rot3()); + for(const auto& key_value: givenGuess) { Key key = key_value.key; const Pose3& pose = givenGuess.at(key); inverseRot.insert(key, pose.rotation().inverse()); @@ -164,9 +158,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const // calculate max node degree & allocate gradient size_t maxNodeDeg = 0; VectorValues grad; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - grad.insert(key,Vector3::Zero()); + grad.insert(key,Z_3x1); size_t currNodeDeg = (adjEdgesMap.at(key)).size(); if(currNodeDeg > maxNodeDeg) maxNodeDeg = currNodeDeg; @@ -180,42 +174,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const double mu_max = maxNodeDeg * rho; double stepsize = 2/mu_max; // = 1/(a b dG) - std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; double maxGrad; // gradient iterations size_t it; - for(it=0; it < maxIter; it++){ + for (it = 0; it < maxIter; it++) { ////////////////////////////////////////////////////////////////////////// // compute the gradient at each node - //std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a - // <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl; maxGrad = 0; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for (const auto& key_value : inverseRot) { Key key = key_value.key; - //std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; - Vector gradKey = Vector3::Zero(); + Vector gradKey = Z_3x1; // collect the gradient for each edge incident on key - for(const size_t& factorId: adjEdgesMap.at(key)){ + for (const size_t& factorId : adjEdgesMap.at(key)) { Rot3 Rij = factorId2RotMap.at(factorId); Rot3 Ri = inverseRot.at(key); - if( key == (pose3Graph.at(factorId))->keys()[0] ){ - Key key1 = (pose3Graph.at(factorId))->keys()[1]; + auto factor = pose3Graph.at(factorId); + const auto& keys = factor->keys(); + if (key == keys[0]) { + Key key1 = keys[1]; Rot3 Rj = inverseRot.at(key1); - gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); - //std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl; - }else if( key == (pose3Graph.at(factorId))->keys()[1] ){ - Key key0 = (pose3Graph.at(factorId))->keys()[0]; + gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); + } else if (key == keys[1]) { + Key key0 = keys[0]; Rot3 Rj = inverseRot.at(key0); gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); - //std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl; - }else{ - std::cout << "Error in gradient computation" << std::endl; + } else { + cout << "Error in gradient computation" << endl; } - } // end of i-th gradient computation - grad.at(key) = stepsize * gradKey; + } // end of i-th gradient computation + grad.at(key) = stepsize * gradKey; double normGradKey = (gradKey).norm(); - //std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl; if(normGradKey>maxGrad) maxGrad = normGradKey; } // end of loop over nodes @@ -230,14 +219,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const break; } // enf of gradient iterations - std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl; - // Return correct rotations - const Rot3& Rref = inverseRot.at(keyAnchor); // This will be set to the identity as so far we included no prior + const Rot3& Rref = inverseRot.at(kAnchorKey); // This will be set to the identity as so far we included no prior Values estimateRot; - for(const Values::ConstKeyValuePair& key_value: inverseRot) { + for(const auto& key_value: inverseRot) { Key key = key_value.key; - if (key != keyAnchor) { + if (key != kAnchorKey) { const Rot3& R = inverseRot.at(key); if(setRefFrame) estimateRot.insert(key, Rref.compose(R.inverse())); @@ -249,11 +236,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const } /* ************************************************************************* */ -void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ +void InitializePose3::createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph) { size_t factorId = 0; - for(const boost::shared_ptr& factor: pose3Graph) { - boost::shared_ptr > pose3Between = - boost::dynamic_pointer_cast >(factor); + for(const auto& factor: pose3Graph) { + auto pose3Between = boost::dynamic_pointer_cast >(factor); if (pose3Between){ Rot3 Rij = pose3Between->measured().rotation(); factorId2RotMap.insert(pair(factorId,Rij)); @@ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, adjEdgesMap.insert(pair >(key2, edge_id)); } }else{ - std::cout << "Error in computeOrientationsGradient" << std::endl; + cout << "Error in createSymbolicGraph" << endl; } factorId++; } } /* ************************************************************************* */ -Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { +Vector3 InitializePose3::gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) { Vector3 logRot = Rot3::Logmap(R1.between(R2)); double th = logRot.norm(); @@ -292,10 +279,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl th = logRot.norm(); } // exclude small or invalid rotations - if (th > 1e-5 && th == th){ // nonzero valid rotations + if (th > 1e-5 && th == th) { // nonzero valid rotations logRot = logRot / th; - }else{ - logRot = Vector3::Zero(); + } else { + logRot = Z_3x1; th = 0.0; } @@ -304,8 +291,7 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl } /* ************************************************************************* */ -Values initializeOrientations(const NonlinearFactorGraph& graph) { - +Values InitializePose3::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); @@ -315,29 +301,30 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { } ///* ************************************************************************* */ -Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { +Values InitializePose3::computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { gttic(InitializePose3_computePoses); // put into Values structure Values initialPose; - for(const Values::ConstKeyValuePair& key_value: initialRot){ + for (const auto& key_value : initialRot) { Key key = key_value.key; const Rot3& rot = initialRot.at(key); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); initialPose.insert(key, initializedPose); } + // add prior noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); - initialPose.insert(keyAnchor, Pose3()); - pose3graph.emplace_shared >(keyAnchor, Pose3(), priorModel); + initialPose.insert(kAnchorKey, Pose3()); + pose3graph.emplace_shared >(kAnchorKey, Pose3(), priorModel); // Create optimizer GaussNewtonParams params; bool singleIter = true; - if(singleIter){ + if (singleIter) { params.maxIterations = 1; - }else{ - std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <(key); estimate.insert(key, pose); } @@ -356,22 +343,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) { } /* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph) { +Values InitializePose3::initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, + bool useGradient) { gttic(InitializePose3_initialize); - - // 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 - Values valueRot3 = computeOrientationsChordal(pose3Graph); - - // Compute the full poses (1 GN iteration on full poses) - return computePoses(pose3Graph, valueRot3); -} - -/* ************************************************************************* */ -Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { Values initialValues; // We "extract" the Pose3 subgraph of the original graph: this @@ -380,27 +354,18 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b // Get orientations from relative orientation measurements Values orientations; - if(useGradient) + if (useGradient) orientations = computeOrientationsGradient(pose3Graph, givenGuess); else orientations = computeOrientationsChordal(pose3Graph); -// orientations.print("orientations\n"); - // Compute the full poses (1 GN iteration on full poses) return computePoses(pose3Graph, orientations); - - // for(const Values::ConstKeyValuePair& key_value: orientations) { - // Key key = key_value.key; - // if (key != keyAnchor) { - // const Point3& pos = givenGuess.at(key).translation(); - // const Rot3& rot = orientations.at(key); - // Pose3 initializedPoses = Pose3(rot, pos); - // initialValues.insert(key, initializedPoses); - // } - // } - // return initialValues; } -} // end of namespace lago -} // end of namespace gtsam +/* ************************************************************************* */ +Values InitializePose3::initialize(const NonlinearFactorGraph& graph) { + return initialize(graph, Values(), false); +} + +} // namespace gtsam diff --git a/gtsam/slam/InitializePose3.h b/gtsam/slam/InitializePose3.h index dba5ceac3..ce1b28854 100644 --- a/gtsam/slam/InitializePose3.h +++ b/gtsam/slam/InitializePose3.h @@ -20,40 +20,68 @@ #pragma once -#include +#include +#include #include #include -#include -#include +#include namespace gtsam { typedef std::map > KeyVectorMap; -typedef std::map KeyRotMap; +typedef std::map KeyRotMap; -namespace InitializePose3 { +struct GTSAM_EXPORT InitializePose3 { + static GaussianFactorGraph buildLinearOrientationGraph( + const NonlinearFactorGraph& g); -GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); + static Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); -GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsChordal( + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); + /** + * Return the orientations of a graph including only BetweenFactors + */ + static Values computeOrientationsGradient( + const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, + size_t maxIter = 10000, const bool setRefFrame = true); -GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, - const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); + static void createSymbolicGraph(KeyVectorMap& adjEdgesMap, + KeyRotMap& factorId2RotMap, + const NonlinearFactorGraph& pose3Graph); -GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, - const NonlinearFactorGraph& pose3Graph); + static Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, + const double b); -GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b); + /** + * Select the subgraph of betweenFactors and transforms priors into between + * wrt a fictitious node + */ + static NonlinearFactorGraph buildPose3graph( + const NonlinearFactorGraph& graph); -GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); + static Values computePoses(NonlinearFactorGraph& pose3graph, + Values& initialRot); -GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements using chordal method. + */ + static Values initializeOrientations(const NonlinearFactorGraph& graph); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); + /** + * "extract" the Pose3 subgraph of the original graph, get orientations from + * relative orientation measurements (using either gradient or chordal + * method), and finish up with 1 GN iteration on full poses. + */ + static Values initialize(const NonlinearFactorGraph& graph, + const Values& givenGuess, bool useGradient = false); -GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); - -} // end of namespace lago -} // end of namespace gtsam + /// Calls initialize above using Chordal method. + static Values initialize(const NonlinearFactorGraph& graph); +}; +} // end of namespace gtsam diff --git a/gtsam/slam/tests/testInitializePose3.cpp b/gtsam/slam/tests/testInitializePose3.cpp index e6f08286a..6fe8b3d7c 100644 --- a/gtsam/slam/tests/testInitializePose3.cpp +++ b/gtsam/slam/tests/testInitializePose3.cpp @@ -70,6 +70,17 @@ NonlinearFactorGraph graph() { g.add(PriorFactor(x0, pose0, model)); return g; } + +NonlinearFactorGraph graph2() { + NonlinearFactorGraph g; + g.add(BetweenFactor(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0))); + g.add(BetweenFactor(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information + g.add(BetweenFactor(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin + g.add(PriorFactor(x0, pose0, model)); + return g; +} } /* *************************************************************************** */ @@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) { EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); } +/* *************************************************************************** */ +TEST( InitializePose3, orientationsPrecisions ) { + NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph2()); + + Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); + + // comparison is up to M_PI, that's why we add some multiples of 2*M_PI + EXPECT(assert_equal(simple::R0, initial.at(x0), 1e-6)); + EXPECT(assert_equal(simple::R1, initial.at(x1), 1e-6)); + EXPECT(assert_equal(simple::R2, initial.at(x2), 1e-6)); + EXPECT(assert_equal(simple::R3, initial.at(x3), 1e-6)); +} + /* *************************************************************************** */ TEST( InitializePose3, orientationsGradientSymbolicGraph ) { NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());