cv::Mat toMat(Eigen::Matrix3Xd emat){
double* p = new double[emat.rows()*emat.cols()];
int w = emat.cols();
for (int i=0;i<emat.rows();i++)
for (int j=0;j<w;j++)
p[i*w+j] = emat(i,j);
cv::Mat result(emat.rows(),emat.cols(),CV_64F,p);
return result;
}
TEST(OdometryCalibration, Jacobian) {
Odometry odometry;
odometry.ux = 2.0;
odometry.uy = 3.0;
odometry.utheta = M_PI;
const Eigen::Matrix3Xd jacobian = OdometryCalibration::jacobian(odometry);
ASSERT_EQ(3, (int) jacobian.rows());
ASSERT_EQ(9, (int) jacobian.cols());
ASSERT_DOUBLE_EQ(-2.0, (double) jacobian(0, 0));
ASSERT_DOUBLE_EQ(-3.0, (double) jacobian(0, 1));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(0, 2));
ASSERT_DOUBLE_EQ(0.0, (double) jacobian(0, 3));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(2, 8));
ASSERT_DOUBLE_EQ(-15-3*M_PI, jacobian.sum());
}