Ejemplo n.º 1
0
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());
}