Standard formatting
dellaert
parent
f097ceef38
commit
0498a4550b
|
|
@ -39,11 +39,11 @@ private:
|
|||
GTSAM_CONCEPT_MANIFOLD_TYPE(Calibration)
|
||||
|
||||
// Get dimensions of calibration type at compile time
|
||||
static const int DimK = FixedDimension<Calibration>::value;
|
||||
static const int DimK = FixedDimension<Calibration>::value;
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 + DimK };
|
||||
enum {dimension = 6 + DimK};
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -292,7 +292,7 @@ public:
|
|||
|
||||
if (!Dpose && !Dpoint && !Dcal) {
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -305,11 +305,11 @@ public:
|
|||
Dpc_pose.leftCols<3>() = Dpc_rot;
|
||||
|
||||
// camera to normalized image coordinate
|
||||
Matrix23 Dpn_pc; // 2*3
|
||||
Matrix23 Dpn_pc;// 2*3
|
||||
const Point2 pn = project_to_camera(pc, Dpn_pc);
|
||||
|
||||
// uncalibration
|
||||
Matrix2 Dpi_pn; // 2*2
|
||||
Matrix2 Dpi_pn;// 2*2
|
||||
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
|
||||
|
||||
// chain the Jacobian matrices
|
||||
|
|
@ -317,7 +317,7 @@ public:
|
|||
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)
|
||||
*Dpoint = (Dpi_pc * Dpc_point).leftCols<2>();// only 2dof are important for the point (direction-only)
|
||||
return pi;
|
||||
}
|
||||
|
||||
|
|
@ -346,7 +346,7 @@ public:
|
|||
|
||||
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
|
||||
calculateDpose(pn, d, Dpi_pn, (*Dcamera).leftCols(6));
|
||||
(*Dcamera).rightCols(DimK) = Dcal; // Jacobian wrt calib
|
||||
(*Dcamera).rightCols(DimK) = Dcal;// Jacobian wrt calib
|
||||
}
|
||||
if (Dpoint) {
|
||||
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
|
||||
|
|
@ -467,7 +467,7 @@ private:
|
|||
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) = //
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) =//
|
||||
Dpi_pn * Dpn_pose;
|
||||
}
|
||||
|
||||
|
|
@ -485,12 +485,12 @@ private:
|
|||
// 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), //
|
||||
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) = //
|
||||
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) =//
|
||||
Dpi_pn * Dpn_point;
|
||||
}
|
||||
|
||||
|
|
@ -504,11 +504,14 @@ private:
|
|||
|
||||
};
|
||||
|
||||
template<typename Calibration>
|
||||
struct traits<PinholeCamera<Calibration> > : public internal::Manifold<
|
||||
PinholeCamera<Calibration> > {
|
||||
};
|
||||
|
||||
template<typename Calibration>
|
||||
struct traits< PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
|
||||
|
||||
template<typename Calibration>
|
||||
struct traits< const PinholeCamera<Calibration> > : public internal::Manifold<PinholeCamera<Calibration> > {};
|
||||
struct traits<const PinholeCamera<Calibration> > : public internal::Manifold<
|
||||
PinholeCamera<Calibration> > {
|
||||
};
|
||||
|
||||
} // \ gtsam
|
||||
|
|
|
|||
|
|
@ -40,7 +40,9 @@ private:
|
|||
|
||||
public:
|
||||
|
||||
enum { dimension = 6 };
|
||||
enum {
|
||||
dimension = 6
|
||||
};
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
|
@ -70,8 +72,8 @@ public:
|
|||
* (theta 0 = looking in direction of positive X axis)
|
||||
* @param height camera height
|
||||
*/
|
||||
static PinholePose Level(const boost::shared_ptr<Calibration>& K, const Pose2& pose2,
|
||||
double height) {
|
||||
static PinholePose Level(const boost::shared_ptr<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);
|
||||
|
|
@ -235,8 +237,7 @@ public:
|
|||
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
|
||||
* @param Dpoint is the Jacobian w.r.t. point3
|
||||
*/
|
||||
Point2 project2(
|
||||
const Point3& pw,
|
||||
Point2 project2(const Point3& pw,
|
||||
OptionalJacobian<2, 6> Dcamera = boost::none,
|
||||
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>
|
||||
struct traits< PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {};
|
||||
|
||||
template<typename Calibration>
|
||||
struct traits< const PinholePose<Calibration> > : public internal::Manifold<PinholePose<Calibration> > {};
|
||||
struct traits<const PinholePose<Calibration> > : public internal::Manifold<
|
||||
PinholePose<Calibration> > {
|
||||
};
|
||||
|
||||
} // \ gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue