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

Merged from branch 'trunk'

release/4.3a0
Richard Roberts 2012-08-03 21:09:59 +00:00
parent 061b6ddc08 a9864dab23
commit 6f0c0fc7c5
3 changed files with 48 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.cproject
View File

@ -2192,22 +2192,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="wrap_gtsam_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_unstable_clean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>wrap_gtsam_unstable_clean</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap_gtsam_distclean" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

47
gtsam.h
View File

@ -1063,15 +1063,25 @@ class InvertedOrdering {
class NonlinearFactorGraph { class NonlinearFactorGraph {
NonlinearFactorGraph(); NonlinearFactorGraph();
void print(string s) const; NonlinearFactorGraph(const gtsam::NonlinearFactorGraph& graph);
size_t size() const;
double error(const gtsam::Values& c) const; // FactorGraph
double probPrime(const gtsam::Values& c) const; void print(string s) const;
gtsam::NonlinearFactor* at(size_t i) const; bool equals(const gtsam::NonlinearFactorGraph& fg, double tol) const;
size_t size() const;
bool empty() const;
void remove(size_t i);
size_t nrFactors() const;
void add(const gtsam::NonlinearFactor* factor); void add(const gtsam::NonlinearFactor* factor);
gtsam::Ordering* orderingCOLAMD(const gtsam::Values& c) const; gtsam::NonlinearFactor* at(size_t i) const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
double probPrime(const gtsam::Values& values) const;
gtsam::Ordering* orderingCOLAMD(const gtsam::Values& values) const;
// Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const; // Ordering* orderingCOLAMDConstrained(const gtsam::Values& c, const std::map<gtsam::Key,int>& constraints) const;
gtsam::GaussianFactorGraph* linearize(const gtsam::Values& c, const gtsam::Ordering& ordering) const; gtsam::GaussianFactorGraph* linearize(const gtsam::Values& values,
const gtsam::Ordering& ordering) const;
gtsam::NonlinearFactorGraph clone() const; gtsam::NonlinearFactorGraph clone() const;
}; };
@ -1382,6 +1392,29 @@ class ISAM2 {
gtsam::ISAM2Params params() const; gtsam::ISAM2Params params() const;
}; };
#include <gtsam/nonlinear/NonlinearISAM.h>
class NonlinearISAM {
NonlinearISAM();
NonlinearISAM(int reorderInterval);
void print(string s) const;
void printStats() const;
void saveGraph(string s) const;
gtsam::Values estimate() const;
Matrix marginalCovariance(size_t key) const;
int reorderInterval() const;
int reorderCounter() const;
void update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& initialValues);
void reorder_relinearize();
void addKey(size_t key);
void setOrdering(const gtsam::Ordering& new_ordering);
// These might be expensive as instead of a reference the wrapper will make a copy
gtsam::GaussianISAM bayesTree() const;
gtsam::Values getLinearizationPoint() const;
gtsam::Ordering getOrdering() const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
};
//************************************************************************* //*************************************************************************
// Nonlinear factor types // Nonlinear factor types
//************************************************************************* //*************************************************************************

10
gtsam_unstable/gtsam_unstable.h
View File

@ -172,15 +172,21 @@ virtual class DGroundConstraint : gtsam::NonlinearFactor {
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam_unstable/slam/PoseTranslationPrior.h> #include <gtsam_unstable/slam/PoseTranslationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}> template<POSE>
virtual class PoseTranslationPrior : gtsam::NonlinearFactor { virtual class PoseTranslationPrior : gtsam::NonlinearFactor {
PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); PoseTranslationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
}; };
typedef gtsam::PoseTranslationPrior<gtsam::Pose2> PoseTranslationPrior2D;
typedef gtsam::PoseTranslationPrior<gtsam::Pose3> PoseTranslationPrior3D;
#include <gtsam_unstable/slam/PoseRotationPrior.h> #include <gtsam_unstable/slam/PoseRotationPrior.h>
template<POSE = {gtsam::Pose2,gtsam::Pose3}> template<POSE>
virtual class PoseRotationPrior : gtsam::NonlinearFactor { virtual class PoseRotationPrior : gtsam::NonlinearFactor {
PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel); PoseRotationPrior(size_t key, const POSE& pose_z, const gtsam::noiseModel::Base* noiseModel);
}; };
typedef gtsam::PoseRotationPrior<gtsam::Pose2> PoseRotationPrior2D;
typedef gtsam::PoseRotationPrior<gtsam::Pose3> PoseRotationPrior3D;
} //\namespace gtsam } //\namespace gtsam