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

Updating unit tests for new NonlinearOptimizer

release/4.3a0
Richard Roberts 2012-02-28 20:55:50 +00:00
parent 8748f483b0
commit 9312b0a128
18 changed files with 107 additions and 181 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
.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="false" 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="5" 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">
@ -2186,4 +2186,5 @@
</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></value>
<value>-j5</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>

5
gtsam/nonlinear/GaussNewtonOptimizer.h
View File

@ -69,6 +69,7 @@ public:
/**
* This class performs Gauss-Newton nonlinear optimization
* TODO: use make_shared
*/
class GaussNewtonOptimizer : public NonlinearOptimizer {
@ -107,9 +108,9 @@ public:
* @param params The optimization parameters
*/
GaussNewtonOptimizer(const SharedGraph& graph, const SharedValues& values,
const SharedGNParams& params = SharedGNParams(),
const GaussNewtonParams& params = GaussNewtonParams(),
const SharedOrdering& ordering = SharedOrdering()) :
NonlinearOptimizer(graph, values, params ? params : SharedGNParams(new GaussNewtonParams())),
NonlinearOptimizer(graph, values, SharedGNParams(new GaussNewtonParams(params))),
gnParams_(boost::static_pointer_cast<const GaussNewtonParams>(params_)),
colamdOrdering_(!ordering || ordering->size() == 0),
ordering_(colamdOrdering_ ? graph_->orderingCOLAMD(*values_) : ordering) {}

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -90,6 +90,7 @@ public:
/**
* This class performs Levenberg-Marquardt nonlinear optimization
* TODO: use make_shared
*/
class LevenbergMarquardtOptimizer : public NonlinearOptimizer {
@ -130,9 +131,9 @@ public:
* @param params The optimization parameters
*/
LevenbergMarquardtOptimizer(const SharedGraph& graph, const SharedValues& values,
const SharedLMParams& params = SharedLMParams(),
const LevenbergMarquardtParams& params = LevenbergMarquardtParams(),
const SharedOrdering& ordering = SharedOrdering()) :
NonlinearOptimizer(graph, values, params ? params : SharedLMParams(new LevenbergMarquardtParams())),
NonlinearOptimizer(graph, values, SharedLMParams(new LevenbergMarquardtParams(params))),
lmParams_(boost::static_pointer_cast<const LevenbergMarquardtParams>(params_)),
colamdOrdering_(!ordering || ordering->size() == 0),
ordering_(colamdOrdering_ ? graph_->orderingCOLAMD(*values_) : ordering),

2
gtsam/slam/planarSLAM.cpp
View File

@ -17,8 +17,6 @@
#include <gtsam/slam/planarSLAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
// Use planarSLAM namespace for specific SLAM instance
namespace planarSLAM {

9
gtsam/slam/planarSLAM.h
View File

@ -24,7 +24,7 @@
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/geometry/Pose2.h>
@ -110,15 +110,10 @@ namespace planarSLAM {
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values();
}
};
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // planarSLAM

2
gtsam/slam/pose2SLAM.cpp
View File

@ -21,8 +21,6 @@
// Use pose2SLAM namespace for specific SLAM instance
template class gtsam::NonlinearOptimizer<pose2SLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianSequentialSolver>;
namespace pose2SLAM {
/* ************************************************************************* */

14
gtsam/slam/pose2SLAM.h
View File

@ -24,7 +24,7 @@
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
@ -97,20 +97,10 @@ namespace pose2SLAM {
/// Optimize
Values optimize(const Values& initialEstimate) {
typedef NonlinearOptimizer<Graph> Optimizer;
return *Optimizer::optimizeLM(*this, initialEstimate,
NonlinearOptimizationParameters::LAMBDA);
return *LevenbergMarquardtOptimizer(*this, initialEstimate).optimize()->values();
}
};
/// The sequential optimizer
typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
GaussianSequentialSolver> OptimizerSequential;
/// The multifrontal optimizer
typedef NonlinearOptimizer<Graph, GaussianFactorGraph,
GaussianMultifrontalSolver> Optimizer;
} // pose2SLAM

3
gtsam/slam/pose3SLAM.h
View File

@ -61,9 +61,6 @@ namespace gtsam {
void addHardConstraint(Index i, const Pose3& p);
};
/// Optimizer
typedef NonlinearOptimizer<Graph> Optimizer;
} // pose3SLAM
/**

48
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -16,7 +16,7 @@ using namespace boost;
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h>
@ -62,8 +62,6 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
/* ************************************************************************* */
@ -175,11 +173,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -220,11 +216,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -263,12 +257,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -323,13 +314,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -366,12 +353,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-2, 1e-2, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */

47
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -16,7 +16,7 @@ using namespace boost;
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h>
@ -63,8 +63,6 @@ double getGaussian()
return sqrt(-2.0f * (double)log(S) / S) * V1;
}
typedef NonlinearOptimizer<Graph> Optimizer;
const SharedNoiseModel sigma1(noiseModel::Unit::Create(1));
/* ************************************************************************* */
@ -177,11 +175,9 @@ TEST( GeneralSFMFactor, optimize_defaultK ) {
// Create an ordering of the variables
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * 1e-5 * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * 1e-5 * nMeasurements);
}
/* ************************************************************************* */
@ -222,11 +218,9 @@ TEST( GeneralSFMFactor, optimize_varK_SingleMeasurementError ) {
const double reproj_error = 1e-5;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -265,12 +259,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixCameras ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -321,12 +312,9 @@ TEST( GeneralSFMFactor, optimize_varK_FixLandmarks ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-3, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */
@ -363,12 +351,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
const double reproj_error = 1e-5 ;
shared_ptr<Ordering> ordering = getOrdering(X,L);
NonlinearOptimizationParameters::shared_ptr params (
new NonlinearOptimizationParameters(1e-5, 1e-5, 0.0, 100, 1e-5, 10, NonlinearOptimizationParameters::SILENT));
Optimizer optimizer(graph, values, ordering, params);
Optimizer optimizer2 = optimizer.levenbergMarquardt();
EXPECT(optimizer2.error() < 0.5 * reproj_error * nMeasurements);
NonlinearOptimizer::auto_ptr optimizer =
LevenbergMarquardtOptimizer(graph, values, LevenbergMarquardtParams(), ordering).optimize();
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
}
/* ************************************************************************* */

25
gtsam/slam/tests/testPose2SLAM.cpp
View File

@ -160,17 +160,16 @@ TEST(Pose2Graph, optimize) {
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += kx0, kx1;
typedef NonlinearOptimizer<pose2SLAM::Graph> Optimizer;
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer0(fg, initial, ordering, params);
Optimizer optimizer = optimizer0.levenbergMarquardt();
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize();
// Check with expected config
Values expected;
expected.insert(pose2SLAM::PoseKey(0), Pose2(0,0,0));
expected.insert(pose2SLAM::PoseKey(1), Pose2(1,2,M_PI_2));
CHECK(assert_equal(expected, *optimizer.values()));
CHECK(assert_equal(expected, *optimizer->values()));
}
/* ************************************************************************* */
@ -200,11 +199,11 @@ TEST(Pose2Graph, optimizeThreePoses) {
*ordering += kx0,kx1,kx2;
// optimize
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize();
Values actual = *optimizer.values();
Values actual = *optimizer->values();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));
@ -243,11 +242,11 @@ TEST_UNSAFE(Pose2SLAM, optimizeCircle) {
*ordering += kx0,kx1,kx2,kx3,kx4,kx5;
// optimize
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
pose2SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose2SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
LevenbergMarquardtParams params;
params.relativeErrorTol = 1e-15;
NonlinearOptimizer::auto_ptr optimizer = LevenbergMarquardtOptimizer(fg, initial, params, ordering).optimize();
Values actual = *optimizer.values();
Values actual = *optimizer->values();
// Check with ground truth
CHECK(assert_equal((const Values&)hexagon, actual));

11
gtsam/slam/tests/testPose3SLAM.cpp
View File

@ -30,6 +30,7 @@ using namespace boost::assign;
#include <gtsam/slam/pose3SLAM.h>
#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
@ -76,11 +77,9 @@ TEST(Pose3Graph, optimizeCircle) {
// Choose an ordering and optimize
shared_ptr<Ordering> ordering(new Ordering);
*ordering += kx0,kx1,kx2,kx3,kx4,kx5;
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
pose3SLAM::Optimizer optimizer0(fg, initial, ordering, params);
pose3SLAM::Optimizer optimizer = optimizer0.levenbergMarquardt();
Values actual = *optimizer.values();
Values actual = *LevenbergMarquardtOptimizer(
fg, initial, LevenbergMarquardtOptimizer::SharedLMParams(), ordering).optimize()->values();
// Check with ground truth
CHECK(assert_equal(hexagon, actual,1e-4));
@ -115,7 +114,7 @@ TEST(Pose3Graph, partial_prior_height) {
// linearization
EXPECT_DOUBLES_EQUAL(2.0, height.error(values), tol);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
Values actual = *LevenbergMarquardtOptimizer(graph, values).optimize()->values();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}
@ -167,7 +166,7 @@ TEST(Pose3Graph, partial_prior_xy) {
Values values;
values.insert(key, init);
Values actual = *pose3SLAM::Optimizer::optimizeLM(graph, values);
Values actual = *LevenbergMarquardtOptimizer(graph, values).optimize()->values();
EXPECT(assert_equal(expected, actual.at<Pose3>(key), tol));
EXPECT_DOUBLES_EQUAL(0.0, graph.error(actual), tol);
}

37
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -21,7 +21,7 @@
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/visualSLAM.h>
@ -50,45 +50,34 @@ TEST( StereoFactor, singlePoint)
{
//Cal3_S2 K(625, 625, 0, 320, 240, 0.5);
boost::shared_ptr<Cal3_S2Stereo> K(new Cal3_S2Stereo(625, 625, 0, 320, 240, 0.5));
boost::shared_ptr<visualSLAM::Graph> graph(new visualSLAM::Graph());
NonlinearFactorGraph graph;
graph->add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
graph.add(visualSLAM::PoseConstraint(PoseKey(1),camera1));
StereoPoint2 z14(320,320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph->add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
graph.add(visualSLAM::StereoFactor(z14,sigma, PoseKey(1), PointKey(1), K));
// Create a configuration corresponding to the ground truth
boost::shared_ptr<Values> values(new Values());
values->insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Values values(new Values());
values.insert(PoseKey(1), camera1); // add camera at z=6.25m looking towards origin
Point3 l1(0, 0, 0);
values->insert(PointKey(1), l1); // add point at origin;
values.insert(PointKey(1), l1); // add point at origin;
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*values);
typedef gtsam::NonlinearOptimizer<visualSLAM::Graph, gtsam::GaussianFactorGraph, gtsam::GaussianMultifrontalSolver> Optimizer; // optimization engine for this domain
double absoluteThreshold = 1e-9;
double relativeThreshold = 1e-5;
int maxIterations = 100;
NonlinearOptimizationParameters::verbosityLevel verbose = NonlinearOptimizationParameters::SILENT;
NonlinearOptimizationParameters parameters(absoluteThreshold, relativeThreshold, 0,
maxIterations, 1.0, 10, verbose, NonlinearOptimizationParameters::BOUNDED);
Optimizer optimizer(graph, values, ordering, make_shared<NonlinearOptimizationParameters>(parameters));
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, values)));
// We expect the initial to be zero because config is the ground truth
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed
Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, afterOneIteration.error(), 1e-9);
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, afterOneIteration->error(), 1e-9);
// Complete solution
Optimizer final = optimizer.gaussNewton();
NonlinearOptimizer::auto_ptr final = optimizer->optimize();
DOUBLES_EQUAL(0.0, final.error(), 1e-6);
DOUBLES_EQUAL(0.0, final->error(), 1e-6);
}
/* ************************************************************************* */

