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

add dynamic size matrix uncalibrate in Cal3DS2_Base, now wrapper compiles

release/4.3a0
Jing Dong 2014-11-09 17:02:22 -05:00
parent 80b7fdd932
commit 8161cc28ad
4 changed files with 26 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/geometry/Cal3DS2.cpp
View File

@ -47,19 +47,6 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const {
return T2.vector() - vector();
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
Matrix29 H1f;
Matrix2 H2f;
Point2 u = Base::uncalibrate(p,H1f,H2f);
if (H1)
*H1 = H1f;
if (H2)
*H2 = H2f;
return u;
}
}
/* ************************************************************************* */

15
gtsam/geometry/Cal3DS2.h
View File

@ -87,21 +87,6 @@ public:
/// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable
/// @}
/// @name Standard Interface
/// @{
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
private:

23
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -91,8 +91,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Eigen::Matrix<double, 2, 9>&> H1,
boost::optional<Eigen::Matrix<double, 2, 2>&> H2) const {
boost::optional<Matrix29&> H1,
boost::optional<Matrix2&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@ -126,6 +126,25 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1 || H2) {
Matrix29 D1;
Matrix2 D2;
Point2 pu = uncalibrate(p, D1, D2);
if (H1)
*H1 = D1;
if (H2)
*H2 = D2;
return pu;
} else {
return uncalibrate(p);
}
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {

6
gtsam/geometry/Cal3DS2_Base.h
View File

@ -18,7 +18,6 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -114,10 +113,15 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix2&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;