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

@dellaert. Works with quaternion switch on in gcc 4.7, 4.8 and MSVC 2012. Pls Merge if compiles on MAC

release/4.3a0
nsrinivasan7 2014-12-05 16:01:10 -05:00
parent f5db91a56f
commit a5877a96b8
1 changed files with 4 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Rot3Q.cpp
View File

@ -87,22 +87,9 @@ namespace gtsam {
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); } return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = R2.transpose(); if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3; if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_); return Rot3(quaternion_ * R2.quaternion_);
@ -114,7 +101,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const { Rot3 Rot3::inverse(boost::optional<Matrix3&> H1) const {
if (H1) *H1 = -matrix(); if (H1) *H1 = -matrix();
return Rot3(quaternion_.inverse()); return Rot3(quaternion_.inverse());
} }
@ -129,7 +116,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2, Rot3 Rot3::between(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*matrix()); if (H1) *H1 = -(R2.transpose()*matrix());
if (H2) *H2 = I3; if (H2) *H2 = I3;
return between_default(*this, R2); return between_default(*this, R2);
@ -137,7 +124,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p, Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
Matrix R = matrix(); Matrix R = matrix();
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = R; if (H2) *H2 = R;