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

setting up the gradient method

release/4.3a0
Luca 2014-09-02 18:38:12 -04:00
parent 16bb90387c
commit 15abbc484b
3 changed files with 39 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
gtsam/slam/InitializePose3.cpp
View File

@ -72,7 +72,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
/* ************************************************************************* */ /* ************************************************************************* */
// Transform VectorValues into valid Rot3 // Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientations); gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus Matrix ppm = Matrix::Zero(3,3); // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1; ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
@ -131,9 +131,26 @@ NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose2> // Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
gttic(InitializePose3_computeOrientations); gttic(InitializePose3_computeOrientationsChordal);
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
// Solve the LFG
VectorValues relaxedRot3 = relaxedGraph.optimize();
// normalize and compute Rot3
return normalizeRelaxedRotations(relaxedRot3);
}
/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess) {
gttic(InitializePose3_computeOrientationsGradient);
// regularize measurements and plug everything in a factor graph // regularize measurements and plug everything in a factor graph
GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
@ -153,7 +170,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph pose3Graph = buildPose3graph(graph); NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
return computeOrientations(pose3Graph); return computeOrientationsChordal(pose3Graph);
} }
///* ************************************************************************* */ ///* ************************************************************************* */
@ -200,18 +217,26 @@ Values initialize(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph pose3Graph = buildPose3graph(graph); NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements // Get orientations from relative orientation measurements
Values valueRot3 = computeOrientations(pose3Graph); Values valueRot3 = computeOrientationsChordal(pose3Graph);
// Compute the full poses (1 GN iteration on full poses) // Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, valueRot3); return computePoses(pose3Graph, valueRot3);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess) { Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
Values initialValues; Values initialValues;
// get the orientation estimates from LAGO // We "extract" the Pose3 subgraph of the original graph: this
Values orientations = initializeOrientations(graph); // is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements
Values orientations;
if(useGradient)
orientations = computeOrientationsGradient(pose3Graph, givenGuess);
else
orientations = computeOrientationsChordal(pose3Graph);
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
Key key = key_value.key; Key key = key_value.key;

6
gtsam/slam/InitializePose3.h
View File

@ -32,9 +32,7 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFact
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
GTSAM_EXPORT Values computeOrientations(const NonlinearFactorGraph& pose3Graph); GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph);
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph); GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
@ -42,7 +40,7 @@ GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& init
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess); GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
} // end of namespace lago } // end of namespace lago
} // end of namespace gtsam } // end of namespace gtsam

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

@ -80,7 +80,9 @@ TEST( InitializePose3, buildPose3graph ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( InitializePose3, orientations ) { TEST( InitializePose3, orientations ) {
Values initial = InitializePose3::initializeOrientations(simple::graph()); NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
Values initial = InitializePose3::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));