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

Changed Matrix.h to correct return values amd impleemnted rectangular matrix types. 

Also changed block operations to <<
release/4.3a0
Natesh Srinivasan 2014-12-03 15:16:55 -05:00
parent deff8b1e25
commit aad0b2876b
18 changed files with 189 additions and 126 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
gtsam/base/Matrix.h
View File

@ -49,7 +49,7 @@ typedef Eigen::Matrix<double,4,1> Matrix41;
typedef Eigen::Matrix<double,5,1> Matrix51;
typedef Eigen::Matrix<double,6,1> Matrix61;
typedef Eigen::Matrix<double,1,1> Matrix1;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4;
@ -78,46 +78,129 @@ typedef Eigen::Block<const Matrix> ConstSubMatrix;
// Identity Matrices
static const int I_1x1 = 1;
static const Matrix2 I_2x2 = Matrix2::Identity();
static const Matrix3 I_3x3 = Matrix3::Identity();
static const Matrix4 I_4x4 = Matrix4::Identity();
static const Matrix5 I_5x5 = Matrix5::Identity();
static const Matrix6 I_6x6 = Matrix6::Identity();
// Negative Identity
static const int _I_1x1 = -1;
static const Matrix2 _I_2x2 = -I_2x2;
static const Matrix3 _I_3x3 = -I_3x3;
static const Matrix4 _I_4x4 = -I_4x4;
static const Matrix5 _I_5x5 = -I_5x5;
static const Matrix6 _I_6x6 = -I_6x6;
static const Eigen::MatrixBase<Matrix1>::IdentityReturnType I_1x1 = Matrix1::Identity();
static const Eigen::MatrixBase<Matrix2>::IdentityReturnType I_2x2 = Matrix2::Identity();
static const Eigen::MatrixBase<Matrix3>::IdentityReturnType I_3x3 = Matrix3::Identity();
static const Eigen::MatrixBase<Matrix4>::IdentityReturnType I_4x4 = Matrix4::Identity();
static const Eigen::MatrixBase<Matrix5>::IdentityReturnType I_5x5 = Matrix5::Identity();
static const Eigen::MatrixBase<Matrix6>::IdentityReturnType I_6x6 = Matrix6::Identity();
// Zero matrices
// TODO : Make these for rectangular sized matrices as well.
static const int Z_1x1 = 0;
static const Matrix2 Z_2x2 = Matrix2::Zero();
static const Matrix3 Z_3x3 = Matrix3::Zero();
static const Matrix4 Z_4x4 = Matrix4::Zero();
static const Matrix5 Z_5x5 = Matrix5::Zero();
static const Matrix6 Z_6x6 = Matrix6::Zero();
static const Eigen::DenseBase<Matrix1>::ConstantReturnType Z_1x1 = Matrix1::Zero();
static const Eigen::DenseBase<Matrix2>::ConstantReturnType Z_2x2 = Matrix2::Zero();
static const Eigen::DenseBase<Matrix3>::ConstantReturnType Z_3x3 = Matrix3::Zero();
static const Eigen::DenseBase<Matrix4>::ConstantReturnType Z_4x4 = Matrix4::Zero();
static const Eigen::DenseBase<Matrix5>::ConstantReturnType Z_5x5 = Matrix5::Zero();
static const Eigen::DenseBase<Matrix6>::ConstantReturnType Z_6x6 = Matrix6::Zero();
static const Eigen::DenseBase<Matrix12>::ConstantReturnType Z_1x2 = Matrix12::Zero();
static const Eigen::DenseBase<Matrix13>::ConstantReturnType Z_1x3 = Matrix13::Zero();
static const Eigen::DenseBase<Matrix14>::ConstantReturnType Z_1x4 = Matrix14::Zero();
static const Eigen::DenseBase<Matrix15>::ConstantReturnType Z_1x5 = Matrix15::Zero();
static const Eigen::DenseBase<Matrix16>::ConstantReturnType Z_1x6 = Matrix16::Zero();
static const Eigen::DenseBase<Matrix23>::ConstantReturnType Z_2x3 = Matrix23::Zero();
static const Eigen::DenseBase<Matrix24>::ConstantReturnType Z_2x4 = Matrix24::Zero();
static const Eigen::DenseBase<Matrix25>::ConstantReturnType Z_2x5 = Matrix25::Zero();
static const Eigen::DenseBase<Matrix26>::ConstantReturnType Z_2x6 = Matrix26::Zero();
static const Eigen::DenseBase<Matrix27>::ConstantReturnType Z_2x7 = Matrix27::Zero();
static const Eigen::DenseBase<Matrix28>::ConstantReturnType Z_2x8 = Matrix28::Zero();
static const Eigen::DenseBase<Matrix29>::ConstantReturnType Z_2x9 = Matrix29::Zero();
static const Eigen::DenseBase<Matrix32>::ConstantReturnType Z_3x2 = Matrix32::Zero();
static const Eigen::DenseBase<Matrix34>::ConstantReturnType Z_3x4 = Matrix34::Zero();
static const Eigen::DenseBase<Matrix35>::ConstantReturnType Z_3x5 = Matrix35::Zero();
static const Eigen::DenseBase<Matrix36>::ConstantReturnType Z_3x6 = Matrix36::Zero();
static const Eigen::DenseBase<Matrix37>::ConstantReturnType Z_3x7 = Matrix37::Zero();
static const Eigen::DenseBase<Matrix38>::ConstantReturnType Z_3x8 = Matrix38::Zero();
static const Eigen::DenseBase<Matrix39>::ConstantReturnType Z_3x9 = Matrix39::Zero();
static const Eigen::DenseBase<Matrix21>::ConstantReturnType Z_2x1 = Matrix21::Zero();
static const Eigen::DenseBase<Matrix31>::ConstantReturnType Z_3x1 = Matrix31::Zero();
static const Eigen::DenseBase<Matrix41>::ConstantReturnType Z_4x1 = Matrix41::Zero();
static const Eigen::DenseBase<Matrix51>::ConstantReturnType Z_5x1 = Matrix51::Zero();
static const Eigen::DenseBase<Matrix61>::ConstantReturnType Z_6x1 = Matrix61::Zero();
// Ones matrices
// TODO : Make these for rectangular sized matrices as well.
static const int O_1x1 = 1;
static const Matrix2 O_2x2 = Matrix2::Ones();
static const Matrix3 O_3x3 = Matrix3::Ones();
static const Matrix4 O_4x4 = Matrix4::Ones();
static const Matrix5 O_5x5 = Matrix5::Ones();
static const Matrix6 O_6x6 = Matrix6::Ones();
static const Eigen::DenseBase<Matrix1>::ConstantReturnType O_1x1 = Matrix1::Ones();
static const Eigen::DenseBase<Matrix2>::ConstantReturnType O_2x2 = Matrix2::Ones();
static const Eigen::DenseBase<Matrix3>::ConstantReturnType O_3x3 = Matrix3::Ones();
static const Eigen::DenseBase<Matrix4>::ConstantReturnType O_4x4 = Matrix4::Ones();
static const Eigen::DenseBase<Matrix5>::ConstantReturnType O_5x5 = Matrix5::Ones();
static const Eigen::DenseBase<Matrix6>::ConstantReturnType O_6x6 = Matrix6::Ones();
static const Eigen::DenseBase<Matrix12>::ConstantReturnType O_1x2 = Matrix12::Ones();
static const Eigen::DenseBase<Matrix13>::ConstantReturnType O_1x3 = Matrix13::Ones();
static const Eigen::DenseBase<Matrix14>::ConstantReturnType O_1x4 = Matrix14::Ones();
static const Eigen::DenseBase<Matrix15>::ConstantReturnType O_1x5 = Matrix15::Ones();
static const Eigen::DenseBase<Matrix16>::ConstantReturnType O_1x6 = Matrix16::Ones();
static const Eigen::DenseBase<Matrix23>::ConstantReturnType O_2x3 = Matrix23::Ones();
static const Eigen::DenseBase<Matrix24>::ConstantReturnType O_2x4 = Matrix24::Ones();
static const Eigen::DenseBase<Matrix25>::ConstantReturnType O_2x5 = Matrix25::Ones();
static const Eigen::DenseBase<Matrix26>::ConstantReturnType O_2x6 = Matrix26::Ones();
static const Eigen::DenseBase<Matrix27>::ConstantReturnType O_2x7 = Matrix27::Ones();
static const Eigen::DenseBase<Matrix28>::ConstantReturnType O_2x8 = Matrix28::Ones();
static const Eigen::DenseBase<Matrix29>::ConstantReturnType O_2x9 = Matrix29::Ones();
static const Eigen::DenseBase<Matrix32>::ConstantReturnType O_3x2 = Matrix32::Ones();
static const Eigen::DenseBase<Matrix34>::ConstantReturnType O_3x4 = Matrix34::Ones();
static const Eigen::DenseBase<Matrix35>::ConstantReturnType O_3x5 = Matrix35::Ones();
static const Eigen::DenseBase<Matrix36>::ConstantReturnType O_3x6 = Matrix36::Ones();
static const Eigen::DenseBase<Matrix37>::ConstantReturnType O_3x7 = Matrix37::Ones();
static const Eigen::DenseBase<Matrix38>::ConstantReturnType O_3x8 = Matrix38::Ones();
static const Eigen::DenseBase<Matrix39>::ConstantReturnType O_3x9 = Matrix39::Ones();
static const Eigen::DenseBase<Matrix21>::ConstantReturnType O_2x1 = Matrix21::Ones();
static const Eigen::DenseBase<Matrix31>::ConstantReturnType O_3x1 = Matrix31::Ones();
static const Eigen::DenseBase<Matrix41>::ConstantReturnType O_4x1 = Matrix41::Ones();
static const Eigen::DenseBase<Matrix51>::ConstantReturnType O_5x1 = Matrix51::Ones();
static const Eigen::DenseBase<Matrix61>::ConstantReturnType O_6x1 = Matrix61::Ones();
// Negative Ones
static const int _O_1x1 = -1;
static const Matrix2 _O_2x2 = -O_2x2;
static const Matrix3 _O_3x3 = -O_3x3;
static const Matrix4 _O_4x4 = -O_4x4;
static const Matrix5 _O_5x5 = -O_5x5;
static const Matrix6 _O_6x6 = -O_6x6;
static const Eigen::DenseBase<Matrix1>::ConstantReturnType _O_1x1 = Matrix1::Constant(-1);
static const Eigen::DenseBase<Matrix2>::ConstantReturnType _O_2x2 = Matrix2::Constant(-1);
static const Eigen::DenseBase<Matrix3>::ConstantReturnType _O_3x3 = Matrix3::Constant(-1);
static const Eigen::DenseBase<Matrix4>::ConstantReturnType _O_4x4 = Matrix4::Constant(-1);
static const Eigen::DenseBase<Matrix5>::ConstantReturnType _O_5x5 = Matrix5::Constant(-1);
static const Eigen::DenseBase<Matrix6>::ConstantReturnType _O_6x6 = Matrix6::Constant(-1);
static const Eigen::DenseBase<Matrix12>::ConstantReturnType _O_1x2 = Matrix12::Constant(-1);
static const Eigen::DenseBase<Matrix13>::ConstantReturnType _O_1x3 = Matrix13::Constant(-1);
static const Eigen::DenseBase<Matrix14>::ConstantReturnType _O_1x4 = Matrix14::Constant(-1);
static const Eigen::DenseBase<Matrix15>::ConstantReturnType _O_1x5 = Matrix15::Constant(-1);
static const Eigen::DenseBase<Matrix16>::ConstantReturnType _O_1x6 = Matrix16::Constant(-1);
static const Eigen::DenseBase<Matrix23>::ConstantReturnType _O_2x3 = Matrix23::Constant(-1);
static const Eigen::DenseBase<Matrix24>::ConstantReturnType _O_2x4 = Matrix24::Constant(-1);
static const Eigen::DenseBase<Matrix25>::ConstantReturnType _O_2x5 = Matrix25::Constant(-1);
static const Eigen::DenseBase<Matrix26>::ConstantReturnType _O_2x6 = Matrix26::Constant(-1);
static const Eigen::DenseBase<Matrix27>::ConstantReturnType _O_2x7 = Matrix27::Constant(-1);
static const Eigen::DenseBase<Matrix28>::ConstantReturnType _O_2x8 = Matrix28::Constant(-1);
static const Eigen::DenseBase<Matrix29>::ConstantReturnType _O_2x9 = Matrix29::Constant(-1);
static const Eigen::DenseBase<Matrix32>::ConstantReturnType _O_3x2 = Matrix32::Constant(-1);
static const Eigen::DenseBase<Matrix34>::ConstantReturnType _O_3x4 = Matrix34::Constant(-1);
static const Eigen::DenseBase<Matrix35>::ConstantReturnType _O_3x5 = Matrix35::Constant(-1);
static const Eigen::DenseBase<Matrix36>::ConstantReturnType _O_3x6 = Matrix36::Constant(-1);
static const Eigen::DenseBase<Matrix37>::ConstantReturnType _O_3x7 = Matrix37::Constant(-1);
static const Eigen::DenseBase<Matrix38>::ConstantReturnType _O_3x8 = Matrix38::Constant(-1);
static const Eigen::DenseBase<Matrix39>::ConstantReturnType _O_3x9 = Matrix39::Constant(-1);
static const Eigen::DenseBase<Matrix21>::ConstantReturnType _O_2x1 = Matrix21::Constant(-1);
static const Eigen::DenseBase<Matrix31>::ConstantReturnType _O_3x1 = Matrix31::Constant(-1);
static const Eigen::DenseBase<Matrix41>::ConstantReturnType _O_4x1 = Matrix41::Constant(-1);
static const Eigen::DenseBase<Matrix51>::ConstantReturnType _O_5x1 = Matrix51::Constant(-1);
static const Eigen::DenseBase<Matrix61>::ConstantReturnType _O_6x1 = Matrix61::Constant(-1);
// Matlab-like syntax

