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

removed redundant "Lago" from several function names

release/4.3a0
dellaert 2014-05-31 13:13:14 -04:00
parent fe33c80b5f
commit 1d43a1f206
4 changed files with 54 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -48,7 +48,7 @@ int main(const int argc, const char *argv[]) {
graphWithPrior.print(); graphWithPrior.print();
std::cout << "Computing LAGO estimate" << std::endl; std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = lago::initializeLago(graphWithPrior); Values estimateLago = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl; std::cout << "done!" << std::endl;
if (argc < 3) { if (argc < 3) {

26
gtsam/nonlinear/lago.cpp
View File

@ -26,8 +26,12 @@
namespace gtsam { namespace gtsam {
namespace lago { namespace lago {
static Matrix I = eye(1); static const Matrix I = eye(1);
static Matrix I3 = eye(3); static const Matrix I3 = eye(3);
static const Key keyAnchor = symbol('Z',9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
static const noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* ************************************************************************* */ /* ************************************************************************* */
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree, double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
@ -216,7 +220,7 @@ PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){
// Find a minimum spanning tree // Find a minimum spanning tree
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
@ -244,18 +248,18 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this // We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph // is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph); NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
return computeLagoOrientations(pose2Graph, useOdometricPath); return computeOrientations(pose2Graph, useOdometricPath);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { Values computePoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) {
// Linearized graph on full poses // Linearized graph on full poses
GaussianFactorGraph linearPose2graph; GaussianFactorGraph linearPose2graph;
@ -315,25 +319,25 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this // We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph // is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph); NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath); VectorValues orientationsLago = computeOrientations(pose2Graph, useOdometricPath);
// Compute the full poses // Compute the full poses
return computeLagoPoses(pose2Graph, orientationsLago); return computePoses(pose2Graph, orientationsLago);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess) {
Values initialGuessLago; Values initialGuessLago;
// get the orientation estimates from LAGO // get the orientation estimates from LAGO
VectorValues orientations = initializeOrientationsLago(graph); VectorValues orientations = initializeOrientations(graph);
// for all nodes in the tree // for all nodes in the tree
for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){

11
gtsam/nonlinear/lago.h
View File

@ -44,9 +44,6 @@ namespace gtsam {
namespace lago { namespace lago {
typedef std::map<Key,double> key2doubleMap; typedef std::map<Key,double> key2doubleMap;
const Key keyAnchor = symbol('Z',9999999);
noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree) /* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree)
* for a node (nodeKey). The function starts at the nodes and moves towards the root * for a node (nodeKey). The function starts at the nodes and moves towards the root
@ -83,16 +80,16 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<s
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */ /* Returns the orientations of a graph including only BetweenFactors<Pose2> */
GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true); GTSAM_EXPORT VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */ /* LAGO: Returns the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); GTSAM_EXPORT VectorValues initializeOrientations(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */ /* Returns the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */ /* Only corrects the orientation part in initialGuess */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& initialGuess);
} // end of namespace lago } // end of namespace lago
} // end of namespace gtsam } // end of namespace gtsam

68
gtsam/nonlinear/tests/testLago.cpp
View File

@ -148,25 +148,25 @@ TEST( Lago, regularizedMeasurements ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) { TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false; bool useOdometricPath = false;
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath); VectorValues initial = initializeOrientations(simple::graph(), useOdometricPath);
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); VectorValues initial = initializeOrientations(simple::graph());
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -174,26 +174,26 @@ TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false; bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model)); g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); VectorValues initial = initializeOrientations(g, useOdometricPath);
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model)); g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g); VectorValues initial = initializeOrientations(g);
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -201,26 +201,26 @@ TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false; bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model)); g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath); VectorValues initial = initializeOrientations(g, useOdometricPath);
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) { TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model)); g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g); VectorValues initial = initializeOrientations(g);
// 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((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -234,7 +234,7 @@ TEST( Lago, smallGraphValues ) {
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
// lago does not touch the Cartesian part and only fixed the orientations // lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph(), initialGuess); Values actual = lago::initialize(simple::graph(), initialGuess);
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
@ -250,7 +250,7 @@ TEST( Lago, smallGraphValues ) {
TEST( Lago, smallGraph2 ) { TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations // lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph()); Values actual = lago::initialize(simple::graph());
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
@ -275,7 +275,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = initializeOrientationsLago(graphWithPrior); VectorValues actualVV = initializeOrientations(graphWithPrior);
Values actual; Values actual;
Key keyAnc = symbol('Z',9999999); Key keyAnc = symbol('Z',9999999);
for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
@ -310,7 +310,7 @@ TEST( Lago, largeGraphNoisy ) {
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = initializeLago(graphWithPrior); Values actual = lago::initialize(graphWithPrior);
NonlinearFactorGraph gmatlab; NonlinearFactorGraph gmatlab;
Values expected; Values expected;