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

Standard formatting

release/4.3a0
dellaert 2015-02-21 07:29:08 +01:00
parent f097ceef38
commit 0498a4550b
2 changed files with 52 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

73
gtsam/geometry/PinholeCamera.h
View File

@ -39,11 +39,11 @@ private:
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration) GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
// Get dimensions of calibration type at compile time // Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<Calibration>::value; static const int DimK = FixedDimension<Calibration>::value;
public: public:
enum { dimension = 6 + DimK }; enum {dimension = 6 + DimK};
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -54,12 +54,12 @@ public:
/** constructor with pose */ /** constructor with pose */
explicit PinholeCamera(const Pose3& pose) : explicit PinholeCamera(const Pose3& pose) :
pose_(pose) { pose_(pose) {
} }
/** constructor with pose and calibration */ /** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& K) : PinholeCamera(const Pose3& pose, const Calibration& K) :
pose_(pose), K_(K) { pose_(pose), K_(K) {
} }
/// @} /// @}
@ -120,7 +120,7 @@ public:
} }
PinholeCamera(const Vector &v, const Vector &K) : PinholeCamera(const Vector &v, const Vector &K) :
pose_(Pose3::Expmap(v)), K_(K) { pose_(Pose3::Expmap(v)), K_(K) {
} }
/// @} /// @}
@ -130,7 +130,7 @@ public:
/// 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
@ -194,10 +194,10 @@ public:
/// move a cameras according to d /// move a cameras according to d
PinholeCamera retract(const Vector& d) const { PinholeCamera retract(const Vector& d) const {
if ((size_t) d.size() == 6) if ((size_t) d.size() == 6)
return PinholeCamera(pose().retract(d), calibration()); return PinholeCamera(pose().retract(d), calibration());
else else
return PinholeCamera(pose().retract(d.head(6)), return PinholeCamera(pose().retract(d.head(6)),
calibration().retract(d.tail(calibration().dim()))); calibration().retract(d.tail(calibration().dim())));
} }
/// return canonical coordinate /// return canonical coordinate
@ -228,12 +228,12 @@ public:
OptionalJacobian<2, 3> Dpoint = boost::none) { OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0) if (P.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
double d = 1.0 / P.z(); double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d; const double u = P.x() * d, v = P.y() * d;
if (Dpoint) if (Dpoint)
*Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
return Point2(u, v); return Point2(u, v);
} }
@ -271,12 +271,12 @@ public:
// chain the Jacobian matrices // chain the Jacobian matrices
if (Dpose) if (Dpose)
calculateDpose(pn, d, Dpi_pn, *Dpose); calculateDpose(pn, d, Dpi_pn, *Dpose);
if (Dpoint) if (Dpoint)
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
return pi; return pi;
} else } else
return K_.uncalibrate(pn, Dcal); return K_.uncalibrate(pn, Dcal);
} }
/** project a point at infinity from world coordinate to the image /** project a point at infinity from world coordinate to the image
@ -292,7 +292,7 @@ public:
if (!Dpose && !Dpoint && !Dcal) { if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter) const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
const Point2 pn = project_to_camera(pc); // project the point to the camera const Point2 pn = project_to_camera(pc);// project the point to the camera
return K_.uncalibrate(pn); return K_.uncalibrate(pn);
} }
@ -305,19 +305,19 @@ public:
Dpc_pose.leftCols<3>() = Dpc_rot; Dpc_pose.leftCols<3>() = Dpc_rot;
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix23 Dpn_pc; // 2*3 Matrix23 Dpn_pc;// 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc); const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration // uncalibration
Matrix2 Dpi_pn; // 2*2 Matrix2 Dpi_pn;// 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices // chain the Jacobian matrices
const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc; const Matrix23 Dpi_pc = Dpi_pn * Dpn_pc;
if (Dpose) if (Dpose)
*Dpose = Dpi_pc * Dpc_pose; *Dpose = Dpi_pc * Dpc_pose;
if (Dpoint) if (Dpoint)
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>(); // only 2dof are important for the point (direction-only) *Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
return pi; return pi;
} }
@ -346,7 +346,7 @@ public:
if (Dcamera) { // TODO why does leftCols<6>() not compile ?? if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6)); calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
(*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib (*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib
} }
if (Dpoint) { if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint); calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
@ -383,7 +383,7 @@ public:
Matrix16 Dpose_; Matrix16 Dpose_;
double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint); double result = pose_.range(point, Dcamera ? &Dpose_ : 0, Dpoint);
if (Dcamera) if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero(); *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result; return result;
} }
@ -401,7 +401,7 @@ public:
Matrix16 Dpose_; Matrix16 Dpose_;
double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose); double result = pose_.range(pose, Dcamera ? &Dpose_ : 0, Dpose);
if (Dcamera) if (Dcamera)
*Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero(); *Dcamera << Dpose_, Eigen::Matrix<double, 1, DimK>::Zero();
return result; return result;
} }
@ -417,13 +417,13 @@ public:
const PinholeCamera<CalibrationB>& camera, // const PinholeCamera<CalibrationB>& camera, //
OptionalJacobian<1, dimension> Dcamera = boost::none, OptionalJacobian<1, dimension> Dcamera = boost::none,
// OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother = // OptionalJacobian<1, 6 + traits::dimension<CalibrationB>::value> Dother =
boost::optional<Matrix&> Dother = boost::optional<Matrix&> Dother =
boost::none) const { boost::none) const {
Matrix16 Dcamera_, Dother_; Matrix16 Dcamera_, Dother_;
double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0, double result = pose_.range(camera.pose(), Dcamera ? &Dcamera_ : 0,
Dother ? &Dother_ : 0); Dother ? &Dother_ : 0);
if (Dcamera) { if (Dcamera) {
Dcamera->resize(1, 6 + DimK); Dcamera->resize(1, 6 + DimK);
*Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero(); *Dcamera << Dcamera_, Eigen::Matrix<double, 1, DimK>::Zero();
} }
if (Dother) { if (Dother) {
@ -467,8 +467,8 @@ private:
Matrix26 Dpn_pose; Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; 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); assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpose) =//
Dpi_pn * Dpn_pose; Dpi_pn * Dpn_pose;
} }
/** /**
@ -485,13 +485,13 @@ private:
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
Matrix23 Dpn_point; Matrix23 Dpn_point;
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, 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); /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d; Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3); assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) =//
Dpi_pn * Dpn_point; Dpi_pn * Dpn_point;
} }
/** Serialization function */ /** Serialization function */
@ -504,11 +504,14 @@ private:
}; };
template<typename Calibration>
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
};
template<typename Calibration> template<typename Calibration>
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {}; struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
PinholeCamera<Calibration> > {
template<typename Calibration> };
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
} // \ gtsam } // \ gtsam