3
gtsam/geometry/Cal3Bundler.cpp
View File

@ -141,8 +141,7 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix2 Dp;
uncalibrate(p, Dcal, Dp);
Matrix25 H;
H.block<2,2>(0,0) = Dp;
H.block<2,3>(0,2) = Dcal;
H << Dp, Dcal;
return H;
}

6
gtsam/geometry/Cal3Unified.cpp
View File

@ -81,9 +81,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
Vector2 DU;
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
*H1 << H1base, H2base * DU;
}
// Inlined derivative for points
@ -95,7 +93,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU;
*H2 << H2base * DU;
}
return puncalib;

4
gtsam/geometry/Cal3_S2.h
View File

@ -173,8 +173,8 @@ public:
inline Cal3_S2 between(const Cal3_S2& q,
OptionalJacobian<5,5> H1=boost::none,
OptionalJacobian<5,5> H2=boost::none) const {
if(H1) *H1 = _I_5x5;
if(H2) *H2 = I_5x5;
if(H1) *H1 << -I_5x5;
if(H2) *H2 << I_5x5;
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}

21
gtsam/geometry/EssentialMatrix.cpp
View File

@ -27,10 +27,8 @@ EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
// First get 2*3 derivative from Unit3::FromPoint3
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
H->block<3, 3>(0, 0) << I_3x3; // upper left
H->block<2, 3>(3, 0).setZero(); // lower left
H->block<3, 3>(0, 3) << Z_3x3; // upper right
H->block<2, 3>(3, 3) = D_direction_1T2 * _1R2_.matrix(); // lower right
*H << I_3x3, Z_3x3, //
Z_2x3, D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
}
}
@ -69,9 +67,8 @@ Point3 EssentialMatrix::transform_to(const Point3& p,
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix35 H;
H.block<3,3>(0,0) = DE_.block<3, 3>(0, 0);
H.block<3,2>(0,3) = -aRb_.transpose() * aTb_.basis();
Matrix35 H;
H << DE_.block<3, 3>(0, 0), -aRb_.transpose() * aTb_.basis();
*DE = H;
}
return q;
@ -93,11 +90,8 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE) {
HE->setZero();
HE->block<3, 3>(0, 0) = cRb.matrix(); // a change in aRb_ will yield a rotated change in c1Rc2
HE->block<2, 2>(3, 3) = D_c1Tc2_aTb; // (2*2)
}
if (HE) *HE << cRb.matrix(), Z_3x2, //
Z_2x3, D_c1Tc2_aTb;
if (HR) {
throw runtime_error(
"EssentialMatrix::rotate: derivative HR not implemented yet");
@ -118,8 +112,7 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
H->block<1,3>(0,0) = HR;
H->block<1,2>(0,3) = HD;
*H << HR, HD;
}
return dot(vA, E_ * vB);
}

