void dTv1(mat_t& drt3_dt1, mat_t& dt3_dt1) { drt3_dt1.create(3, 3); dt3_dt1.create(3, 3); for(int i = 0; i < 3; ++i) { ev.setTo(Scalar(0)); ev(i, 0) = eps; composeRT( rv1, tv1 + ev, rv2, tv2, rv3_p, tv3_p); composeRT( rv1, tv1 - ev, rv2, tv2, rv3_m, tv3_m); drt3_dt1.col(i) = rv3_p - rv3_m; dt3_dt1.col(i) = tv3_p - tv3_m; } drt3_dt1 /= 2 * eps; dt3_dt1 /= 2 * eps; }
void dRv2(mat_t& dr3_dr2, mat_t& dt3_dr2) { dr3_dr2.create(3, 3); dt3_dr2.create(3, 3); for(int i = 0; i < 3; ++i) { ev.setTo(Scalar(0)); ev(i, 0) = eps; composeRT( rv1, tv1, rv2 + ev, tv2, rv3_p, tv3_p); composeRT( rv1, tv1, rv2 - ev, tv2, rv3_m, tv3_m); dr3_dr2.col(i) = rv3_p - rv3_m; dt3_dr2.col(i) = tv3_p - tv3_m; } dr3_dr2 /= 2 * eps; dt3_dr2 /= 2 * eps; }