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

All unit tests compile

release/4.3a0
Richard Roberts 2012-03-22 17:46:43 +00:00
parent 13ce0e59f0
commit be386ed6bd
5 changed files with 56 additions and 65 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.cproject
View File

@ -21,7 +21,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="7" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@ -285,6 +285,7 @@
</profile>
</scannerConfigBuildInfo>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
<storageModule moduleId="org.eclipse.cdt.make.core.buildtargets">
<buildTargets>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -2044,6 +2045,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor.run" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j7</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2221,5 +2230,4 @@
</target>
</buildTargets>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cproject>

2
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j5</value>
<value>-j7</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

12
tests/testNonlinearEquality.cpp
View File

@ -195,7 +195,7 @@ TEST ( NonlinearEquality, allow_error_optimize ) {
// optimize
Ordering ordering;
ord.push_back(key1);
ordering.push_back(key1);
NonlinearOptimizer::auto_ptr result = LevenbergMarquardtOptimizer(graph, init, ordering).optimize();
// verify
@ -229,8 +229,8 @@ TEST ( NonlinearEquality, allow_error_optimize_with_factors ) {
// optimize
Ordering ordering;
ord.push_back(key1);
Values actual = *LevenbergMarquardtOptimizer(graph, values, ordering).optimize()->values();
ordering.push_back(key1);
Values actual = *LevenbergMarquardtOptimizer(graph, init, ordering).optimize()->values();
// verify
Values expected;
@ -399,7 +399,7 @@ TEST( testNonlinearEqualityConstraint, odo_simple_optimize ) {
new eq2D::OdoEqualityConstraint(
truth_pt1.between(truth_pt2), key1, key2));
Graph graph;
NonlinearFactorGraph graph;
graph.push_back(constraint1);
graph.push_back(constraint2);
graph.push_back(factor);
@ -423,7 +423,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
* constrained to a particular value
*/
Graph graph;
NonlinearFactorGraph graph;
Symbol x1('x',1), x2('x',2);
Symbol l1('l',1), l2('l',2);
@ -460,7 +460,7 @@ TEST (testNonlinearEqualityConstraint, two_pose ) {
/* ********************************************************************* */
TEST (testNonlinearEqualityConstraint, map_warp ) {
// get a graph
Graph graph;
NonlinearFactorGraph graph;
// keys
Symbol x1('x',1), x2('x',2);

90
tests/testNonlinearOptimizer.cpp
View File

@ -34,7 +34,7 @@ using namespace boost;
// template definitions
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
using namespace gtsam;
@ -68,7 +68,7 @@ TEST( NonlinearOptimizer, iterateLM )
// LM iterate with lambda 0 should be the same
NonlinearOptimizer::auto_ptr iterated2 = LevenbergMarquardtOptimizer(fg, config, LevenbergMarquardtParams(), ord).update(0.0)->iterate();
CHECK(assert_equal(*iterated1.values(), *iterated2.values(), 1e-9));
CHECK(assert_equal(*iterated1->values(), *iterated2->values(), 1e-9));
}
/* ************************************************************************* */
@ -93,18 +93,12 @@ TEST( NonlinearOptimizer, optimize )
shared_ptr<Ordering> ord(new Ordering());
ord->push_back(kx(1));
// initial optimization state is the same in both cases tested
boost::shared_ptr<NonlinearOptimizationParameters> params = boost::make_shared<NonlinearOptimizationParameters>();
params->relDecrease_ = 1e-5;
params->absDecrease_ = 1e-5;
Optimizer optimizer(fg, c0, ord, params);
// Gauss-Newton
NonlinearOptimizer::auto_ptr actual1 = *GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize();
NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, GaussNewtonParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual1->values())),tol);
// Levenberg-Marquardt
Optimizer actual2 = *LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimizer();
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, LevenbergMarquardtParams(), ord).optimize();
DOUBLES_EQUAL(0,fg->error(*(actual2->values())),tol);
}
@ -118,7 +112,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer )
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0);
Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg->error(*actual),tol);
}
@ -131,7 +125,7 @@ TEST( NonlinearOptimizer, SimpleLMOptimizer_noshared )
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_values actual = Optimizer::optimizeLM(fg, c0);
Values::const_shared_ptr actual = LevenbergMarquardtOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg.error(*actual),tol);
}
@ -145,7 +139,7 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer )
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0);
Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg->error(*actual),tol);
}
@ -158,15 +152,17 @@ TEST( NonlinearOptimizer, SimpleGNOptimizer_noshared )
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Optimizer::shared_values actual = Optimizer::optimizeGN(fg, c0);
Values::const_shared_ptr actual = GaussNewtonOptimizer(fg, c0).optimize()->values();
DOUBLES_EQUAL(0,fg.error(*actual),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, optimization_method )
{
GaussNewtonParams params;
params.elimination = GaussNewtonParams::QR;
LevenbergMarquardtParams paramsQR;
paramsQR.factorization = LevenbergMarquardtParams::QR;
LevenbergMarquardtParams paramsLDL;
paramsLDL.factorization = LevenbergMarquardtParams::LDL;
EXPECT(false);
example::Graph fg = example::createReallyNonlinearFactorGraph();
@ -175,79 +171,67 @@ TEST( NonlinearOptimizer, optimization_method )
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
Values actualMFQR = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(true), MULTIFRONTAL, LM);
Values actualMFQR = *LevenbergMarquardtOptimizer(fg, c0, paramsQR).optimize()->values();
DOUBLES_EQUAL(0,fg.error(actualMFQR),tol);
Values actualMFLDL = optimize<example::Graph>(
fg, c0, *NonlinearOptimizationParameters().newFactorization(false), MULTIFRONTAL, LM);
Values actualMFLDL = *LevenbergMarquardtOptimizer(fg, c0, paramsLDL).optimize()->values();
DOUBLES_EQUAL(0,fg.error(actualMFLDL),tol);
}
/* ************************************************************************* */
TEST( NonlinearOptimizer, Factorization )
{
typedef NonlinearOptimizer<pose2SLAM::Graph, GaussianFactorGraph, GaussianSequentialSolver > Optimizer;
Values config;
config.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
config.insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.));
boost::shared_ptr<Values> config(new Values);
config->insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
config->insert(pose2SLAM::PoseKey(2), Pose2(1.5,0.,0.));
pose2SLAM::Graph graph;
graph.addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
boost::shared_ptr<pose2SLAM::Graph> graph(new pose2SLAM::Graph);
graph->addPrior(1, Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph->addOdometry(1,2, Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering;
ordering.push_back(pose2SLAM::PoseKey(1));
ordering.push_back(pose2SLAM::PoseKey(2));
boost::shared_ptr<Ordering> ordering(new Ordering);
ordering->push_back(pose2SLAM::PoseKey(1));
ordering->push_back(pose2SLAM::PoseKey(2));
Optimizer optimizer(graph, config, ordering);
Optimizer optimized = optimizer.iterateLM();
NonlinearOptimizer::auto_ptr optimized = LevenbergMarquardtOptimizer(graph, config, ordering).iterate();
Values expected;
expected.insert(pose2SLAM::PoseKey(1), Pose2(0.,0.,0.));
expected.insert(pose2SLAM::PoseKey(2), Pose2(1.,0.,0.));
CHECK(assert_equal(expected, *optimized.values(), 1e-5));
CHECK(assert_equal(expected, *optimized->values(), 1e-5));
}
/* ************************************************************************* */
TEST_UNSAFE(NonlinearOptimizer, NullFactor) {
shared_ptr<example::Graph> fg(new example::Graph(
example::createReallyNonlinearFactorGraph()));
example::Graph fg = example::createReallyNonlinearFactorGraph();
// Add null factor
fg->push_back(example::Graph::sharedFactor());
fg.push_back(example::Graph::sharedFactor());
// test error at minimum
Point2 xstar(0,0);
Values cstar;
cstar.insert(simulated2D::PoseKey(1), xstar);
DOUBLES_EQUAL(0.0,fg->error(cstar),0.0);
DOUBLES_EQUAL(0.0,fg.error(cstar),0.0);
// test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
Point2 x0(3,3);
boost::shared_ptr<Values> c0(new Values);
c0->insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg->error(*c0),1e-3);
Values c0;
c0.insert(simulated2D::PoseKey(1), x0);
DOUBLES_EQUAL(199.0,fg.error(c0),1e-3);
// optimize parameters
shared_ptr<Ordering> ord(new Ordering());
ord->push_back(kx(1));
// initial optimization state is the same in both cases tested
boost::shared_ptr<NonlinearOptimizationParameters> params = boost::make_shared<NonlinearOptimizationParameters>();
params->relDecrease_ = 1e-5;
params->absDecrease_ = 1e-5;
Optimizer optimizer(fg, c0, ord, params);
Ordering ord;
ord.push_back(kx(1));
// Gauss-Newton
Optimizer actual1 = optimizer.gaussNewton();
DOUBLES_EQUAL(0,fg->error(*(actual1.values())),tol);
NonlinearOptimizer::auto_ptr actual1 = GaussNewtonOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual1->values()),tol);
// Levenberg-Marquardt
Optimizer actual2 = optimizer.levenbergMarquardt();
DOUBLES_EQUAL(0,fg->error(*(actual2.values())),tol);
NonlinearOptimizer::auto_ptr actual2 = LevenbergMarquardtOptimizer(fg, c0, ord).optimize();
DOUBLES_EQUAL(0,fg.error(*actual2->values()),tol);
}
///* ************************************************************************* */

5
tests/testRot3Optimization.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
@ -47,8 +47,7 @@ TEST(Rot3, optimize) {
fg.add(Between(Symbol('r',j), Symbol('r',(j+1)%6), Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
}
NonlinearOptimizationParameters params;
Values final = optimize(fg, initial, params);
Values final = *GaussNewtonOptimizer(fg, initial).optimize()->values();
EXPECT(assert_equal(truth, final, 1e-5));
}