New example with smart PCG
dellaert
parent
9f51aad0fc
commit
0914e2c8d5
24
.cproject
24
.cproject
|
|
@ -2303,14 +2303,6 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMwSPCG_easy.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>Pose2SLAMwSPCG_easy.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="UGM_small.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
|
|
@ -2447,6 +2439,22 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="Pose2SLAMwSPCG.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>Pose2SLAMwSPCG.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SFMExample_SmartFactorPCG.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j4</buildArguments>
|
||||
<buildTarget>SFMExample_SmartFactorPCG.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>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,118 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 SFMExample_SmartFactorPCG.cpp
|
||||
* @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
// For an explanation of these headers, see SFMExample_SmartFactor.cpp
|
||||
#include "SFMdata.h"
|
||||
#include <gtsam/slam/SmartProjectionPoseFactor.h>
|
||||
|
||||
// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/linear/PCGSolver.h>
|
||||
#include <gtsam/linear/Preconditioner.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Make the typename short so it looks much cleaner
|
||||
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main(int argc, char* argv[]) {
|
||||
|
||||
// Define the camera calibration parameters
|
||||
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
|
||||
|
||||
// Define the camera observation noise model
|
||||
noiseModel::Isotropic::shared_ptr measurementNoise =
|
||||
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
|
||||
|
||||
// Create the set of ground-truth landmarks and poses
|
||||
vector<Point3> points = createPoints();
|
||||
vector<Pose3> poses = createPoses();
|
||||
|
||||
// Create a factor graph
|
||||
NonlinearFactorGraph graph;
|
||||
|
||||
// Simulated measurements from each camera pose, adding them to the factor graph
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
|
||||
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
|
||||
SmartFactor::shared_ptr smartfactor(new SmartFactor());
|
||||
|
||||
for (size_t i = 0; i < poses.size(); ++i) {
|
||||
|
||||
// generate the 2D measurement
|
||||
SimpleCamera camera(poses[i], *K);
|
||||
Point2 measurement = camera.project(points[j]);
|
||||
|
||||
// call add() function to add measurement into a single factor, here we need to add:
|
||||
smartfactor->add(measurement, i, measurementNoise, K);
|
||||
}
|
||||
|
||||
// insert the smart factor in the graph
|
||||
graph.push_back(smartfactor);
|
||||
}
|
||||
|
||||
// Add a prior on pose x0. This indirectly specifies where the origin is.
|
||||
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
|
||||
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
|
||||
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
|
||||
|
||||
// Fix the scale ambiguity by adding a prior
|
||||
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise));
|
||||
|
||||
// Create the initial estimate to the solution
|
||||
Values initialEstimate;
|
||||
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
|
||||
for (size_t i = 0; i < poses.size(); ++i)
|
||||
initialEstimate.insert(i, poses[i].compose(delta));
|
||||
|
||||
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
|
||||
// We indicate that an iterative linear solver should be used.
|
||||
// In addition, the *type* of the iterativeParams decides on the type of
|
||||
// iterative solver, in this case the SPCG (subgraph PCG)
|
||||
LevenbergMarquardtParams parameters;
|
||||
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
|
||||
PCGSolverParameters::shared_ptr pcg =
|
||||
boost::make_shared<PCGSolverParameters>();
|
||||
pcg->preconditioner_ =
|
||||
boost::make_shared<BlockJacobiPreconditionerParameters>();
|
||||
parameters.iterativeParams = pcg;
|
||||
|
||||
LevenbergMarquardtOptimizer Optimizer(graph, initialEstimate, parameters);
|
||||
Values result = Optimizer.optimize();
|
||||
|
||||
// Display result as in SFMExample_SmartFactor.run
|
||||
result.print("Final results:\n");
|
||||
Values landmark_result;
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
SmartFactor::shared_ptr smart = //
|
||||
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
|
||||
if (smart) {
|
||||
boost::optional<Point3> point = smart->point(result);
|
||||
if (point) // ignore if boost::optional return NULL
|
||||
landmark_result.insert(j, *point);
|
||||
}
|
||||
}
|
||||
|
||||
landmark_result.print("Landmark results:\n");
|
||||
|
||||
return 0;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
Loading…
Reference in New Issue