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

initialize with intrinsic coordinates which has radial distortion modeled

release/4.3a0
Varun Agrawal 2021-06-02 12:08:07 -04:00
parent c252937cee
commit c2f4cc82bf
1 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/geometry/Cal3Bundler.cpp
View File

@ -99,21 +99,26 @@ Point2 Cal3Bundler::calibrate(const Point2& pi, OptionalJacobian<2, 3> Dcal,
double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_; double x = (pi.x() - u0_) / fx_, y = (pi.y() - v0_) / fx_;
const Point2 invKPi(x, y); const Point2 invKPi(x, y);
// initialize pn with distortion included Point2 pn;
double px = pi.x(), py = pi.y(), rr = px * px + py * py; double px = pi.x(), py = pi.y();
double g = (1 + k1_ * rr + k2_ * rr * rr);
Point2 pn = invKPi / g;
// 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 = 0;
for (iteration = 0; iteration < maxIterations; ++iteration) { do {
if (distance2(uncalibrate(pn), pi) <= tol_) break; // initialize pn with distortion included
px = pn.x(), py = pn.y(); double rr = (px * px) + (py * py);
rr = (px * px) + (py * py);
double g = (1 + k1_ * rr + k2_ * rr * rr); double g = (1 + k1_ * rr + k2_ * rr * rr);
pn = invKPi / g; pn = invKPi / g;
}
if (distance2(uncalibrate(pn), pi) <= tol_) break;
// Set px and py using intrinsic coordinates since that is where radial
// distortion correction is done.
px = pn.x(), py = pn.y();
iteration++;
} while (iteration < maxIterations);
if (iteration >= maxIterations) if (iteration >= maxIterations)
throw std::runtime_error( throw std::runtime_error(
@ -149,4 +154,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H; return H;
} }
} // \ namespace gtsam } // namespace gtsam