12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Modernized, documented

release/4.3a0
Frank Dellaert 2019-03-19 15:47:01 -04:00
parent 9a16cf97a7
commit 37eba50932
3 changed files with 129 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

178
gtsam/slam/InitializePose3.cpp
View File

@ -29,13 +29,9 @@
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
namespace InitializePose3 { namespace initialize_pose3 {
static const Matrix I9 = I_9x9; static const Key kAnchorKey = symbol('Z', 9999999);
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33 = Z_3x3;
static const Key keyAnchor = symbol('Z', 9999999);
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
@ -43,26 +39,29 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
GaussianFactorGraph linearGraph; GaussianFactorGraph linearGraph;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9); noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
for(const boost::shared_ptr<NonlinearFactor>& factor: g) { for(const auto& factor: g) {
Matrix3 Rij; Matrix3 Rij;
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between) if (pose3Between)
Rij = pose3Between->measured().rotation().matrix(); Rij = pose3Between->measured().rotation().matrix();
else else
std::cout << "Error in buildLinearOrientationGraph" << std::endl; cout << "Error in buildLinearOrientationGraph" << endl;
const KeyVector& keys = factor->keys(); const auto& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
Matrix M9 = Z_9x9; Matrix M9 = Z_9x9;
M9.block(0,0,3,3) = Rij; M9.block(0,0,3,3) = Rij;
M9.block(3,3,3,3) = Rij; M9.block(3,3,3,3) = Rij;
M9.block(6,6,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, model);
} }
// prior on the anchor orientation // 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(),
model);
return linearGraph; return linearGraph;
} }
@ -75,23 +74,15 @@ Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3; Values validRot3;
for(const VectorValues::value_type& it: relaxedRot3) { for(const auto& it: relaxedRot3) {
Key key = it.first; Key key = it.first;
if (key != keyAnchor) { if (key != kAnchorKey) {
const Vector& rotVector = it.second; Matrix3 R; R << 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);
Matrix U, V; Vector s; Matrix U, V; Vector s;
svd(rotMat, U, s, V); svd(R.transpose(), U, s, V);
Matrix3 normalizedRotMat = U * V.transpose(); 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) if(normalizedRotMat.determinant() < 0)
normalizedRotMat = U * ppm * V.transpose(); normalizedRotMat = U * ppm * V.transpose();
@ -103,31 +94,27 @@ 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 buildPose3graph(const NonlinearFactorGraph& graph) {
gttic(InitializePose3_buildPose3graph); gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph; NonlinearFactorGraph pose3Graph;
for(const boost::shared_ptr<NonlinearFactor>& factor: graph) { for(const auto& factor: graph) {
// recast to a between on Pose3 // recast to a between on Pose3
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = const auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between) if (pose3Between)
pose3Graph.add(pose3Between); pose3Graph.add(pose3Between);
// recast PriorFactor<Pose3> to BetweenFactor<Pose3> // recast PriorFactor<Pose3> to BetweenFactor<Pose3>
boost::shared_ptr<PriorFactor<Pose3> > pose3Prior = const auto pose3Prior = boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
if (pose3Prior) if (pose3Prior)
pose3Graph.emplace_shared<BetweenFactor<Pose3> >(keyAnchor, pose3Prior->keys()[0], pose3Graph.emplace_shared<BetweenFactor<Pose3> >(kAnchorKey, pose3Prior->keys()[0],
pose3Prior->prior(), pose3Prior->noiseModel()); pose3Prior->prior(), pose3Prior->noiseModel());
} }
return pose3Graph; return pose3Graph;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
gttic(InitializePose3_computeOrientationsChordal); gttic(InitializePose3_computeOrientationsChordal);
@ -142,14 +129,16 @@ Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3> Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) { const Values& givenGuess,
const size_t maxIter,
const bool setRefFrame) {
gttic(InitializePose3_computeOrientationsGradient); gttic(InitializePose3_computeOrientationsGradient);
// this works on the inverse rotations, according to Tron&Vidal,2011 // this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot; Values inverseRot;
inverseRot.insert(keyAnchor, Rot3()); inverseRot.insert(kAnchorKey, Rot3());
for(const Values::ConstKeyValuePair& key_value: givenGuess) { for(const auto& key_value: givenGuess) {
Key key = key_value.key; Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key); const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse()); inverseRot.insert(key, pose.rotation().inverse());
@ -164,9 +153,9 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
// calculate max node degree & allocate gradient // calculate max node degree & allocate gradient
size_t maxNodeDeg = 0; size_t maxNodeDeg = 0;
VectorValues grad; VectorValues grad;
for(const Values::ConstKeyValuePair& key_value: inverseRot) { for(const auto& key_value: inverseRot) {
Key key = key_value.key; Key key = key_value.key;
grad.insert(key,Vector3::Zero()); grad.insert(key,Z_3x1);
size_t currNodeDeg = (adjEdgesMap.at(key)).size(); size_t currNodeDeg = (adjEdgesMap.at(key)).size();
if(currNodeDeg > maxNodeDeg) if(currNodeDeg > maxNodeDeg)
maxNodeDeg = currNodeDeg; maxNodeDeg = currNodeDeg;
@ -180,42 +169,37 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
double mu_max = maxNodeDeg * rho; double mu_max = maxNodeDeg * rho;
double stepsize = 2/mu_max; // = 1/(a b dG) 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; double maxGrad;
// gradient iterations // gradient iterations
size_t it; size_t it;
for(it=0; it < maxIter; it++){ for (it = 0; it < maxIter; it++) {
////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////
// compute the gradient at each node // 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; maxGrad = 0;
for(const Values::ConstKeyValuePair& key_value: inverseRot) { for (const auto& key_value : inverseRot) {
Key key = key_value.key; Key key = key_value.key;
//std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl; Vector gradKey = Z_3x1;
Vector gradKey = Vector3::Zero();
// collect the gradient for each edge incident on key // 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 Rij = factorId2RotMap.at(factorId);
Rot3 Ri = inverseRot.at<Rot3>(key); Rot3 Ri = inverseRot.at<Rot3>(key);
if( key == (pose3Graph.at(factorId))->keys()[0] ){ auto factor = pose3Graph.at(factorId);
Key key1 = (pose3Graph.at(factorId))->keys()[1]; const auto& keys = factor->keys();
if (key == keys[0]) {
Key key1 = keys[1];
Rot3 Rj = inverseRot.at<Rot3>(key1); Rot3 Rj = inverseRot.at<Rot3>(key1);
gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b); 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 == keys[1]) {
}else if( key == (pose3Graph.at(factorId))->keys()[1] ){ Key key0 = keys[0];
Key key0 = (pose3Graph.at(factorId))->keys()[0];
Rot3 Rj = inverseRot.at<Rot3>(key0); Rot3 Rj = inverseRot.at<Rot3>(key0);
gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b); 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 {
}else{ cout << "Error in gradient computation" << endl;
std::cout << "Error in gradient computation" << std::endl;
} }
} // end of i-th gradient computation } // end of i-th gradient computation
grad.at(key) = stepsize * gradKey; grad.at(key) = stepsize * gradKey;
double normGradKey = (gradKey).norm(); double normGradKey = (gradKey).norm();
//std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
if(normGradKey>maxGrad) if(normGradKey>maxGrad)
maxGrad = normGradKey; maxGrad = normGradKey;
} // end of loop over nodes } // end of loop over nodes
@ -230,14 +214,12 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
break; break;
} // enf of gradient iterations } // enf of gradient iterations
std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl;
// Return correct rotations // 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; Values estimateRot;
for(const Values::ConstKeyValuePair& key_value: inverseRot) { for(const auto& key_value: inverseRot) {
Key key = key_value.key; Key key = key_value.key;
if (key != keyAnchor) { if (key != kAnchorKey) {
const Rot3& R = inverseRot.at<Rot3>(key); const Rot3& R = inverseRot.at<Rot3>(key);
if(setRefFrame) if(setRefFrame)
estimateRot.insert(key, Rref.compose(R.inverse())); estimateRot.insert(key, Rref.compose(R.inverse()));
@ -249,11 +231,11 @@ Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const
} }
/* ************************************************************************* */ /* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph) {
size_t factorId = 0; size_t factorId = 0;
for(const boost::shared_ptr<NonlinearFactor>& factor: pose3Graph) { for(const auto& factor: pose3Graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between = auto pose3Between = boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){ if (pose3Between){
Rot3 Rij = pose3Between->measured().rotation(); Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij)); factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
@ -275,7 +257,7 @@ void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id)); adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
} }
}else{ }else{
std::cout << "Error in computeOrientationsGradient" << std::endl; cout << "Error in createSymbolicGraph" << endl;
} }
factorId++; factorId++;
} }
@ -292,10 +274,10 @@ Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const doubl
th = logRot.norm(); th = logRot.norm();
} }
// exclude small or invalid rotations // 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; logRot = logRot / th;
}else{ } else {
logRot = Vector3::Zero(); logRot = Z_3x1;
th = 0.0; th = 0.0;
} }
@ -320,24 +302,25 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure // put into Values structure
Values initialPose; Values initialPose;
for(const Values::ConstKeyValuePair& key_value: initialRot){ for (const auto& key_value : initialRot) {
Key key = key_value.key; Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key); const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0)); Pose3 initializedPose = Pose3(rot, Point3(0, 0, 0));
initialPose.insert(key, initializedPose); initialPose.insert(key, initializedPose);
} }
// add prior // add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
initialPose.insert(keyAnchor, Pose3()); initialPose.insert(kAnchorKey, Pose3());
pose3graph.emplace_shared<PriorFactor<Pose3> >(keyAnchor, Pose3(), priorModel); pose3graph.emplace_shared<PriorFactor<Pose3> >(kAnchorKey, Pose3(), priorModel);
// Create optimizer // Create optimizer
GaussNewtonParams params; GaussNewtonParams params;
bool singleIter = true; bool singleIter = true;
if(singleIter){ if (singleIter) {
params.maxIterations = 1; params.maxIterations = 1;
}else{ } else {
std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <<std::endl; cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" << endl;
params.setVerbosity("TERMINATION"); params.setVerbosity("TERMINATION");
} }
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params); GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
@ -345,9 +328,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
// put into Values structure // put into Values structure
Values estimate; Values estimate;
for(const Values::ConstKeyValuePair& key_value: GNresult) { for (const auto& key_value : GNresult) {
Key key = key_value.key; Key key = key_value.key;
if (key != keyAnchor) { if (key != kAnchorKey) {
const Pose3& pose = GNresult.at<Pose3>(key); const Pose3& pose = GNresult.at<Pose3>(key);
estimate.insert(key, pose); estimate.insert(key, pose);
} }
@ -356,22 +339,9 @@ Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph) { Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess,
bool useGradient) {
gttic(InitializePose3_initialize); 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; Values initialValues;
// We "extract" the Pose3 subgraph of the original graph: this // We "extract" the Pose3 subgraph of the original graph: this
@ -380,27 +350,19 @@ Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, b
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
Values orientations; Values orientations;
if(useGradient) if (useGradient)
orientations = computeOrientationsGradient(pose3Graph, givenGuess); orientations = computeOrientationsGradient(pose3Graph, givenGuess);
else else
orientations = computeOrientationsChordal(pose3Graph); orientations = computeOrientationsChordal(pose3Graph);
// orientations.print("orientations\n");
// Compute the full poses (1 GN iteration on full poses) // Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations); 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 initialize(const NonlinearFactorGraph& graph) {
return initialize(graph, Values(), false);
}
} // namespace initialize_pose3
} // namespace gtsam

57
gtsam/slam/InitializePose3.h
View File

@ -20,40 +20,65 @@
#pragma once #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/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam { namespace gtsam {
typedef std::map<Key, std::vector<size_t> > KeyVectorMap; typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
typedef std::map<Key, Rot3 > KeyRotMap; typedef std::map<Key, Rot3> KeyRotMap;
namespace InitializePose3 { namespace initialize_pose3 {
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g); GTSAM_EXPORT GaussianFactorGraph
buildLinearOrientationGraph(const NonlinearFactorGraph& g);
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); /**
* Return the orientations of a graph including only BetweenFactors<Pose3>
*/
GTSAM_EXPORT Values
computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, /**
const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true); * Return the orientations of a graph including only BetweenFactors<Pose3>
*/
GTSAM_EXPORT Values computeOrientationsGradient(
const NonlinearFactorGraph& pose3Graph, const Values& givenGuess,
size_t maxIter = 10000, const bool setRefFrame = true);
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap,
KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph); const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT 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);
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); /**
* Select the subgraph of betweenFactors and transforms priors into between wrt
* a fictitious node
*/
GTSAM_EXPORT NonlinearFactorGraph
buildPose3graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT 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 either gradient or chordal method),
* and finish up with 1 GN iteration on full poses.
*/
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
const Values& givenGuess,
bool useGradient = false);
/// Calls initialize above using Chordal method.
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); } // namespace initialize_pose3
} // end of namespace lago
} // end of namespace gtsam } // end of namespace gtsam

