void evaluatePoseWithRotation(const EdgeModel &originalEdgeModel, const PoseRT &est_cam, const PoseRT &ground_cam, PoseError &poseError)
{
EdgeModel groundModel, estimatedModel;
originalEdgeModel.rotate_cam(ground_cam, groundModel);
originalEdgeModel.rotate_cam(est_cam, estimatedModel);
const double eps = 1e-4;
CV_Assert(groundModel.hasRotationSymmetry);
CV_Assert(estimatedModel.hasRotationSymmetry);
Point3d tvecPoint = groundModel.getObjectCenter() - estimatedModel.getObjectCenter();
double tvecDiff = norm(tvecPoint);
double cosAngle = groundModel.upStraightDirection.ddot(estimatedModel.upStraightDirection);
cosAngle = std::min(cosAngle, 1.0);
cosAngle = std::max(cosAngle, -1.0);
double hartleyDiff = acos(cosAngle);
PoseRT diff_cam = est_cam * ground_cam.inv();
Mat Rt_diff_obj = groundModel.Rt_obj2cam.inv(DECOMP_SVD) * diff_cam.getProjectiveMatrix() * groundModel.Rt_obj2cam;
Mat rvec_diff_obj, tvec_diff_obj;
getRvecTvec(Rt_diff_obj, rvec_diff_obj, tvec_diff_obj);
/*
point2col(tvecPoint, *tvec);
Point3d rvecPoint = estimatedModel.rotationAxis.cross(groundModel.rotationAxis);
rvecPoint *= hartleyDiff / norm(rvecPoint);
point2col(rvecPoint, *rvec);
*/
poseError.init(PoseRT(rvec_diff_obj, tvec_diff_obj), hartleyDiff, tvecDiff);
/*
Point3d zRvecPoint_obj = estimatedModel.rotationAxis.cross(groundModel.rotationAxis);
CV_Assert(norm(zRvecPoint_obj) > eps);
zRvecPoint_obj *= hartleyDiff / norm(zRvecPoint_obj);
Mat zRvec_obj;
point2col(zRvecPoint_obj, zRvec_obj);
const int dim = 3;
Point3d tvecPoint_obj = groundModel.getObjectCenter() - estimatedModel.getObjectCenter();
zRvec_obj = Mat::zeros(dim, 1, CV_64FC1);
Mat zTvec_obj;
point2col(tvecPoint_obj, zTvec_obj);
//zTvec_obj = Mat::zeros(dim, 1, CV_64FC1);
PoseRT zPose_obj = PoseRT(zRvec_obj, zTvec_obj);
Mat withoutZRotation_Rt = estimatedModel.Rt_obj2cam * zPose_obj.getProjectiveMatrix() * estimatedModel.Rt_obj2cam.inv(DECOMP_SVD) * est_cam.getProjectiveMatrix();
PoseRT withoutZRotationPose = PoseRT(withoutZRotation_Rt);
double xyRotationDiff, xyTranslationDiff;
PoseRT::computeDistance(ground_cam, withoutZRotationPose, xyRotationDiff, xyTranslationDiff, groundModel.Rt_obj2cam);
//PoseRT::computeDistance(ground_cam, withoutZRotationPose, xyRotationDiff, xyTranslationDiff);
cout << "xy: " << xyTranslationDiff << " " << xyRotationDiff * 180.0 / CV_PI<< endl;
*/
}
void PoseRT::computeObjectDistance(const PoseRT &pose1, const PoseRT &pose2, double &rotationDistance, double &translationDistance)
{
PoseRT diff_cam = pose1.inv() * pose2;
rotationDistance = norm(diff_cam.getRvec());
translationDistance = norm(diff_cam.getTvec());
}