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

Fixed VectorValues, added linearize and denseJacobian/denseHessian

release/4.3a0
Frank Dellaert 2011-10-28 16:25:15 +00:00
parent e1c984f2b4
commit 980ed4d590
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -73,3 +73,9 @@ initialEstimate.print('initial estimate');
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
result = graph.optimize_(initialEstimate); result = graph.optimize_(initialEstimate);
result.print('final result'); result.print('final result');
%% Print out the corresponding dense matrix
ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord);
A_b = gfg.denseJacobian;
AtA_Atb = gfg.denseHessian;

11
gtsam.h
View File

@ -55,8 +55,7 @@ class VectorValues {
void print(string s) const; void print(string s) const;
bool equals(const VectorValues& expected, double tol) const; bool equals(const VectorValues& expected, double tol) const;
size_t size() const; size_t size() const;
void reserve(size_t nVars, size_t totalDims); void insert(size_t j, const Vector& value);
size_t push_back_preallocated(Vector vector);
}; };
class GaussianConditional { class GaussianConditional {
@ -109,6 +108,8 @@ class GaussianFactorGraph {
double error(const VectorValues& c) const; double error(const VectorValues& c) const;
double probPrime(const VectorValues& c) const; double probPrime(const VectorValues& c) const;
void combine(const GaussianFactorGraph& lfg); void combine(const GaussianFactorGraph& lfg);
Matrix denseJacobian() const;
Matrix denseHessian() const;
}; };
class Landmark2 { class Landmark2 {
@ -133,9 +134,13 @@ class PlanarSLAMValues {
class PlanarSLAMGraph { class PlanarSLAMGraph {
PlanarSLAMGraph(); PlanarSLAMGraph();
void print(string s) const;
double error(const PlanarSLAMValues& c) const; double error(const PlanarSLAMValues& c) const;
Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const; Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
void print(string s) const; GaussianFactorGraph* linearize(const PlanarSLAMValues& config, const Ordering& ordering) const;
void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model); void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model);
void addPoseConstraint(size_t i, const Pose2& p); void addPoseConstraint(size_t i, const Pose2& p);
void addOdometry(size_t i, size_t j, const Pose2& z, void addOdometry(size_t i, size_t j, const Pose2& z,