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
commit
e19f3c5902
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);
|
||||
Values estimateLago = lago::initialize(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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -10,38 +10,57 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file LagoInitializer.h
|
||||
* @file lago.h
|
||||
* @author Luca Carlone
|
||||
* @author Frank Dellaert
|
||||
* @date May 14, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/LagoInitializer.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/lago.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>
|
||||
|
||||
namespace gtsam {
|
||||
namespace lago {
|
||||
|
||||
static Matrix I = eye(1);
|
||||
static Matrix I3 = eye(3);
|
||||
static const Matrix I = eye(1);
|
||||
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;
|
||||
Key key_child = nodeKey; // the node
|
||||
Key key_parent = 0; // the initialization does not matter
|
||||
while(1){
|
||||
while (1) {
|
||||
// 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;
|
||||
// we sum the delta theta corresponding to the edge parent->child
|
||||
nodeTheta += deltaThetaMap.at(key_child);
|
||||
// we get the parent
|
||||
key_parent = tree.at(key_child); // the parent
|
||||
// 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);
|
||||
break;
|
||||
}
|
||||
|
|
@ -55,15 +74,14 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
|
|||
const PredecessorMap<Key>& tree) {
|
||||
|
||||
key2doubleMap thetaToRootMap;
|
||||
key2doubleMap::const_iterator it;
|
||||
|
||||
// Orientation of the roo
|
||||
thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
|
||||
|
||||
// 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
|
||||
Key nodeKey = it->first;
|
||||
Key nodeKey = it.first;
|
||||
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
|
||||
thetaToRootMap);
|
||||
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
|
||||
|
|
@ -73,35 +91,38 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
|
|||
|
||||
/* ************************************************************************* */
|
||||
void getSymbolicGraph(
|
||||
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
|
||||
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
|
||||
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
|
||||
key2doubleMap& deltaThetaMap,
|
||||
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
|
||||
|
||||
// Get keys for which you want the orientation
|
||||
size_t id=0;
|
||||
size_t id = 0;
|
||||
// Loop over the factors
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g){
|
||||
if (factor->keys().size() == 2){
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
|
||||
if (factor->keys().size() == 2) {
|
||||
Key key1 = factor->keys()[0];
|
||||
Key key2 = factor->keys()[1];
|
||||
// recast to a between
|
||||
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
if (!pose2Between) continue;
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
if (!pose2Between)
|
||||
continue;
|
||||
// get the orientation - measured().theta();
|
||||
double deltaTheta = pose2Between->measured().theta();
|
||||
// insert (directed) orientations in the map "deltaThetaMap"
|
||||
bool inTree=false;
|
||||
if(tree.at(key1)==key2){ // key2 -> key1
|
||||
bool inTree = false;
|
||||
if (tree.at(key1) == key2) { // key2 -> key1
|
||||
deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
|
||||
inTree = true;
|
||||
} else if(tree.at(key2)==key1){ // key1 -> key2
|
||||
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
|
||||
} else if (tree.at(key2) == key1) { // key1 -> key2
|
||||
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
|
||||
inTree = true;
|
||||
}
|
||||
// store factor slot, distinguishing spanning tree edges from chordsIds
|
||||
if(inTree == true)
|
||||
if (inTree == true)
|
||||
spanningTreeIds.push_back(id);
|
||||
else // it's a chord!
|
||||
else
|
||||
// it's a chord!
|
||||
chordsIds.push_back(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) {
|
||||
|
||||
// Get the relative rotation measurement from the between factor
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
if (!pose2Between)
|
||||
throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!");
|
||||
throw std::invalid_argument(
|
||||
"buildLinearOrientationGraph: invalid between factor!");
|
||||
deltaTheta = (Vector(1) << pose2Between->measured().theta());
|
||||
|
||||
// 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::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonalModel)
|
||||
throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model "
|
||||
"(current version assumes diagonal noise model)!");
|
||||
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement
|
||||
throw std::invalid_argument(
|
||||
"buildLinearOrientationGraph: invalid noise model "
|
||||
"(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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
|
||||
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
|
||||
GaussianFactorGraph buildLinearOrientationGraph(
|
||||
const std::vector<size_t>& spanningTreeIds,
|
||||
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
|
||||
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree) {
|
||||
|
||||
GaussianFactorGraph lagoGraph;
|
||||
Vector deltaTheta;
|
||||
noiseModel::Diagonal::shared_ptr model_deltaTheta;
|
||||
|
||||
// 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();
|
||||
Key key1 = keys[0], key2 = keys[1];
|
||||
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
|
||||
BOOST_FOREACH(const size_t& factorId, chordsIds){
|
||||
BOOST_FOREACH(const size_t& factorId, chordsIds) {
|
||||
const FastVector<Key>& keys = g[factorId]->keys();
|
||||
Key key1 = keys[0], key2 = keys[1];
|
||||
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
|
||||
double key1_DeltaTheta_key2 = deltaTheta(0);
|
||||
///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 k = boost::math::round(k2pi_noise/(2*M_PI));
|
||||
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 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
|
||||
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
|
||||
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
|
||||
Vector deltaThetaRegularized = (Vector(1)
|
||||
<< key1_DeltaTheta_key2 - 2 * k * M_PI);
|
||||
lagoGraph.add(
|
||||
JacobianFactor(key1, -I, key2, I, deltaThetaRegularized,
|
||||
model_deltaTheta));
|
||||
}
|
||||
// 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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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;
|
||||
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph){
|
||||
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
|
||||
|
||||
// recast to a between on Pose2
|
||||
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
if (pose2Between)
|
||||
pose2Graph.add(pose2Between);
|
||||
|
||||
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
|
||||
boost::shared_ptr< PriorFactor<Pose2> > pose2Prior =
|
||||
boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
|
||||
boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
|
||||
if (pose2Prior)
|
||||
pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
|
||||
pose2Prior->prior(), pose2Prior->get_noiseModel()));
|
||||
pose2Graph.add(
|
||||
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
|
||||
pose2Prior->prior(), pose2Prior->get_noiseModel()));
|
||||
}
|
||||
return pose2Graph;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
|
||||
static PredecessorMap<Key> findOdometricPath(
|
||||
const NonlinearFactorGraph& pose2Graph) {
|
||||
|
||||
PredecessorMap<Key> tree;
|
||||
Key minKey;
|
||||
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 key2 = std::max(factor->keys()[0], factor->keys()[1]);
|
||||
if(minUnassigned){
|
||||
if (minUnassigned) {
|
||||
minKey = key1;
|
||||
minUnassigned = false;
|
||||
}
|
||||
if( key2 - key1 == 1){ // consecutive keys
|
||||
if (key2 - key1 == 1) { // consecutive keys
|
||||
tree.insert(key2, key1);
|
||||
if(key1 < minKey)
|
||||
if (key1 < minKey)
|
||||
minKey = key1;
|
||||
}
|
||||
}
|
||||
tree.insert(minKey,keyAnchor);
|
||||
tree.insert(keyAnchor,keyAnchor); // root
|
||||
tree.insert(minKey, keyAnchor);
|
||||
tree.insert(keyAnchor, keyAnchor); // root
|
||||
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
|
||||
PredecessorMap<Key> tree;
|
||||
if (useOdometricPath)
|
||||
tree = findOdometricPath(pose2Graph);
|
||||
else
|
||||
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
|
||||
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(pose2Graph);
|
||||
|
||||
// Create a linear factor graph (LFG) of scalars
|
||||
key2doubleMap deltaThetaMap;
|
||||
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);
|
||||
|
||||
// temporary structure to correct wraparounds along loops
|
||||
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
|
||||
|
||||
// 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
|
||||
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
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
|
||||
|
||||
// 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
|
||||
GaussianFactorGraph linearPose2graph;
|
||||
|
||||
// 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::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
|
||||
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
|
||||
|
||||
if(pose2Between){
|
||||
if (pose2Between) {
|
||||
Key key1 = pose2Between->keys()[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];
|
||||
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
|
||||
|
||||
double dx = pose2Between->measured().x();
|
||||
double dy = pose2Between->measured().y();
|
||||
|
||||
Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy);
|
||||
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs
|
||||
Matrix J1 = - I3;
|
||||
J1(0,2) = s1*dx + c1*dy;
|
||||
J1(1,2) = -c1*dx + s1*dy;
|
||||
Vector globalDeltaCart = (Vector(2) << c1 * dx - s1 * dy, s1 * dx
|
||||
+ c1 * dy);
|
||||
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
|
||||
Matrix J1 = -I3;
|
||||
J1(0, 2) = s1 * dx + c1 * dy;
|
||||
J1(1, 2) = -c1 * dx + s1 * dy;
|
||||
// Retrieve the noise model for the relative rotation
|
||||
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));
|
||||
}else{
|
||||
throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!");
|
||||
linearPose2graph.add(
|
||||
JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
|
||||
} else {
|
||||
throw std::invalid_argument(
|
||||
"computeLagoPoses: cannot manage non between factor here!");
|
||||
}
|
||||
}
|
||||
// add prior
|
||||
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
|
||||
linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel));
|
||||
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(
|
||||
(Vector(3) << 1e-2, 1e-2, 1e-4));
|
||||
linearPose2graph.add(
|
||||
JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0), priorModel));
|
||||
|
||||
// optimize
|
||||
VectorValues posesLago = linearPose2graph.optimize();
|
||||
|
||||
// put into Values structure
|
||||
Values initialGuessLago;
|
||||
for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){
|
||||
Key key = it->first;
|
||||
if (key != keyAnchor){
|
||||
Vector poseVector = posesLago.at(key);
|
||||
Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2));
|
||||
BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
|
||||
Key key = it.first;
|
||||
if (key != keyAnchor) {
|
||||
const Vector& poseVector = it.second;
|
||||
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
|
||||
orientationsLago.at(key)(0) + poseVector(2));
|
||||
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
|
||||
// is done to properly model priors and avoiding operating on a larger graph
|
||||
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
|
||||
|
||||
// Get orientations from relative orientation measurements
|
||||
VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath);
|
||||
VectorValues orientationsLago = computeOrientations(pose2Graph,
|
||||
useOdometricPath);
|
||||
|
||||
// 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;
|
||||
|
||||
// get the orientation estimates from LAGO
|
||||
VectorValues orientations = initializeOrientationsLago(graph);
|
||||
VectorValues orientations = initializeOrientations(graph);
|
||||
|
||||
// for all nodes in the tree
|
||||
for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){
|
||||
Key key = it->first;
|
||||
if (key != keyAnchor){
|
||||
Pose2 pose = initialGuess.at<Pose2>(key);
|
||||
Vector orientation = orientations.at(key);
|
||||
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
|
||||
BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
|
||||
Key key = it.first;
|
||||
if (key != keyAnchor) {
|
||||
const Pose2& pose = initialGuess.at<Pose2>(key);
|
||||
const Vector& orientation = it.second;
|
||||
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
|
||||
initialGuessLago.insert(key, poseLago);
|
||||
}
|
||||
}
|
||||
return initialGuessLago;
|
||||
}
|
||||
|
||||
} // end of namespace lago
|
||||
} // end of namespace gtsam
|
||||
|
|
@ -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
|
||||
|
|
@ -19,20 +19,14 @@
|
|||
* @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/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 <boost/math/constants/constants.hpp>
|
||||
|
||||
#include <cmath>
|
||||
|
||||
using namespace std;
|
||||
|
|
@ -77,10 +71,10 @@ TEST( Lago, checkSTandChords ) {
|
|||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
|
||||
key2doubleMap deltaThetaMap;
|
||||
lago::key2doubleMap deltaThetaMap;
|
||||
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
|
||||
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[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[x3], x0);
|
||||
|
||||
key2doubleMap expected;
|
||||
lago::key2doubleMap expected;
|
||||
expected[x0]= 0;
|
||||
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[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> 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;
|
||||
actual = computeThetasToRoot(deltaThetaMap, tree);
|
||||
lago::key2doubleMap actual;
|
||||
actual = lago::computeThetasToRoot(deltaThetaMap, tree);
|
||||
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
|
||||
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
|
||||
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
|
||||
|
|
@ -125,14 +119,14 @@ TEST( Lago, regularizedMeasurements ) {
|
|||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
|
||||
key2doubleMap deltaThetaMap;
|
||||
lago::key2doubleMap deltaThetaMap;
|
||||
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
|
||||
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();
|
||||
// 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));
|
||||
|
|
@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) {
|
|||
/* *************************************************************************** */
|
||||
TEST( Lago, smallGraphVectorValues ) {
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.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) << 0.0), initial.at(x0), 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), initial.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 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;
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.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) << 0.0), initial.at(x0), 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), initial.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, multiplePosePriorsSP ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 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;
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.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) << 0.0), initial.at(x0), 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), initial.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
|
||||
}
|
||||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, multiplePoseAndRotPriorsSP ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
|
||||
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 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));
|
||||
|
||||
// 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
|
||||
Values expected;
|
||||
|
|
@ -249,7 +243,7 @@ TEST( Lago, smallGraphValues ) {
|
|||
TEST( Lago, smallGraph2 ) {
|
||||
|
||||
// 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
|
||||
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));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
||||
VectorValues actualVV = initializeOrientationsLago(graphWithPrior);
|
||||
VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
|
||||
Values actual;
|
||||
Key keyAnc = symbol('Z',9999999);
|
||||
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));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
|
||||
Values actual = initializeLago(graphWithPrior);
|
||||
Values actual = lago::initialize(graphWithPrior);
|
||||
|
||||
NonlinearFactorGraph gmatlab;
|
||||
Values expected;
|
||||
Loading…
Reference in New Issue