24
gtsam/geometry/PinholePose.h
View File

@ -40,7 +40,9 @@ private:
public: public:
enum { dimension = 6 }; enum {
dimension = 6
};
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -70,8 +72,8 @@ public:
* (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 PinholePose Level(const boost::shared_ptr<Calibration>& K, const Pose2& pose2, static PinholePose Level(const boost::shared_ptr<Calibration>& K,
double height) { 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);
@ -214,7 +216,7 @@ public:
OptionalJacobian<2, 3> Dpoint = boost::none) { OptionalJacobian<2, 3> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0) if (P.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
double d = 1.0 / P.z(); double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d; const double u = P.x() * d, v = P.y() * d;
@ -235,8 +237,7 @@ public:
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
*/ */
Point2 project2( Point2 project2(const Point3& pw,
const Point3& pw,
OptionalJacobian<2, 6> Dcamera = boost::none, OptionalJacobian<2, 6> Dcamera = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const { OptionalJacobian<2, 3> Dpoint = boost::none) const {
@ -388,11 +389,14 @@ private:
}; };
template<typename Calibration>
struct traits<PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
};
template<typename Calibration> template<typename Calibration>
struct traits< PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {}; struct traits<const PinholePose<Calibration> > : public internal::Manifold<
PinholePose<Calibration> > {
template<typename Calibration> };
struct traits< const PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {};
} // \ gtsam } // \ gtsam