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

Finish lago-namespace 

Introduced namespace, renamed some functions, cleaned up headers, made some functions static, made examples work without arguments, moved some eclipse targets into tests folder, added 2 others in examples.
release/4.3a0
dellaert 2014-05-31 13:46:51 -04:00
parent 0ce70befc4 8d54460d2c
commit e19f3c5902
7 changed files with 418 additions and 358 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
.cproject
View File

@ -568,6 +568,7 @@
</target> </target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget> <buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -575,6 +576,7 @@
</target> </target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget> <buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -622,6 +624,7 @@
</target> </target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget> <buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -629,6 +632,7 @@
</target> </target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget> <buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -636,6 +640,7 @@
</target> </target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget> <buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -651,6 +656,7 @@
</target> </target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget> <buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -728,46 +734,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1114,6 +1080,7 @@
</target> </target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1159,6 +1126,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1239,14 +1214,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1351,6 +1318,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -1433,7 +1416,6 @@
</target> </target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget> <buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1473,7 +1455,6 @@
</target> </target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget> <buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1481,7 +1462,6 @@
</target> </target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget> <buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1495,22 +1475,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1768,6 +1732,7 @@
</target> </target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget> <buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1775,6 +1740,7 @@
</target> </target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget> <buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1782,6 +1748,7 @@
</target> </target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget> <buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -1789,6 +1756,7 @@
</target> </target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand> <buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget> <buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2547,6 +2515,7 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2554,6 +2523,7 @@
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget> <buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2561,6 +2531,7 @@
</target> </target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
@ -2830,6 +2801,62 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j4</buildArguments>
@ -2848,7 +2875,6 @@
</target> </target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget> <buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>

35
examples/Pose2SLAMExample_g2o.cpp
View File

@ -18,27 +18,22 @@
* @author Luca Carlone * @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/dataset.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <fstream> #include <fstream>
#include <sstream>
using namespace std; using namespace std;
using namespace gtsam; 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) if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; g2oFile = "../../examples/Data/noisyToyGraph.txt";
const string g2oFile = argv[1]; else
g2oFile = argv[1];
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Values initial; Values initial;
@ -46,7 +41,8 @@ int main(const int argc, const char *argv[]){
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph; 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.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
@ -54,10 +50,13 @@ int main(const int argc, const char *argv[]){
Values result = optimizer.optimize(); Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl; std::cout << "Optimization complete" << std::endl;
const string outputFile = argv[2]; if (argc < 3) {
std::cout << "Writing results to file: " << outputFile << std::endl; result.print("result");
writeG2o(outputFile, graph, result); } else {
std::cout << "done! " << 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;
}
return 0; return 0;
} }

40
examples/Pose2SLAMExample_lago.cpp
View File

@ -12,33 +12,29 @@
/** /**
* @file Pose2SLAMExample_lago.cpp * @file Pose2SLAMExample_lago.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem * @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 * Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
* @date May 15, 2014 * @date May 15, 2014
* @author Luca Carlone * @author Luca Carlone
*/ */
#include <gtsam/geometry/Pose2.h> #include <gtsam/nonlinear/lago.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LagoInitializer.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <fstream> #include <fstream>
#include <sstream>
using namespace std; using namespace std;
using namespace gtsam; 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) if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; g2oFile = "../../examples/Data/noisyToyGraph.txt";
const string g2oFile = argv[1]; else
g2oFile = argv[1];
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
Values initial; Values initial;
@ -46,17 +42,23 @@ int main(const int argc, const char *argv[]){
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph; 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.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();
std::cout << "Computing LAGO estimate" << std::endl; std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = initializeLago(graphWithPrior); Values estimateLago = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl; std::cout << "done!" << std::endl;
const string outputFile = argv[2]; if (argc < 3) {
std::cout << "Writing results to file: " << outputFile << std::endl; estimateLago.print("estimateLago");
writeG2o(outputFile, graph, estimateLago); } else {
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;
}
return 0; return 0;
} }

100
gtsam/nonlinear/LagoInitializer.h
View File