34
gtsam/slam/tests/testVSLAM.cpp
View File

@ -23,7 +23,7 @@
using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
using namespace std;
@ -102,16 +102,16 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values())));
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values())));
}
@ -139,16 +139,16 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, GaussNewtonParams(), ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values())));
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values())));
}
@ -170,20 +170,18 @@ TEST( Graph, CHECK_ORDERING)
initialEstimate->insert(PointKey(3), landmark3);
initialEstimate->insert(PointKey(4), landmark4);
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initialEstimate);
// Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth
visualSLAM::Optimizer optimizer(graph, initialEstimate, ordering);
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started
// with the ground truth
visualSLAM::Optimizer afterOneIteration = optimizer.iterate();
DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9);
NonlinearOptimizer::auto_ptr afterOneIteration = optimizer->iterate();
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// check if correct
CHECK(assert_equal(*initialEstimate,*(afterOneIteration.values())));
CHECK(assert_equal(*initialEstimate,*(afterOneIteration->values())));
}
/* ************************************************************************* */

1
gtsam/slam/visualSLAM.cpp
View File

@ -16,7 +16,6 @@
*/
#include <gtsam/slam/visualSLAM.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
namespace gtsam {

3
gtsam/slam/visualSLAM.h
View File

@ -145,7 +145,4 @@ namespace visualSLAM {
}; // Graph
/// typedef for Optimizer. The current default will use the multi-frontal solver
typedef NonlinearOptimizer<Graph> Optimizer;
} // namespaces

