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

wrapped Pose2.compose

release/4.3a0
Frank Dellaert 2011-11-04 04:27:55 +00:00
parent 4cab1e6722
commit db07ba5183
2 changed files with 40 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
gtsam.h
View File

@ -37,7 +37,8 @@ class Pose2 {
double x() const; double x() const;
double y() const; double y() const;
double theta() const; double theta() const;
size_t dim() const; int dim() const;
Pose2* compose_(const Pose2& p2);
Pose2* between_(const Pose2& p2); Pose2* between_(const Pose2& p2);
}; };
@ -52,19 +53,19 @@ class SharedDiagonal {
class VectorValues { class VectorValues {
VectorValues(); VectorValues();
VectorValues(size_t nVars, size_t varDim); VectorValues(int nVars, int varDim);
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; int size() const;
void insert(size_t j, const Vector& value); void insert(int j, const Vector& value);
}; };
class GaussianConditional { class GaussianConditional {
GaussianConditional(size_t key, Vector d, Matrix R, Vector sigmas); GaussianConditional(int key, Vector d, Matrix R, Vector sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
Vector sigmas); Vector sigmas);
GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, GaussianConditional(int key, Vector d, Matrix R, int name1, Matrix S,
size_t name2, Matrix T, Vector sigmas); int name2, Matrix T, Vector sigmas);
void print(string s) const; void print(string s) const;
bool equals(const GaussianConditional &cg, double tol) const; bool equals(const GaussianConditional &cg, double tol) const;
}; };
@ -86,11 +87,11 @@ class GaussianFactor {
class JacobianFactor { class JacobianFactor {
JacobianFactor(); JacobianFactor();
JacobianFactor(Vector b_in); JacobianFactor(Vector b_in);
JacobianFactor(size_t i1, Matrix A1, Vector b, const SharedDiagonal& model); JacobianFactor(int i1, Matrix A1, Vector b, const SharedDiagonal& model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, Vector b, JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, Vector b,
const SharedDiagonal& model); const SharedDiagonal& model);
JacobianFactor(size_t i1, Matrix A1, size_t i2, Matrix A2, size_t i3, JacobianFactor(int i1, Matrix A1, int i2, Matrix A2, int i3, Matrix A3,
Matrix A3, Vector b, const SharedDiagonal& model); Vector b, const SharedDiagonal& model);
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactor& lf, double tol) const; bool equals(const GaussianFactor& lf, double tol) const;
bool empty() const; bool empty() const;
@ -104,13 +105,13 @@ class GaussianFactorGraph {
void print(string s) const; void print(string s) const;
bool equals(const GaussianFactorGraph& lfgraph, double tol) const; bool equals(const GaussianFactorGraph& lfgraph, double tol) const;
size_t size() const; int size() const;
void push_back(GaussianFactor* ptr_f); void push_back(GaussianFactor* ptr_f);
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 denseJacobian() const;
Matrix denseHessian() const; Matrix denseHessian() const;
Matrix sparseJacobian_() const; Matrix sparseJacobian_() const;
}; };
@ -132,9 +133,11 @@ class Landmark2 {
double y(); double y();
}; };
class Ordering{ class Ordering {
void print(string s) const; Ordering();
bool equals(const Ordering& ord, double tol) const; void print(string s) const;
bool equals(const Ordering& ord, double tol) const;
void push_back(string key);
}; };
class PlanarSLAMValues { class PlanarSLAMValues {
@ -152,17 +155,22 @@ class PlanarSLAMGraph {
double error(const PlanarSLAMValues& c) const; double error(const PlanarSLAMValues& c) const;
Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const; Ordering* orderingCOLAMD(const PlanarSLAMValues& config) const;
GaussianFactorGraph* linearize(const PlanarSLAMValues& config, const Ordering& ordering) const; GaussianFactorGraph* linearize(const PlanarSLAMValues& config,
const Ordering& ordering) const;
void addPrior(size_t i, const Pose2& p, const SharedNoiseModel& model); void addPrior(int i, const Pose2& p, const SharedNoiseModel& model);
void addPoseConstraint(size_t i, const Pose2& p); void addPoseConstraint(int i, const Pose2& p);
void addOdometry(size_t i, size_t j, const Pose2& z, void addOdometry(int i, int j, const Pose2& z, const SharedNoiseModel& model);
const SharedNoiseModel& model); void addBearing(int i, int j, const Rot2& z, const SharedNoiseModel& model);
void addBearing(size_t i, size_t j, const Rot2& z, void addRange(int i, int j, double z, const SharedNoiseModel& model);
const SharedNoiseModel& model); void addBearingRange(int i, int j, const Rot2& z1, double z2,
void addRange(size_t i, size_t j, double z, const SharedNoiseModel& model);
void addBearingRange(size_t i, size_t j, const Rot2& z1, double z2,
const SharedNoiseModel& model); const SharedNoiseModel& model);
PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate); PlanarSLAMValues* optimize_(const PlanarSLAMValues& initialEstimate);
}; };
class PlanarSLAMOdometry {
PlanarSLAMOdometry(int key1, int key2, const Pose2& measured,
const SharedNoiseModel& model);
void print(string s) const;
GaussianFactor* linearize(const PlanarSLAMValues& c, const Ordering& ordering) const;
};

5
gtsam/geometry/Pose2.h
View File

@ -104,6 +104,11 @@ namespace gtsam {
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; boost::optional<Matrix&> H2 = boost::none) const;
/// MATLAB version returns shared pointer
boost::shared_ptr<Pose2> compose_(const Pose2& p2) {
return boost::shared_ptr<Pose2>(new Pose2(compose(p2)));
}
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Point2 operator*(const Point2& point) const { return transform_from(point);} inline Point2 operator*(const Point2& point) const { return transform_from(point);}