added testVSLAMGraph unit test. will fail until we can add a constraint to either one of the two cameras.
Chris Beall
parent
03f865d4b1
commit
ef0fc9fbac
90
.cproject
90
.cproject
|
|
@ -1,4 +1,4 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
|
|
@ -298,22 +298,6 @@
|
|||
</storageModule>
|
||||
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
|
||||
<buildTargets>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-k</buildArguments>
|
||||
|
|
@ -324,7 +308,7 @@
|
|||
</target>
|
||||
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testSimpleCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -347,22 +331,22 @@
|
|||
</target>
|
||||
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testCalibratedCamera.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianConditional.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testConditionalGaussian.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>testGaussianConditional.run</buildTarget>
|
||||
<buildTarget>testConditionalGaussian.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testPose2.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -370,7 +354,7 @@
|
|||
</target>
|
||||
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testRot3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -383,18 +367,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactor.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="testLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGaussianFactorGraph.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -408,7 +392,7 @@
|
|||
</target>
|
||||
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testPose3.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -430,31 +414,31 @@
|
|||
</target>
|
||||
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testNonlinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="timeLinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactor.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>timeLinearFactor.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<target name="timeLinearFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>timeGaussianFactorGraph.run</buildTarget>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>timeLinearFactorGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testGaussianBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -469,7 +453,7 @@
|
|||
</target>
|
||||
<target name="testSymbolicBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testSymbolicBayesNet.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
|
|
@ -484,7 +468,7 @@
|
|||
</target>
|
||||
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testVector.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -492,7 +476,7 @@
|
|||
</target>
|
||||
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testMatrix.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
|
|
@ -505,6 +489,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="testVSLAMGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>testVSLAMGraph.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildTarget>install</buildTarget>
|
||||
|
|
@ -526,6 +518,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>install</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments></buildArguments>
|
||||
<buildTarget>check</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
</buildTargets>
|
||||
</storageModule>
|
||||
</cconfiguration>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,123 @@
|
|||
/**
|
||||
* @file testVSLAMGraph.cpp
|
||||
* @brief Unit test for two cameras and four landmarks
|
||||
* single camera
|
||||
* @author Chris Beall
|
||||
* @author Frank Dellaert
|
||||
* @author Viorela Ila
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include "VSLAMGraph.h"
|
||||
#include "NonlinearFactorGraph-inl.h"
|
||||
#include "NonlinearOptimizer-inl.h"
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
typedef NonlinearOptimizer<VSLAMGraph,VSLAMConfig> Optimizer;
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 landmark1_local(-1.0,-1.0, 0.0);
|
||||
Point3 landmark2_local(-1.0, 1.0, 0.0);
|
||||
Point3 landmark3_local( 1.0, 1.0, 0.0);
|
||||
Point3 landmark4_local( 1.0,-1.0, 0.0);
|
||||
|
||||
Pose3 camera1_local(Matrix_(3,3,
|
||||
1., 0., 0.,
|
||||
0.,-1., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,6.25));
|
||||
|
||||
Pose3 camera2_local(Matrix_(3,3,
|
||||
1., 0., 0.,
|
||||
0.,-1., 0.,
|
||||
0., 0.,-1.
|
||||
),
|
||||
Point3(0,0,5.00));
|
||||
|
||||
Point3 landmark1() { return landmark1_local;}
|
||||
Point3 landmark2() { return landmark2_local;}
|
||||
Point3 landmark3() { return landmark3_local;}
|
||||
Point3 landmark4() { return landmark4_local;}
|
||||
|
||||
Pose3 camera1() { return camera1_local;}
|
||||
Pose3 camera2() { return camera2_local;}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
VSLAMGraph testGraph() {
|
||||
Point2 z11(-100, 100);
|
||||
Point2 z12(-100, -100);
|
||||
Point2 z13(100, -100);
|
||||
Point2 z14(100, 100);
|
||||
Point2 z21(-125, 125);
|
||||
Point2 z22(-125, -125);
|
||||
Point2 z23(125, -125);
|
||||
Point2 z24(125, 125);
|
||||
|
||||
double sigma = 1;
|
||||
Cal3_S2 K(625, 625, 0, 0, 0);
|
||||
VSLAMGraph g;
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z11.vector(), sigma, 1, 1, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z12.vector(), sigma, 1, 2, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z13.vector(), sigma, 1, 3, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z14.vector(), sigma, 1, 4, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z21.vector(), sigma, 2, 1, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z22.vector(), sigma, 2, 2, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z23.vector(), sigma, 2, 3, K)));
|
||||
g.push_back(VSLAMFactor::shared_ptr(new VSLAMFactor(z24.vector(), sigma, 2, 4, K)));
|
||||
|
||||
return g;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( VSLAMGraph, optimizeLM)
|
||||
{
|
||||
// build a graph
|
||||
VSLAMGraph graph = testGraph();
|
||||
|
||||
// Create a configuration corresponding to the ground truth
|
||||
VSLAMConfig groundTruth;
|
||||
groundTruth.addCameraPose(1, camera1());
|
||||
groundTruth.addCameraPose(2, camera2());
|
||||
groundTruth.addLandmarkPoint(1, landmark1());
|
||||
groundTruth.addLandmarkPoint(2, landmark2());
|
||||
groundTruth.addLandmarkPoint(3, landmark3());
|
||||
groundTruth.addLandmarkPoint(4, landmark4());
|
||||
|
||||
// Create an ordering of the variables
|
||||
list<string> keys;
|
||||
keys.push_back("l1");
|
||||
keys.push_back("l2");
|
||||
keys.push_back("l3");
|
||||
keys.push_back("l4");
|
||||
keys.push_back("x1");
|
||||
keys.push_back("x2");
|
||||
Ordering ordering(keys);
|
||||
|
||||
// optimize it
|
||||
double relativeErrorTreshold=1e-6, absoluteErrorTreshold=1.0;
|
||||
int verbosity=0;
|
||||
|
||||
Optimizer::shared_config initialConfig(new VSLAMConfig(groundTruth));
|
||||
Optimizer optimizer(graph, ordering, initialConfig, 1e-5);
|
||||
|
||||
// We expect the initial to be zero because config is the ground truth
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
// Iterate once, and the config should not have changed because we started
|
||||
// with the ground truth
|
||||
Optimizer afterOneIteration = optimizer.iterate();
|
||||
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
|
||||
|
||||
//afterOneIteration.config()->print();
|
||||
|
||||
// check if correct
|
||||
CHECK(groundTruth.equals(*(afterOneIteration.config())) );
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; TestRegistry::runAllTests(tr); return 0;}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue