TEST(MEKFGyroAccMagTestGroup, vect_measurement_jacobian)
{
Eigen::Quaternionf att(Eigen::AngleAxisf(M_PI/2, Eigen::Vector3f::UnitZ()));
Eigen::Vector3f v_i(0, 1, 0); // expectation of v in inertial frame
// expectation of v is along [0,1,0] in inertial and [1,0,0] in body frame,
// because the body frame is oriented +90deg yaw
k.reset(att);
Eigen::Vector3f v_b = att.conjugate()._transformVector(v_i); // [1,0,0]
Eigen::Matrix3f b_to_m = k.vect_measurement_basis_b_frame(v_i);
//std::cout << std::endl << v_b << std::endl;
Eigen::Matrix<float, 2, 3> Ha = MEKFGyroAccMag::vect_measurement_jacobian(b_to_m, v_b);
Eigen::Matrix<float, 2, 3> Ha_exp;
// sensitivity of z (= last 2 components of v_m) with respect to a:
// with respect to a1 (=roll right) = [0, 0]' (v_b is [1, 0, 0], along roll axis)
// with respect to a2 (=pitch up) = [0, 1]' (v_b goes down (positive))
// with respect to a3 (=yaw right) = [-1, 0]' (v_b goes left (negative))
Ha_exp << 0, 0, -1,
0, 1, 0;
//std::cout << std::endl << Ha << std::endl;
CHECK_TRUE(Ha.isApprox(Ha_exp));
}
bool k_means_tree<Point, K, Data, Lp>::compare_centroids(const Eigen::Matrix<float, rows, dim>& centroids,
const Eigen::Matrix<float, rows, dim>& last_centroids) const
{
return centroids.isApprox(last_centroids, 1e-30f);//1e-30f);
}
// Generate random 3D points
Eigen::Matrix<double, 3, Eigen::Dynamic> points = eight::utils::samplePointsInBox(Eigen::Vector3d(-500, -500, 300), Eigen::Vector3d(500, 500, 1500), nPoints);
// Assume the first camera at origin and the second freely transformed.
Eigen::AffineCompact3d t0;
t0.setIdentity();
Eigen::AffineCompact3d t1;
t1 = Eigen::Translation3d(15.0, 0.0, 3.5) * Eigen::AngleAxisd(0.25*M_PI, Eigen::Vector3d(0.5, -0.3, 0.2).normalized());
// Generate projected image points
Eigen::Matrix<double, 3, 4> cam0 = eight::cameraMatrix(k, t0);
Eigen::Matrix<double, 3, 4> cam1 = eight::cameraMatrix(k, t1);
Eigen::Matrix<double, 2, Eigen::Dynamic> image0 = eight::perspectiveProject(points, cam0).colwise().hnormalized();
Eigen::Matrix<double, 2, Eigen::Dynamic> image1 = eight::perspectiveProject(points, cam1).colwise().hnormalized();
Eigen::Matrix3d F = eight::fundamentalMatrix(image0, image1);
std::cout << F << std::endl;
Eigen::Matrix3d E = eight::essentialMatrix(k, F);
Eigen::Matrix<double, 3, 4> pose = eight::pose(E, k, image0, image1);
std::cout << "Should be: " << std::endl << t1.matrix() << std::endl;
std::cout << "Pose: " << std::endl << pose << std::endl;
// Note: Translation can only be compared up to scale.
Eigen::Matrix<double, 3, 4> tm = t1.matrix();
tm.block<3, 1>(0, 3).normalize();
REQUIRE(pose.isApprox(tm, 1e-3));
}