2
gtsam/geometry/Point2.h
View File

@ -139,7 +139,7 @@ public:
inline Point2 between(const Point2& q,
OptionalJacobian<2,2> H1=boost::none,
OptionalJacobian<2,2> H2=boost::none) const {
if(H1) *H1 = _I_2x2;
if(H1) *H1 = -I_2x2;
if(H2) *H2 = I_2x2;
return q - (*this);
}

8
gtsam/geometry/Point3.cpp
View File

@ -66,9 +66,9 @@ Point3 Point3::operator/(double s) const {
Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1= I_3x3;
*H1 << I_3x3;
if (H2)
*H2= I_3x3;
*H2 << I_3x3;
return *this + q;
}
@ -76,9 +76,9 @@ Point3 Point3::add(const Point3 &q, OptionalJacobian<3,3> H1,
Point3 Point3::sub(const Point3 &q, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
if (H1)
*H1 = I_3x3;
*H1 << I_3x3;
if (H2)
*H2 = I_3x3;
*H2 << I_3x3;
return *this - q;
}

10
gtsam/geometry/Point3.h
View File

@ -95,8 +95,8 @@ namespace gtsam {
inline Point3 compose(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if (H1) H1->setIdentity();
if (H2) H2->setIdentity();
if (H1) *H1 << I_3x3;
if (H2) *H2 << I_3x3;
return *this + p2;
}
@ -107,8 +107,8 @@ namespace gtsam {
inline Point3 between(const Point3& p2,
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const {
if(H1) *H1 = _I_3x3;
if(H2) H2->setIdentity();
if(H1) *H1 << -I_3x3;
if(H2) *H2 << I_3x3;
return p2 - *this;
}
@ -172,7 +172,7 @@ namespace gtsam {
if (H2) {
*H2 << -x_+p2.x(), -y_+p2.y(), -z_+p2.z();
*H2 = *H2 *(1./d);
*H2 << *H2 *(1./d);
}
return d;
}

32
gtsam/geometry/Pose2.cpp
View File

@ -125,7 +125,7 @@ Matrix3 Pose2::AdjointMap() const {
/* ************************************************************************* */
Pose2 Pose2::inverse(OptionalJacobian<3,3> H1) const {
if (H1) *H1 = -AdjointMap();
if (H1) *H1 << -AdjointMap();
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
}
@ -139,7 +139,7 @@ Point2 Pose2::transform_to(const Point2& point,
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
if (H2) *H2 = r_.transpose();
if (H2) *H2 << r_.transpose();
return q;
}
@ -148,8 +148,8 @@ Point2 Pose2::transform_to(const Point2& point,
Pose2 Pose2::compose(const Pose2& p2, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
// TODO: inline and reuse?
if(H1) *H1 = p2.inverse().AdjointMap();
if(H2) *H2 = I_3x3;
if(H1) *H1 << p2.inverse().AdjointMap();
if(H2) *H2 << I_3x3;
return (*this)*p2;
}
@ -162,11 +162,8 @@ Point2 Pose2::transform_from(const Point2& p,
const Matrix2 R = r_.matrix();
Matrix21 Drotate1;
Drotate1 << -q.y(), q.x();
if (H1) {
H1->block<2,2>(0,0) = R; // [R R_{pi/2}q]
H1->block<2,1>(0,2) = Drotate1;
}
if (H2) *H2 = R; // R
if (H1) *H1 << R, Drotate1;
if (H2) *H2 << R; // R
}
return q + t_;
}
@ -200,7 +197,7 @@ Pose2 Pose2::between(const Pose2& p2, OptionalJacobian<3,3> H1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I_3x3;
if (H2) *H2 << I_3x3;
return Pose2(R,t);
}
@ -214,8 +211,8 @@ Rot2 Pose2::bearing(const Point2& point,
if (!H1 && !H2) return Rot2::relativeBearing(d);
Matrix12 D_result_d;
Rot2 result = Rot2::relativeBearing(d, D_result_d);
if (H1) *H1 = D_result_d * (D1);
if (H2) *H2 = D_result_d * (D2);
if (H1) *H1 << D_result_d * (D1);
if (H2) *H2 << D_result_d * (D2);
return result;
}
@ -226,8 +223,7 @@ Rot2 Pose2::bearing(const Pose2& pose,
Rot2 result = bearing(pose.t(), H1, H2 ? &D2 : 0);
if (H2) {
Matrix12 H2_ = D2 * pose.r().matrix();
H2->setZero();
H2->block<1,2>(0,0) = H2_;
*H2 << H2_, Z_1x1;
}
return result;
}
@ -242,9 +238,9 @@ double Pose2::range(const Point2& point,
Matrix23 H1_;
H1_ << -r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
*H1 << H * H1_;
}
if (H2) *H2 = H;
if (H2) *H2 << H;
return r;
}
@ -261,14 +257,14 @@ double Pose2::range(const Pose2& pose,
H1_ <<
-r_.c(), r_.s(), 0.0,
-r_.s(), -r_.c(), 0.0;
*H1 = H * H1_;
*H1 << H * H1_;
}
if (H2) {
Matrix23 H2_;
H2_ <<
pose.r_.c(), -pose.r_.s(), 0.0,
pose.r_.s(), pose.r_.c(), 0.0;
*H2 = H * H2_;
*H2 << H * H2_;
}
return r;
}

24
gtsam/geometry/Pose3.cpp
View File

@ -274,17 +274,14 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
/* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
if (H1)
*H1 = p2.inverse().AdjointMap();
if (H2)
*H2 = I_6x6;
if (H1) *H1 << p2.inverse().AdjointMap();
if (H2) *H2 << I_6x6;
return (*this) * p2;
}
/* ************************************************************************* */
Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1)
*H1 = -AdjointMap();
if (H1) *H1 << -AdjointMap();
Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt * (-t_));
}
@ -294,10 +291,8 @@ Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
OptionalJacobian<6,6> H2) const {
Pose3 result = inverse() * p2;
if (H1)
*H1 = -result.inverse().AdjointMap();
if (H2)
*H2 = I_6x6;
if (H1) *H1 << -result.inverse().AdjointMap();
if (H2) *H2 << I_6x6;
return result;
}
@ -313,10 +308,8 @@ double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1,
d2);
Matrix13 D_result_d ;
D_result_d << x / n, y / n, z / n;
if (H1)
*H1 = D_result_d * (D1);
if (H2)
*H2 = D_result_d * (D2);
if (H1) *H1 << D_result_d * D1;
if (H2) *H2 << D_result_d * D2;
return n;
}
@ -327,8 +320,7 @@ double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) {
Matrix13 H2_ = D2 * pose.rotation().matrix();
H2->setZero();
H2->block<1,3>(0,3) = H2_;
*H2 << Z_1x3, H2_;
}
return r;
}

