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_;
const Point2 invKPi(x, y);
// initialize pn with distortion included
double px = pi.x(), py = pi.y(), rr = px * px + py * py;
double g = (1 + k1_ * rr + k2_ * rr * rr);
Point2 pn = invKPi / g;
Point2 pn;
double px = pi.x(), py = pi.y();
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (distance2(uncalibrate(pn), pi) <= tol_) break;
px = pn.x(), py = pn.y();
rr = (px * px) + (py * py);
int iteration = 0;
do {
// initialize pn with distortion included
double rr = (px * px) + (py * py);
double g = (1 + k1_ * rr + k2_ * rr * rr);
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)
throw std::runtime_error(
@ -149,4 +154,4 @@ Matrix25 Cal3Bundler::D2d_intrinsic_calibration(const Point2& p) const {
return H;
}
} // \ namespace gtsam
} // namespace gtsam