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

PoseRTV

release/4.3a0
Mike Bosse 2014-12-20 20:22:34 +01:00
parent 6e5dbcf2a3
commit b7204b100d
4 changed files with 43 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/base/Matrix.cpp
View File

@ -238,11 +238,6 @@ istream& operator>>(istream& inputStream, Matrix& destinationMatrix) {
return inputStream;
}
/* ************************************************************************* */
void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}
/* ************************************************************************* */
Matrix diag(const std::vector<Matrix>& Hs) {
size_t rows = 0, cols = 0;

5
gtsam/base/Matrix.h
View File

@ -238,7 +238,10 @@ Eigen::Block<const MATRIX> sub(const MATRIX& A, size_t i1, size_t i2, size_t j1,
* @param i is the row of the upper left corner insert location
* @param j is the column of the upper left corner insert location
*/
GTSAM_EXPORT void insertSub(Matrix& fullMatrix, const Matrix& subMatrix, size_t i, size_t j);
template <typename Derived1, typename Derived2>
GTSAM_EXPORT void insertSub(Eigen::MatrixBase<Derived1>& fullMatrix, const Eigen::MatrixBase<Derived2>& subMatrix, size_t i, size_t j) {
fullMatrix.block(i, j, subMatrix.rows(), subMatrix.cols()) = subMatrix;
}
/**
* Create a matrix with submatrices along its diagonal

36
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -71,7 +71,15 @@ Vector9 PoseRTV::Logmap(const PoseRTV& p) {
}
/* ************************************************************************* */
PoseRTV PoseRTV::retract(const Vector& v) const {
PoseRTV PoseRTV::retract(const Vector& v,
OptionalJacobian<dimension, dimension> Horigin,
OptionalJacobian<dimension, dimension> Hv) const {
if (Horigin) {
CONCEPT_NOT_IMPLEMENTED;
}
if (Hv) {
CONCEPT_NOT_IMPLEMENTED;
}
assert(v.size() == 9);
// First order approximation
Pose3 newPose = Rt_.retract(sub(v, 0, 6));
@ -80,7 +88,15 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
}
/* ************************************************************************* */
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
Vector PoseRTV::localCoordinates(const PoseRTV& p1,
OptionalJacobian<dimension, dimension> Horigin,
OptionalJacobian<dimension, dimension> Hp) const {
if (Horigin) {
CONCEPT_NOT_IMPLEMENTED;
}
if (Hp) {
CONCEPT_NOT_IMPLEMENTED;
}
const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation
Vector6 poseLogmap = x0.localCoordinates(x1);
@ -90,7 +106,7 @@ Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
/* ************************************************************************* */
PoseRTV inverse_(const PoseRTV& p) { return p.inverse(); }
PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
PoseRTV PoseRTV::inverse(OptionalJacobian<dimension,dimension> H1) const {
if (H1) *H1 = numericalDerivative11<PoseRTV,PoseRTV>(inverse_, *this, 1e-5);
return PoseRTV(Rt_.inverse(), v_.inverse());
}
@ -98,8 +114,8 @@ PoseRTV PoseRTV::inverse(boost::optional<Matrix&> H1) const {
/* ************************************************************************* */
PoseRTV compose_(const PoseRTV& p1, const PoseRTV& p2) { return p1.compose(p2); }
PoseRTV PoseRTV::compose(const PoseRTV& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
OptionalJacobian<dimension,dimension> H1,
OptionalJacobian<dimension,dimension> H2) const {
if (H1) *H1 = numericalDerivative21(compose_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(compose_, *this, p, 1e-5);
return PoseRTV(Rt_.compose(p.Rt_), v_.compose(p.v_));
@ -108,8 +124,8 @@ PoseRTV PoseRTV::compose(const PoseRTV& p,
/* ************************************************************************* */
PoseRTV between_(const PoseRTV& p1, const PoseRTV& p2) { return p1.between(p2); }
PoseRTV PoseRTV::between(const PoseRTV& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
OptionalJacobian<dimension,dimension> H1,
OptionalJacobian<dimension,dimension> H2) const {
if (H1) *H1 = numericalDerivative21(between_, *this, p, 1e-5);
if (H2) *H2 = numericalDerivative22(between_, *this, p, 1e-5);
return inverse().compose(p);
@ -227,7 +243,7 @@ Point3 PoseRTV::translationIntegration(const Rot3& r2, const Velocity3& v2, doub
/* ************************************************************************* */
double range_(const PoseRTV& A, const PoseRTV& B) { return A.range(B); }
double PoseRTV::range(const PoseRTV& other,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<1,dimension> H1, OptionalJacobian<1,dimension> H2) const {
if (H1) *H1 = numericalDerivative21(range_, *this, other, 1e-5);
if (H2) *H2 = numericalDerivative22(range_, *this, other, 1e-5);
return t().distance(other.t());
@ -239,8 +255,8 @@ PoseRTV transformed_from_(const PoseRTV& global, const Pose3& transform) {
}
PoseRTV PoseRTV::transformed_from(const Pose3& trans,
boost::optional<Matrix&> Dglobal,
boost::optional<Matrix&> Dtrans) const {
OptionalJacobian<dimension,dimension> Dglobal,
OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans) const {
// Note that we rotate the velocity
Matrix DVr, DTt;
Velocity3 newvel = trans.rotation().rotate(v_, DVr, DTt);

25
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -80,8 +80,8 @@ public:
* - v(3-5): Point3
* - v(6-8): Translational velocity
*/
PoseRTV retract(const Vector& v) const;
Vector localCoordinates(const PoseRTV& p) const;
PoseRTV retract(const Vector& v, OptionalJacobian<dimension, dimension> Horigin=boost::none, OptionalJacobian<dimension, dimension> Hv=boost::none) const;
Vector localCoordinates(const PoseRTV& p, OptionalJacobian<dimension, dimension> Horigin=boost::none,OptionalJacobian<dimension, dimension> Hp=boost::none) const;
// Lie
/**
@ -94,24 +94,25 @@ public:
static PoseRTV identity() { return PoseRTV(); }
/** Derivatives calculated numerically */
PoseRTV inverse(boost::optional<Matrix&> H1=boost::none) const;
PoseRTV inverse(OptionalJacobian<dimension,dimension> H1=boost::none) const;
/** Derivatives calculated numerically */
PoseRTV compose(const PoseRTV& p,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const;
PoseRTV operator*(const PoseRTV& p) { return compose(p); }
/** Derivatives calculated numerically */
PoseRTV between(const PoseRTV& p,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const;
// measurement functions
/** Derivatives calculated numerically */
double range(const PoseRTV& other,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<1,dimension> H1=boost::none,
OptionalJacobian<1,dimension> H2=boost::none) const;
// IMU-specific
@ -158,8 +159,8 @@ public:
* Note: the transform jacobian convention is flipped
*/
PoseRTV transformed_from(const Pose3& trans,
boost::optional<Matrix&> Dglobal=boost::none,
boost::optional<Matrix&> Dtrans=boost::none) const;
OptionalJacobian<dimension,dimension> Dglobal=boost::none,
OptionalJacobian<dimension,traits_x<Pose3>::dimension> Dtrans=boost::none) const;
// Utility functions
@ -187,6 +188,6 @@ private:
template<>
struct traits_x<PoseRTV> : public internal::LieGroup<PoseRTV> {};
struct traits_x<PoseRTV> : public internal::LieGroup<PoseRTV, multiplicative_group_tag> {};
} // \namespace gtsam