Merged in feature/pose3_init (pull request #400)
Pose3 Initialization in Python/MATLAB Approved-by: Luca Carlone <luca.carlone@gatech.edu>release/4.3a0
Frank Dellaert
commit
c53e1ec653
|
|
@ -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)
|
||||
|
|
@ -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()
|
||||
20
gtsam.h
20
gtsam.h
|
|
@ -2523,6 +2523,26 @@ class BetweenFactorPose3s
|
|||
gtsam::BetweenFactorPose3* at(size_t i) const;
|
||||
};
|
||||
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
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<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
|
||||
|
||||
|
|
|
|||
|
|
@ -16,7 +16,7 @@
|
|||
* @date August, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
|
|
@ -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<NonlinearFactor>& factor: g) {
|
||||
for(const auto& factor: g) {
|
||||
Matrix3 Rij;
|
||||
double rotationPrecision = 1.0;
|
||||
|
||||
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
if (pose3Between)
|
||||
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(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<NonlinearFactor>& factor: graph) {
|
||||
for(const auto& factor: graph) {
|
||||
|
||||
// recast to a between on Pose3
|
||||
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
const auto 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);
|
||||
const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
|
||||
if (pose3Prior)
|
||||
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0],
|
||||
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
|
||||
pose3Prior->prior(), pose3Prior->noiseModel());
|
||||
}
|
||||
return pose3Graph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
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<Pose3>
|
||||
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<Pose3>(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<Rot3>(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<Rot3>(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<Rot3>(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<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
|
||||
const Rot3& Rref = inverseRot.at<Rot3>(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<Rot3>(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<NonlinearFactor>& factor: pose3Graph) {
|
||||
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
for(const auto& factor: pose3Graph) {
|
||||
auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||
if (pose3Between){
|
||||
Rot3 Rij = pose3Between->measured().rotation();
|
||||
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
|
||||
|
|
@ -275,14 +262,14 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
|||
adjEdgesMap.insert(pair<Key, vector<size_t> >(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<Rot3>(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<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel);
|
||||
initialPose.insert(kAnchorKey, Pose3());
|
||||
pose3graph.emplace_shared<PriorFactor<Pose3> >(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" <<std::endl;
|
||||
} else {
|
||||
cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" << endl;
|
||||
params.setVerbosity("TERMINATION");
|
||||
}
|
||||
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
|
||||
|
|
@ -345,9 +332,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
|||
|
||||
// put into Values structure
|
||||
Values estimate;
|
||||
for(const Values::ConstKeyValuePair& key_value: GNresult) {
|
||||
for (const auto& key_value : GNresult) {
|
||||
Key key = key_value.key;
|
||||
if (key != keyAnchor) {
|
||||
if (key != kAnchorKey) {
|
||||
const Pose3& pose = GNresult.at<Pose3>(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<Pose3>(key).translation();
|
||||
// const Rot3& rot = orientations.at<Rot3>(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
|
||||
|
|
|
|||
|
|
@ -20,40 +20,68 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/inference/graph.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/inference/graph.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
|
||||
typedef std::map<Key, Rot3 > KeyRotMap;
|
||||
typedef std::map<Key, Rot3> 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<Pose3>
|
||||
*/
|
||||
static Values computeOrientationsChordal(
|
||||
const NonlinearFactorGraph& pose3Graph);
|
||||
|
||||
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
|
||||
/**
|
||||
* Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||
*/
|
||||
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
|
||||
|
|
|
|||
|
|
@ -70,6 +70,17 @@ NonlinearFactorGraph graph() {
|
|||
g.add(PriorFactor<Pose3>(x0, pose0, model));
|
||||
return g;
|
||||
}
|
||||
|
||||
NonlinearFactorGraph graph2() {
|
||||
NonlinearFactorGraph g;
|
||||
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), noiseModel::Isotropic::Precision(6, 1.0)));
|
||||
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), noiseModel::Isotropic::Precision(6, 1.0)));
|
||||
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
|
||||
g.add(BetweenFactor<Pose3>(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<Pose3>(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<Pose3>(x0, pose0, model));
|
||||
return g;
|
||||
}
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
|
|
@ -91,6 +102,19 @@ TEST( InitializePose3, orientations ) {
|
|||
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(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<Rot3>(x0), 1e-6));
|
||||
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
|
||||
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
|
||||
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
|
||||
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||
|
|
|
|||
Loading…
Reference in New Issue