From 48d159120ddd95fece32938bc66a18e05638a7c2 Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Thu, 11 Jun 2015 11:40:40 -0400 Subject: [PATCH 01/10] 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 02/10] 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 03/10] 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 4c4c72adb4cf18bebabe75d920bfee0b63672e3b Mon Sep 17 00:00:00 2001 From: zhaoyang Date: Fri, 12 Jun 2015 13:56:47 -0400 Subject: [PATCH 04/10] 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 05/10] 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 06/10] 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 07/10] 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 08/10] 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 09/10] 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 10/10] 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);