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

testCal3S2

release/4.3a0
nsrinivasan7 2014-11-27 10:34:44 -05:00
parent 61666f22d6
commit c9936763e2
5 changed files with 6 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/geometry/Cal3_S2.cpp
View File

@ -71,21 +71,6 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
return true;
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) {
Dcal->resize(2,5);
*Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
}
if (Dp) {
Dp->resize(2,2);
*Dp << fx_, s_, 0.0, fy_;
}
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const {

10
gtsam/geometry/Cal3_S2.h
View File

@ -158,16 +158,6 @@ public:
Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
/**
* convert image coordinates uv to intrinsic coordinates xy
* @param p point in image coordinates

8
gtsam/geometry/PinholeCamera.h
View File

@ -353,7 +353,7 @@ public:
const Point3& pw, //
boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint,
boost::optional<Matrix&> Dcal) const {
boost::optional<Matrix25&> Dcal) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
@ -365,7 +365,7 @@ public:
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix Dpi_pn(2, 2);
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
@ -392,7 +392,7 @@ public:
const Point3& pw, //
boost::optional<Matrix&> Dpose = boost::none,
boost::optional<Matrix&> Dpoint = boost::none,
boost::optional<Matrix&> Dcal = boost::none) const {
boost::optional<Matrix25&> Dcal = boost::none) const {
if (!Dpose && !Dpoint && !Dcal) {
const Point3 pc = pose_.rotation().unrotate(pw); // get direction in camera frame (translation does not matter)
@ -412,7 +412,7 @@ public:
const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration
Matrix Dpi_pn; // 2*2
Matrix2 Dpi_pn; // 2*2
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices

2
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -60,7 +60,7 @@ TEST( Cal3_S2, calibrate_homogeneous) {
Point2 uncalibrate_(const Cal3_S2& k, const Point2& pt) { return k.uncalibrate(pt); }
TEST( Cal3_S2, Duncalibrate1)
{
Matrix computed; K.uncalibrate(p, computed, boost::none);
Matrix25 computed; K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p);
CHECK(assert_equal(numerical,computed,1e-8));
}

2
gtsam/slam/SmartFactorBase.h
View File

@ -280,7 +280,7 @@ public:
Vector bi;
try {
bi = -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
bi = -(cameras[i].project(point, Fi, Ei, static_cast<Matrix25&>(Hcali)) - this->measured_.at(i)).vector();
if(body_P_sensor_){
Pose3 w_Pose_body = (cameras[i].pose()).compose(body_P_sensor_->inverse());
Matrix J(6, 6);