@ -1,100 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file LagoInitializer.h
* @brief Initialize Pose2 in a factor graph using LAGO
* (Linear Approximation for Graph Optimization). see papers:
*
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
* approximation for planar pose graph optimization, IJRR, 2014.
*
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
* for graph-based simultaneous localization and mapping, RSS, 2011.
*
* @param graph: nonlinear factor graph (can include arbitrary factors but we assume
* that there is a subgraph involving Pose2 and betweenFactors). Also in the current
* version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
* and a prior on x0. This assumption can be relaxed by using the extra argument
* useOdometricPath = false, although this part of code is not stable yet.
* @return Values: initial guess from LAGO (only pose2 are initialized)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
typedef std::map<Key,double> key2doubleMap;
const Key keyAnchor = symbol('Z',9999999);
noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree)
* for a node (nodeKey). The function starts at the nodes and moves towards the root
* summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
* The root is assumed to have orientation zero.
*/
GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping)
* for all node wrt the root (root has zero orientation)
*/
GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree);
/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g,
* and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
*/
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */
GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */
GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
} // end of namespace gtsam

247
gtsam/nonlinear/LagoInitializer.cpp → gtsam/nonlinear/lago.cpp
View File

@ -10,38 +10,57 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file LagoInitializer.h * @file lago.h
* @author Luca Carlone * @author Luca Carlone
* @author Frank Dellaert * @author Frank Dellaert
* @date May 14, 2014 * @date May 14, 2014
*/ */
#include <gtsam/nonlinear/LagoInitializer.h> #include <gtsam/nonlinear/lago.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <boost/math/special_functions.hpp> #include <boost/math/special_functions.hpp>
namespace gtsam { namespace gtsam {
namespace lago {
static Matrix I = eye(1); static const Matrix I = eye(1);
static Matrix I3 = eye(3); static const Matrix I3 = eye(3);
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* ************************************************************************* */ /* ************************************************************************* */
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree, /**
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) { * Compute the cumulative orientation (without wrapping) wrt the root of a
* spanning tree (tree) for a node (nodeKey). The function starts at the nodes and
* moves towards the root summing up the (directed) rotation measurements.
* Relative measurements are encoded in "deltaThetaMap".
* The root is assumed to have orientation zero.
*/
static double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap) {
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
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
break; break;
// we sum the delta theta corresponding to the edge parent->child // we sum the delta theta corresponding to the edge parent->child
nodeTheta += deltaThetaMap.at(key_child); nodeTheta += deltaThetaMap.at(key_child);
// we get the parent // we get the parent
key_parent = tree.at(key_child); // the parent key_parent = tree.at(key_child); // the parent
// we check if we connected to some part of the tree we know // we check if we connected to some part of the tree we know
if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){ if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) {
nodeTheta += thetaFromRootMap.at(key_parent); nodeTheta += thetaFromRootMap.at(key_parent);
break; break;
} }
@ -55,15 +74,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) { const PredecessorMap<Key>& tree) {
key2doubleMap thetaToRootMap; key2doubleMap thetaToRootMap;
key2doubleMap::const_iterator it;
// Orientation of the roo // Orientation of the roo
thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0)); thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree // for all nodes in the tree
for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){ BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
// compute the orientation wrt root // compute the orientation wrt root
Key nodeKey = it->first; Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap, double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap); thetaToRootMap);
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta)); thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
@ -73,35 +91,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
/* ************************************************************************* */ /* ************************************************************************* */
void getSymbolicGraph( void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap, /*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){ key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
// Get keys for which you want the orientation // Get keys for which you want the orientation
size_t id=0; size_t id = 0;
// Loop over the factors // Loop over the factors
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g){ BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
if (factor->keys().size() == 2){ if (factor->keys().size() == 2) {
Key key1 = factor->keys()[0]; Key key1 = factor->keys()[0];
Key key2 = factor->keys()[1]; Key key2 = factor->keys()[1];
// recast to a between // recast to a between
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between) continue; if (!pose2Between)
continue;
// get the orientation - measured().theta(); // get the orientation - measured().theta();
double deltaTheta = pose2Between->measured().theta(); double deltaTheta = pose2Between->measured().theta();
// insert (directed) orientations in the map "deltaThetaMap" // insert (directed) orientations in the map "deltaThetaMap"
bool inTree=false; bool inTree = false;
if(tree.at(key1)==key2){ // key2 -> key1 if (tree.at(key1) == key2) { // key2 -> key1
deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta)); deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
inTree = true; inTree = true;
} else if(tree.at(key2)==key1){ // key1 -> key2 } else if (tree.at(key2) == key1) { // key1 -> key2
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta)); deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
inTree = true; inTree = true;
} }
// store factor slot, distinguishing spanning tree edges from chordsIds // store factor slot, distinguishing spanning tree edges from chordsIds
if(inTree == true) if (inTree == true)
spanningTreeIds.push_back(id); spanningTreeIds.push_back(id);
else // it's a chord! else
// it's a chord!
chordsIds.push_back(id); chordsIds.push_back(id);
} }
id++; id++;
@ -109,14 +130,16 @@ void getSymbolicGraph(
} }
/* ************************************************************************* */ /* ************************************************************************* */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor, // Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) { Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
// Get the relative rotation measurement from the between factor // Get the relative rotation measurement from the between factor
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between) if (!pose2Between)
throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!"); throw std::invalid_argument(
"buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta()); deltaTheta = (Vector(1) << pose2Between->measured().theta());
// Retrieve the noise model for the relative rotation // Retrieve the noise model for the relative rotation
@ -124,114 +147,130 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
boost::shared_ptr<noiseModel::Diagonal> diagonalModel = boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model); boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel) if (!diagonalModel)
throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model " throw std::invalid_argument(
"(current version assumes diagonal noise model)!"); "buildLinearOrientationGraph: invalid noise model "
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement "(current version assumes diagonal noise model)!");
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta); model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds, GaussianFactorGraph buildLinearOrientationGraph(
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){ const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree) {
GaussianFactorGraph lagoGraph; GaussianFactorGraph lagoGraph;
Vector deltaTheta; Vector deltaTheta;
noiseModel::Diagonal::shared_ptr model_deltaTheta; noiseModel::Diagonal::shared_ptr model_deltaTheta;
// put original measurements in the spanning tree // put original measurements in the spanning tree
BOOST_FOREACH(const size_t& factorId, spanningTreeIds){ BOOST_FOREACH(const size_t& factorId, spanningTreeIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta)); lagoGraph.add(
JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
} }
// put regularized measurements in the chordsIds // put regularized measurements in the chordsIds
BOOST_FOREACH(const size_t& factorId, chordsIds){ BOOST_FOREACH(const size_t& factorId, chordsIds) {
const FastVector<Key>& keys = g[factorId]->keys(); const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1]; Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta); getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0); double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl; ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1)
double k = boost::math::round(k2pi_noise/(2*M_PI)); - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = boost::math::round(k2pi_noise / (2 * M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI); Vector deltaThetaRegularized = (Vector(1)
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta)); << key1_DeltaTheta_key2 - 2 * k * M_PI);
lagoGraph.add(
JacobianFactor(key1, -I, key2, I, deltaThetaRegularized,
model_deltaTheta));
} }
// prior on the anchor orientation // prior on the anchor orientation
lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise)); lagoGraph.add(
JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise));
return lagoGraph; return lagoGraph;
} }
/* ************************************************************************* */ /* ************************************************************************* */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){ // Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
NonlinearFactorGraph pose2Graph; NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph){ BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
// recast to a between on Pose2 // recast to a between on Pose2
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (pose2Between) if (pose2Between)
pose2Graph.add(pose2Between); pose2Graph.add(pose2Between);
// recast PriorFactor<Pose2> to BetweenFactor<Pose2> // recast PriorFactor<Pose2> to BetweenFactor<Pose2>
boost::shared_ptr< PriorFactor<Pose2> > pose2Prior = boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor); boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
if (pose2Prior) if (pose2Prior)
pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0], pose2Graph.add(
pose2Prior->prior(), pose2Prior->get_noiseModel())); BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->get_noiseModel()));
} }
return pose2Graph; return pose2Graph;
} }
/* ************************************************************************* */ /* ************************************************************************* */
PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) { static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
Key minKey; Key minKey;
bool minUnassigned = true; bool minUnassigned = true;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph){ BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) {
Key key1 = std::min(factor->keys()[0], factor->keys()[1]); Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
Key key2 = std::max(factor->keys()[0], factor->keys()[1]); Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
if(minUnassigned){ if (minUnassigned) {
minKey = key1; minKey = key1;
minUnassigned = false; minUnassigned = false;
} }
if( key2 - key1 == 1){ // consecutive keys if (key2 - key1 == 1) { // consecutive keys
tree.insert(key2, key1); tree.insert(key2, key1);
if(key1 < minKey) if (key1 < minKey)
minKey = key1; minKey = key1;
} }
} }
tree.insert(minKey,keyAnchor); tree.insert(minKey, keyAnchor);
tree.insert(keyAnchor,keyAnchor); // root tree.insert(keyAnchor, keyAnchor); // root
return tree; return tree;
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){ // Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) {
// Find a minimum spanning tree // Find a minimum spanning tree
PredecessorMap<Key> tree; PredecessorMap<Key> tree;
if (useOdometricPath) if (useOdometricPath)
tree = findOdometricPath(pose2Graph); tree = findOdometricPath(pose2Graph);
else else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph); tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(pose2Graph);
// 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);
// temporary structure to correct wraparounds along loops // temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
// 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);
// Solve the LFG // Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize(); VectorValues orientationsLago = lagoGraph.optimize();
@ -240,70 +279,81 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo
} }
/* ************************************************************************* */ /* ************************************************************************* */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { VectorValues initializeOrientations(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, useOdometricPath); return computeOrientations(pose2Graph, useOdometricPath);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) { Values computePoses(const NonlinearFactorGraph& pose2graph,
VectorValues& orientationsLago) {
// Linearized graph on full poses // Linearized graph on full poses
GaussianFactorGraph linearPose2graph; GaussianFactorGraph linearPose2graph;
// We include the linear version of each between factor // We include the linear version of each between factor
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph){ BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between = boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if(pose2Between){ if (pose2Between) {
Key key1 = pose2Between->keys()[0]; Key key1 = pose2Between->keys()[0];
double theta1 = orientationsLago.at(key1)(0); double theta1 = orientationsLago.at(key1)(0);
double s1 = sin(theta1); double c1 = cos(theta1); double s1 = sin(theta1);
double c1 = cos(theta1);
Key key2 = pose2Between->keys()[1]; Key key2 = pose2Between->keys()[1];
double theta2 = orientationsLago.at(key2)(0); double theta2 = orientationsLago.at(key2)(0);
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
double dx = pose2Between->measured().x(); double dx = pose2Between->measured().x();
double dy = pose2Between->measured().y(); double dy = pose2Between->measured().y();
Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy); Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs + c1 * dy);
Matrix J1 = - I3; Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
J1(0,2) = s1*dx + c1*dy; Matrix J1 = -I3;
J1(1,2) = -c1*dx + s1*dy; J1(0, 2) = s1 * dx + c1 * dy;
J1(1, 2) = -c1 * dx + s1 * dy;
// Retrieve the noise model for the relative rotation // Retrieve the noise model for the relative rotation
boost::shared_ptr<noiseModel::Diagonal> diagonalModel = boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(pose2Between->get_noiseModel()); boost::dynamic_pointer_cast<noiseModel::Diagonal>(
pose2Between->get_noiseModel());
linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel)); linearPose2graph.add(
}else{ JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!"); } else {
throw std::invalid_argument(
"computeLagoPoses: cannot manage non between factor here!");
} }
} }
// add prior // add prior
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(
linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel)); (Vector(3) << 1e-2, 1e-2, 1e-4));
linearPose2graph.add(
JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel));
// optimize // optimize
VectorValues posesLago = linearPose2graph.optimize(); VectorValues posesLago = linearPose2graph.optimize();
// put into Values structure // put into Values structure
Values initialGuessLago; Values initialGuessLago;
for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){ BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
Key key = it->first; Key key = it.first;
if (key != keyAnchor){ if (key != keyAnchor) {
Vector poseVector = posesLago.at(key); const Vector& poseVector = it.second;
Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2)); Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
orientationsLago.at(key)(0) + poseVector(2));
initialGuessLago.insert(key, poseLago); initialGuessLago.insert(key, poseLago);
} }
} }
@ -311,37 +361,40 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) { Values initialize(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
VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath); VectorValues orientationsLago = computeOrientations(pose2Graph,
useOdometricPath);
// Compute the full poses // Compute the full poses
return computeLagoPoses(pose2Graph, orientationsLago); return computePoses(pose2Graph, orientationsLago);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) { Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess) {
Values initialGuessLago; Values initialGuessLago;
// get the orientation estimates from LAGO // get the orientation estimates from LAGO
VectorValues orientations = initializeOrientationsLago(graph); VectorValues orientations = initializeOrientations(graph);
// for all nodes in the tree // for all nodes in the tree
for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){ BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
Key key = it->first; Key key = it.first;
if (key != keyAnchor){ if (key != keyAnchor) {
Pose2 pose = initialGuess.at<Pose2>(key); const Pose2& pose = initialGuess.at<Pose2>(key);
Vector orientation = orientations.at(key); const Vector& orientation = it.second;
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0)); Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
initialGuessLago.insert(key, poseLago); initialGuessLago.insert(key, poseLago);
} }
} }
return initialGuessLago; return initialGuessLago;
} }
} // end of namespace lago
} // end of namespace gtsam } // end of namespace gtsam

