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