moved projectPointAtInfinity down
parent
0fee8f37a6
commit
fb47cf8961
|
@ -31,14 +31,6 @@ namespace gtsam {
|
||||||
template<typename Calibration>
|
template<typename Calibration>
|
||||||
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
|
class GTSAM_EXPORT PinholeCamera: public PinholeBaseK<Calibration> {
|
||||||
|
|
||||||
public:
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Some classes template on either PinholeCamera or StereoCamera,
|
|
||||||
* and this typedef informs those classes what "project" returns.
|
|
||||||
*/
|
|
||||||
typedef Point2 Measurement;
|
|
||||||
|
|
||||||
private:
|
private:
|
||||||
|
|
||||||
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
|
typedef PinholeBaseK<Calibration> Base; ///< base class has 3D pose as private member
|
||||||
|
@ -235,47 +227,6 @@ public:
|
||||||
return pi;
|
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 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
|
|
||||||
*/
|
|
||||||
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
|
|
||||||
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
|
|
||||||
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
|
||||||
|
|
||||||
if (!Dpose && !Dpoint && !Dcal) {
|
|
||||||
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
|
|
||||||
const Point2 pn = Base::project_to_camera(pc); // project the point to the camera
|
|
||||||
return K_.uncalibrate(pn);
|
|
||||||
}
|
|
||||||
|
|
||||||
// world to camera coordinate
|
|
||||||
Matrix3 Dpc_rot, Dpc_point;
|
|
||||||
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
|
||||||
|
|
||||||
Matrix36 Dpc_pose;
|
|
||||||
Dpc_pose.setZero();
|
|
||||||
Dpc_pose.leftCols<3>() = Dpc_rot;
|
|
||||||
|
|
||||||
// camera to normalized image coordinate
|
|
||||||
Matrix23 Dpn_pc; // 2*3
|
|
||||||
const Point2 pn = Base::project_to_camera(pc, Dpn_pc);
|
|
||||||
|
|
||||||
// uncalibration
|
|
||||||
Matrix2 Dpi_pn; // 2*2
|
|
||||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
|
||||||
|
|
||||||
// chain the Jacobian matrices
|
|
||||||
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
|
|
||||||
if (Dpose)
|
|
||||||
*Dpose = Dpi_pc * Dpc_pose;
|
|
||||||
if (Dpoint)
|
|
||||||
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only)
|
|
||||||
return pi;
|
|
||||||
}
|
|
||||||
|
|
||||||
/** project a point from world coordinate to the image, fixed Jacobians
|
/** project a point from world coordinate to the image, fixed Jacobians
|
||||||
* @param pw is a point in the world coordinate
|
* @param pw is a point in the world coordinate
|
||||||
*/
|
*/
|
||||||
|
|
|
@ -35,7 +35,10 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
|
||||||
|
|
||||||
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
|
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
|
||||||
|
|
||||||
public :
|
// Get dimensions of calibration type at compile time
|
||||||
|
static const int DimK = FixedDimension<Calibration>::value;
|
||||||
|
|
||||||
|
public :
|
||||||
|
|
||||||
/// @name Standard Constructors
|
/// @name Standard Constructors
|
||||||
/// @{
|
/// @{
|
||||||
|
@ -78,7 +81,19 @@ public :
|
||||||
return pn;
|
return pn;
|
||||||
}
|
}
|
||||||
|
|
||||||
/** project a point from world coordinate to the image, fixed Jacobians
|
/** project a point from world coordinate to the image
|
||||||
|
* @param pw is a point in the world coordinates
|
||||||
|
*/
|
||||||
|
Point2 project(const Point3& pw) const {
|
||||||
|
|
||||||
|
// project to normalized coordinates
|
||||||
|
const Point2 pn = PinholeBase::project2(pw);
|
||||||
|
|
||||||
|
// uncalibrate to pixel coordinates
|
||||||
|
return calibration().uncalibrate(pn);
|
||||||
|
}
|
||||||
|
|
||||||
|
/** project a point from world coordinate to the image, w 2 derivatives
|
||||||
* @param pw is a point in the world coordinates
|
* @param pw is a point in the world coordinates
|
||||||
*/
|
*/
|
||||||
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
Point2 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
|
||||||
|
@ -99,6 +114,47 @@ public :
|
||||||
return pi;
|
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 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
|
||||||
|
*/
|
||||||
|
Point2 projectPointAtInfinity(const Point3& pw, OptionalJacobian<2, 6> Dpose =
|
||||||
|
boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
|
||||||
|
OptionalJacobian<2, DimK> Dcal = boost::none) const {
|
||||||
|
|
||||||
|
if (!Dpose && !Dpoint && !Dcal) {
|
||||||
|
const Point3 pc = this->pose().rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
|
||||||
|
const Point2 pn = PinholeBase::project_to_camera(pc);// project the point to the camera
|
||||||
|
return calibration().uncalibrate(pn);
|
||||||
|
}
|
||||||
|
|
||||||
|
// world to camera coordinate
|
||||||
|
Matrix3 Dpc_rot, Dpc_point;
|
||||||
|
const Point3 pc = this->pose().rotation().unrotate(pw, Dpc_rot, Dpc_point);
|
||||||
|
|
||||||
|
Matrix36 Dpc_pose;
|
||||||
|
Dpc_pose.setZero();
|
||||||
|
Dpc_pose.leftCols<3>() = Dpc_rot;
|
||||||
|
|
||||||
|
// camera to normalized image coordinate
|
||||||
|
Matrix23 Dpn_pc;// 2*3
|
||||||
|
const Point2 pn = PinholeBase::project_to_camera(pc, Dpn_pc);
|
||||||
|
|
||||||
|
// uncalibration
|
||||||
|
Matrix2 Dpi_pn;// 2*2
|
||||||
|
const Point2 pi = calibration().uncalibrate(pn, Dcal, Dpi_pn);
|
||||||
|
|
||||||
|
// chain the Jacobian matrices
|
||||||
|
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
|
||||||
|
if (Dpose)
|
||||||
|
*Dpose = Dpi_pc * Dpc_pose;
|
||||||
|
if (Dpoint)
|
||||||
|
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
|
||||||
|
return pi;
|
||||||
|
}
|
||||||
|
|
||||||
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
|
||||||
Point3 backproject(const Point2& p, double depth) const {
|
Point3 backproject(const Point2& p, double depth) const {
|
||||||
const Point2 pn = calibration().calibrate(p);
|
const Point2 pn = calibration().calibrate(p);
|
||||||
|
|
Loading…
Reference in New Issue