86
gtsam/nonlinear/lago.h Normal file
View File

@ -0,0 +1,86 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file lago.h
* @brief Initialize Pose2 in a factor graph using LAGO
* (Linear Approximation for Graph Optimization). see papers:
*
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
* approximation for planar pose graph optimization, IJRR, 2014.
*
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
* for graph-based simultaneous localization and mapping, RSS, 2011.
*
* @param graph: nonlinear factor graph (can include arbitrary factors but we assume
* that there is a subgraph involving Pose2 and betweenFactors). Also in the current
* version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
* and a prior on x0. This assumption can be relaxed by using the extra argument
* useOdometricPath = false, although this part of code is not stable yet.
* @return Values: initial guess from LAGO (only pose2 are initialized)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
namespace gtsam {
namespace lago {
typedef std::map<Key, double> key2doubleMap;
/**
* Compute the cumulative orientations (without wrapping)
* for all nodes wrt the root (root has zero orientation).
*/
GTSAM_EXPORT key2doubleMap computeThetasToRoot(
const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
/**
* Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
* to the tree and to g, and stores the factor slots corresponding to edges in the
* tree and to chordsIds wrt this tree.
* Also it computes deltaThetaMap which is a fast way to encode relative
* orientations along the tree: for a node key2, s.t. tree[key2]=key1,
* the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1]
*/
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/** Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/** LAGO: Return the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientations(
const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/** Return the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
bool useOdometricPath = true);
/** Only correct the orientation part in initialGuess */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess);
} // end of namespace lago
} // end of namespace gtsam

