setting up the gradient method
							parent
							
								
									16bb90387c
								
							
						
					
					
						commit
						15abbc484b
					
				|  | @ -72,7 +72,7 @@ GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) { | |||
| /* ************************************************************************* */ | ||||
| // Transform VectorValues into valid Rot3
 | ||||
| Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) { | ||||
|   gttic(InitializePose3_computeOrientations); | ||||
|   gttic(InitializePose3_computeOrientationsChordal); | ||||
| 
 | ||||
|   Matrix ppm = Matrix::Zero(3,3); // plus plus minus
 | ||||
|   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>
 | ||||
| Values computeOrientations(const NonlinearFactorGraph& pose3Graph) { | ||||
|   gttic(InitializePose3_computeOrientations); | ||||
| // Return the orientations of a graph including only BetweenFactors<Pose3>
 | ||||
| Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) { | ||||
|   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
 | ||||
|   GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph); | ||||
|  | @ -153,7 +170,7 @@ Values initializeOrientations(const NonlinearFactorGraph& graph) { | |||
|   NonlinearFactorGraph pose3Graph = buildPose3graph(graph); | ||||
| 
 | ||||
|   // 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); | ||||
| 
 | ||||
|   // Get orientations from relative orientation measurements
 | ||||
|   Values valueRot3 = computeOrientations(pose3Graph); | ||||
|   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) { | ||||
| Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) { | ||||
|   Values initialValues; | ||||
| 
 | ||||
|   // get the orientation estimates from LAGO
 | ||||
|   Values orientations = initializeOrientations(graph); | ||||
|   // 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 orientations; | ||||
|   if(useGradient) | ||||
|     orientations = computeOrientationsGradient(pose3Graph, givenGuess); | ||||
|   else | ||||
|     orientations = computeOrientationsChordal(pose3Graph); | ||||
| 
 | ||||
|   BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) { | ||||
|     Key key = key_value.key; | ||||
|  |  | |||
|  | @ -32,9 +32,7 @@ GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFact | |||
| 
 | ||||
| GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3); | ||||
| 
 | ||||
| GTSAM_EXPORT Values computeOrientations(const NonlinearFactorGraph& pose3Graph); | ||||
| 
 | ||||
| GTSAM_EXPORT Values initializeOrientations(const NonlinearFactorGraph& graph); | ||||
| GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph); | ||||
| 
 | ||||
| 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, const Values& givenGuess); | ||||
| GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false); | ||||
| 
 | ||||
| } // end of namespace lago
 | ||||
| } // end of namespace gtsam
 | ||||
|  |  | |||
|  | @ -80,7 +80,9 @@ TEST( InitializePose3, buildPose3graph ) { | |||
| 
 | ||||
| /* *************************************************************************** */ | ||||
| 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
 | ||||
|   EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6)); | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue