Refactored derivatives
dellaert
parent
fd62c6f0e6
commit
29e5faeef0
|
|
@ -80,6 +80,28 @@ Pose3 PinholeBase::LookatPose(const Point3& eye, const Point3& target,
|
|||
return Pose3(Rot3(xc, yc, zc), eye);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix26 PinholeBase::Dpose(const Point2& pn, double d) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
double uv = u * v, uu = u * u, vv = v * v;
|
||||
Matrix26 Dpn_pose;
|
||||
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
|
||||
return Dpn_pose;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix23 PinholeBase::Dpoint(const Point2& pn, double d, const Matrix3& R) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
Matrix23 Dpn_point;
|
||||
Dpn_point << //
|
||||
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
|
||||
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
|
||||
Dpn_point *= d;
|
||||
return Dpn_point;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
|
||||
return CalibratedCamera(LevelPose(pose2, height));
|
||||
|
|
@ -93,32 +115,20 @@ CalibratedCamera CalibratedCamera::Lookat(const Point3& eye,
|
|||
|
||||
/* ************************************************************************* */
|
||||
Point2 CalibratedCamera::project(const Point3& point,
|
||||
OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint) const {
|
||||
OptionalJacobian<2, 6> Dcamera, OptionalJacobian<2, 3> Dpoint) const {
|
||||
|
||||
Point3 q = pose().transform_to(point);
|
||||
Point2 intrinsic = project_to_camera(q);
|
||||
Matrix3 Rt; // calculated by transform_to if needed
|
||||
const Point3 q = pose().transform_to(point, boost::none, Dpoint ? &Rt : 0);
|
||||
Point2 pn = project_to_camera(q);
|
||||
|
||||
// Check if point is in front of camera
|
||||
if (q.z() <= 0)
|
||||
throw CheiralityException();
|
||||
|
||||
if (Dpose || Dpoint) {
|
||||
// optimized version, see CalibratedCamera.nb
|
||||
if (Dcamera || Dpoint) {
|
||||
const double z = q.z(), d = 1.0 / z;
|
||||
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
|
||||
if (Dpose)
|
||||
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v), -uv, -u, 0., -d, d
|
||||
* v;
|
||||
if (Dpoint) {
|
||||
const Matrix3 R(pose().rotation().matrix());
|
||||
Matrix23 Dpoint_;
|
||||
Dpoint_ << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0)
|
||||
- u * R(2, 2), R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1)
|
||||
- v * R(2, 2);
|
||||
*Dpoint = d * Dpoint_;
|
||||
}
|
||||
if (Dcamera)
|
||||
*Dcamera = PinholeBase::Dpose(pn, d);
|
||||
if (Dpoint)
|
||||
*Dpoint = PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
|
||||
}
|
||||
return intrinsic;
|
||||
return pn;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -19,10 +19,11 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
class Point2;
|
||||
|
||||
class GTSAM_EXPORT CheiralityException: public ThreadsafeException<
|
||||
CheiralityException> {
|
||||
public:
|
||||
|
|
@ -134,45 +135,16 @@ protected:
|
|||
* Calculate Jacobian with respect to pose
|
||||
* @param pn projection in normalized coordinates
|
||||
* @param d disparity (inverse depth)
|
||||
* @param Dpi_pn derivative of uncalibrate with respect to pn
|
||||
* @param Dpose Output argument, can be matrix or block, assumed right size !
|
||||
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
|
||||
*/
|
||||
template<typename Derived>
|
||||
static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
|
||||
Eigen::MatrixBase<Derived> const & Dpose) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
double uv = u * v, uu = u * u, vv = v * v;
|
||||
Matrix26 Dpn_pose;
|
||||
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
|
||||
assert(Dpose.rows()==2 && Dpose.cols()==6);
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
|
||||
Dpi_pn * Dpn_pose;
|
||||
}
|
||||
static Matrix26 Dpose(const Point2& pn, double d);
|
||||
|
||||
/**
|
||||
* Calculate Jacobian with respect to point
|
||||
* @param pn projection in normalized coordinates
|
||||
* @param d disparity (inverse depth)
|
||||
* @param Dpi_pn derivative of uncalibrate with respect to pn
|
||||
* @param Dpoint Output argument, can be matrix or block, assumed right size !
|
||||
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
|
||||
* @param R rotation matrix
|
||||
*/
|
||||
template<typename Derived>
|
||||
static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
|
||||
const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
|
||||
// optimized version of derivatives, see CalibratedCamera.nb
|
||||
const double u = pn.x(), v = pn.y();
|
||||
Matrix23 Dpn_point;
|
||||
Dpn_point << //
|
||||
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
|
||||
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
|
||||
Dpn_point *= d;
|
||||
assert(Dpoint.rows()==2 && Dpoint.cols()==3);
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
|
||||
Dpi_pn * Dpn_point;
|
||||
}
|
||||
static Matrix23 Dpoint(const Point2& pn, double d, const Matrix3& R);
|
||||
|
||||
private:
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <cmath>
|
||||
|
||||
|
|
@ -85,7 +86,8 @@ public:
|
|||
OptionalJacobian<2, 6> Dcamera = boost::none,
|
||||
OptionalJacobian<2, 3> Dpoint = boost::none) const {
|
||||
|
||||
const Point3 pc = pose().transform_to(pw);
|
||||
Matrix3 Rt; // calculated by transform_to if needed
|
||||
const Point3 pc = pose().transform_to(pw, boost::none, Dpoint ? &Rt : 0);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
|
||||
if (!Dcamera && !Dpoint) {
|
||||
|
|
@ -98,9 +100,9 @@ public:
|
|||
const Point2 pi = calibration().uncalibrate(pn, boost::none, Dpi_pn);
|
||||
|
||||
if (Dcamera)
|
||||
calculateDpose(pn, d, Dpi_pn, *Dcamera);
|
||||
*Dcamera = Dpi_pn * PinholeBase::Dpose(pn, d);
|
||||
if (Dpoint)
|
||||
calculateDpoint(pn, d, pose().rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
*Dpoint = Dpi_pn * PinholeBase::Dpoint(pn, d, Rt.transpose()); // TODO transpose
|
||||
|
||||
return pi;
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue