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

completed lago example

release/4.3a0
Luca 2014-05-16 15:27:05 -04:00
parent 29b1c92ab8
commit 11db29b1d8
1 changed files with 103 additions and 52 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
examples/tests/testPlanarSLAMExample_lago.cpp
View File

@ -202,13 +202,13 @@ GaussianFactorGraph buildOrientationGraph(const vector<size_t>& spanningTree, co
}
/* ************************************************************************* */
VectorValues initializeLago(const NonlinearFactorGraph& graph) {
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
VectorValues initializeLago(const NonlinearFactorGraph& graph, vector<Key>& keysInBinary) {
// Find a minimum spanning tree
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(graph);
// Create a linear factor graph (LFG) of scalars
vector<Key> keysInBinary;
map<Key, double> deltaThetaMap;
vector<size_t> spanningTree; // ids of between factors forming the spanning tree T
vector<size_t> chords; // ids of between factors corresponding to chords wrt T
@ -226,6 +226,34 @@ VectorValues initializeLago(const NonlinearFactorGraph& graph) {
return estimateLago;
}
/* ************************************************************************* */
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
VectorValues initializeLago(const NonlinearFactorGraph& graph) {
vector<Key> keysInBinary;
return initializeLago(graph, keysInBinary);
}
/* ************************************************************************* */
// returns the orientations of the Pose2 in the connected sub-graph defined by BetweenFactor<Pose2>
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
Values initialGuessLago;
// get the orientation estimates from LAGO
vector<Key> keysInBinary;
VectorValues orientations = initializeLago(graph, keysInBinary);
// plug the orientations in the initialGuess
for(size_t i=0; i<keysInBinary.size(); i++){
Key key = keysInBinary[i];
Pose2 pose = initialGuess.at<Pose2>(key);
Vector orientation = orientations.at(key);
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
initialGuessLago.insert(key, poseLago);
}
return initialGuessLago;
}
namespace simple {
// We consider a small graph:
@ -332,7 +360,7 @@ TEST( Lago, regularizedMeasurements ) {
}
/* *************************************************************************** */
TEST( Lago, smallGraph_GTmeasurements ) {
TEST( Lago, smallGraphVectorValues ) {
VectorValues initialGuessLago = initializeLago(simple::graph());
@ -343,6 +371,29 @@ TEST( Lago, smallGraph_GTmeasurements ) {
EXPECT(assert_equal((Vector(1) << 1.5 * PI - 2*PI), initialGuessLago.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphValues ) {
// we set the orientations in the initial guess to zero
Values initialGuess;
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.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
Values actual = initializeLago(simple::graph(), initialGuess);
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* ************************************************************************* */
int main() {
TestResult tr;