6
gtsam/geometry/Pose3.h
View File

@ -70,6 +70,12 @@ public:
R_(R), t_(t) {
}
/** Construct from R,t, where t \in vector3 */
Pose3(const Rot3& R, const Vector3& t) :
R_(R), t_(t) {
}
/** Construct from Pose2 */
explicit Pose3(const Pose2& pose2);

8
gtsam/geometry/Rot2.h
View File

@ -118,8 +118,8 @@ namespace gtsam {
/** Compose - make a new rotation by adding angles */
inline Rot2 compose(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << I_1x1; // set to 1x1 identity matrix
if (H2) *H2 << I_1x1; // set to 1x1 identity matrix
if (H1) *H1 << 1; // set to 1x1 identity matrix
if (H2) *H2 << 1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ - s_ * R.s_, s_ * R.c_ + c_ * R.s_);
}
@ -131,8 +131,8 @@ namespace gtsam {
/** Between using the default implementation */
inline Rot2 between(const Rot2& R, OptionalJacobian<1,1> H1 =
boost::none, OptionalJacobian<1,1> H2 = boost::none) const {
if (H1) *H1 << _I_1x1; // set to 1x1 identity matrix
if (H2) *H2 << I_1x1; // set to 1x1 identity matrix
if (H1) *H1 << -1; // set to 1x1 identity matrix
if (H2) *H2 << 1; // set to 1x1 identity matrix
return fromCosSin(c_ * R.c_ + s_ * R.s_, -s_ * R.c_ + c_ * R.s_);
}

16
gtsam/geometry/Rot3M.cpp
View File

@ -141,10 +141,8 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2) const {
if (H1)
*H1 = R2.transpose();
if (H2)
*H2 = I_3x3;
if (H1) *H1 << R2.transpose();
if (H2) *H2 << I_3x3;
return *this * R2;
}
@ -161,15 +159,15 @@ Matrix3 Rot3::transpose() const {
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
if (H1) *H1 = -rot_;
if (H1) *H1 << -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I_3x3;
if (H1) *H1 << -(R2.transpose()*rot_);
if (H2) *H2 << I_3x3;
Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
}
@ -178,8 +176,8 @@ Rot3 Rot3::between (const Rot3& R2,
Point3 Rot3::rotate(const Point3& p,
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
if (H1) *H1 << rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 << rot_;
}
return Point3(rot_ * p.vector());
}

