diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h index 5741d71b3..f5a8b4469 100644 --- a/gtsam/geometry/CalibratedCamera.h +++ b/gtsam/geometry/CalibratedCamera.h @@ -18,9 +18,8 @@ #pragma once -#include -#include #include +#include namespace gtsam { @@ -88,26 +87,6 @@ public: return pose_; } - /// compose the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera compose(const CalibratedCamera &c, - OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 = - boost::none) const { - return CalibratedCamera(pose_.compose(c.pose(), H1, H2)); - } - - /// between the two camera poses: TODO Frank says this might not make sense - inline const CalibratedCamera between(const CalibratedCamera& c, - OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 = - boost::none) const { - return CalibratedCamera(pose_.between(c.pose(), H1, H2)); - } - - /// invert the camera pose: TODO Frank says this might not make sense - inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 = - boost::none) const { - return CalibratedCamera(pose_.inverse(H1)); - } - /** * Create a level camera at the given 2D pose and height * @param pose2 specifies the location and viewing direction @@ -152,7 +131,8 @@ public: * @return the intrinsic coordinates of the projected point */ Point2 project(const Point3& point, - OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const; + OptionalJacobian<2, 6> Dpose = boost::none, + OptionalJacobian<2, 3> Dpoint = boost::none) const; /** * projects a 3-dimensional point in camera coordinates into the @@ -174,8 +154,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none, - OptionalJacobian<1,3> H2 = boost::none) const { + double range(const Point3& point, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 3> H2 = boost::none) const { return pose_.range(point, H1, H2); } @@ -186,8 +166,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none, - OptionalJacobian<1,6> H2=boost::none) const { + double range(const Pose3& pose, OptionalJacobian<1, 6> H1 = boost::none, + OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(pose, H1, H2); } @@ -198,8 +178,8 @@ public: * @param H2 optionally computed Jacobian with respect to the 3D point * @return range (double) */ - double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 = - boost::none, OptionalJacobian<1,6> H2 = boost::none) const { + double range(const CalibratedCamera& camera, OptionalJacobian<1, 6> H1 = + boost::none, OptionalJacobian<1, 6> H2 = boost::none) const { return pose_.range(camera.pose_, H1, H2); } @@ -223,15 +203,16 @@ private: namespace traits { template<> -struct GTSAM_EXPORT is_group : public boost::true_type{ +struct GTSAM_EXPORT is_group : public boost::true_type { }; template<> -struct GTSAM_EXPORT is_manifold : public boost::true_type{ +struct GTSAM_EXPORT is_manifold : public boost::true_type { }; template<> -struct GTSAM_EXPORT dimension : public boost::integral_constant{ +struct GTSAM_EXPORT dimension : public boost::integral_constant< + int, 6> { }; } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 4c2422824..41176d0d8 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -18,16 +18,9 @@ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include #include +#include +#include namespace gtsam { @@ -42,8 +35,9 @@ private: Pose3 pose_; Calibration K_; - // Get dimension of calibration type at compile time - static const int DimK = traits::dimension::value; + // Get dimensions of calibration type and This at compile time + static const int DimK = traits::dimension::value, // + DimC = 6 + DimK; public: @@ -172,6 +166,18 @@ public: /// @name Manifold /// @{ + /// Manifold dimension + inline size_t dim() const { + return DimC; + } + + /// Manifold dimension + inline static size_t Dim() { + return DimC; + } + + typedef Eigen::Matrix VectorK6; + /// move a cameras according to d PinholeCamera retract(const Vector& d) const { if ((size_t) d.size() == 6) @@ -181,8 +187,6 @@ public: calibration().retract(d.tail(calibration().dim()))); } - typedef Eigen::Matrix VectorK6; - /// return canonical coordinate VectorK6 localCoordinates(const PinholeCamera& T2) const { VectorK6 d; // TODO: why does d.head<6>() not compile?? @@ -191,16 +195,6 @@ public: return d; } - /// Manifold dimension - inline size_t dim() const { - return Dim(); - } - - /// Manifold dimension - inline static size_t Dim() { - return Dim(); - } - /// @} /// @name Transformations and measurement functions /// @{ @@ -315,7 +309,7 @@ public: */ Point2 project2( const Point3& pw, // - OptionalJacobian<2, Dim()> Dcamera = boost::none, + OptionalJacobian<2, DimC> Dcamera = boost::none, OptionalJacobian<2, 3> Dpoint = boost::none) const { const Point3 pc = pose_.transform_to(pw); @@ -359,85 +353,73 @@ public: /** * Calculate range to a landmark * @param point 3D location of landmark - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpoint the optionally computed Jacobian with respect to the landmark * @return range (double) */ double range( const Point3& point, // - OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 3> Dpoint = boost::none) const { - double result = pose_.range(point, Dpose, Dpoint); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block<1, DimK>(0, 6).setZero(); - } + Matrix16 Dpose; + double result = pose_.range(point, Dcamera ? &Dpose : 0, Dpoint); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another pose * @param pose Other SO(3) pose - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dpose2 the optionally computed Jacobian with respect to the other pose * @return range (double) */ double range( const Pose3& pose, // - boost::optional Dpose = boost::none, - boost::optional Dpose2 = boost::none) const { - double result = pose_.range(pose, Dpose, Dpose2); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6> Dpose2 = boost::none) const { + Matrix16 Dpose; + double result = pose_.range(pose, Dcamera ? &Dpose : 0, Dpose2); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ template double range( const PinholeCamera& camera, // - boost::optional Dpose = boost::none, - boost::optional Dother = boost::none) const { - double result = pose_.range(camera.pose_, Dpose, Dother); - if (Dpose) { - // Add columns of zeros to Jacobian for calibration - Matrix& H1r(*Dpose); - H1r.conservativeResize(Eigen::NoChange, Dim()); - H1r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } - if (Dother) { - // Add columns of zeros to Jacobian for calibration - Matrix& H2r(*Dother); - H2r.conservativeResize(Eigen::NoChange, Dim()); - H2r.block(0, 6, 1, DimK) = Matrix::Zero(1, DimK); - } + OptionalJacobian<1, DimC> Dcamera = boost::none, + OptionalJacobian<1, 6 + CalibrationB::Dim()> Dother = boost::none) const { + Matrix16 Dpose, Dpose2; + double result = pose_.range(camera.pose(), Dcamera ? &Dpose : 0, + Dother ? &Dpose2 : 0); + if (Dcamera) + *Dcamera << Dpose, Eigen::Matrix::Zero(); + if (Dother) + *Dother << Dpose2, Eigen::Matrix::Zero(); return result; } /** * Calculate range to another camera * @param camera Other camera - * @param Dpose the optionally computed Jacobian with respect to pose + * @param Dcamera the optionally computed Jacobian with respect to pose * @param Dother the optionally computed Jacobian with respect to the other camera * @return range (double) */ double range( const CalibratedCamera& camera, // - OptionalJacobian<1, 6> Dpose = boost::none, + OptionalJacobian<1, DimC> Dcamera = boost::none, OptionalJacobian<1, 6> Dother = boost::none) const { - return pose_.range(camera.pose_, Dpose, Dother); + return range(camera.pose_, Dcamera, Dother); } private: