Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc. Small edit to not compute Dcal if not asked.
Frank Dellaert
parent
71466e8476
commit
fa140cb0a3
|
|
@ -31,30 +31,35 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
/**
|
||||
* A pinhole camera class that has a Pose3 and a Calibration.
|
||||
* @addtogroup geometry
|
||||
* \nosubgrouping
|
||||
*/
|
||||
template <typename Calibration>
|
||||
class PinholeCamera : public DerivedValue<PinholeCamera<Calibration> > {
|
||||
private:
|
||||
template<typename Calibration>
|
||||
class PinholeCamera: public DerivedValue<PinholeCamera<Calibration> > {
|
||||
private:
|
||||
Pose3 pose_;
|
||||
Calibration K_;
|
||||
|
||||
public:
|
||||
public:
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/** default constructor */
|
||||
PinholeCamera() {}
|
||||
PinholeCamera() {
|
||||
}
|
||||
|
||||
/** constructor with pose */
|
||||
explicit PinholeCamera(const Pose3& pose):pose_(pose){}
|
||||
explicit PinholeCamera(const Pose3& pose) :
|
||||
pose_(pose) {
|
||||
}
|
||||
|
||||
/** 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
|
||||
|
|
@ -67,7 +72,8 @@ namespace gtsam {
|
|||
* (theta 0 = looking in direction of positive X axis)
|
||||
* @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 Point3 x(st, -ct, 0), y(0, 0, -1), z(ct, st, 0);
|
||||
const Rot3 wRc(x, y, z);
|
||||
|
|
@ -90,13 +96,14 @@ namespace gtsam {
|
|||
* doesn't need to be on the image plane nor orthogonal to the viewing axis
|
||||
* @param K optional calibration parameter
|
||||
*/
|
||||
static PinholeCamera Lookat(const Point3& eye, const Point3& target, const Point3& upVector, const Calibration& K = Calibration()) {
|
||||
Point3 zc = target-eye;
|
||||
zc = zc/zc.norm();
|
||||
static PinholeCamera Lookat(const Point3& eye, const Point3& target,
|
||||
const Point3& upVector, const Calibration& K = Calibration()) {
|
||||
Point3 zc = target - eye;
|
||||
zc = zc / zc.norm();
|
||||
Point3 xc = (-upVector).cross(zc); // minus upVector since yc is pointing down
|
||||
xc = xc/xc.norm();
|
||||
xc = xc / xc.norm();
|
||||
Point3 yc = zc.cross(xc);
|
||||
Pose3 pose3(Rot3(xc,yc,zc), eye);
|
||||
Pose3 pose3(Rot3(xc, yc, zc), eye);
|
||||
return PinholeCamera(pose3, K);
|
||||
}
|
||||
|
||||
|
|
@ -120,34 +127,43 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals (const PinholeCamera &camera, double tol = 1e-9) const {
|
||||
return pose_.equals(camera.pose(), tol) &&
|
||||
K_.equals(camera.calibration(), tol) ;
|
||||
bool equals(const PinholeCamera &camera, double tol = 1e-9) const {
|
||||
return pose_.equals(camera.pose(), tol)
|
||||
&& K_.equals(camera.calibration(), tol);
|
||||
}
|
||||
|
||||
/// print
|
||||
void print(const std::string& s = "PinholeCamera") const {
|
||||
pose_.print(s+".pose");
|
||||
K_.print(s+".calibration");
|
||||
pose_.print(s + ".pose");
|
||||
K_.print(s + ".calibration");
|
||||
}
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
/// @{
|
||||
|
||||
virtual ~PinholeCamera() {}
|
||||
virtual ~PinholeCamera() {
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline Pose3& pose() { return pose_; }
|
||||
inline Pose3& pose() {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return pose
|
||||
inline const Pose3& pose() const { return pose_; }
|
||||
inline const Pose3& pose() const {
|
||||
return pose_;
|
||||
}
|
||||
|
||||
/// return calibration
|
||||
inline Calibration& calibration() { return K_; }
|
||||
inline Calibration& calibration() {
|
||||
return K_;
|
||||
}
|
||||
|
||||
/// 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
|
||||
|
|
@ -155,17 +171,19 @@ namespace gtsam {
|
|||
|
||||
/// compose two cameras: TODO Frank says this might not make sense
|
||||
inline const PinholeCamera compose(const PinholeCamera &c,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
PinholeCamera result( pose_.compose(c.pose(), H1, H2), K_ );
|
||||
if(H1) {
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
PinholeCamera result(pose_.compose(c.pose(), H1, H2), K_);
|
||||
if (H1) {
|
||||
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());
|
||||
}
|
||||
if(H2) {
|
||||
if (H2) {
|
||||
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());
|
||||
}
|
||||
return result;
|
||||
|
|
@ -173,28 +191,32 @@ namespace gtsam {
|
|||
|
||||
/// between two cameras: TODO Frank says this might not make sense
|
||||
inline const PinholeCamera between(const PinholeCamera& c,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
PinholeCamera result( pose_.between(c.pose(), H1, H2), K_ );
|
||||
if(H1) {
|
||||
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
|
||||
boost::none) const {
|
||||
PinholeCamera result(pose_.between(c.pose(), H1, H2), K_);
|
||||
if (H1) {
|
||||
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());
|
||||
}
|
||||
if(H2) {
|
||||
if (H2) {
|
||||
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());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
/// inverse camera: TODO Frank says this might not make sense
|
||||
inline const PinholeCamera inverse(boost::optional<Matrix&> H1=boost::none) const {
|
||||
PinholeCamera result( pose_.inverse(H1), K_ );
|
||||
if(H1) {
|
||||
inline const PinholeCamera inverse(
|
||||
boost::optional<Matrix&> H1 = boost::none) const {
|
||||
PinholeCamera result(pose_.inverse(H1), K_);
|
||||
if (H1) {
|
||||
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());
|
||||
}
|
||||
return result;
|
||||
|
|
@ -202,7 +224,7 @@ namespace gtsam {
|
|||
|
||||
/// compose two cameras: TODO Frank says this might not make sense
|
||||
inline const PinholeCamera compose(const Pose3 &c) const {
|
||||
return PinholeCamera( pose_.compose(c), K_ );
|
||||
return PinholeCamera(pose_.compose(c), K_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
@ -211,26 +233,31 @@ namespace gtsam {
|
|||
|
||||
/// move a cameras according to d
|
||||
PinholeCamera retract(const Vector& d) const {
|
||||
if ((size_t) d.size() == pose_.dim() )
|
||||
return PinholeCamera(pose().retract(d), calibration()) ;
|
||||
if ((size_t) d.size() == pose_.dim())
|
||||
return PinholeCamera(pose().retract(d), calibration());
|
||||
else
|
||||
return PinholeCamera(pose().retract(d.head(pose().dim())),
|
||||
calibration().retract(d.tail(calibration().dim()))) ;
|
||||
calibration().retract(d.tail(calibration().dim())));
|
||||
}
|
||||
|
||||
/// return canonical coordinate
|
||||
Vector localCoordinates(const PinholeCamera& T2) const {
|
||||
Vector d(dim());
|
||||
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;
|
||||
}
|
||||
|
||||
/// Manifold dimension
|
||||
inline size_t dim() const { return pose_.dim() + K_.dim(); }
|
||||
inline size_t dim() const {
|
||||
return pose_.dim() + K_.dim();
|
||||
}
|
||||
|
||||
/// 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
|
||||
|
|
@ -239,13 +266,14 @@ namespace gtsam {
|
|||
/**
|
||||
* projects a 3-dimensional point in camera coordinates into the
|
||||
* 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,
|
||||
boost::optional<Matrix&> H1 = boost::none){
|
||||
if (H1) {
|
||||
boost::optional<Matrix&> Dpoint = boost::none) {
|
||||
if (Dpoint) {
|
||||
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());
|
||||
}
|
||||
|
|
@ -253,139 +281,145 @@ namespace gtsam {
|
|||
/**
|
||||
* projects a 3-dimensional point at infinity (direction-only) in camera coordinates into the
|
||||
* 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,
|
||||
boost::optional<Matrix&> H1 = boost::none){
|
||||
if (H1) {
|
||||
boost::optional<Matrix&> Dpoint = boost::none) {
|
||||
if (Dpoint) {
|
||||
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());
|
||||
}
|
||||
|
||||
/// Project a point into the image and check depth
|
||||
inline std::pair<Point2,bool> projectSafe(const Point3& pw) const {
|
||||
const Point3 pc = pose_.transform_to(pw) ;
|
||||
const Point2 pn = project_to_camera(pc) ;
|
||||
return std::make_pair(K_.uncalibrate(pn), pc.z()>0);
|
||||
inline std::pair<Point2, bool> projectSafe(const Point3& pw) const {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
|
||||
}
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param H1 is the jacobian w.r.t. pose3
|
||||
* @param H2 is the jacobian w.r.t. point3
|
||||
* @param H3 is the jacobian w.r.t. calibration
|
||||
* @param pw is a point in world coordinates
|
||||
* @param Dpose is the Jacobian w.r.t. pose3
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 project(const Point3& pw,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
inline Point2 project(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none,
|
||||
boost::optional<Matrix&> Dcal = boost::none) const {
|
||||
|
||||
if (!H1 && !H2 && !H3) {
|
||||
const Point3 pc = pose_.transform_to(pw) ;
|
||||
if ( pc.z() <= 0 ) throw CheiralityException();
|
||||
const Point2 pn = project_to_camera(pc) ;
|
||||
if (!Dpose && !Dpoint && !Dcal) {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
if (pc.z() <= 0)
|
||||
throw CheiralityException();
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
// world to camera coordinate
|
||||
Matrix Hc1 /* 3*6 */, Hc2 /* 3*3 */ ;
|
||||
const Point3 pc = pose_.transform_to(pw, Hc1, Hc2) ;
|
||||
if( pc.z() <= 0 ) throw CheiralityException();
|
||||
Matrix Dpc_pose /* 3*6 */, Dpc_point /* 3*3 */;
|
||||
const Point3 pc = pose_.transform_to(pw, Dpc_pose, Dpc_point);
|
||||
if (pc.z() <= 0)
|
||||
throw CheiralityException();
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix Hn; // 2*3
|
||||
const Point2 pn = project_to_camera(pc, Hn) ;
|
||||
Matrix Dpn_pc; // 2*3
|
||||
const Point2 pn = project_to_camera(pc, Dpn_pc);
|
||||
|
||||
// uncalibration
|
||||
Matrix Hk, Hi; // 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
|
||||
Matrix Dpi_pn; // 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the jacobian matrices
|
||||
const Matrix tmp = Hi*Hn ;
|
||||
if (H1) *H1 = tmp * Hc1 ;
|
||||
if (H2) *H2 = tmp * Hc2 ;
|
||||
if (H3) *H3 = Hk;
|
||||
// chain the Jacobian matrices
|
||||
const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
|
||||
if (Dpose)
|
||||
*Dpose = Dpi_pc * Dpc_pose;
|
||||
if (Dpoint)
|
||||
*Dpoint = Dpi_pc * Dpc_point;
|
||||
return pi;
|
||||
}
|
||||
|
||||
/** 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 H1 is the jacobian w.r.t. pose3
|
||||
* @param H2 is the jacobian w.r.t. point3
|
||||
* @param H3 is the jacobian w.r.t. calibration
|
||||
* @param Dpose is the Jacobian w.r.t. pose3
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
* @param Dcal is the Jacobian w.r.t. calibration
|
||||
*/
|
||||
inline Point2 projectPointAtInfinity(const Point3& pw,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none,
|
||||
boost::optional<Matrix&> H3 = boost::none) const {
|
||||
inline Point2 projectPointAtInfinity(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none,
|
||||
boost::optional<Matrix&> Dcal = boost::none) const {
|
||||
|
||||
if (!H1 && !H2 && !H3) {
|
||||
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) ;
|
||||
if (!Dpose && !Dpoint && !Dcal) {
|
||||
const Point3 pc = pose_.rotation().unrotate(pw);
|
||||
const Point2 pn = projectPointAtInfinityToCamera(pc);
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
// world to camera coordinate
|
||||
Matrix Hc1_rot /* 3*3 */, Hc2 /* 3*3 */ ;
|
||||
const Point3 pc = pose_.rotation().unrotate(pw, Hc1_rot, Hc2) ;
|
||||
// if( pc.z() <= 0 ) throw CheiralityException(); // this does not make sense for point at infinity
|
||||
Matrix Dpc_rot /* 3*3 */, Dpc_point /* 3*3 */;
|
||||
const Point3 pc = pose_.rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||
|
||||
Matrix Hc1 = Matrix::Zero(3,6);
|
||||
Hc1.block(0,0,3,3) = Hc1_rot;
|
||||
Matrix Dpc_pose = Matrix::Zero(3, 6);
|
||||
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix Hn; // 2*3
|
||||
const Point2 pn = projectPointAtInfinityToCamera(pc, Hn) ;
|
||||
Matrix Dpn_pc; // 2*3
|
||||
const Point2 pn = projectPointAtInfinityToCamera(pc, Dpn_pc);
|
||||
|
||||
// uncalibration
|
||||
Matrix Hk, Hi; // 2*2
|
||||
// Matrix H23 = Matrix::Zero(3,2);
|
||||
// H23.block(0,0,2,2) = Matrix::Identity(2,2);
|
||||
const Point2 pi = K_.uncalibrate(pn, Hk, Hi);
|
||||
Matrix Dpi_pn; // 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the jacobian matrices
|
||||
const Matrix tmp = Hi*Hn;
|
||||
if (H1) *H1 = tmp * Hc1;
|
||||
if (H2) *H2 = (tmp * Hc2).block(0,0,3,2);
|
||||
if (H3) *H3 = Hk;
|
||||
// chain the Jacobian matrices
|
||||
const Matrix Dpi_pc = Dpi_pn * Dpn_pc;
|
||||
if (Dpose)
|
||||
*Dpose = Dpi_pc * Dpc_pose;
|
||||
if (Dpoint)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).block(0, 0, 3, 2);
|
||||
return pi;
|
||||
}
|
||||
|
||||
|
||||
/** project a point from world coordinate to the image
|
||||
* @param pw is a point in the world coordinate
|
||||
* @param H1 is the jacobian w.r.t. [pose3 calibration]
|
||||
* @param H2 is the jacobian w.r.t. point3
|
||||
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
*/
|
||||
inline Point2 project2(const Point3& pw,
|
||||
boost::optional<Matrix&> H1 = boost::none,
|
||||
boost::optional<Matrix&> H2 = boost::none) const {
|
||||
inline Point2 project2(
|
||||
const Point3& pw, //
|
||||
boost::optional<Matrix&> Dcamera = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const {
|
||||
|
||||
if (!H1 && !H2) {
|
||||
const Point3 pc = pose_.transform_to(pw) ;
|
||||
if ( pc.z() <= 0 ) throw CheiralityException();
|
||||
const Point2 pn = project_to_camera(pc) ;
|
||||
if (!Dcamera && !Dpoint) {
|
||||
const Point3 pc = pose_.transform_to(pw);
|
||||
if (pc.z() <= 0)
|
||||
throw CheiralityException();
|
||||
const Point2 pn = project_to_camera(pc);
|
||||
return K_.uncalibrate(pn);
|
||||
}
|
||||
|
||||
Matrix Htmp1, Htmp2, Htmp3;
|
||||
const Point2 pi = this->project(pw, Htmp1, Htmp2, Htmp3);
|
||||
if (H1) {
|
||||
*H1 = Matrix(2, this->dim());
|
||||
H1->leftCols(pose().dim()) = Htmp1 ; // jacobian wrt pose3
|
||||
H1->rightCols(calibration().dim()) = Htmp3 ; // jacobian wrt calib
|
||||
Matrix Dpose, Dp, Dcal;
|
||||
const Point2 pi = this->project(pw, Dpose, Dp, Dcal);
|
||||
if (Dcamera) {
|
||||
*Dcamera = Matrix(2, this->dim());
|
||||
Dcamera->leftCols(pose().dim()) = Dpose; // Jacobian wrt pose3
|
||||
Dcamera->rightCols(calibration().dim()) = Dcal; // Jacobian wrt calib
|
||||
}
|
||||
if (H2) *H2 = Htmp2;
|
||||
if (Dpoint)
|
||||
*Dpoint = Dp;
|
||||
return pi;
|
||||
}
|
||||
|
||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||
inline Point3 backproject(const Point2& p, double depth) const {
|
||||
const Point2 pn = K_.calibrate(p);
|
||||
const Point3 pc(pn.x()*depth, pn.y()*depth, depth);
|
||||
const Point3 pc(pn.x() * depth, pn.y() * depth, depth);
|
||||
return pose_.transform_from(pc);
|
||||
}
|
||||
|
||||
|
|
@ -399,17 +433,18 @@ namespace gtsam {
|
|||
/**
|
||||
* Calculate range to a landmark
|
||||
* @param point 3D location of landmark
|
||||
* @param H1 the optionally computed Jacobian with respect to pose
|
||||
* @param H2 the optionally computed Jacobian with respect to the landmark
|
||||
* @param Dpose 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,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(point, H1, H2);
|
||||
if(H1) {
|
||||
double range(
|
||||
const Point3& point, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpoint = boost::none) const {
|
||||
double result = pose_.range(point, Dpose, Dpoint);
|
||||
if (Dpose) {
|
||||
// Add columns of zeros to Jacobian for calibration
|
||||
Matrix& H1r(*H1);
|
||||
Matrix& H1r(*Dpose);
|
||||
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + 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
|
||||
* @param pose Other SO(3) pose
|
||||
* @param H1 the optionally computed Jacobian with respect to pose
|
||||
* @param H2 the optionally computed Jacobian with respect to the landmark
|
||||
* @param Dpose 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<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(pose, H1, H2);
|
||||
if(H1) {
|
||||
double range(
|
||||
const Pose3& pose, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dpose2 = boost::none) const {
|
||||
double result = pose_.range(pose, Dpose, Dpose2);
|
||||
if (Dpose) {
|
||||
// Add columns of zeros to Jacobian for calibration
|
||||
Matrix& H1r(*H1);
|
||||
Matrix& H1r(*Dpose);
|
||||
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + 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
|
||||
* @param camera Other camera
|
||||
* @param H1 the optionally computed Jacobian with respect to pose
|
||||
* @param H2 the optionally computed Jacobian with respect to the landmark
|
||||
* @param Dpose the optionally computed Jacobian with respect to pose
|
||||
* @param Dother the optionally computed Jacobian with respect to the other camera
|
||||
* @return range (double)
|
||||
*/
|
||||
template<class CalibrationB>
|
||||
double range(const PinholeCamera<CalibrationB>& camera,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
double result = pose_.range(camera.pose_, H1, H2);
|
||||
if(H1) {
|
||||
double range(
|
||||
const PinholeCamera<CalibrationB>& camera, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dother = boost::none) const {
|
||||
double result = pose_.range(camera.pose_, Dpose, Dother);
|
||||
if (Dpose) {
|
||||
// Add columns of zeros to Jacobian for calibration
|
||||
Matrix& H1r(*H1);
|
||||
Matrix& H1r(*Dpose);
|
||||
H1r.conservativeResize(Eigen::NoChange, pose_.dim() + 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
|
||||
Matrix& H2r(*H2);
|
||||
H2r.conservativeResize(Eigen::NoChange, camera.pose().dim() + camera.calibration().dim());
|
||||
H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) = Matrix::Zero(1, camera.calibration().dim());
|
||||
Matrix& H2r(*Dother);
|
||||
H2r.conservativeResize(Eigen::NoChange,
|
||||
camera.pose().dim() + camera.calibration().dim());
|
||||
H2r.block(0, camera.pose().dim(), 1, camera.calibration().dim()) =
|
||||
Matrix::Zero(1, camera.calibration().dim());
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
|
@ -466,14 +505,16 @@ namespace gtsam {
|
|||
/**
|
||||
* Calculate range to another camera
|
||||
* @param camera Other camera
|
||||
* @param H1 the optionally computed Jacobian with respect to pose
|
||||
* @param H2 the optionally computed Jacobian with respect to the landmark
|
||||
* @param Dpose 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,
|
||||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const {
|
||||
return pose_.range(camera.pose_, H1, H2); }
|
||||
double range(
|
||||
const CalibratedCamera& camera, //
|
||||
boost::optional<Matrix&> Dpose = boost::none,
|
||||
boost::optional<Matrix&> Dother = boost::none) const {
|
||||
return pose_.range(camera.pose_, Dpose, Dother);
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -490,5 +531,4 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(K_);
|
||||
}
|
||||
/// @}
|
||||
};
|
||||
}
|
||||
};}
|
||||
|
|
|
|||
Loading…
Reference in New Issue