void PoseRT::computeDistance(const PoseRT &pose1, const PoseRT &pose2, double &rotationDistance, double &translationDistance, const cv::Mat &Rt_obj2cam)
{
Mat Rt_diff_cam = pose1.getProjectiveMatrix() * pose2.getProjectiveMatrix().inv(DECOMP_SVD);
Mat Rt_diff_obj = Rt_diff_cam;
if (!Rt_obj2cam.empty())
{
Rt_diff_obj = Rt_obj2cam.inv(DECOMP_SVD) * Rt_diff_cam * Rt_obj2cam;
}
Mat rvec_diff_obj, tvec_diff_obj;
getRvecTvec(Rt_diff_obj, rvec_diff_obj, tvec_diff_obj);
rotationDistance = norm(rvec_diff_obj);
translationDistance = norm(tvec_diff_obj);
}
void evaluatePose(const EdgeModel &testEdgeModel, const cv::Mat &rvec_est_cam, const cv::Mat &tvec_est_cam, const PoseRT &ground_cam, string prefix = "")
{
double distance = 0;
std::cout << prefix + "Hausdorff distance: " << distance << endl;
Mat Rt_est_cam2test;
createProjectiveMatrix(rvec_est_cam, tvec_est_cam, Rt_est_cam2test);
Mat Rt_diff_cam = ground_cam.getProjectiveMatrix() * Rt_est_cam2test.inv(DECOMP_SVD);
Mat Rt_obj2cam = testEdgeModel.Rt_obj2cam;
Mat Rt_diff_obj = Rt_obj2cam.inv(DECOMP_SVD) * Rt_diff_cam * Rt_obj2cam;
Mat rvec_diff_obj, tvec_diff_obj;
getRvecTvec(Rt_diff_obj, rvec_diff_obj, tvec_diff_obj);
double hartleyDiff = norm(rvec_diff_obj);
const double radians2degrees = 180.0 / CV_PI;
cout << "Hartley diff (deg): " << radians2degrees * hartleyDiff << endl;
cout << "Angle diff (deg): " << 0 << endl;
cout << "Normal diff (deg): " << 0 << endl;
cout << "Tvec diff: " << norm(tvec_diff_obj) << endl;
}