From 0f95890215b3979e0d6b926104c40a5c8a9728e7 Mon Sep 17 00:00:00 2001 From: Natesh Srinivasan Date: Wed, 3 Dec 2014 16:34:58 -0500 Subject: [PATCH] done with geometry --- gtsam/base/Matrix.h | 40 ---------------------------- gtsam/geometry/StereoCamera.cpp | 23 +++++++--------- gtsam/geometry/StereoCamera.h | 10 +++---- gtsam/geometry/TriangulationFactor.h | 4 +-- gtsam/geometry/Unit3.h | 1 - gtsam/geometry/triangulation.h | 2 +- 6 files changed, 18 insertions(+), 62 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 960373a62..d1c343fcd 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -163,46 +163,6 @@ static const Eigen::DenseBase::ConstantReturnType O_4x1 = Matrix41::On static const Eigen::DenseBase::ConstantReturnType O_5x1 = Matrix51::Ones(); static const Eigen::DenseBase::ConstantReturnType O_6x1 = Matrix61::Ones(); - -// Negative Ones -static const Eigen::DenseBase::ConstantReturnType _O_1x1 = Matrix1::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x2 = Matrix2::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x3 = Matrix3::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_4x4 = Matrix4::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_5x5 = Matrix5::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_6x6 = Matrix6::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_1x2 = Matrix12::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x3 = Matrix13::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x4 = Matrix14::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x5 = Matrix15::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_1x6 = Matrix16::Constant(-1); - - -static const Eigen::DenseBase::ConstantReturnType _O_2x3 = Matrix23::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x4 = Matrix24::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x5 = Matrix25::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x6 = Matrix26::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x7 = Matrix27::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x8 = Matrix28::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_2x9 = Matrix29::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_3x2 = Matrix32::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x4 = Matrix34::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x5 = Matrix35::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x6 = Matrix36::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x7 = Matrix37::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x8 = Matrix38::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x9 = Matrix39::Constant(-1); - -static const Eigen::DenseBase::ConstantReturnType _O_2x1 = Matrix21::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_3x1 = Matrix31::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_4x1 = Matrix41::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_5x1 = Matrix51::Constant(-1); -static const Eigen::DenseBase::ConstantReturnType _O_6x1 = Matrix61::Constant(-1); - - - // Matlab-like syntax /** diff --git a/gtsam/geometry/StereoCamera.cpp b/gtsam/geometry/StereoCamera.cpp index f48c188aa..0e5197289 100644 --- a/gtsam/geometry/StereoCamera.cpp +++ b/gtsam/geometry/StereoCamera.cpp @@ -30,7 +30,7 @@ namespace gtsam { /* ************************************************************************* */ StereoPoint2 StereoCamera::project(const Point3& point, - boost::optional H1, boost::optional H2) const { + OptionalJacobian<3,6> H1, OptionalJacobian<3,3> H2) const { #ifdef STEREOCAMERA_CHAIN_RULE const Point3 q = leftCamPose_.transform_to(point, H1, H2); @@ -57,26 +57,23 @@ namespace gtsam { if (H1 || H2) { #ifdef STEREOCAMERA_CHAIN_RULE // just implement chain rule - Matrix D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian + Matrix3 D_project_point = Dproject_to_stereo_camera1(q); // 3x3 Jacobian if (H1) *H1 = D_project_point*(*H1); if (H2) *H2 = D_project_point*(*H2); #else // optimized version, see StereoCamera.nb if (H1) { const double v1 = v/fy, v2 = fx*v1, dx=d*x; - *H1 = (Matrix(3, 6) << - uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, + *H1 << uL*v1, -fx-dx*uL, v2, -dfx, 0.0, d*uL, uR*v1, -fx-dx*uR, v2, -dfx, 0.0, d*uR, - fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v - ).finished(); + fy + v*v1, -dx*v , -x*dfy, 0.0, -dfy, d*v; } if (H2) { - const Matrix R(leftCamPose_.rotation().matrix()); - *H2 = d * (Matrix(3, 3) << - fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, - fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, - fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v - ).finished(); + const Matrix3 R(leftCamPose_.rotation().matrix()); + *H2 << fx*R(0, 0) - R(0, 2)*uL, fx*R(1, 0) - R(1, 2)*uL, fx*R(2, 0) - R(2, 2)*uL, + fx*R(0, 0) - R(0, 2)*uR, fx*R(1, 0) - R(1, 2)*uR, fx*R(2, 0) - R(2, 2)*uR, + fy*R(0, 1) - R(0, 2)*v , fy*R(1, 1) - R(1, 2)*v , fy*R(2, 1) - R(2, 2)*v; + *H2 << d * (*H2); } #endif } @@ -86,7 +83,7 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { + Matrix3 StereoCamera::Dproject_to_stereo_camera1(const Point3& P) const { double d = 1.0 / P.z(), d2 = d*d; const Cal3_S2Stereo& K = *K_; double f_x = K.fx(), f_y = K.fy(), b = K.baseline(); diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h index dfefc4bec..4c19126e6 100644 --- a/gtsam/geometry/StereoCamera.h +++ b/gtsam/geometry/StereoCamera.h @@ -95,8 +95,8 @@ public: } Pose3 between(const StereoCamera &camera, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const { + OptionalJacobian<6,6> H1=boost::none, + OptionalJacobian<6,6> H2=boost::none) const { return leftCamPose_.between(camera.pose(), H1, H2); } @@ -119,8 +119,8 @@ public: * @param H3 IGNORED (for calibration) */ StereoPoint2 project(const Point3& point, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + OptionalJacobian<3, 6> H1 = boost::none, + OptionalJacobian<3, 3> H2 = boost::none) const; /** * @@ -138,7 +138,7 @@ public: private: /** utility functions */ - Matrix Dproject_to_stereo_camera1(const Point3& P) const; + Matrix3 Dproject_to_stereo_camera1(const Point3& P) const; friend class boost::serialization::access; template diff --git a/gtsam/geometry/TriangulationFactor.h b/gtsam/geometry/TriangulationFactor.h index fc8d546d3..9dcd5a5c3 100644 --- a/gtsam/geometry/TriangulationFactor.h +++ b/gtsam/geometry/TriangulationFactor.h @@ -112,14 +112,14 @@ public: } /// Evaluate error h(x)-z and optionally derivatives - Vector evaluateError(const Point3& point, boost::optional H2 = + Vector evaluateError(const Point3& point, OptionalJacobian<2,3> H2 = boost::none) const { try { Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_); return error.vector(); } catch (CheiralityException& e) { if (H2) - *H2 = zeros(2, 3); + *H2 << Z_2x3; if (verboseCheirality_) std::cout << e.what() << ": Landmark " << DefaultKeyFormatter(this->key()) << " moved behind camera" diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 5dfa7375e..f83404e74 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -147,7 +147,6 @@ private: /// @name Advanced Interface /// @{ - /** Serialization function */ friend class boost::serialization::access; template diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index fabcc4c02..864a60664 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -53,7 +53,7 @@ public: * @return Triangulated Point3 */ GTSAM_EXPORT Point3 triangulateDLT( - const std::vector& projection_matrices, + const std::vector& projection_matrices, // TODO: Use the fact that projection matrices sizes are known at compile time const std::vector& measurements, double rank_tol); ///