12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc. Small edit to not compute Dcal if not asked.

release/4.3a0
Frank Dellaert 2013-10-12 05:17:01 +00:00
parent 71466e8476
commit fa140cb0a3
1 changed files with 473 additions and 433 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

302
gtsam/geometry/PinholeCamera.h
View File

@ -48,13 +48,18 @@ namespace gtsam {
/// @{ /// @{
/** default constructor */ /** default constructor */
PinholeCamera() {} PinholeCamera() {
}
/** constructor with pose */ /** constructor with pose */
explicit PinholeCamera(const Pose3& pose):pose_(pose){} explicit PinholeCamera(const Pose3& pose) :
pose_(pose) {
}
/** constructor with pose and calibration */ /** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& K):pose_(pose),K_(K) {} PinholeCamera(const Pose3& pose, const Calibration& K) :
pose_(pose), K_(K) {
}
/// @} /// @}
/// @name Named Constructors /// @name Named Constructors
@ -67,7 +72,8 @@ namespace gtsam {
* (theta 0 = looking in direction of positive X axis) * (theta 0 = looking in direction of positive X axis)
* @param height camera height * @param height camera height
*/ */
static PinholeCamera Level(const Calibration &K, const Pose2& pose2, double height) { static PinholeCamera Level(const Calibration &K, const Pose2& pose2,
double height) {
const double st = sin(pose2.theta()), ct = cos(pose2.theta()); const double st = sin(pose2.theta()), ct = cos(pose2.theta());
const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0); const Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
const Rot3 wRc(x, y, z); const Rot3 wRc(x, y, z);
@ -90,7 +96,8 @@ namespace gtsam {
* doesn't need to be on the image plane nor orthogonal to the viewing axis * doesn't need to be on the image plane nor orthogonal to the viewing axis
* @param K optional calibration parameter * @param K optional calibration parameter
*/ */
static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) { static PinholeCamera Lookat(const Point3& eye, const Point3& target,
const Point3& upVector, const Calibration& K = Calibration()) {
Point3 zc = target - eye; Point3 zc = target - eye;
zc = zc / zc.norm(); zc = zc / zc.norm();
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
@ -121,8 +128,8 @@ namespace gtsam {
/// assert equality up to a tolerance /// assert equality up to a tolerance
bool equals(const PinholeCamera &camera, double tol = 1e-9) const { bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) && return pose_.equals(camera.pose(), tol)
K_.equals(camera.calibration(), tol) ; && K_.equals(camera.calibration(), tol);
} }
/// print /// print
@ -135,19 +142,28 @@ namespace gtsam {
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
virtual ~PinholeCamera() {} virtual ~PinholeCamera() {
}
/// return pose /// return pose
inline Pose3& pose() { return pose_; } inline Pose3& pose() {
return pose_;
}
/// return pose /// return pose
inline const Pose3& pose() const { return pose_; } inline const Pose3& pose() const {
return pose_;
}
/// return calibration /// return calibration
inline Calibration& calibration() { return K_; } inline Calibration& calibration() {
return K_;
}
/// return calibration /// return calibration
inline const Calibration& calibration() const { return K_; } inline const Calibration& calibration() const {
return K_;
}
/// @} /// @}
/// @name Group ?? Frank says this might not make sense /// @name Group ?? Frank says this might not make sense
@ -155,17 +171,19 @@ namespace gtsam {
/// compose two cameras: TODO Frank says this might not make sense /// compose two cameras: TODO Frank says this might not make sense
inline const PinholeCamera compose(const PinholeCamera &c, inline const PinholeCamera compose(const PinholeCamera &c,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2=boost::none) const { boost::none) const {
PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_); PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_);
if (H1) { if (H1) {
H1->conservativeResize(Dim(), Dim()); H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
} }
if (H2) { if (H2) {
H2->conservativeResize(Dim(), Dim()); H2->conservativeResize(Dim(), Dim());
H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
} }
return result; return result;
@ -173,28 +191,32 @@ namespace gtsam {
/// between two cameras: TODO Frank says this might not make sense /// between two cameras: TODO Frank says this might not make sense
inline const PinholeCamera between(const PinholeCamera& c, inline const PinholeCamera between(const PinholeCamera& c,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::optional<Matrix&> H2=boost::none) const { boost::none) const {
PinholeCamera result(pose_.between(c.pose(), H1, H2), K_); PinholeCamera result(pose_.between(c.pose(), H1, H2), K_);
if (H1) { if (H1) {
H1->conservativeResize(Dim(), Dim()); H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
} }
if (H2) { if (H2) {
H2->conservativeResize(Dim(), Dim()); H2->conservativeResize(Dim(), Dim());
H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); H2->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); H2->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
} }
return result; return result;
} }
/// inverse camera: TODO Frank says this might not make sense /// inverse camera: TODO Frank says this might not make sense
inline const PinholeCamera inverse(boost::optional<Matrix&> H1=boost::none) const { inline const PinholeCamera inverse(
boost::optional<Matrix&> H1 = boost::none) const {
PinholeCamera result(pose_.inverse(H1), K_); PinholeCamera result(pose_.inverse(H1), K_);
if (H1) { if (H1) {
H1->conservativeResize(Dim(), Dim()); H1->conservativeResize(Dim(), Dim());
H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),Calibration::Dim()); H1->topRightCorner(Pose3::Dim(), Calibration::Dim()) = zeros(Pose3::Dim(),
Calibration::Dim());
H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim()); H1->bottomRows(Calibration::Dim()) = zeros(Calibration::Dim(), Dim());
} }
return result; return result;
@ -222,15 +244,20 @@ namespace gtsam {
Vector localCoordinates(const PinholeCamera& T2) const { Vector localCoordinates(const PinholeCamera& T2) const {
Vector d(dim()); Vector d(dim());
d.head(pose().dim()) = pose().localCoordinates(T2.pose()); d.head(pose().dim()) = pose().localCoordinates(T2.pose());
d.tail(calibration().dim()) = calibration().localCoordinates(T2.calibration()); d.tail(calibration().dim()) = calibration().localCoordinates(
T2.calibration());
return d; return d;
} }
/// Manifold dimension /// Manifold dimension
inline size_t dim() const { return pose_.dim() + K_.dim(); } inline size_t dim() const {
return pose_.dim() + K_.dim();
}
/// Manifold dimension /// Manifold dimension
inline static size_t Dim() { return Pose3::Dim() + Calibration::Dim(); } inline static size_t Dim() {
return Pose3::Dim() + Calibration::Dim();
}
/// @} /// @}
/// @name Transformations and measurement functions /// @name Transformations and measurement functions
@ -239,13 +266,14 @@ namespace gtsam {
/** /**
* projects a 3-dimensional point in camera coordinates into the * projects a 3-dimensional point in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied * camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative * @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/ */
inline static Point2 project_to_camera(const Point3& P, inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> H1 = boost::none){ boost::optional<Matrix&> Dpoint = boost::none) {
if (H1) { if (Dpoint) {
double d = 1.0 / P.z(), d2 = d * d; double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); *Dpoint = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
} }
return Point2(P.x() / P.z(), P.y() / P.z()); return Point2(P.x() / P.z(), P.y() / P.z());
} }
@ -253,13 +281,15 @@ namespace gtsam {
/** /**
* projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the * projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the
* camera and returns a 2-dimensional point, no calibration applied * camera and returns a 2-dimensional point, no calibration applied
* With optional 2by3 derivative * TODO: Frank says: this function seems to be identical as the above
* @param P A point in camera coordinates
* @param Dpoint is the 2*3 Jacobian w.r.t. P
*/ */
inline static Point2 projectPointAtInfinityToCamera(const Point3& P, inline static Point2 projectPointAtInfinityToCamera(const Point3& P,
boost::optional<Matrix&> H1 = boost::none){ boost::optional<Matrix&> Dpoint = boost::none) {
if (H1) { if (Dpoint) {
double d = 1.0 / P.z(), d2 = d * d; double d = 1.0 / P.z(), d2 = d * d;
*H1 = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); *Dpoint = Matrix_(2, 3, d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2);
} }
return Point2(P.x() / P.z(), P.y() / P.z()); return Point2(P.x() / P.z(), P.y() / P.z());
} }
@ -272,113 +302,117 @@ namespace gtsam {
} }
/** project a point from world coordinate to the image /** project a point from world coordinate to the image
* @param pw is a point in the world coordinate * @param pw is a point in world coordinates
* @param H1 is the jacobian w.r.t. pose3 * @param Dpose is the Jacobian w.r.t. pose3
* @param H2 is the jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* @param H3 is the jacobian w.r.t. calibration * @param Dcal is the Jacobian w.r.t. calibration
*/ */
inline Point2 project(const Point3& pw, inline Point2 project(
boost::optional<Matrix&> H1 = boost::none, const Point3& pw, //
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
if (!H1 && !H2 && !H3) { if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
if ( pc.z() <= 0 ) throw CheiralityException(); if (pc.z() <= 0)
throw CheiralityException();
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn); return K_.uncalibrate(pn);
} }
// world to camera coordinate // world to camera coordinate
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ; Matrix Dpc_pose /* 3*6 */, Dpc_point /* 3*3 */;
const Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ; const Point3 pc = pose_.transform_to(pw, Dpc_pose, Dpc_point);
if( pc.z() <= 0 ) throw CheiralityException(); if (pc.z() <= 0)
throw CheiralityException();
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix Hn; // 2*3 Matrix Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Hn) ; const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration // uncalibration
Matrix Hk, Hi; // 2*2 Matrix Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Hk, Hi); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the jacobian matrices // chain the Jacobian matrices
const Matrix tmp = Hi*Hn ; const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
if (H1) *H1 = tmp * Hc1 ; if (Dpose)
if (H2) *H2 = tmp * Hc2 ; *Dpose = Dpi_pc * Dpc_pose;
if (H3) *H3 = Hk; if (Dpoint)
*Dpoint = Dpi_pc * Dpc_point;
return pi; return pi;
} }
/** project a point at infinity from world coordinate to the image /** project a point at infinity from world coordinate to the image
* @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf) * @param pw is a point in the world coordinate (it is pw = lambda*[pw_x pw_y pw_z] with lambda->inf)
* @param H1 is the jacobian w.r.t. pose3 * @param Dpose is the Jacobian w.r.t. pose3
* @param H2 is the jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
* @param H3 is the jacobian w.r.t. calibration * @param Dcal is the Jacobian w.r.t. calibration
*/ */
inline Point2 projectPointAtInfinity(const Point3& pw, inline Point2 projectPointAtInfinity(
boost::optional<Matrix&> H1 = boost::none, const Point3& pw, //
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
if (!H1 && !H2 && !H3) { if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); const Point3 pc = pose_.rotation().unrotate(pw);
// if ( pc.z() <= 0 ) throw CheiralityException(); // this does not make sense for point at infinity
const Point2 pn = projectPointAtInfinityToCamera(pc); const Point2 pn = projectPointAtInfinityToCamera(pc);
return K_.uncalibrate(pn); return K_.uncalibrate(pn);
} }
// world to camera coordinate // world to camera coordinate
Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ; Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ; const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
// if( pc.z() <= 0 ) throw CheiralityException(); // this does not make sense for point at infinity
Matrix Hc1 = Matrix::Zero(3,6); Matrix Dpc_pose = Matrix::Zero(3, 6);
Hc1.block(0,0,3,3) = Hc1_rot; Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix Hn; // 2*3 Matrix Dpn_pc; // 2*3
const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ; const Point2 pn = projectPointAtInfinityToCamera(pc, Dpn_pc);
// uncalibration // uncalibration
Matrix Hk, Hi; // 2*2 Matrix Dpi_pn; // 2*2
// Matrix H23 = Matrix::Zero(3,2); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// H23.block(0,0,2,2) = Matrix::Identity(2,2);
const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
// chain the jacobian matrices // chain the Jacobian matrices
const Matrix tmp = Hi*Hn; const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
if (H1) *H1 = tmp * Hc1; if (Dpose)
if (H2) *H2 = (tmp * Hc2).block(0,0,3,2); *Dpose = Dpi_pc * Dpc_pose;
if (H3) *H3 = Hk; if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 3, 2);
return pi; return pi;
} }
/** project a point from world coordinate to the image /** project a point from world coordinate to the image
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinate
* @param H1 is the jacobian w.r.t. [pose3 calibration] * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param H2 is the jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
*/ */
inline Point2 project2(const Point3& pw, inline Point2 project2(
boost::optional<Matrix&> H1 = boost::none, const Point3& pw, //
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> Dcamera = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const {
if (!H1 && !H2) { if (!Dcamera && !Dpoint) {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
if ( pc.z() <= 0 ) throw CheiralityException(); if (pc.z() <= 0)
throw CheiralityException();
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn); return K_.uncalibrate(pn);
} }
Matrix Htmp1, Htmp2, Htmp3; Matrix Dpose, Dp, Dcal;
const Point2 pi = this->project(pw, Htmp1, Htmp2, Htmp3); const Point2 pi = this->project(pw, Dpose, Dp, Dcal);
if (H1) { if (Dcamera) {
*H1 = Matrix(2, this->dim()); *Dcamera = Matrix(2, this->dim());
H1->leftCols(pose().dim()) = Htmp1 ; // jacobian wrt pose3 Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3
H1->rightCols(calibration().dim()) = Htmp3 ; // jacobian wrt calib Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib
} }
if (H2) *H2 = Htmp2; if (Dpoint)
*Dpoint = Dp;
return pi; return pi;
} }
@ -399,17 +433,18 @@ namespace gtsam {
/** /**
* Calculate range to a landmark * Calculate range to a landmark
* @param point 3D location of landmark * @param point 3D location of landmark
* @param H1 the optionally computed Jacobian with respect to pose * @param Dpose the optionally computed Jacobian with respect to pose
* @param H2 the optionally computed Jacobian with respect to the landmark * @param Dpoint the optionally computed Jacobian with respect to the landmark
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(
boost::optional<Matrix&> H1=boost::none, const Point3& point, //
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> Dpose = boost::none,
double result = pose_.range(point, H1, H2); boost::optional<Matrix&> Dpoint = boost::none) const {
if(H1) { double result = pose_.range(point, Dpose, Dpoint);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration // Add columns of zeros to Jacobian for calibration
Matrix& H1r(*H1); Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
} }
@ -419,17 +454,18 @@ namespace gtsam {
/** /**
* Calculate range to another pose * Calculate range to another pose
* @param pose Other SO(3) pose * @param pose Other SO(3) pose
* @param H1 the optionally computed Jacobian with respect to pose * @param Dpose the optionally computed Jacobian with respect to pose
* @param H2 the optionally computed Jacobian with respect to the landmark * @param Dpose2 the optionally computed Jacobian with respect to the other pose
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, double range(
boost::optional<Matrix&> H1=boost::none, const Pose3& pose, //
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> Dpose = boost::none,
double result = pose_.range(pose, H1, H2); boost::optional<Matrix&> Dpose2 = boost::none) const {
if(H1) { double result = pose_.range(pose, Dpose, Dpose2);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration // Add columns of zeros to Jacobian for calibration
Matrix& H1r(*H1); Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
} }
@ -439,26 +475,29 @@ namespace gtsam {
/** /**
* Calculate range to another camera * Calculate range to another camera
* @param camera Other camera * @param camera Other camera
* @param H1 the optionally computed Jacobian with respect to pose * @param Dpose the optionally computed Jacobian with respect to pose
* @param H2 the optionally computed Jacobian with respect to the landmark * @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double) * @return range (double)
*/ */
template<class CalibrationB> template<class CalibrationB>
double range(const PinholeCamera<CalibrationB>& camera, double range(
boost::optional<Matrix&> H1=boost::none, const PinholeCamera<CalibrationB>& camera, //
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> Dpose = boost::none,
double result = pose_.range(camera.pose_, H1, H2); boost::optional<Matrix&> Dother = boost::none) const {
if(H1) { double result = pose_.range(camera.pose_, Dpose, Dother);
if (Dpose) {
// Add columns of zeros to Jacobian for calibration // Add columns of zeros to Jacobian for calibration
Matrix& H1r(*H1); Matrix& H1r(*Dpose);
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim()); H1r.conservativeResize(Eigen::NoChange, pose_.dim() + K_.dim());
H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim()); H1r.block(0, pose_.dim(), 1, K_.dim()) = Matrix::Zero(1, K_.dim());
} }
if(H2) { if (Dother) {
// Add columns of zeros to Jacobian for calibration // Add columns of zeros to Jacobian for calibration
Matrix& H2r(*H2); Matrix& H2r(*Dother);
H2r.conservativeResize(Eigen::NoChange, camera.pose().dim() + camera.calibration().dim()); H2r.conservativeResize(Eigen::NoChange,
H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = Matrix::Zero(1, camera.calibration().dim()); camera.pose().dim() + camera.calibration().dim());
H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) =
Matrix::Zero(1, camera.calibration().dim());
} }
return result; return result;
} }
@ -466,14 +505,16 @@ namespace gtsam {
/** /**
* Calculate range to another camera * Calculate range to another camera
* @param camera Other camera * @param camera Other camera
* @param H1 the optionally computed Jacobian with respect to pose * @param Dpose the optionally computed Jacobian with respect to pose
* @param H2 the optionally computed Jacobian with respect to the landmark * @param Dother the optionally computed Jacobian with respect to the other camera
* @return range (double) * @return range (double)
*/ */
double range(const CalibratedCamera& camera, double range(
boost::optional<Matrix&> H1=boost::none, const CalibratedCamera& camera, //
boost::optional<Matrix&> H2=boost::none) const { boost::optional<Matrix&> Dpose = boost::none,
return pose_.range(camera.pose_, H1, H2); } boost::optional<Matrix&> Dother = boost::none) const {
return pose_.range(camera.pose_, Dpose, Dother);
}
private: private:
@ -490,5 +531,4 @@ private:
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
/// @} /// @}
}; };}
}