14
gtsam/geometry/SimpleCamera.cpp
View File

@ -21,27 +21,27 @@
namespace gtsam {
SimpleCamera simpleCamera(const Matrix& P) {
SimpleCamera simpleCamera(const Matrix34& P) {
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
Matrix A = P.topLeftCorner(3, 3);
Vector a = P.col(3);
Matrix3 A = P.topLeftCorner(3, 3);
Vector3 a = P.col(3);
// do RQ decomposition to get s*K and cRw angles
Matrix sK;
Vector xyz;
Matrix3 sK;
Vector3 xyz;
boost::tie(sK, xyz) = RQ(A);
// Recover scale factor s and K
double s = sK(2, 2);
Matrix K = sK / s;
Matrix3 K = sK / s;
// Recover cRw itself, and its inverse
Rot3 cRw = Rot3::RzRyRx(xyz);
Rot3 wRc = cRw.inverse();
// Now, recover T from a = - s K cRw T = - A T
Vector T = -(A.inverse() * a);
Vector3 T = -(A.inverse() * a);
return SimpleCamera(Pose3(wRc, T),
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
}

2
gtsam/geometry/SimpleCamera.h
View File

@ -27,5 +27,5 @@ namespace gtsam {
typedef PinholeCamera<Cal3_S2> SimpleCamera;
/// Recover camera from 3*4 camera matrix
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix& P);
GTSAM_EXPORT SimpleCamera simpleCamera(const Matrix34& P);
}

3
gtsam/geometry/StereoCamera.cpp
View File

@ -30,8 +30,7 @@ namespace gtsam {
/* ************************************************************************* */
StereoPoint2 StereoCamera::project(const Point3& point,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2,
boost::optional<Matrix&> H3) const {
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
#ifdef STEREOCAMERA_CHAIN_RULE
const Point3 q = leftCamPose_.transform_to(point, H1, H2);

3
gtsam/geometry/StereoCamera.h
View File

@ -120,8 +120,7 @@ public:
*/
StereoPoint2 project(const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const;
boost::optional<Matrix&> H2 = boost::none) const;
/**
*

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -49,7 +49,7 @@ TEST( Rot3, chart)
/* ************************************************************************* */
TEST( Rot3, constructor)
{
Rot3 expected(I_3x3);
Rot3 expected((Matrix)I_3x3);
Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));