From c75a76c705f1f6cefbdee3d6f9e7765edcd619de Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Mon, 8 Jun 2015 20:30:55 -0400 Subject: [PATCH 01/16] Moved raw access method (possibly to be removed!) to base class as does not assume regular... --- gtsam/linear/JacobianFactor.cpp | 45 ++++++++++++++++++++++++++++++ gtsam/linear/JacobianFactor.h | 11 ++++++++ gtsam/slam/RegularJacobianFactor.h | 44 ----------------------------- 3 files changed, 56 insertions(+), 44 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 11025fc0f..597925eea 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -533,6 +533,51 @@ void JacobianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, transposeMultiplyAdd(alpha, Ax, y); } +/* ************************************************************************* */ +/** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Note: this is not assuming a fixed dimension for the variables, + * but requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO Frank asks: why is this here if not regular ???? + */ +void JacobianFactor::multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const { + + /// Use Eigen magic to access raw memory + typedef Eigen::Map VectorMap; + typedef Eigen::Map ConstVectorMap; + + if (empty()) + return; + Vector Ax = zero(Ab_.rows()); + + /// Just iterate over all A matrices and multiply in correct config part (looping over keys) + /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' + /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); + } + /// Deal with noise properly, need to Double* whiten as we are dividing by variance + if (model_) { + model_->whitenInPlace(Ax); + model_->whitenInPlace(Ax); + } + + /// multiply with alpha + Ax *= alpha; + + /// Again iterate over all A matrices and insert Ai^T into y + for (size_t pos = 0; pos < size(); ++pos) { + size_t offset = accumulatedDims[keys_[pos]]; + size_t dim = accumulatedDims[keys_[pos] + 1] - offset; + VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; + } +} + /* ************************************************************************* */ VectorValues JacobianFactor::gradientAtZero() const { VectorValues g; diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 194ba5c67..319d9faba 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -283,6 +283,17 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; + /** + * Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x + * Requires the vector accumulatedDims to tell the dimension of + * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, + * then accumulatedDims is [0 3 9 11 13] + * NOTE: size of accumulatedDims is size of keys + 1!! + * TODO(frank): we should probably kill this if no longer needed + */ + void multiplyHessianAdd(double alpha, const double* x, double* y, + const std::vector& accumulatedDims) const; + /// A'*b for Jacobian VectorValues gradientAtZero() const; diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 38e1407f0..1531258cb 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -182,50 +182,6 @@ public: return model_ ? model_->whiten(Ax) : Ax; } - /** Raw memory access version of multiplyHessianAdd y += alpha * A'*A*x - * Note: this is not assuming a fixed dimension for the variables, - * but requires the vector accumulatedDims to tell the dimension of - * each variable: e.g.: x0 has dim 3, x2 has dim 6, x3 has dim 2, - * then accumulatedDims is [0 3 9 11 13] - * NOTE: size of accumulatedDims is size of keys + 1!! - * TODO Frank asks: why is this here if not regular ???? - */ - void multiplyHessianAdd(double alpha, const double* x, double* y, - const std::vector& accumulatedDims) const { - - /// Use Eigen magic to access raw memory - typedef Eigen::Map VectorMap; - typedef Eigen::Map ConstVectorMap; - - if (empty()) - return; - Vector Ax = zero(Ab_.rows()); - - /// Just iterate over all A matrices and multiply in correct config part (looping over keys) - /// E.g.: Jacobian A = [A0 A1 A2] multiplies x = [x0 x1 x2]' - /// Hence: Ax = A0 x0 + A1 x1 + A2 x2 (hence we loop over the keys and accumulate) - for (size_t pos = 0; pos < size(); ++pos) { - size_t offset = accumulatedDims[keys_[pos]]; - size_t dim = accumulatedDims[keys_[pos] + 1] - offset; - Ax += Ab_(pos) * ConstVectorMap(x + offset, dim); - } - /// Deal with noise properly, need to Double* whiten as we are dividing by variance - if (model_) { - model_->whitenInPlace(Ax); - model_->whitenInPlace(Ax); - } - - /// multiply with alpha - Ax *= alpha; - - /// Again iterate over all A matrices and insert Ai^T into y - for (size_t pos = 0; pos < size(); ++pos) { - size_t offset = accumulatedDims[keys_[pos]]; - size_t dim = accumulatedDims[keys_[pos] + 1] - offset; - VectorMap(y + offset, dim) += Ab_(pos).transpose() * Ax; - } - } - }; // end class RegularJacobianFactor From 48d159120ddd95fece32938bc66a18e05638a7c2 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 11 Jun 2015 11:40:40 -0400 Subject: [PATCH 02/16] create a backproject2, with optional Jacobians --- gtsam/geometry/StereoCamera.cpp | 23 +++++++++++++ gtsam/geometry/StereoCamera.h | 8 +++++ gtsam/geometry/tests/testStereoCamera.cpp | 42 +++++++++++++++++++++-- 3 files changed, 70 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9e5b88b31..3d334b2dd 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -95,4 +95,27 @@ namespace gtsam { return world_point; } + /* ************************************************************************* */ + Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, + OptionalJacobian<3, 3> H2) { + const Cal3_S2Stereo& K = *K_; + const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline(); + + Vector measured = z.vector(); + double Z = b * fx / (measured[0] - measured[1]); + double X = Z * (measured[0] - cx) / fx; + double Y = Z * (measured[2] - cy) / fy; + + if(H1 || H2) { + if(H1) { + // do something here + } + if(H2) { + + } + } + + return leftCamPose_.transform_from(Point3(X, Y, Z)); + } + } diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index 35cf437e9..f09626c9d 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -137,6 +137,14 @@ public: /// back-project a measurement Point3 backproject(const StereoPoint2& z) const; + /** Back-project the 2D point and compute optional derivatives + * @param H1 derivative with respect to pose + * @param H2 derivative with respect to point + */ + Point3 backproject2(const StereoPoint2& z, + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; + /// @} /// @name Deprecated /// @{ diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 45f26c244..6d6972215 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -99,13 +99,13 @@ TEST( StereoCamera, Dproject) Matrix actual1; stereoCam.project2(landmark, actual1, boost::none); CHECK(assert_equal(expected1,actual1,1e-7)); - Matrix expected2 = numericalDerivative32(project3,camPose, landmark, *K); + Matrix expected2 = numericalDerivative32(project3, camPose, landmark, *K); Matrix actual2; stereoCam.project2(landmark, boost::none, actual2); CHECK(assert_equal(expected2,actual2,1e-7)); } /* ************************************************************************* */ -TEST( StereoCamera, backproject) +TEST( StereoCamera, backproject_case1) { Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); StereoCamera stereoCam2(Pose3(), K2); @@ -117,7 +117,7 @@ TEST( StereoCamera, backproject) } /* ************************************************************************* */ -TEST( StereoCamera, backproject2) +TEST( StereoCamera, backproject_case2) { Rot3 R(0.589511291, -0.804859792, 0.0683931805, -0.804435942, -0.592650676, -0.0405925523, @@ -132,6 +132,42 @@ TEST( StereoCamera, backproject2) CHECK(assert_equal(z, actual, 1e-3)); } +static Point3 backproject3(const Pose3& pose, const StereoPoint2& point, const Cal3_S2Stereo& K) { + return StereoCamera(pose, boost::make_shared(K)).backproject(point); +} + +/* ************************************************************************* */ +TEST( StereoCamera, backproject2_case1) +{ + Cal3_S2Stereo::shared_ptr K2(new Cal3_S2Stereo(1500, 1500, 0, 320, 240, 0.5)); + StereoCamera stereoCam2(Pose3(), K2); + + Point3 expected_point(1.2, 2.3, 4.5); + StereoPoint2 stereo_point = stereoCam2.project(expected_point); + + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); + CHECK(assert_equal(expected_point, actual_point, 1e-8)); + + Matrix expected_jacobian = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian, actual_jacobian_1, acutal_jacobian_2)); +} + +TEST( StereoCamera, backproject2_case2) +{ + Rot3 R(0.589511291, -0.804859792, 0.0683931805, + -0.804435942, -0.592650676, -0.0405925523, + 0.0732045588, -0.0310882277, -0.996832359); + Point3 t(53.5239823, 23.7866016, -4.42379876); + Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612)); + StereoCamera camera(Pose3(R,t), K); + StereoPoint2 z(184.812, 129.068, 714.768); + + Point3 l = camera.backproject2(z); + StereoPoint2 actual = camera.project(l); + CHECK(assert_equal(z, actual, 1e-3)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ From cb76d321d3030adba11113b2181e4695e85ec8de Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 11 Jun 2015 17:16:50 -0400 Subject: [PATCH 03/16] add back-projection derivative w.r.t. point, only the equation not finished yet. --- gtsam/geometry/StereoCamera.cpp | 20 +++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 3d334b2dd..1a7d7a6db 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -101,18 +101,32 @@ namespace gtsam { const Cal3_S2Stereo& K = *K_; const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline(); - Vector measured = z.vector(); + Vector3 measured = z.vector(); // u_L, u_R, v + double d = measured[0] - measured[1]; // disparity + double Z = b * fx / (measured[0] - measured[1]); double X = Z * (measured[0] - cx) / fx; double Y = Z * (measured[2] - cy) / fy; if(H1 || H2) { if(H1) { - // do something here + // do something here, w.r.t pose } if(H2) { - + double d_2 = d*d; + double z_partial_x = -fx*b/d_2, z_partial_y = fx*b/d_2; + *H2 << z_partial_x * X/Z + Z/fx, z_partial_y *X/Z, 0, + z_partial_x * Y/Z, z_partial_y *Y/Z, Z/fy, + z_partial_x, z_partial_y, 0; } + + Matrix point_H1, point_H2; + const Point3 point = leftCamPose_.transform_from(Point3(X,Y,Z), point_H1, point_H2); + + *H1 = point_H1 * (*H1); + *H2 = point_H2 * (*H2); + + return point; } return leftCamPose_.transform_from(Point3(X, Y, Z)); From a5d74f77d742deae18cb3ae08f3f6f4aa02cc9d7 Mon Sep 17 00:00:00 2001 From: lvzhaoyang Date: Fri, 12 Jun 2015 10:47:02 -0400 Subject: [PATCH 04/16] add test case. correct constness for backproject2 definition --- gtsam/geometry/StereoCamera.cpp | 6 +++--- gtsam/geometry/tests/testStereoCamera.cpp | 17 ++++++++++++++--- 2 files changed, 17 insertions(+), 6 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 1a7d7a6db..9f8d58d4a 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -97,9 +97,9 @@ namespace gtsam { /* ************************************************************************* */ Point3 StereoCamera::backproject2(const StereoPoint2& z, OptionalJacobian<3, 6> H1, - OptionalJacobian<3, 3> H2) { + OptionalJacobian<3, 3> H2) const { const Cal3_S2Stereo& K = *K_; - const double fx = K.fx(), fy = K.fy(), cx = K.cx(), cy = K.cy(), b = K.baseline(); + const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline(); Vector3 measured = z.vector(); // u_L, u_R, v double d = measured[0] - measured[1]; // disparity @@ -110,7 +110,7 @@ namespace gtsam { if(H1 || H2) { if(H1) { - // do something here, w.r.t pose + } if(H2) { double d_2 = d*d; diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 6d6972215..00329cb3c 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -149,8 +149,11 @@ TEST( StereoCamera, backproject2_case1) Point3 actual_point = stereoCam2.backproject2(stereo_point, actual_jacobian_1, actual_jacobian_2); CHECK(assert_equal(expected_point, actual_point, 1e-8)); - Matrix expected_jacobian = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); - CHECK(assert_equal(expected_jacobian, actual_jacobian_1, acutal_jacobian_2)); + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3)); } TEST( StereoCamera, backproject2_case2) @@ -163,9 +166,17 @@ TEST( StereoCamera, backproject2_case2) StereoCamera camera(Pose3(R,t), K); StereoPoint2 z(184.812, 129.068, 714.768); - Point3 l = camera.backproject2(z); + Matrix actual_jacobian_1, actual_jacobian_2; + Point3 l = camera.backproject2(z, actual_jacobian_1, actual_jacobian_2); + StereoPoint2 actual = camera.project(l); CHECK(assert_equal(z, actual, 1e-3)); + + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, camera, z, *K); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3)); + + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, camera, z, *K); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3)); } /* ************************************************************************* */ From fb34c099ec463c4ab2462fa780f6364586ed6df0 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 13:35:45 -0400 Subject: [PATCH 05/16] fix the RegularJacobianFactor overload issue, for mutiplyHessianAdd function. --- gtsam/slam/RegularJacobianFactor.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/slam/RegularJacobianFactor.h b/gtsam/slam/RegularJacobianFactor.h index 1531258cb..f954cba88 100644 --- a/gtsam/slam/RegularJacobianFactor.h +++ b/gtsam/slam/RegularJacobianFactor.h @@ -68,6 +68,8 @@ public: JacobianFactor(keys, augmentedMatrix, sigmas) { } + using JacobianFactor::multiplyHessianAdd; + /** y += alpha * A'*A*x */ virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const { From 4c4c72adb4cf18bebabe75d920bfee0b63672e3b Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 13:56:47 -0400 Subject: [PATCH 06/16] oops, this should be derivative against pose in test --- gtsam/geometry/tests/testStereoCamera.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 00329cb3c..1e0d2037e 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -172,10 +172,10 @@ TEST( StereoCamera, backproject2_case2) StereoPoint2 actual = camera.project(l); CHECK(assert_equal(z, actual, 1e-3)); - Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, camera, z, *K); + Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K); CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3)); - Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, camera, z, *K); + Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K); CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3)); } From 9ac223ec7dfa40e58ec739f7820ed3b213d83b42 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 15:08:58 -0400 Subject: [PATCH 07/16] correct the chain rule in Jacobian --- gtsam/geometry/StereoCamera.cpp | 26 ++++++++++++++------------ 1 file changed, 14 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 9f8d58d4a..31db86bc1 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -109,22 +109,24 @@ namespace gtsam { double Y = Z * (measured[2] - cy) / fy; if(H1 || H2) { - if(H1) { - } - if(H2) { - double d_2 = d*d; - double z_partial_x = -fx*b/d_2, z_partial_y = fx*b/d_2; - *H2 << z_partial_x * X/Z + Z/fx, z_partial_y *X/Z, 0, - z_partial_x * Y/Z, z_partial_y *Y/Z, Z/fy, - z_partial_x, z_partial_y, 0; - } + double d_2 = d*d; + double z_partial_x = -fx*b/d_2, z_partial_y = fx*b/d_2; + Matrix3 partial_to_point; + partial_to_point << z_partial_x * X/Z + Z/fx, z_partial_y *X/Z, 0, + z_partial_x * Y/Z, z_partial_y *Y/Z, Z/fy, + z_partial_x, z_partial_y, 0; - Matrix point_H1, point_H2; + Eigen::Matrix point_H1; + Eigen::Matrix point_H2; const Point3 point = leftCamPose_.transform_from(Point3(X,Y,Z), point_H1, point_H2); - *H1 = point_H1 * (*H1); - *H2 = point_H2 * (*H2); + if(H1) { + *H1 = point_H1; + } + if(H2) { + *H2 = point_H2 * partial_to_point; + } return point; } From 4c9b1de675c2ad8eae6ef6a91a8e3813ba0c4a49 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 15:09:28 -0400 Subject: [PATCH 08/16] Tests passed with real settings --- gtsam/geometry/tests/testStereoCamera.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/tests/testStereoCamera.cpp b/gtsam/geometry/tests/testStereoCamera.cpp index 1e0d2037e..3adf2257d 100644 --- a/gtsam/geometry/tests/testStereoCamera.cpp +++ b/gtsam/geometry/tests/testStereoCamera.cpp @@ -150,10 +150,10 @@ TEST( StereoCamera, backproject2_case1) CHECK(assert_equal(expected_point, actual_point, 1e-8)); Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(), stereo_point, *K2); - CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3)); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(), stereo_point, *K2); - CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3)); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); } TEST( StereoCamera, backproject2_case2) @@ -173,10 +173,10 @@ TEST( StereoCamera, backproject2_case2) CHECK(assert_equal(z, actual, 1e-3)); Matrix expected_jacobian_to_pose = numericalDerivative31(backproject3, Pose3(R,t), z, *K); - CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-3)); + CHECK(assert_equal(expected_jacobian_to_pose, actual_jacobian_1, 1e-6)); Matrix expected_jacobian_to_point = numericalDerivative32(backproject3, Pose3(R,t), z, *K); - CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-3)); + CHECK(assert_equal(expected_jacobian_to_point, actual_jacobian_2, 1e-6)); } /* ************************************************************************* */ From 2a2b885cdd260b80959b4dc8cf01935ba9a08693 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 16:07:36 -0400 Subject: [PATCH 09/16] change local variable naming... --- gtsam/geometry/StereoCamera.cpp | 34 ++++++++++++++++----------------- 1 file changed, 16 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 31db86bc1..7520cf723 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -101,37 +101,35 @@ namespace gtsam { const Cal3_S2Stereo& K = *K_; const double fx = K.fx(), fy = K.fy(), cx = K.px(), cy = K.py(), b = K.baseline(); - Vector3 measured = z.vector(); // u_L, u_R, v - double d = measured[0] - measured[1]; // disparity + double uL = z.uL(), uR = z.uR(), v = z.v(); + double disparity = uL - uR; - double Z = b * fx / (measured[0] - measured[1]); - double X = Z * (measured[0] - cx) / fx; - double Y = Z * (measured[2] - cy) / fy; + double local_z = b * fx / disparity; + const Point3 local_point(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); if(H1 || H2) { - double d_2 = d*d; + double d_2 = disparity*disparity; double z_partial_x = -fx*b/d_2, z_partial_y = fx*b/d_2; - Matrix3 partial_to_point; - partial_to_point << z_partial_x * X/Z + Z/fx, z_partial_y *X/Z, 0, - z_partial_x * Y/Z, z_partial_y *Y/Z, Z/fy, + double x_over_z = local_point.x() / local_point.z(), + y_over_z = local_point.y() / local_point.z(); + + Matrix3 D_local_z; + D_local_z << z_partial_x * x_over_z + local_point.z()/fx, z_partial_y * x_over_z, 0, + z_partial_x * y_over_z, z_partial_y * y_over_z, local_point.z()/fy, z_partial_x, z_partial_y, 0; - Eigen::Matrix point_H1; - Eigen::Matrix point_H2; - const Point3 point = leftCamPose_.transform_from(Point3(X,Y,Z), point_H1, point_H2); + Matrix3 D_point_local; + const Point3 world_point = leftCamPose_.transform_from(local_point, H1, D_point_local); - if(H1) { - *H1 = point_H1; - } if(H2) { - *H2 = point_H2 * partial_to_point; + *H2 = D_point_local * D_local_z; } - return point; + return world_point; } - return leftCamPose_.transform_from(Point3(X, Y, Z)); + return leftCamPose_.transform_from(local_point); } } From 8440e3c3b2fe366dd6759ddfd5cbc307416e8e2c Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 16:53:32 -0400 Subject: [PATCH 10/16] cool, a simplified D_local_z jacobian --- gtsam/geometry/StereoCamera.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 7520cf723..36efb61dd 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -108,16 +108,13 @@ namespace gtsam { const Point3 local_point(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); if(H1 || H2) { - - double d_2 = disparity*disparity; - double z_partial_x = -fx*b/d_2, z_partial_y = fx*b/d_2; - double x_over_z = local_point.x() / local_point.z(), - y_over_z = local_point.y() / local_point.z(); - + double z_partial_uR = local_z/disparity; + double x_partial_uR = local_point.x()/disparity; + double y_partial_uR = local_point.y()/disparity; Matrix3 D_local_z; - D_local_z << z_partial_x * x_over_z + local_point.z()/fx, z_partial_y * x_over_z, 0, - z_partial_x * y_over_z, z_partial_y * y_over_z, local_point.z()/fy, - z_partial_x, z_partial_y, 0; + D_local_z << -x_partial_uR + local_point.z()/fx, x_partial_uR, 0, + -y_partial_uR, y_partial_uR, local_point.z() / fy, + -z_partial_uR, z_partial_uR, 0; Matrix3 D_point_local; const Point3 world_point = leftCamPose_.transform_from(local_point, H1, D_point_local); From 3d18d70d6925fa6d12c4b4aa1d4232b322c0b33b Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 16:55:38 -0400 Subject: [PATCH 11/16] change naming for local_point & world_point --- gtsam/geometry/StereoCamera.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index 36efb61dd..b04143d8d 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -91,8 +91,8 @@ namespace gtsam { double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double X = Z * (measured[0] - K_->px()) / K_->fx(); double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 world_point = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world_point; + Point3 world = leftCamPose_.transform_from(Point3(X, Y, Z)); + return world; } /* ************************************************************************* */ @@ -105,28 +105,28 @@ namespace gtsam { double disparity = uL - uR; double local_z = b * fx / disparity; - const Point3 local_point(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); + const Point3 local(local_z * (uL - cx)/ fx, local_z * (v - cy) / fy, local_z); if(H1 || H2) { double z_partial_uR = local_z/disparity; - double x_partial_uR = local_point.x()/disparity; - double y_partial_uR = local_point.y()/disparity; + double x_partial_uR = local.x()/disparity; + double y_partial_uR = local.y()/disparity; Matrix3 D_local_z; - D_local_z << -x_partial_uR + local_point.z()/fx, x_partial_uR, 0, - -y_partial_uR, y_partial_uR, local_point.z() / fy, + D_local_z << -x_partial_uR + local.z()/fx, x_partial_uR, 0, + -y_partial_uR, y_partial_uR, local.z() / fy, -z_partial_uR, z_partial_uR, 0; Matrix3 D_point_local; - const Point3 world_point = leftCamPose_.transform_from(local_point, H1, D_point_local); + const Point3 world = leftCamPose_.transform_from(local, H1, D_point_local); if(H2) { *H2 = D_point_local * D_local_z; } - return world_point; + return world; } - return leftCamPose_.transform_from(local_point); + return leftCamPose_.transform_from(local); } } From abe6b9cec61839a74a6e875919c1e08c683dbb4d Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 17:27:36 -0400 Subject: [PATCH 12/16] change 'world' to 'point' --- gtsam/geometry/StereoCamera.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index b04143d8d..5c6646454 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -91,8 +91,8 @@ namespace gtsam { double Z = K_->baseline() * K_->fx() / (measured[0] - measured[1]); double X = Z * (measured[0] - K_->px()) / K_->fx(); double Y = Z * (measured[2] - K_->py()) / K_->fy(); - Point3 world = leftCamPose_.transform_from(Point3(X, Y, Z)); - return world; + Point3 point = leftCamPose_.transform_from(Point3(X, Y, Z)); + return point; } /* ************************************************************************* */ @@ -117,13 +117,13 @@ namespace gtsam { -z_partial_uR, z_partial_uR, 0; Matrix3 D_point_local; - const Point3 world = leftCamPose_.transform_from(local, H1, D_point_local); + const Point3 point = leftCamPose_.transform_from(local, H1, D_point_local); if(H2) { *H2 = D_point_local * D_local_z; } - return world; + return point; } return leftCamPose_.transform_from(local); From a18875b598eb454378cf05de3ed4df5278fd87c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 16:05:09 -0700 Subject: [PATCH 13/16] Changed headers to GTSAM-style (near to far) --- gtsam/base/FastDefaultAllocator.h | 5 ++--- gtsam/base/FastMap.h | 2 +- gtsam/base/timing.cpp | 22 +++++++++++++--------- gtsam/base/timing.h | 13 ++++++++----- 4 files changed, 24 insertions(+), 18 deletions(-) diff --git a/gtsam/base/FastDefaultAllocator.h b/gtsam/base/FastDefaultAllocator.h index 156a87f55..bf5cfc498 100644 --- a/gtsam/base/FastDefaultAllocator.h +++ b/gtsam/base/FastDefaultAllocator.h @@ -17,8 +17,7 @@ */ #pragma once - -#include +#include // Configuration from CMake #if !defined GTSAM_ALLOCATOR_BOOSTPOOL && !defined GTSAM_ALLOCATOR_TBB && !defined GTSAM_ALLOCATOR_STL # ifdef GTSAM_USE_TBB @@ -85,4 +84,4 @@ namespace gtsam }; } -} \ No newline at end of file +} diff --git a/gtsam/base/FastMap.h b/gtsam/base/FastMap.h index 7cd6e28b8..65d532191 100644 --- a/gtsam/base/FastMap.h +++ b/gtsam/base/FastMap.h @@ -19,9 +19,9 @@ #pragma once #include -#include #include #include +#include namespace gtsam { diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 36f1c2f5f..8df669227 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -16,18 +16,22 @@ * @date Oct 5, 2010 */ -#include -#include -#include -#include -#include -#include -#include -#include - #include #include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + namespace gtsam { namespace internal { diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 7a43ef884..49c43712a 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -17,12 +17,15 @@ */ #pragma once -#include -#include -#include -#include -#include #include +#include + +#include +#include +#include + +#include +#include // This file contains the GTSAM timing instrumentation library, a low-overhead method for // learning at a medium-fine level how much time various components of an algorithm take From 15966a65f253ddff73f135789e3662b73bc563b6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 16:05:18 -0700 Subject: [PATCH 14/16] Small reformat --- gtsam/base/SymmetricBlockMatrix.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/base/SymmetricBlockMatrix.cpp b/gtsam/base/SymmetricBlockMatrix.cpp index 546b6a7f1..0fbdfeefc 100644 --- a/gtsam/base/SymmetricBlockMatrix.cpp +++ b/gtsam/base/SymmetricBlockMatrix.cpp @@ -61,13 +61,13 @@ VerticalBlockMatrix SymmetricBlockMatrix::choleskyPartial( // Split conditional - // Create one big conditionals with many frontal variables. - gttic(Construct_conditional); - const size_t varDim = offset(nFrontals); - VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); - Ab.full() = matrix_.topRows(varDim); - Ab.full().triangularView().setZero(); - gttoc(Construct_conditional); + // Create one big conditionals with many frontal variables. + gttic(Construct_conditional); + const size_t varDim = offset(nFrontals); + VerticalBlockMatrix Ab = VerticalBlockMatrix::LikeActiveViewOf(*this, varDim); + Ab.full() = matrix_.topRows(varDim); + Ab.full().triangularView().setZero(); + gttoc(Construct_conditional); gttic(Remaining_factor); // Take lower-right block of Ab_ to get the remaining factor From 75e072396c5f3b8c2e97b4e0a803031c0645e53f Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 14 Jun 2015 16:05:39 -0700 Subject: [PATCH 15/16] Refactored and renamed some internals --- gtsam/base/timing.cpp | 39 +++++++++++++---------------- gtsam/base/timing.h | 58 +++++++++++++++++++++++++++---------------- 2 files changed, 53 insertions(+), 44 deletions(-) diff --git a/gtsam/base/timing.cpp b/gtsam/base/timing.cpp index 8df669227..b76746ba0 100644 --- a/gtsam/base/timing.cpp +++ b/gtsam/base/timing.cpp @@ -35,9 +35,9 @@ namespace gtsam { namespace internal { -GTSAM_EXPORT boost::shared_ptr timingRoot( +GTSAM_EXPORT boost::shared_ptr gTimingRoot( new TimingOutline("Total", getTicTocID("Total"))); -GTSAM_EXPORT boost::weak_ptr timingCurrent(timingRoot); +GTSAM_EXPORT boost::weak_ptr gCurrentTimer(gTimingRoot); /* ************************************************************************* */ // Implementation of TimingOutline @@ -54,8 +54,8 @@ void TimingOutline::add(size_t usecs, size_t usecsWall) { } /* ************************************************************************* */ -TimingOutline::TimingOutline(const std::string& label, size_t myId) : - myId_(myId), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( +TimingOutline::TimingOutline(const std::string& label, size_t id) : + id_(id), t_(0), tWall_(0), t2_(0.0), tIt_(0), tMax_(0), tMin_(0), n_(0), myOrder_( 0), lastChildOrder_(0), label_(label) { #ifdef GTSAM_USING_NEW_BOOST_TIMERS timer_.stop(); @@ -157,7 +157,7 @@ const boost::shared_ptr& TimingOutline::child(size_t child, } /* ************************************************************************* */ -void TimingOutline::ticInternal() { +void TimingOutline::tic() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(timer_.is_stopped()); timer_.start(); @@ -173,7 +173,7 @@ void TimingOutline::ticInternal() { } /* ************************************************************************* */ -void TimingOutline::tocInternal() { +void TimingOutline::toc() { #ifdef GTSAM_USING_NEW_BOOST_TIMERS assert(!timer_.is_stopped()); @@ -216,7 +216,6 @@ void TimingOutline::finishedIteration() { } /* ************************************************************************* */ -// Generate or retrieve a unique global ID number that will be used to look up tic_/toc statements size_t getTicTocID(const char *descriptionC) { const std::string description(descriptionC); // Global (static) map from strings to ID numbers and current next ID number @@ -236,37 +235,33 @@ size_t getTicTocID(const char *descriptionC) { } /* ************************************************************************* */ -void ticInternal(size_t id, const char *labelC) { +void tic(size_t id, const char *labelC) { const std::string label(labelC); - if (ISDEBUG("timing-verbose")) - std::cout << "gttic_(" << id << ", " << label << ")" << std::endl; boost::shared_ptr node = // - timingCurrent.lock()->child(id, label, timingCurrent); - timingCurrent = node; - node->ticInternal(); + gCurrentTimer.lock()->child(id, label, gCurrentTimer); + gCurrentTimer = node; + node->tic(); } /* ************************************************************************* */ -void tocInternal(size_t id, const char *label) { - if (ISDEBUG("timing-verbose")) - std::cout << "gttoc(" << id << ", " << label << ")" << std::endl; - boost::shared_ptr current(timingCurrent.lock()); - if (id != current->myId_) { - timingRoot->print(); +void toc(size_t id, const char *label) { + boost::shared_ptr current(gCurrentTimer.lock()); + if (id != current->id_) { + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: gttoc(\"%s\") called when last tic was \"%s\".") % label % current->label_).str()); } if (!current->parent_.lock()) { - timingRoot->print(); + gTimingRoot->print(); throw std::invalid_argument( (boost::format( "gtsam timing: Mismatched tic/toc: extra gttoc(\"%s\"), already at the root") % label).str()); } - current->tocInternal(); - timingCurrent = current->parent_; + current->toc(); + gCurrentTimer = current->parent_; } } // namespace internal diff --git a/gtsam/base/timing.h b/gtsam/base/timing.h index 49c43712a..a904c5f08 100644 --- a/gtsam/base/timing.h +++ b/gtsam/base/timing.h @@ -128,16 +128,21 @@ namespace gtsam { namespace internal { + // Generate/retrieve a unique global ID number that will be used to look up tic/toc statements GTSAM_EXPORT size_t getTicTocID(const char *description); - GTSAM_EXPORT void ticInternal(size_t id, const char *label); - GTSAM_EXPORT void tocInternal(size_t id, const char *label); + + // Create new TimingOutline child for gCurrentTimer, make it gCurrentTimer, and call tic method + GTSAM_EXPORT void tic(size_t id, const char *label); + + // Call toc on gCurrentTimer and then set gCurrentTimer to the parent of gCurrentTimer + GTSAM_EXPORT void toc(size_t id, const char *label); /** * Timing Entry, arranged in a tree */ class GTSAM_EXPORT TimingOutline { protected: - size_t myId_; + size_t id_; size_t t_; size_t tWall_; double t2_ ; ///< cache the \sum t_i^2 @@ -179,29 +184,38 @@ namespace gtsam { void print2(const std::string& outline = "", const double parentTotal = -1.0) const; const boost::shared_ptr& child(size_t child, const std::string& label, const boost::weak_ptr& thisPtr); - void ticInternal(); - void tocInternal(); + void tic(); + void toc(); void finishedIteration(); - GTSAM_EXPORT friend void tocInternal(size_t id, const char *label); + GTSAM_EXPORT friend void toc(size_t id, const char *label); }; // \TimingOutline /** - * No documentation + * Small class that calls internal::tic at construction, and internol::toc when destroyed */ class AutoTicToc { - private: + private: size_t id_; - const char *label_; + const char* label_; bool isSet_; - public: - AutoTicToc(size_t id, const char* label) : id_(id), label_(label), isSet_(true) { ticInternal(id_, label_); } - void stop() { tocInternal(id_, label_); isSet_ = false; } - ~AutoTicToc() { if(isSet_) stop(); } + + public: + AutoTicToc(size_t id, const char* label) + : id_(id), label_(label), isSet_(true) { + tic(id_, label_); + } + void stop() { + toc(id_, label_); + isSet_ = false; + } + ~AutoTicToc() { + if (isSet_) stop(); + } }; - GTSAM_EXTERN_EXPORT boost::shared_ptr timingRoot; - GTSAM_EXTERN_EXPORT boost::weak_ptr timingCurrent; + GTSAM_EXTERN_EXPORT boost::shared_ptr gTimingRoot; + GTSAM_EXTERN_EXPORT boost::weak_ptr gCurrentTimer; } // Tic and toc functions that are always active (whether or not ENABLE_TIMING is defined) @@ -213,7 +227,7 @@ namespace gtsam { // tic #define gttic_(label) \ static const size_t label##_id_tic = ::gtsam::internal::getTicTocID(#label); \ - ::gtsam::internal::AutoTicToc label##_obj = ::gtsam::internal::AutoTicToc(label##_id_tic, #label) + ::gtsam::internal::AutoTicToc label##_obj(label##_id_tic, #label) // toc #define gttoc_(label) \ @@ -231,26 +245,26 @@ namespace gtsam { // indicate iteration is finished inline void tictoc_finishedIteration_() { - ::gtsam::internal::timingRoot->finishedIteration(); } + ::gtsam::internal::gTimingRoot->finishedIteration(); } // print inline void tictoc_print_() { - ::gtsam::internal::timingRoot->print(); } + ::gtsam::internal::gTimingRoot->print(); } // print mean and standard deviation inline void tictoc_print2_() { - ::gtsam::internal::timingRoot->print2(); } + ::gtsam::internal::gTimingRoot->print2(); } // get a node by label and assign it to variable #define tictoc_getNode(variable, label) \ static const size_t label##_id_getnode = ::gtsam::internal::getTicTocID(#label); \ const boost::shared_ptr variable = \ - ::gtsam::internal::timingCurrent.lock()->child(label##_id_getnode, #label, ::gtsam::internal::timingCurrent); + ::gtsam::internal::gCurrentTimer.lock()->child(label##_id_getnode, #label, ::gtsam::internal::gCurrentTimer); // reset inline void tictoc_reset_() { - ::gtsam::internal::timingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); - ::gtsam::internal::timingCurrent = ::gtsam::internal::timingRoot; } + ::gtsam::internal::gTimingRoot.reset(new ::gtsam::internal::TimingOutline("Total", ::gtsam::internal::getTicTocID("Total"))); + ::gtsam::internal::gCurrentTimer = ::gtsam::internal::gTimingRoot; } #ifdef ENABLE_TIMING #define gttic(label) gttic_(label) From 33e412f2ee1fa3ccb55104b072c199e3590c267a Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 15 Jun 2015 01:05:48 -0700 Subject: [PATCH 16/16] Code review --- gtsam/linear/Scatter.cpp | 5 +++-- gtsam/linear/Scatter.h | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.h | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/Scatter.cpp b/gtsam/linear/Scatter.cpp index 2602e08ba..942b42160 100644 --- a/gtsam/linear/Scatter.cpp +++ b/gtsam/linear/Scatter.cpp @@ -10,9 +10,10 @@ * -------------------------------------------------------------------------- */ /** - * @file HessianFactor.cpp + * @file Scatter.cpp * @author Richard Roberts - * @date Dec 8, 2010 + * @author Frank Dellaert + * @date June 2015 */ #include diff --git a/gtsam/linear/Scatter.h b/gtsam/linear/Scatter.h index e1df2d658..7a37ba1e0 100644 --- a/gtsam/linear/Scatter.h +++ b/gtsam/linear/Scatter.h @@ -14,7 +14,7 @@ * @brief Maps global variable indices to slot indices * @author Richard Roberts * @author Frank Dellaert - * @date Dec 8, 2010 + * @date June 2015 */ #pragma once diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index 315e95512..e5561af48 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -46,7 +46,7 @@ public: public: - double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-4) + double lambdaInitial; ///< The initial Levenberg-Marquardt damping term (default: 1e-5) double lambdaFactor; ///< The amount by which to multiply or divide lambda when adjusting lambda (default: 10.0) double lambdaUpperBound; ///< The maximum lambda to try before assuming the optimization has failed (default: 1e5) double lambdaLowerBound; ///< The minimum lambda used in LM (default: 0)