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

completed all calibration files

release/4.3a0
nsrinivasan7 2014-12-02 12:40:18 -05:00
parent 595afb51fe
commit 22bbde6fe0
10 changed files with 72 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam/geometry/Cal3Bundler.cpp
View File

@ -34,15 +34,17 @@ Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) :
}
/* ************************************************************************* */
Matrix Cal3Bundler::K() const {
Matrix3 Cal3Bundler::K() const {
Matrix3 K;
K << f_, 0, u0_, 0, f_, v0_, 0, 0, 1;
return K;
}
/* ************************************************************************* */
Vector Cal3Bundler::k() const {
return (Vector(4) << k1_, k2_, 0, 0).finished();
Vector4 Cal3Bundler::k() const {
Vector4 rvalue_;
rvalue_ << k1_, k2_, 0, 0;
return rvalue_;
}
/* ************************************************************************* */
@ -120,25 +122,27 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix Dp;
Matrix2 Cal3Bundler::D2d_intrinsic(const Point2& p) const {
Matrix2 Dp;
uncalibrate(p, boost::none, Dp);
return Dp;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix Dcal;
Matrix23 Cal3Bundler::D2d_calibration(const Point2& p) const {
Matrix23 Dcal;
uncalibrate(p, Dcal, boost::none);
return Dcal;
}
/* ************************************************************************* */
Matrix Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix Dcal, Dp;
Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
Matrix23 Dcal;
Matrix2 Dp;
uncalibrate(p, Dcal, Dp);
Matrix H(2, 5);
H << Dp, Dcal;
Matrix25 H;
H.block<2,2>(0,0) = Dp;
H.block<2,3>(0,2) = Dcal;
return H;
}

10
gtsam/geometry/Cal3Bundler.h
View File

@ -69,8 +69,8 @@ public:
/// @name Standard Interface
/// @{
Matrix K() const; ///< Standard 3*3 calibration matrix
Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
Matrix3 K() const; ///< Standard 3*3 calibration matrix
Vector4 k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector3 vector() const;
@ -120,13 +120,13 @@ public:
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_intrinsic(const Point2& p) const;
Matrix2 D2d_intrinsic(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_calibration(const Point2& p) const;
Matrix23 D2d_calibration(const Point2& p) const;
/// @deprecated might be removed in next release, use uncalibrate
Matrix D2d_intrinsic_calibration(const Point2& p) const;
Matrix25 D2d_intrinsic_calibration(const Point2& p) const;
/// @}
/// @name Manifold

12
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -28,8 +28,10 @@ Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
Matrix3 Cal3DS2_Base::K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/* ************************************************************************* */
@ -39,7 +41,7 @@ Vector Cal3DS2_Base::vector() const {
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print((Matrix)K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
@ -156,7 +158,7 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
Matrix2 Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
@ -167,7 +169,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
Matrix29 Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;

6
gtsam/geometry/Cal3DS2_Base.h
View File

@ -45,7 +45,7 @@ protected:
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Matrix3 K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
@ -121,10 +121,10 @@ public:
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
Matrix2 D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
Matrix29 D2d_calibration(const Point2& p) const ;
private:

5
gtsam/geometry/Cal3Unified.cpp
View File

@ -52,8 +52,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
/* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
OptionalJacobian<2,10> H1,
OptionalJacobian<2,2> H2) const {
// this part of code is modified from Cal3DS2,
// since the second part of this model (after project to normalized plane)
@ -82,7 +82,6 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
-ys * sqrt_nx * xi_sqrt_nx2;
H1->resize(2,10);
H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
}

4
gtsam/geometry/Cal3Unified.h
View File

@ -96,8 +96,8 @@ public:
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
OptionalJacobian<2,10> Dcal = boost::none,
OptionalJacobian<2,2> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& p, const double tol=1e-5) const;

2
gtsam/geometry/Cal3_S2.cpp
View File

@ -53,7 +53,7 @@ Cal3_S2::Cal3_S2(const std::string &path) :
/* ************************************************************************* */
void Cal3_S2::print(const std::string& s) const {
gtsam::print(matrix(), s);
gtsam::print((Matrix)matrix(), s);
}
/* ************************************************************************* */

16
gtsam/geometry/Cal3_S2.h
View File

@ -125,20 +125,24 @@ public:
}
/// return calibration matrix K
Matrix K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0).finished();
Matrix3 K() const {
Matrix3 K;
K << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0;
return K;
}
/** @deprecated The following function has been deprecated, use K above */
Matrix matrix() const {
Matrix3 matrix() const {
return K();
}
/// return inverted calibration matrix inv(K)
Matrix matrix_inverse() const {
Matrix3 matrix_inverse() const {
const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_;
return (Matrix(3, 3) << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0).finished();
Matrix3 K_inverse;
K_inverse << 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0,
1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0;
return K_inverse;
}
/**

29
gtsam/geometry/CalibratedCamera.cpp
View File

@ -33,10 +33,10 @@ CalibratedCamera::CalibratedCamera(const Vector &v) :
/* ************************************************************************* */
Point2 CalibratedCamera::project_to_camera(const Point3& P,
boost::optional<Matrix&> H1) {
OptionalJacobian<2,3> H1) {
if (H1) {
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).finished();
(*H1) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2;
}
return Point2(P.x() / P.z(), P.y() / P.z());
}
@ -59,10 +59,12 @@ CalibratedCamera CalibratedCamera::Level(const Pose2& pose2, double height) {
/* ************************************************************************* */
Point2 CalibratedCamera::project(const Point3& point,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const {
OptionalJacobian<2,6> Dpose, OptionalJacobian<2,3> Dpoint) const {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
Point3 q = pose_.transform_to(point, Dpose, Dpoint);
Matrix36 Dpose_;
Matrix3 Dpoint_;
Point3 q = pose_.transform_to(point, Dpose_, Dpoint_);
#else
Point3 q = pose_.transform_to(point);
#endif
@ -75,23 +77,24 @@ Point2 CalibratedCamera::project(const Point3& point,
if (Dpose || Dpoint) {
#ifdef CALIBRATEDCAMERA_CHAIN_RULE
// just implement chain rule
Matrix H;
Matrix23 H;
project_to_camera(q,H);
if (Dpose) *Dpose = H * (*Dpose);
if (Dpoint) *Dpoint = H * (*Dpoint);
if (Dpose) *Dpose = H * (*Dpose_);
if (Dpoint) *Dpoint = H * (*Dpoint_);
#else
// optimized version, see CalibratedCamera.nb
const double z = q.z(), d = 1.0 / z;
const double u = intrinsic.x(), v = intrinsic.y(), uv = u * v;
if (Dpose)
*Dpose = (Matrix(2, 6) << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v).finished();
*Dpose << uv, -(1. + u * u), v, -d, 0., d * u, (1. + v * v),
-uv, -u, 0., -d, d * v;
if (Dpoint) {
const Matrix R(pose_.rotation().matrix());
*Dpoint = d
* (Matrix(2, 3) << R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2),
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)).finished();
R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
*Dpoint = d * Dpoint_;
}
#endif
}

23
gtsam/geometry/CalibratedCamera.h
View File

@ -90,20 +90,20 @@ public:
/// compose the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera compose(const CalibratedCamera &c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
OptionalJacobian<6,6> H1=boost::none, OptionalJacobian<6,6> H2 =
boost::none) const {
return CalibratedCamera(pose_.compose(c.pose(), H1, H2));
}
/// between the two camera poses: TODO Frank says this might not make sense
inline const CalibratedCamera between(const CalibratedCamera& c,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
OptionalJacobian<6,6> H1 = boost::none, OptionalJacobian<6,6> H2 =
boost::none) const {
return CalibratedCamera(pose_.between(c.pose(), H1, H2));
}
/// invert the camera pose: TODO Frank says this might not make sense
inline const CalibratedCamera inverse(boost::optional<Matrix&> H1 =
inline const CalibratedCamera inverse(OptionalJacobian<6,6> H1 =
boost::none) const {
return CalibratedCamera(pose_.inverse(H1));
}
@ -152,8 +152,7 @@ public:
* @return the intrinsic coordinates of the projected point
*/
Point2 project(const Point3& point,
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none) const;
OptionalJacobian<2,6> Dpose=boost::none, OptionalJacobian<2,3> Dpoint=boost::none) const;
/**
* projects a 3-dimensional point in camera coordinates into the
@ -161,7 +160,7 @@ public:
* With optional 2by3 derivative
*/
static Point2 project_to_camera(const Point3& cameraPoint,
boost::optional<Matrix&> H1 = boost::none);
OptionalJacobian<2, 3> H1 = boost::none);
/**
* backproject a 2-dimensional point to a 3-dimension point
@ -175,8 +174,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Point3& point, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
double range(const Point3& point, OptionalJacobian<1,6> H1 = boost::none,
OptionalJacobian<1,3> H2 = boost::none) const {
return pose_.range(point, H1, H2);
}
@ -187,8 +186,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const Pose3& pose, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
double range(const Pose3& pose, OptionalJacobian<1,6> H1=boost::none,
OptionalJacobian<1,6> H2=boost::none) const {
return pose_.range(pose, H1, H2);
}
@ -199,8 +198,8 @@ public:
* @param H2 optionally computed Jacobian with respect to the 3D point
* @return range (double)
*/
double range(const CalibratedCamera& camera, boost::optional<Matrix&> H1 =
boost::none, boost::optional<Matrix&> H2 = boost::none) const {
double range(const CalibratedCamera& camera, OptionalJacobian<1,6> H1 =
boost::none, OptionalJacobian<1,6> H2 = boost::none) const {
return pose_.range(camera.pose_, H1, H2);
}