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;
}