24
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -74,15 +74,15 @@ NonlinearFactorGraph graph() {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, buildPose3graph ) { TEST( InitializePose3, buildPose3graph ) {
NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3graph = initialize_pose3::buildPose3graph(simple::graph());
// pose3graph.print(""); // pose3graph.print("");
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientations ) { TEST( InitializePose3, orientations ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
Values initial = InitializePose3::computeOrientationsChordal(pose3Graph); Values initial = initialize_pose3::computeOrientationsChordal(pose3Graph);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // 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::R0, initial.at<Rot3>(x0), 1e-6));
@ -93,12 +93,12 @@ TEST( InitializePose3, orientations ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientationsGradientSymbolicGraph ) { TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
KeyVectorMap adjEdgesMap; KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap; KeyRotMap factorId2RotMap;
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph); initialize_pose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9); EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
@ -132,7 +132,7 @@ TEST( InitializePose3, singleGradient ) {
double a = 6.010534238540223; double a = 6.010534238540223;
double b = 1.0; double b = 1.0;
Vector actual = InitializePose3::gradientTron(R1, R2, a, b); Vector actual = initialize_pose3::gradientTron(R1, R2, a, b);
Vector expected = Vector3::Zero(); Vector expected = Vector3::Zero();
expected(2) = 1.962658662803917; expected(2) = 1.962658662803917;
@ -142,7 +142,7 @@ TEST( InitializePose3, singleGradient ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, iterationGradient ) { TEST( InitializePose3, iterationGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations // Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@ -154,7 +154,7 @@ TEST( InitializePose3, iterationGradient ) {
size_t maxIter = 1; // test gradient at the first iteration size_t maxIter = 1; // test gradient at the first iteration
bool setRefFrame = false; bool setRefFrame = false;
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame); Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281, Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
0.033572116359134, 0.999436104312325, -0.000621610948719, 0.033572116359134, 0.999436104312325, -0.000621610948719,
@ -183,7 +183,7 @@ TEST( InitializePose3, iterationGradient ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientationsGradient ) { TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph()); NonlinearFactorGraph pose3Graph = initialize_pose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations // Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01)); Rot3 Rpert = Rot3::Expmap(Vector3(0.01, 0.01, 0.01));
@ -194,7 +194,7 @@ TEST( InitializePose3, orientationsGradient ) {
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) )); givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3(0,0,0)) ));
// do 10 gradient iterations // do 10 gradient iterations
bool setRefFrame = false; bool setRefFrame = false;
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame); Values orientations = initialize_pose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
// const Key keyAnchor = symbol('Z', 9999999); // const Key keyAnchor = symbol('Z', 9999999);
// givenPoses.insert(keyAnchor,simple::pose0); // givenPoses.insert(keyAnchor,simple::pose0);
@ -228,7 +228,7 @@ TEST( InitializePose3, posesWithGivenGuess ) {
givenPoses.insert(x2,simple::pose2); givenPoses.insert(x2,simple::pose2);
givenPoses.insert(x3,simple::pose3); givenPoses.insert(x3,simple::pose3);
Values initial = InitializePose3::initialize(simple::graph(), givenPoses); Values initial = initialize_pose3::initialize(simple::graph(), givenPoses);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(givenPoses, initial, 1e-6)); EXPECT(assert_equal(givenPoses, initial, 1e-6));
@ -245,7 +245,7 @@ TEST( InitializePose3, initializePoses )
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6); noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel)); inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
Values initial = InitializePose3::initialize(*inputGraph); Values initial = initialize_pose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !! EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!
} }