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

cleaning code

release/4.3a0
Luca 2014-05-28 13:22:09 -04:00
parent 461047b242
commit a805034273
3 changed files with 18 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
gtsam/nonlinear/LagoInitializer.cpp
View File

@ -31,7 +31,6 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
double nodeTheta = 0; double nodeTheta = 0;
Key key_child = nodeKey; // the node Key key_child = nodeKey; // the node
Key key_parent = 0; // the initialization does not matter Key key_parent = 0; // the initialization does not matter
///std::cout << "start" << std::endl;
while(1){ while(1){
// We check if we reached the root // We check if we reached the root
if(tree.at(key_child)==key_child) // if we reached the root if(tree.at(key_child)==key_child) // if we reached the root
@ -47,7 +46,6 @@ double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
} }
key_child = key_parent; // we move upwards in the tree key_child = key_parent; // we move upwards in the tree
} }
///std::cout << "end" << std::endl;
return nodeTheta; return nodeTheta;
} }
@ -107,15 +105,6 @@ void getSymbolicGraph(
} }
id++; id++;
} }
///g.print("Before detlta map \n");
key2doubleMap::const_iterator it;
for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){
Key nodeKey = it->first;
///std::cout << "deltaThMAP = key " << DefaultKeyFormatter(nodeKey) << " th= " << it->second << std::endl;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -222,9 +211,8 @@ PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph){ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){
bool useOdometricPath = true;
// Find a minimum spanning tree // Find a minimum spanning tree
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
if (useOdometricPath) if (useOdometricPath)
@ -232,21 +220,15 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph){
else else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph); tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
///std::cout << "found spanning tree" << std::endl;
// Create a linear factor graph (LFG) of scalars // Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap; key2doubleMap deltaThetaMap;
std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph); getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
///std::cout << "found symbolic graph" << std::endl;
// temporary structure to correct wraparounds along loops // temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
///std::cout << "computed orientations from root" << std::endl;
// regularize measurements and plug everything in a factor graph // regularize measurements and plug everything in a factor graph
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree); GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
@ -257,14 +239,14 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph){
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph) { VectorValues initializeOrientationsLago(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); return computeLagoOrientations(pose2Graph, useOdometricPath);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -289,8 +271,6 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta(); double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta();
linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
if(fabs(linearDeltaRot)>M_PI)
std::cout << "linearDeltaRot " << linearDeltaRot << std::endl;
double dx = pose2Between->measured().x(); double dx = pose2Between->measured().x();
double dy = pose2Between->measured().y(); double dy = pose2Between->measured().y();
@ -330,31 +310,16 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph) { Values initializeLago(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
///std::cout << "buildPose2graph" << std::endl;
NonlinearFactorGraph pose2Graph = buildPose2graph(graph); NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
///std::cout << "computeLagoOrientations" << std::endl; VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath);
VectorValues orientationsLago = computeLagoOrientations(pose2Graph);
// VectorValues orientationsLago;
// NonlinearFactorGraph g;
// Values orientationsLagoV;
// readG2o("/home/aspn/Desktop/orientationsNoisyToyGraph.txt", g, orientationsLagoV);
//
// BOOST_FOREACH(const Values::KeyValuePair& key_val, orientationsLagoV){
// Key k = key_val.key;
// double th = orientationsLagoV.at<Pose2>(k).theta();
// orientationsLago.insert(k,(Vector(1) << th));
// }
// orientationsLago.insert(keyAnchor,(Vector(1) << 0.0));
// Compute the full poses // Compute the full poses
///std::cout << "computeLagoPoses" << std::endl;
return computeLagoPoses(pose2Graph, orientationsLago); return computeLagoPoses(pose2Graph, orientationsLago);
} }

6
gtsam/nonlinear/LagoInitializer.h
View File

@ -83,13 +83,13 @@ GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spann
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph); 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> */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph); VectorValues computeLagoOrientations(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 */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph); VectorValues initializeOrientationsLago(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 */
Values initializeLago(const NonlinearFactorGraph& graph); Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */ /* Only corrects the orientation part in initialGuess */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess); Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);

18
gtsam/nonlinear/tests/testLagoInitializer.cpp
View File

@ -144,10 +144,10 @@ TEST( Lago, regularizedMeasurements ) {
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
} }
/* *************************************************************************** * /* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) { TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false;
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); VectorValues initialGuessLago = initializeOrientationsLago(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), initialGuessLago.at(x0), 1e-6));
@ -168,11 +168,12 @@ TEST( Lago, smallGraphVectorValuesSP ) {
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
} }
/* *************************************************************************** * /* *************************************************************************** */
TEST( Lago, multiplePosePriors ) { TEST( Lago, multiplePosePriors ) {
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); VectorValues initialGuessLago = initializeOrientationsLago(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), initialGuessLago.at(x0), 1e-6));
@ -195,11 +196,12 @@ TEST_UNSAFE( Lago, multiplePosePriorsSP ) {
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
} }
/* *************************************************************************** * /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) { TEST( Lago, multiplePoseAndRotPriors ) {
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); VectorValues initialGuessLago = initializeOrientationsLago(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), initialGuessLago.at(x0), 1e-6));
@ -209,7 +211,7 @@ TEST( Lago, multiplePoseAndRotPriors ) {
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) { 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 initialGuessLago = initializeOrientationsLago(g);