37
tests/testBoundingConstraint.cpp
View File

@ -31,11 +31,6 @@ SharedDiagonal soft_model2 = noiseModel::Unit::Create(2);
SharedDiagonal soft_model2_alt = noiseModel::Isotropic::Sigma(2, 0.1);
SharedDiagonal hard_model1 = noiseModel::Constrained::All(1);
typedef NonlinearFactorGraph Graph;
typedef boost::shared_ptr<Graph> shared_graph;
typedef boost::shared_ptr<Values> shared_values;
typedef NonlinearOptimizer<Graph> Optimizer;
// some simple inequality constraints
Symbol key(simulated2D::PoseKey(1));
double mu = 10.0;
@ -150,19 +145,19 @@ TEST( testBoundingConstraint, unary_simple_optimization1) {
Point2 goal_pt(1.0, 2.0);
Point2 start_pt(0.0, 1.0);
shared_graph graph(new Graph());
NonlinearFactorGraph graph;
Symbol x1('x',1);
graph->add(iq2D::PoseXInequality(x1, 1.0, true));
graph->add(iq2D::PoseYInequality(x1, 2.0, true));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
graph.add(iq2D::PoseXInequality(x1, 1.0, true));
graph.add(iq2D::PoseYInequality(x1, 2.0, true));
graph.add(simulated2D::Prior(start_pt, soft_model2, x1));
shared_values initValues(new Values());
initValues->insert(x1, start_pt);
Values initValues;
initValues.insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
CHECK(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */
@ -172,19 +167,19 @@ TEST( testBoundingConstraint, unary_simple_optimization2) {
Point2 goal_pt(1.0, 2.0);
Point2 start_pt(2.0, 3.0);
shared_graph graph(new Graph());
NonlinearFactorGraph graph;
Symbol x1('x',1);
graph->add(iq2D::PoseXInequality(x1, 1.0, false));
graph->add(iq2D::PoseYInequality(x1, 2.0, false));
graph->add(simulated2D::Prior(start_pt, soft_model2, x1));
graph.add(iq2D::PoseXInequality(x1, 1.0, false));
graph.add(iq2D::PoseYInequality(x1, 2.0, false));
graph.add(simulated2D::Prior(start_pt, soft_model2, x1));
shared_values initValues(new Values());
initValues->insert(x1, start_pt);
Values initValues;
initValues.insert(x1, start_pt);
Optimizer::shared_values actual = Optimizer::optimizeLM(graph, initValues);
Values actual = *LevenbergMarquardtOptimizer(graph, initValues).optimize()->values();
Values expected;
expected.insert(x1, goal_pt);
CHECK(assert_equal(expected, *actual, tol));
CHECK(assert_equal(expected, actual, tol));
}
/* ************************************************************************* */