Fixed up examples
							parent
							
								
									732409faf3
								
							
						
					
					
						commit
						4afb11a2ed
					
				
							
								
								
									
										162
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										162
									
								
								.cproject
								
								
								
								
							|  | @ -568,6 +568,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -575,6 +576,7 @@ | |||
| 			</target> | ||||
| 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testBinaryBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -622,6 +624,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -629,6 +632,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testSymbolicFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -636,6 +640,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -651,6 +656,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -728,46 +734,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testValues.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testOrdering.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testKey.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testLinearContainerFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j6 -j8</buildArguments> | ||||
| 				<buildTarget>testWhiteNoiseFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1114,6 +1080,7 @@ | |||
| 			</target> | ||||
| 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testErrors.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1159,6 +1126,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testParticleFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1239,14 +1214,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testParticleFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1351,6 +1318,22 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testImuFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testCombinedImuFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1433,7 +1416,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2DOriented.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1473,7 +1455,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1481,7 +1462,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated3D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1495,22 +1475,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testImuFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testCombinedImuFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
|  | @ -1768,6 +1732,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G DEB</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1775,6 +1740,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G RPM</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1782,6 +1748,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G TGZ</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1789,6 +1756,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2547,6 +2515,7 @@ | |||
| 			</target> | ||||
| 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2554,6 +2523,7 @@ | |||
| 			</target> | ||||
| 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2561,6 +2531,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNetB.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2830,6 +2801,62 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="Pose2SLAMExample_lago.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>Pose2SLAMExample_lago.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="Pose2SLAMExample_g2o.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>Pose2SLAMExample_g2o.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testLago.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testLinearContainerFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testOrdering.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testOrdering.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testValues.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testValues.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testWhiteNoiseFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j4</buildArguments> | ||||
|  | @ -2848,7 +2875,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testGaussianISAM2</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  |  | |||
|  | @ -18,27 +18,22 @@ | |||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/inference/Key.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/slam/BetweenFactor.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/Marginals.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <fstream> | ||||
| #include <sstream> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| int main(const int argc, const char *argv[]) { | ||||
| 
 | ||||
| int main(const int argc, const char *argv[]){ | ||||
| 
 | ||||
|   // Read graph from file
 | ||||
|   string g2oFile; | ||||
|   if (argc < 2) | ||||
|     std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; | ||||
|   const string g2oFile = argv[1]; | ||||
|     g2oFile = "../../examples/Data/noisyToyGraph.txt"; | ||||
|   else | ||||
|     g2oFile = argv[1]; | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   Values initial; | ||||
|  | @ -46,7 +41,8 @@ int main(const int argc, const char *argv[]){ | |||
| 
 | ||||
|   // Add prior on the pose having index (key) = 0
 | ||||
|   NonlinearFactorGraph graphWithPrior = graph; | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = //
 | ||||
|       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); | ||||
|   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); | ||||
| 
 | ||||
|   std::cout << "Optimizing the factor graph" << std::endl; | ||||
|  | @ -54,10 +50,13 @@ int main(const int argc, const char *argv[]){ | |||
|   Values result = optimizer.optimize(); | ||||
|   std::cout << "Optimization complete" << std::endl; | ||||
| 
 | ||||
|   const string outputFile = argv[2]; | ||||
|   std::cout << "Writing results to file: " << outputFile << std::endl; | ||||
|   writeG2o(outputFile, graph, result); | ||||
|   std::cout << "done! " << std::endl; | ||||
| 
 | ||||
|   if (argc < 3) { | ||||
|     result.print("result"); | ||||
|   } else { | ||||
|     const string outputFile = argv[2]; | ||||
|     std::cout << "Writing results to file: " << outputFile << std::endl; | ||||
|     writeG2o(outputFile, graph, result); | ||||
|     std::cout << "done! " << std::endl; | ||||
|   } | ||||
|   return 0; | ||||
| } | ||||
|  |  | |||
|  | @ -12,33 +12,29 @@ | |||
| /**
 | ||||
|  * @file Pose2SLAMExample_lago.cpp | ||||
|  * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem | ||||
|  * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h | ||||
|  * using LAGO (Linear Approximation for Graph Optimization). See class lago.h | ||||
|  * Output is written on a file, in g2o format | ||||
|  * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o | ||||
|  * @date May 15, 2014 | ||||
|  * @author Luca Carlone | ||||
|  */ | ||||
| 
 | ||||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/inference/Key.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <gtsam/nonlinear/lago.h> | ||||
| #include <gtsam/slam/dataset.h> | ||||
| #include <gtsam/nonlinear/LagoInitializer.h> | ||||
| #include <gtsam/nonlinear/NonlinearFactorGraph.h> | ||||
| #include <gtsam/nonlinear/GaussNewtonOptimizer.h> | ||||
| #include <gtsam/nonlinear/Values.h> | ||||
| #include <gtsam/slam/PriorFactor.h> | ||||
| #include <fstream> | ||||
| #include <sstream> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| int main(const int argc, const char *argv[]) { | ||||
| 
 | ||||
| int main(const int argc, const char *argv[]){ | ||||
| 
 | ||||
|   // Read graph from file
 | ||||
|   string g2oFile; | ||||
|   if (argc < 2) | ||||
|     std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; | ||||
|   const string g2oFile = argv[1]; | ||||
|     g2oFile = "../../examples/Data/noisyToyGraph.txt"; | ||||
|   else | ||||
|     g2oFile = argv[1]; | ||||
| 
 | ||||
|   NonlinearFactorGraph graph; | ||||
|   Values initial; | ||||
|  | @ -46,17 +42,23 @@ int main(const int argc, const char *argv[]){ | |||
| 
 | ||||
|   // Add prior on the pose having index (key) = 0
 | ||||
|   NonlinearFactorGraph graphWithPrior = graph; | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); | ||||
|   noiseModel::Diagonal::shared_ptr priorModel = //
 | ||||
|       noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); | ||||
|   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); | ||||
|   graphWithPrior.print(); | ||||
| 
 | ||||
|   std::cout << "Computing LAGO estimate" << std::endl; | ||||
|   Values estimateLago = initializeLago(graphWithPrior); | ||||
|   std::cout << "done!" << std::endl; | ||||
| 
 | ||||
|   const string outputFile = argv[2]; | ||||
|   std::cout << "Writing results to file: " << outputFile << std::endl; | ||||
|   writeG2o(outputFile, graph, estimateLago); | ||||
|   std::cout << "done! " << std::endl; | ||||
|   if (argc < 3) { | ||||
|     estimateLago.print("estimateLago"); | ||||
|   } else { | ||||
|     const string outputFile = argv[2]; | ||||
|     std::cout << "Writing results to file: " << outputFile << std::endl; | ||||
|     writeG2o(outputFile, graph, estimateLago); | ||||
|     std::cout << "done! " << std::endl; | ||||
|   } | ||||
| 
 | ||||
|   return 0; | ||||
| } | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue