initial implementation of chordal relaxation
Luca
parent
bcab483574
commit
2f2a40a737
|
|
@ -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 <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
|
#include <boost/math/special_functions.hpp>
|
||||||
|
|
||||||
|
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<NonlinearFactor>& factor, g) {
|
||||||
|
Matrix3 Rijt;
|
||||||
|
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between)
|
||||||
|
Rijt = pose3Between->measured().rotation().matrix().transpose();
|
||||||
|
else
|
||||||
|
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
|
||||||
|
|
||||||
|
const FastVector<Key>& 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<NonlinearFactor>& factor, graph) {
|
||||||
|
|
||||||
|
// recast to a between on Pose3
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between)
|
||||||
|
pose3Graph.add(pose3Between);
|
||||||
|
|
||||||
|
// recast PriorFactor<Pose3> to BetweenFactor<Pose3>
|
||||||
|
boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
|
||||||
|
boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
|
||||||
|
if (pose3Prior)
|
||||||
|
pose3Graph.add(
|
||||||
|
BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0],
|
||||||
|
pose3Prior->prior(), pose3Prior->get_noiseModel()));
|
||||||
|
}
|
||||||
|
return pose3Graph;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Return the orientations of a graph including only BetweenFactors<Pose2>
|
||||||
|
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<NonlinearFactor>& factor, pose2graph) {
|
||||||
|
//
|
||||||
|
// boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||||
|
// boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(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<noiseModel::Diagonal> diagonalModel =
|
||||||
|
// boost::dynamic_pointer_cast<noiseModel::Diagonal>(
|
||||||
|
// 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<Pose2>(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
|
||||||
|
|
@ -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 <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||||
|
#include <gtsam/linear/VectorValues.h>
|
||||||
|
#include <gtsam/inference/graph.h>
|
||||||
|
|
||||||
|
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<size_t>& spanningTreeIds,
|
||||||
|
// const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
|
||||||
|
// const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& 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
|
||||||
|
|
@ -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 <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
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<Pose3>(x0, x1, pose0.between(pose1), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
|
||||||
|
g.add(PriorFactor<Pose3>(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<Pose3>(0), simple::pose0, 1e-6));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
Loading…
Reference in New Issue