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

Fixed derivative of group action, as explained in LieGroups.pdf

release/4.3a0
dellaert 2016-02-07 22:26:10 -08:00
parent 6e1879a60d
commit fb3eda5eee
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
gtsam_unstable/geometry/Similarity3.cpp
View File

@ -76,10 +76,10 @@ Point3 Similarity3::transform_from(const Point3& p, //
OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const { OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
Point3 q = R_ * p + t_; Point3 q = R_ * p + t_;
if (H1) { if (H1) {
const Matrix3 R = R_.matrix(); // For this derivative, see LieGroups.pdf
Matrix3 DR = s_ * R * skewSymmetric(-p.x(), -p.y(), -p.z()); const Matrix3 sR = s_ * R_.matrix();
// TODO(frank): explain the derivative in lambda Matrix3 DR = sR * skewSymmetric(-p.x(), -p.y(), -p.z());
*H1 << DR, s_ * R, s_ * p.vector(); *H1 << DR, sR, sR * p.vector();
} }
if (H2) if (H2)
*H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix() *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()