22 #include <Eigen/Dense>
30 #ifdef USE_NUMERIC_JACOBIANS
58 std::cout <<
"ERROR: No input images." << std::endl;
72 vector<size_t> valid_image_pair_indices;
76 for (
size_t i = 0; i <
images.size(); i++)
80 bool corners_found[2] = {
false,
false};
82 for (
int lr = 0; lr < 2; lr++)
93 imgSize[lr] = img_gray.
getSize();
107 if (imgSize[lr].y != (
int)img_gray.
getHeight() ||
108 imgSize[lr].
x != (
int)img_gray.
getWidth())
110 std::cout <<
"ERROR: All the images in each left/right "
111 "channel must have the same size."
125 if (corners_found[lr] &&
127 corners_found[lr] =
false;
131 "%s img #%u: %s\n", lr == 0 ?
"LEFT" :
"RIGHT",
132 static_cast<unsigned int>(i),
133 corners_found[lr] ?
"DETECTED" :
"NOT DETECTED");
145 if (corners_found[lr])
162 if (corners_found[0] && corners_found[1])
164 valid_image_pair_indices.push_back(i);
176 bool has_to_redraw_corners =
false;
179 pt_l1 =
images[i].left.detected_corners[1],
180 pt_r0 =
images[i].right.detected_corners[0],
181 pt_r1 =
images[i].right.detected_corners[1];
189 if (Al.x * Ar.x + Al.y * Ar.y < 0)
191 has_to_redraw_corners =
true;
194 images[i].right.detected_corners.begin(),
195 images[i].right.detected_corners.end());
198 if (has_to_redraw_corners)
201 images[i].right.img_original.colorImage(
202 images[i].right.img_checkboard);
203 images[i].right.img_checkboard.drawChessboardCorners(
204 images[i].right.detected_corners, check_size.
x,
212 std::cout << valid_image_pair_indices.size()
213 <<
" valid image pairs.\n";
214 if (valid_image_pair_indices.empty())
217 std::cerr <<
"ERROR: No valid images. Perhaps the checkerboard "
218 "size is incorrect?\n";
225 vector<TPoint3D> obj_points;
226 obj_points.reserve(CORNERS_COUNT);
229 obj_points.emplace_back(
242 const size_t N = valid_image_pair_indices.size();
243 const size_t nObs = 2 * N * CORNERS_COUNT;
248 const double tau = 0.3;
249 const double t1 = 1e-8;
250 const double t2 = 1e-8;
251 const double MAX_LAMBDA = 1e20;
280 size_t nUnknownsCamParams = 0;
283 std::vector<size_t> vars_to_optimize;
288 for (
int calibRound = 0; calibRound < 2; calibRound++)
293 << ((calibRound == 0) ?
"LM calibration round #0: WITHOUT "
294 "distortion ----------\n"
295 :
"LM calibration round #1: ALL "
296 "parameters --------------\n");
302 vars_to_optimize.clear();
303 vars_to_optimize.push_back(0);
304 vars_to_optimize.push_back(1);
305 vars_to_optimize.push_back(2);
306 vars_to_optimize.push_back(3);
316 nUnknownsCamParams = vars_to_optimize.size();
322 const size_t nUnknowns = N * 6 + 6 + 2 * nUnknownsCamParams;
327 lm_stat, res_jacob,
false ,
337 double lambda = tau * H.
asEigen().diagonal().array().maxCoeff();
338 bool done = (minus_g.
array().abs().maxCoeff() < t1);
339 int numItersImproving = 0;
340 bool use_robust =
false;
356 for (
size_t i = 0; i < nUnknowns; i++) HH(i, i) += lambda;
359 Eigen::LLT<Eigen::MatrixXd> llt(
360 HH.asEigen().selfadjointView<Eigen::Lower>());
361 if (llt.info() != Eigen::Success)
365 done = (lambda > MAX_LAMBDA);
368 cout <<
"LM iter#" << iter
369 <<
": Couldn't solve LLt, retrying with larger "
375 llt.solve(minus_g.
asEigen()).eval());
377 const double eps_norm =
eps.norm();
378 if (eps_norm < t2 * (eps_norm + t2))
381 cout <<
"Termination criterion: eps_norm < "
383 << eps_norm <<
" < " << t2 * (eps_norm + t2)
398 new_lm_stat, new_res_jacob, use_robust,
406 if (numItersImproving++ > 5)
407 use_robust = user_wants_use_robust;
411 cout <<
"LM iter#" << iter
412 <<
": Avr.err(px):" << std::sqrt(err / nObs)
413 <<
"->" << std::sqrt(err_new / nObs)
414 <<
" lambda:" << lambda << endl;
417 lm_stat.
swap(new_lm_stat);
418 res_jacob.swap(new_res_jacob);
422 res_jacob, vars_to_optimize, minus_g, H);
425 done = (minus_g.
array().abs().maxCoeff() < t1);
428 lambda = std::max(lambda, 1e-100);
437 cout <<
"LM iter#" << iter <<
": No update: err=" << err
438 <<
" -> err_new=" << err_new
439 <<
" retrying with larger lambda=" << lambda
442 done = (lambda > MAX_LAMBDA);
481 l2r_pose.getAsQuaternion(l2r_quat);
499 for (
unsigned long valid_image_pair_indice : valid_image_pair_indices)
510 const size_t base_idx_H_CPs = H.
cols() - 2 * nUnknownsCamParams;
511 for (
size_t i = 0; i < nUnknownsCamParams; i++)
514 H(base_idx_H_CPs + i, base_idx_H_CPs + i);
516 H(base_idx_H_CPs + nUnknownsCamParams + i,
517 base_idx_H_CPs + nUnknownsCamParams + i);
521 for (
unsigned long idx : valid_image_pair_indices)
556 catch (
const std::exception& e)
558 std::cout << e.what() << std::endl;
580 const double pz_ = 1 / p.
z;
581 const double pz_2 = pz_ * pz_;
585 G(0, 2) = -p.
x * pz_2;
589 G(1, 2) = -p.
y * pz_2;
641 const double x = nP.
x / nP.
z;
642 const double y = nP.
y / nP.
z;
644 const double r2 = x * x + y * y;
645 const double r = std::sqrt(r2);
646 const double r6 = r2 * r2 * r2;
649 const double fx = c[0], fy = c[1];
651 const double k1 = c[4], k2 = c[5], k3 = c[6];
652 const double t1 = c[7], t2 = c[8];
656 fx * (k2 * r2 + k3 * r6 + 6 * t2 * x + 2 * t1 * y +
657 x * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2) + k1 * r + 1);
658 Hb(0, 1) = fx * (2 * t1 * x + 2 * t2 * y +
659 x * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2));
661 Hb(1, 0) = fy * (2 * t1 * x + 2 * t2 * y +
662 y * (2 * k1 * x + 4 * k2 * x * r + 6 * k3 * x * r2));
664 fy * (k2 * r2 + k3 * r6 + 2 * t2 * x + 6 * t1 * y +
665 y * (2 * k1 * y + 4 * k2 * y * r + 6 * k3 * y * r2) + k1 * r + 1);
668 Hc(0, 0) = t2 * (3 * x * x + y * y) + x * (k2 * r2 + k3 * r6 + k1 * r + 1) +
673 Hc(0, 4) = fx * x * r;
674 Hc(0, 5) = fx * x * r2;
675 Hc(0, 6) = fx * x * r6;
676 Hc(0, 7) = 2 * fx * x * y;
677 Hc(0, 8) = fx * (3 * x * x + y * y);
680 Hc(1, 1) = t1 * (x * x + 3 * y * y) + y * (k2 * r2 + k3 * r6 + k1 * r + 1) +
684 Hc(1, 4) = fy * y * r;
685 Hc(1, 5) = fy * y * r2;
686 Hc(1, 6) = fy * y * r6;
687 Hc(1, 7) = fy * (x * x + 3 * y * y);
688 Hc(1, 8) = 2 * fy * x * y;
697 dpl_del.
block<3, 3>(0, 0).setIdentity();
713 P.dot(dr1) + D.
x(), P.dot(dr2) + D.
y(), P.dot(dr3) + D.z());
716 H.block<3, 3>(0, 0).setIdentity();
719 dp_deps =
A.getRotationMatrix().asEigen() * H;
731 const double x = nP.
x / nP.
z;
732 const double y = nP.
y / nP.
z;
736 const double r4 =
square(r2);
737 const double r6 = r2 * r4;
740 const double B = 2 * x * y;
758 const size_t N = res_jac.size();
759 const size_t nMaxUnknowns = N * 6 + 6 + 9 + 9;
767 for (
size_t i = 0; i < N; i++)
769 const size_t nObs = res_jac[i].size();
770 for (
size_t j = 0; j < nObs; j++)
781 minus_g_tot.block<6, 1>(i * 6, 0) -= gij.block<6, 1>(0, 0);
782 minus_g_tot.block<6 + 9 + 9, 1>(N * 6, 0) -=
783 gij.block<6 + 9 + 9, 1>(6, 0);
785 H_tot.block<6, 6>(i * 6, i * 6) += Hij.block<6, 6>(0, 0);
787 H_tot.block<6 + 9 + 9, 6 + 9 + 9>(N * 6, N * 6) +=
788 Hij.block<6 + 9 + 9, 6 + 9 + 9>(6, 6);
789 H_tot.block<6, 6 + 9 + 9>(i * 6, N * 6) +=
790 Hij.block<6, 6 + 9 + 9>(0, 6);
791 H_tot.block<6 + 9 + 9, 6>(N * 6, i * 6) +=
792 Hij.block<6 + 9 + 9, 6>(6, 0);
800 const double cost_lr_angular = 1e10;
807 const size_t N_Cs = var_indxs.size();
808 const size_t nUnknowns = N * 6 + 6 + 2 * N_Cs;
809 const size_t nUnkPoses = N * 6 + 6;
812 H.
setZero(nUnknowns, nUnknowns);
814 minus_g.
asEigen().block(0, 0, nUnkPoses, 1) =
815 minus_g_tot.asEigen().block(0, 0, nUnkPoses, 1);
816 H.
asEigen().block(0, 0, nUnkPoses, nUnkPoses) =
817 H_tot.asEigen().block(0, 0, nUnkPoses, nUnkPoses);
820 for (
size_t i = 0; i < N_Cs; i++)
822 minus_g[nUnkPoses + i] = minus_g_tot[nUnkPoses + var_indxs[i]];
823 minus_g[nUnkPoses + N_Cs + i] =
824 minus_g_tot[nUnkPoses + 9 + var_indxs[i]];
827 for (
size_t k = 0; k < nUnkPoses; k++)
829 for (
size_t i = 0; i < N_Cs; i++)
831 H(nUnkPoses + i, k) = H(k, nUnkPoses + i) =
832 H_tot(k, nUnkPoses + var_indxs[i]);
833 H(nUnkPoses + i + N_Cs, k) = H(k, nUnkPoses + i + N_Cs) =
834 H_tot(k, nUnkPoses + 9 + var_indxs[i]);
838 for (
size_t i = 0; i < N_Cs; i++)
840 for (
size_t j = 0; j < N_Cs; j++)
842 H(nUnkPoses + i, nUnkPoses + j) =
843 H_tot(nUnkPoses + var_indxs[i], nUnkPoses + var_indxs[j]);
844 H(nUnkPoses + N_Cs + i, nUnkPoses + N_Cs + j) = H_tot(
845 nUnkPoses + 9 + var_indxs[i], nUnkPoses + 9 + var_indxs[j]);
872 const std::vector<size_t>& var_indxs,
lm_stat_t& lm_stat)
876 for (
size_t i = 0; i < N; i++)
887 cam_pose = (incrPose +
CPose3D(cam_pose)).asTPose();
903 const size_t idx = 6 * N + 6;
906 for (
size_t i = 0; i < nPC; i++)
914 #ifdef USE_NUMERIC_JACOBIANS
931 obj_point(_obj_point),
933 right2left(_right2left),
939 void numeric_jacob_eval_function(
976 dat.obj_point, cam_params.
leftCamera, incrPose_l + dat.left_cam, px_l);
980 incrPose_rl + dat.right2left + incrPose_l + dat.left_cam, px_r);
983 px_l.
x, px_l.
y, px_r.
x, px_r.
y);
994 const double x = X[0], y = X[1];
996 const double r4 =
square(r2);
997 const double r6 = r2 * r4;
1000 const double B = 2 * x * y;
1026 for (
int i = 0; i < 3; i++)
out[i] = D_p_out[i];
1029 struct TEvalData_A_eps_D_p
1035 void eval_dA_eps_D_p(
1041 const CPose3D A_eps_D = dat.A + (incrPose + dat.D);
1044 for (
int i = 0; i < 3; i++)
out[i] = pt[i];
1055 bool use_robust_kernel,
double kernel_param)
1057 double total_err = 0;
1085 for (
size_t k = 0; k < N; k++)
1088 const size_t nPts = lm_stat.
obj_points.size();
1089 res_jac[k].resize(nPts);
1091 for (
size_t i = 0; i < nPts; i++)
1095 lm_stat.
images[k_idx].left.detected_corners[i];
1097 lm_stat.
images[k_idx].right.detected_corners[i];
1099 px_obs_l.
x, px_obs_l.
y, px_obs_r.
x, px_obs_r.
y);
1122 const double err_sqr_norm = rje.
residual.squaredNorm();
1123 if (use_robust_kernel)
1126 rk.param_sq = kernel_param;
1128 double kernel_1st_deriv, kernel_2nd_deriv;
1129 const double scaled_err =
1130 rk.eval(err_sqr_norm, kernel_1st_deriv, kernel_2nd_deriv);
1133 total_err += scaled_err;
1136 total_err += err_sqr_norm;
1143 #if !defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
1149 TPoint3D pt_wrt_left, pt_wrt_right;
1186 rje.
J.setZero(4, 30);
1187 rje.
J.block<2, 6>(0, 0) = dhl_del.
asEigen();
1188 rje.
J.block<2, 6>(2, 0) = dhr_del.
asEigen();
1189 rje.
J.block<2, 6>(2, 6) = dhr_der.
asEigen();
1190 rje.
J.block<2, 9>(0, 12) = dhl_dcl.
asEigen();
1191 rje.
J.block<2, 9>(2, 21) = dhr_dcr.
asEigen();
1193 #if defined(COMPARE_NUMERIC_JACOBIANS)
1199 #if defined(USE_NUMERIC_JACOBIANS) || defined(COMPARE_NUMERIC_JACOBIANS)
1209 const double x_incrs_val[30] = {
1210 1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7,
1211 1e-6, 1e-6, 1e-6, 1e-7, 1e-7, 1e-7,
1212 1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4,
1213 1e-3, 1e-3, 1e-3, 1e-3, 1e-8, 1e-8, 1e-8, 1e-8, 1e-4
1221 x0, &numeric_jacob_eval_function, x_incrs, dat, rje.
J);
1223 #if defined(COMPARE_NUMERIC_JACOBIANS)
1226 #endif // ---- end of numeric Jacobians ----
1229 #if defined(COMPARE_NUMERIC_JACOBIANS)
1239 << J_num - J_theor << endl
1240 <<
"diff (ratio):\n"
1241 << (J_num - J_theor).cwiseQuotient(J_num) << endl