Fixed H2

2015-03-02 21:08:57 -08:00 · 2015-03-02 21:08:57 -08:00 · 728991e31f
parent 05198c091e
commit 728991e31f
1 changed files with 2 additions and 0 deletions
--- a/gtsam_unstable/geometry/Similarity3.cpp
+++ b/gtsam_unstable/geometry/Similarity3.cpp
@ -65,6 +65,8 @@ bool Similarity3::equals(const Similarity3& sim, double tol) const {

 Point3 Similarity3::transform_from(const Point3& p, //
    OptionalJacobian<3, 7> H1, OptionalJacobian<3, 3> H2) const {
+  if (H2)
+    *H2 = s_ * R_.matrix(); // just 3*3 sub-block of matrix()
  return R_ * (s_ * p) + t_;
 }