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

make tolerance as a constructor param

release/4.3a0
Varun Agrawal 2020-09-23 13:48:40 -04:00
parent 611974c63a
commit b499006b12
2 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
gtsam/geometry/Cal3Bundler.cpp
View File

@ -25,13 +25,13 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Cal3Bundler::Cal3Bundler() : Cal3Bundler::Cal3Bundler() :
f_(1), k1_(0), k2_(0), u0_(0), v0_(0) { f_(1), k1_(0), k2_(0), u0_(0), v0_(0), tol_(1e-5) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0) : Cal3Bundler::Cal3Bundler(double f, double k1, double k2, double u0, double v0,
f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0) { double tol)
} : f_(f), k1_(k1), k2_(k2), u0_(u0), v0_(v0), tol_(tol) {}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Cal3Bundler::K() const { Matrix3 Cal3Bundler::K() const {
@ -94,23 +94,24 @@ Point2 Cal3Bundler::uncalibrate(const Point2& p, //
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol, Point2 Cal3Bundler::calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal, OptionalJacobian<2, 3> Dcal,
OptionalJacobian<2, 2> Dp) const { OptionalJacobian<2, 2> Dp) const {
// Copied from Cal3DS2 :-( // Copied from Cal3DS2 :-(
// but specialized with k1,k2 non-zero only and fx=fy and s=0 // but specialized with k1,k2 non-zero only and fx=fy and s=0
const Point2 invKPi((pi.x() - u0_)/f_, (pi.y() - v0_)/f_); double x = (pi.x() - u0_)/f_, y = (pi.y() - v0_)/f_;
const Point2 invKPi(x, y);
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary // initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi; Point2 pn(x, y);
// iterate until the uncalibrate is close to the actual pixel coordinate // iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10; const int maxIterations = 10;
int iteration; int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) { for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol) if (distance2(uncalibrate(pn), pi) <= tol_)
break; break;
const double x = pn.x(), y = pn.y(), xx = x * x, yy = y * y; const double px = pn.x(), py = pn.y(), xx = px * px, yy = py * py;
const double rr = xx + yy; const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr); const double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g; pn = invKPi / g;
@ -120,7 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, const double tol,
throw std::runtime_error( throw std::runtime_error(
"Cal3Bundler::calibrate fails to converge. need a better initialization"); "Cal3Bundler::calibrate fails to converge. need a better initialization");
const double x = invKPi.x(), y = invKPi.y(); x = invKPi.x(), y = invKPi.y();
const double xx = x * x, yy = y * y; const double xx = x * x, yy = y * y;
const double rr = xx + yy; const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr); const double g = (1 + k1_ * rr + k2_ * rr * rr);

8
gtsam/geometry/Cal3Bundler.h
View File

@ -33,6 +33,7 @@ private:
double f_; ///< focal length double f_; ///< focal length
double k1_, k2_; ///< radial distortion double k1_, k2_; ///< radial distortion
double u0_, v0_; ///< image center, not a parameter to be optimized but a constant double u0_, v0_; ///< image center, not a parameter to be optimized but a constant
double tol_; ///< tolerance value when calibrating
public: public:
@ -51,8 +52,10 @@ public:
* @param k2 second radial distortion coefficient (quartic) * @param k2 second radial distortion coefficient (quartic)
* @param u0 optional image center (default 0), considered a constant * @param u0 optional image center (default 0), considered a constant
* @param v0 optional image center (default 0), considered a constant * @param v0 optional image center (default 0), considered a constant
* @param tol optional calibration tolerance value
*/ */
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0,
double tol = 1e-5);
virtual ~Cal3Bundler() {} virtual ~Cal3Bundler() {}
@ -125,7 +128,7 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in intrinsic coordinates * @return point in intrinsic coordinates
*/ */
Point2 calibrate(const Point2& pi, const double tol = 1e-5, Point2 calibrate(const Point2& pi,
OptionalJacobian<2, 3> Dcal = boost::none, OptionalJacobian<2, 3> Dcal = boost::none,
OptionalJacobian<2, 2> Dp = boost::none) const; OptionalJacobian<2, 2> Dp = boost::none) const;
@ -173,6 +176,7 @@ private:
ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(u0_); ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_); ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(tol_);
} }
/// @} /// @}