106
gtsam/nonlinear/tests/testLagoInitializer.cpp → gtsam/nonlinear/tests/testLago.cpp
View File

@ -19,20 +19,14 @@
* @date May 14, 2014 * @date May 14, 2014
*/ */
#include <gtsam/geometry/Pose2.h> #include <gtsam/nonlinear/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/math/constants/constants.hpp>
#include <cmath> #include <cmath>
using namespace std; using namespace std;
@ -77,10 +71,10 @@ TEST( Lago, checkSTandChords ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1) DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0) DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
@ -100,19 +94,19 @@ TEST( Lago, orientationsOverSpanningTree ) {
EXPECT_LONGS_EQUAL(tree[x2], x0); EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0); EXPECT_LONGS_EQUAL(tree[x3], x0);
key2doubleMap expected; lago::key2doubleMap expected;
expected[x0]= 0; expected[x0]= 0;
expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1)) expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0)) expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3)) expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3))
key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap actual; lago::key2doubleMap actual;
actual = computeThetasToRoot(deltaThetaMap, tree); actual = lago::computeThetasToRoot(deltaThetaMap, tree);
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6); DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6); DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6); DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@ -125,14 +119,14 @@ TEST( Lago, regularizedMeasurements ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap; lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g); lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree); lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree); GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian(); std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded) // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4)); Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));
@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) { TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false; bool useOdometricPath = false;
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath); VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph()); VectorValues initial = lago::initializeOrientations(simple::graph());
// 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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -173,26 +167,26 @@ TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false; 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, useOdometricPath); VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePosePriorsSP ) {
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 initial = lago::initializeOrientations(g);
// 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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -200,26 +194,26 @@ TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false; 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, useOdometricPath); VectorValues initial = lago::initializeOrientations(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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) { 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 initial = lago::initializeOrientations(g);
// 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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6)); EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6)); EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
} }
/* *************************************************************************** */ /* *************************************************************************** */
@ -233,7 +227,7 @@ TEST( Lago, smallGraphValues ) {
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.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 // lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph(), initialGuess); Values actual = lago::initialize(simple::graph(), initialGuess);
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
@ -249,7 +243,7 @@ TEST( Lago, smallGraphValues ) {
TEST( Lago, smallGraph2 ) { TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations // lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph()); Values actual = lago::initialize(simple::graph());
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
@ -274,7 +268,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = initializeOrientationsLago(graphWithPrior); VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
Values actual; Values actual;
Key keyAnc = symbol('Z',9999999); Key keyAnc = symbol('Z',9999999);
for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){ for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
@ -309,7 +303,7 @@ TEST( Lago, largeGraphNoisy ) {
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4)); noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = initializeLago(graphWithPrior); Values actual = lago::initialize(graphWithPrior);
NonlinearFactorGraph gmatlab; NonlinearFactorGraph gmatlab;
Values expected; Values expected;