diff --git a/src/processor/ippe.cpp b/src/processor/ippe.cpp deleted file mode 100644 index abbd4c4f8653ffbdde69192094151ef6afb53adc..0000000000000000000000000000000000000000 --- a/src/processor/ippe.cpp +++ /dev/null @@ -1,1050 +0,0 @@ -#include "base/processor/ippe.h" -#include <opencv2/imgproc.hpp> - -#include <iostream> - -using namespace cv; - -IPPE::PoseSolver::PoseSolver() -{ - -} - -IPPE::PoseSolver::~PoseSolver() -{ - -} - -void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, - cv::OutputArray _rvec1, cv::OutputArray _tvec1, float& err1, cv::OutputArray _rvec2, cv::OutputArray _tvec2, float& err2) -{ - cv::Mat normalizedImagePoints; //undistored version of imagePoints - - if (_cameraMatrix.empty()) { - //there is no camera matrix and image points are given in normalized pixel coordinates. - _imagePoints.copyTo(normalizedImagePoints); - } - else { - //undistort the image points (i.e. put them in normalized pixel coordinates): - cv::undistortPoints(_imagePoints, normalizedImagePoints, _cameraMatrix, _distCoeffs); - } - - //solve: - cv::Mat Ma, Mb; - solveGeneric(_objectPoints, normalizedImagePoints, Ma, Mb); - - //the two poses computed by IPPE (sorted): - cv::Mat M1, M2; - - //sort poses by reprojection error: - sortPosesByReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2); - - //fill outputs - rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); - rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); - - M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); - M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); -} - -void IPPE::PoseSolver::solveGeneric(cv::InputArray _objectPoints, cv::InputArray _normalizedInputPoints, - cv::OutputArray _Ma, cv::OutputArray _Mb) -{ - - //argument checking: - size_t n = _objectPoints.rows() * _objectPoints.cols(); //number of points - int objType = _objectPoints.type(); - int type_input = _normalizedInputPoints.type(); - assert((objType == CV_32FC3) | (objType == CV_64FC3)); - assert((type_input == CV_32FC2) | (type_input == CV_64FC2)); - assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1)); - assert((_objectPoints.rows() >= 4) | (_objectPoints.cols() >= 4)); - assert((_normalizedInputPoints.rows() == 1) | (_normalizedInputPoints.cols() == 1)); - assert(static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols()) == n); - - cv::Mat normalizedInputPoints; - if (type_input == CV_32FC2) { - _normalizedInputPoints.getMat().convertTo(normalizedInputPoints, CV_64FC2); - } - else { - normalizedInputPoints = _normalizedInputPoints.getMat(); - } - - cv::Mat objectInputPoints; - if (type_input == CV_32FC3) { - _objectPoints.getMat().convertTo(objectInputPoints, CV_64FC3); - } - else { - objectInputPoints = _objectPoints.getMat(); - } - - cv::Mat canonicalObjPoints; - cv::Mat MmodelPoints2Canonical; - - //transform object points to the canonical position (zero centred and on the plane z=0): - makeCanonicalObjectPoints(objectInputPoints, canonicalObjPoints, MmodelPoints2Canonical); - - //compute the homography mapping the model's points to normalizedInputPoints - cv::Mat H; - HomographyHO::homographyHO(canonicalObjPoints, _normalizedInputPoints, H); - - //now solve - cv::Mat MaCanon, MbCanon; - solveCanonicalForm(canonicalObjPoints, normalizedInputPoints, H, MaCanon, MbCanon); - - //transform computed poses to account for canonical transform: - cv::Mat Ma = MaCanon * MmodelPoints2Canonical; - cv::Mat Mb = MbCanon * MmodelPoints2Canonical; - - //output poses: - Ma.copyTo(_Ma); - Mb.copyTo(_Mb); -} - -void IPPE::PoseSolver::solveCanonicalForm(cv::InputArray _canonicalObjPoints, cv::InputArray _normalizedInputPoints, cv::InputArray _H, - cv::OutputArray _Ma, cv::OutputArray _Mb) -{ - - _Ma.create(4, 4, CV_64FC1); - _Mb.create(4, 4, CV_64FC1); - - cv::Mat Ma = _Ma.getMat(); - cv::Mat Mb = _Mb.getMat(); - cv::Mat H = _H.getMat(); - - //initialise poses: - Ma.setTo(0); - Ma.at<double>(3, 3) = 1; - Mb.setTo(0); - Mb.at<double>(3, 3) = 1; - - //Compute the Jacobian J of the homography at (0,0): - double j00, j01, j10, j11, v0, v1; - - j00 = H.at<double>(0, 0) - H.at<double>(2, 0) * H.at<double>(0, 2); - j01 = H.at<double>(0, 1) - H.at<double>(2, 1) * H.at<double>(0, 2); - j10 = H.at<double>(1, 0) - H.at<double>(2, 0) * H.at<double>(1, 2); - j11 = H.at<double>(1, 1) - H.at<double>(2, 1) * H.at<double>(1, 2); - - //Compute the transformation of (0,0) into the image: - v0 = H.at<double>(0, 2); - v1 = H.at<double>(1, 2); - - //compute the two rotation solutions: - cv::Mat Ra = Ma.colRange(0, 3).rowRange(0, 3); - cv::Mat Rb = Mb.colRange(0, 3).rowRange(0, 3); - computeRotations(j00, j01, j10, j11, v0, v1, Ra, Rb); - - //for each rotation solution, compute the corresponding translation solution: - cv::Mat ta = Ma.colRange(3, 4).rowRange(0, 3); - cv::Mat tb = Mb.colRange(3, 4).rowRange(0, 3); - computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Ra, ta); - computeTranslation(_canonicalObjPoints, _normalizedInputPoints, Rb, tb); -} - -void IPPE::PoseSolver::solveSquare(float squareLength, InputArray _imagePoints, InputArray _cameraMatrix, InputArray _distCoeffs, - OutputArray _rvec1, OutputArray _tvec1, float& err1, OutputArray _rvec2, OutputArray _tvec2, float& err2) -{ - - //allocate outputs: - _rvec1.create(3, 1, CV_64FC1); - _tvec1.create(3, 1, CV_64FC1); - _rvec2.create(3, 1, CV_64FC1); - _tvec2.create(3, 1, CV_64FC1); - - cv::Mat normalizedInputPoints; //undistored version of imagePoints - cv::Mat objectPoints2D; - - //generate the object points: - generateSquareObjectCorners2D(squareLength, objectPoints2D); - - - cv::Mat H; //homography from canonical object points to normalized pixels - - - if (_cameraMatrix.empty()) { - //this means imagePoints are defined in normalized pixel coordinates, so just copy it: - _imagePoints.copyTo(normalizedInputPoints); - } - else { - //undistort the image points (i.e. put them in normalized pixel coordinates). - cv::undistortPoints(_imagePoints, normalizedInputPoints, _cameraMatrix, _distCoeffs); - } - - //compute H - homographyFromSquarePoints(normalizedInputPoints, squareLength / 2.0f, H); - - //now solve - cv::Mat Ma, Mb; - solveCanonicalForm(objectPoints2D, normalizedInputPoints, H, Ma, Mb); - - //sort poses according to reprojection error: - cv::Mat M1, M2; - cv::Mat objectPoints3D; - generateSquareObjectCorners3D(squareLength, objectPoints3D); - sortPosesByReprojError(objectPoints3D, _imagePoints, _cameraMatrix, _distCoeffs, Ma, Mb, M1, M2, err1, err2); - - //fill outputs - rot2vec(M1.colRange(0, 3).rowRange(0, 3), _rvec1); - rot2vec(M2.colRange(0, 3).rowRange(0, 3), _rvec2); - - M1.colRange(3, 4).rowRange(0, 3).copyTo(_tvec1); - M2.colRange(3, 4).rowRange(0, 3).copyTo(_tvec2); -} - -void IPPE::PoseSolver::generateSquareObjectCorners3D(double squareLength, OutputArray _objectPoints) -{ - _objectPoints.create(1, 4, CV_64FC3); - cv::Mat objectPoints = _objectPoints.getMat(); - objectPoints.ptr<Vec3d>(0)[0] = Vec3d(-squareLength / 2.0, squareLength / 2.0, 0.0); - objectPoints.ptr<Vec3d>(0)[1] = Vec3d(squareLength / 2.0, squareLength / 2.0, 0.0); - objectPoints.ptr<Vec3d>(0)[2] = Vec3d(squareLength / 2.0, -squareLength / 2.0, 0.0); - objectPoints.ptr<Vec3d>(0)[3] = Vec3d(-squareLength / 2.0, -squareLength / 2.0, 0.0); -} - -void IPPE::PoseSolver::generateSquareObjectCorners2D(double squareLength, OutputArray _objectPoints) -{ - _objectPoints.create(1, 4, CV_64FC2); - cv::Mat objectPoints = _objectPoints.getMat(); - objectPoints.ptr<Vec2d>(0)[0] = Vec2d(-squareLength / 2.0, squareLength / 2.0); - objectPoints.ptr<Vec2d>(0)[1] = Vec2d(squareLength / 2.0, squareLength / 2.0); - objectPoints.ptr<Vec2d>(0)[2] = Vec2d(squareLength / 2.0, -squareLength / 2.0); - objectPoints.ptr<Vec2d>(0)[3] = Vec2d(-squareLength / 2.0, -squareLength / 2.0); -} - -double IPPE::PoseSolver::meanSceneDepth(InputArray _objectPoints, InputArray _rvec, InputArray _tvec) -{ - assert((_objectPoints.type() == CV_64FC3) | (_objectPoints.type() == CV_64FC3)); - - size_t n = _objectPoints.rows() * _objectPoints.cols(); - Mat R; - Mat q; - Rodrigues(_rvec, R); - double zBar = 0; - - for (size_t i = 0; i < n; i++) { - cv::Mat p(_objectPoints.getMat().at<Point3d>(i)); - q = R * p + _tvec.getMat(); - double z; - if (q.depth() == CV_64FC1) { - z = q.at<double>(2); - } - else { - z = static_cast<double>(q.at<float>(2)); - } - zBar += z; - - //if (z <= 0) { - // std::cout << "Warning: object point " << i << " projects behind the camera! This should not be allowed. " << std::endl; - //} - } - return zBar / static_cast<double>(n); -} - -void IPPE::PoseSolver::rot2vec(InputArray _R, OutputArray _r) -{ - assert(_R.type() == CV_64FC1); - assert(_R.rows() == 3); - assert(_R.cols() == 3); - - _r.create(3, 1, CV_64FC1); - - cv::Mat R = _R.getMat(); - cv::Mat rvec = _r.getMat(); - - double trace = R.at<double>(0, 0) + R.at<double>(1, 1) + R.at<double>(2, 2); - double w_norm = acos((trace - 1.0) / 2.0); - double c0, c1, c2; - double eps = std::numeric_limits<float>::epsilon(); - double d = 1 / (2 * sin(w_norm)) * w_norm; - if (w_norm < eps) //rotation is the identity - { - rvec.setTo(0); - } - else { - c0 = R.at<double>(2, 1) - R.at<double>(1, 2); - c1 = R.at<double>(0, 2) - R.at<double>(2, 0); - c2 = R.at<double>(1, 0) - R.at<double>(0, 1); - rvec.at<double>(0) = d * c0; - rvec.at<double>(1) = d * c1; - rvec.at<double>(2) = d * c2; - } -} - -void IPPE::PoseSolver::computeTranslation(InputArray _objectPoints, InputArray _normalizedImgPoints, InputArray _R, OutputArray _t) -{ - //This is solved by building the linear system At = b, where t corresponds to the (unknown) translation. - //This is then inverted with the associated normal equations to give t = inv(transpose(A)*A)*transpose(A)*b - //For efficiency we only store the coefficients of (transpose(A)*A) and (transpose(A)*b) - - assert(_objectPoints.type() == CV_64FC2); - assert(_normalizedImgPoints.type() == CV_64FC2); - assert(_R.type() == CV_64FC1); - - assert((_R.rows() == 3) & (_R.cols() == 3)); - assert((_objectPoints.rows() == 1) | (_objectPoints.cols() == 1)); - assert((_normalizedImgPoints.rows() == 1) | (_normalizedImgPoints.cols() == 1)); - size_t n = _normalizedImgPoints.rows() * _normalizedImgPoints.cols(); - assert(n == static_cast<size_t>(_objectPoints.rows() * _objectPoints.cols())); - - cv::Mat objectPoints = _objectPoints.getMat(); - cv::Mat imgPoints = _normalizedImgPoints.getMat(); - - _t.create(3, 1, CV_64FC1); - - cv::Mat R = _R.getMat(); - - //coefficients of (transpose(A)*A) - double ATA00 = n; - double ATA02 = 0; - double ATA11 = n; - double ATA12 = 0; - double ATA20 = 0; - double ATA21 = 0; - double ATA22 = 0; - - //coefficients of (transpose(A)*b) - double ATb0 = 0; - double ATb1 = 0; - double ATb2 = 0; - - //S gives inv(transpose(A)*A)/det(A)^2 - double S00, S01, S02; - double S10, S11, S12; - double S20, S21, S22; - - double rx, ry, rz; - double a2; - double b2; - double bx, by; - - //now loop through each point and increment the coefficients: - for (size_t i = 0; i < n; i++) { - rx = R.at<double>(0, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(0, 1) * objectPoints.at<Vec2d>(i)(1); - ry = R.at<double>(1, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(1, 1) * objectPoints.at<Vec2d>(i)(1); - rz = R.at<double>(2, 0) * objectPoints.at<Vec2d>(i)(0) + R.at<double>(2, 1) * objectPoints.at<Vec2d>(i)(1); - - a2 = -imgPoints.at<Vec2d>(i)(0); - b2 = -imgPoints.at<Vec2d>(i)(1); - - ATA02 = ATA02 + a2; - ATA12 = ATA12 + b2; - ATA20 = ATA20 + a2; - ATA21 = ATA21 + b2; - ATA22 = ATA22 + a2 * a2 + b2 * b2; - - bx = -a2 * rz - rx; - by = -b2 * rz - ry; - - ATb0 = ATb0 + bx; - ATb1 = ATb1 + by; - ATb2 = ATb2 + a2 * bx + b2 * by; - } - - double detAInv = 1.0 / (ATA00 * ATA11 * ATA22 - ATA00 * ATA12 * ATA21 - ATA02 * ATA11 * ATA20); - - //construct S: - S00 = ATA11 * ATA22 - ATA12 * ATA21; - S01 = ATA02 * ATA21; - S02 = -ATA02 * ATA11; - S10 = ATA12 * ATA20; - S11 = ATA00 * ATA22 - ATA02 * ATA20; - S12 = -ATA00 * ATA12; - S20 = -ATA11 * ATA20; - S21 = -ATA00 * ATA21; - S22 = ATA00 * ATA11; - - //solve t: - Mat t = _t.getMat(); - t.at<double>(0) = detAInv * (S00 * ATb0 + S01 * ATb1 + S02 * ATb2); - t.at<double>(1) = detAInv * (S10 * ATb0 + S11 * ATb1 + S12 * ATb2); - t.at<double>(2) = detAInv * (S20 * ATb0 + S21 * ATb1 + S22 * ATb2); -} - -void IPPE::PoseSolver::computeRotations(double j00, double j01, double j10, double j11, double p, double q, OutputArray _R1, OutputArray _R2) -{ - //This is fairly optimized code which makes it hard to understand. The matlab code is certainly easier to read. - _R1.create(3, 3, CV_64FC1); - _R2.create(3, 3, CV_64FC1); - - double a00, a01, a10, a11, ata00, ata01, ata11, b00, b01, b10, b11, binv00, binv01, binv10, binv11; - //double rv00, rv01, rv02, rv10, rv11, rv12, rv20, rv21, rv22; - double rtilde00, rtilde01, rtilde10, rtilde11; - double rtilde00_2, rtilde01_2, rtilde10_2, rtilde11_2; - double b0, b1, gamma, dtinv; - double sp; - - Mat Rv; - cv::Mat v(3,1,CV_64FC1); - v.at<double>(0) = p; - v.at<double>(1) = q; - v.at<double>(2) = 1; - rotateVec2ZAxis(v,Rv); - Rv = Rv.t(); - - - //setup the 2x2 SVD decomposition: - double rv00, rv01, rv02; - double rv10, rv11, rv12; - double rv20, rv21, rv22; - rv00 = Rv.at<double>(0,0); - rv01 = Rv.at<double>(0,1); - rv02 = Rv.at<double>(0,2); - - rv10 = Rv.at<double>(1,0); - rv11 = Rv.at<double>(1,1); - rv12 = Rv.at<double>(1,2); - - rv20 = Rv.at<double>(2,0); - rv21 = Rv.at<double>(2,1); - rv22 = Rv.at<double>(2,2); - - b00 = rv00 - p * rv20; - b01 = rv01 - p * rv21; - b10 = rv10 - q * rv20; - b11 = rv11 - q * rv21; - - dtinv = 1.0 / ((b00 * b11 - b01 * b10)); - - binv00 = dtinv * b11; - binv01 = -dtinv * b01; - binv10 = -dtinv * b10; - binv11 = dtinv * b00; - - a00 = binv00 * j00 + binv01 * j10; - a01 = binv00 * j01 + binv01 * j11; - a10 = binv10 * j00 + binv11 * j10; - a11 = binv10 * j01 + binv11 * j11; - - //compute the largest singular value of A: - ata00 = a00 * a00 + a01 * a01; - ata01 = a00 * a10 + a01 * a11; - ata11 = a10 * a10 + a11 * a11; - - gamma = sqrt(0.5 * (ata00 + ata11 + sqrt((ata00 - ata11) * (ata00 - ata11) + 4.0 * ata01 * ata01))); - - //reconstruct the full rotation matrices: - rtilde00 = a00 / gamma; - rtilde01 = a01 / gamma; - rtilde10 = a10 / gamma; - rtilde11 = a11 / gamma; - - rtilde00_2 = rtilde00 * rtilde00; - rtilde01_2 = rtilde01 * rtilde01; - rtilde10_2 = rtilde10 * rtilde10; - rtilde11_2 = rtilde11 * rtilde11; - - b0 = sqrt(-rtilde00_2 - rtilde10_2 + 1); - b1 = sqrt(-rtilde01_2 - rtilde11_2 + 1); - sp = (-rtilde00 * rtilde01 - rtilde10 * rtilde11); - - if (sp < 0) { - b1 = -b1; - } - - //store results: - Mat R1 = _R1.getMat(); - Mat R2 = _R2.getMat(); - - R1.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (b0)*rv02; - R1.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (b1)*rv02; - R1.at<double>(0, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv00 + (b0 * rtilde01 - b1 * rtilde00) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; - R1.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (b0)*rv12; - R1.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (b1)*rv12; - R1.at<double>(1, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv10 + (b0 * rtilde01 - b1 * rtilde00) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; - R1.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (b0)*rv22; - R1.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (b1)*rv22; - R1.at<double>(2, 2) = (b1 * rtilde10 - b0 * rtilde11) * rv20 + (b0 * rtilde01 - b1 * rtilde00) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; - - R2.at<double>(0, 0) = (rtilde00)*rv00 + (rtilde10)*rv01 + (-b0) * rv02; - R2.at<double>(0, 1) = (rtilde01)*rv00 + (rtilde11)*rv01 + (-b1) * rv02; - R2.at<double>(0, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv00 + (b1 * rtilde00 - b0 * rtilde01) * rv01 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv02; - R2.at<double>(1, 0) = (rtilde00)*rv10 + (rtilde10)*rv11 + (-b0) * rv12; - R2.at<double>(1, 1) = (rtilde01)*rv10 + (rtilde11)*rv11 + (-b1) * rv12; - R2.at<double>(1, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv10 + (b1 * rtilde00 - b0 * rtilde01) * rv11 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv12; - R2.at<double>(2, 0) = (rtilde00)*rv20 + (rtilde10)*rv21 + (-b0) * rv22; - R2.at<double>(2, 1) = (rtilde01)*rv20 + (rtilde11)*rv21 + (-b1) * rv22; - R2.at<double>(2, 2) = (b0 * rtilde11 - b1 * rtilde10) * rv20 + (b1 * rtilde00 - b0 * rtilde01) * rv21 + (rtilde00 * rtilde11 - rtilde01 * rtilde10) * rv22; -} - - -void IPPE::PoseSolver::homographyFromSquarePoints(InputArray _targetPoints, double halfLength, OutputArray H_) -{ - - assert((_targetPoints.type() == CV_32FC2) | (_targetPoints.type() == CV_64FC2)); - - cv::Mat pts = _targetPoints.getMat(); - H_.create(3, 3, CV_64FC1); - Mat H = H_.getMat(); - - double p1x, p1y; - double p2x, p2y; - double p3x, p3y; - double p4x, p4y; - - if (_targetPoints.type() == CV_32FC2) { - p1x = -pts.at<Vec2f>(0)(0); - p1y = -pts.at<Vec2f>(0)(1); - - p2x = -pts.at<Vec2f>(1)(0); - p2y = -pts.at<Vec2f>(1)(1); - - p3x = -pts.at<Vec2f>(2)(0); - p3y = -pts.at<Vec2f>(2)(1); - - p4x = -pts.at<Vec2f>(3)(0); - p4y = -pts.at<Vec2f>(3)(1); - } - else { - p1x = -pts.at<Vec2d>(0)(0); - p1y = -pts.at<Vec2d>(0)(1); - - p2x = -pts.at<Vec2d>(1)(0); - p2y = -pts.at<Vec2d>(1)(1); - - p3x = -pts.at<Vec2d>(2)(0); - p3y = -pts.at<Vec2d>(2)(1); - - p4x = -pts.at<Vec2d>(3)(0); - p4y = -pts.at<Vec2d>(3)(1); - } - - //analytic solution: - double detsInv = -1 / (halfLength * (p1x * p2y - p2x * p1y - p1x * p4y + p2x * p3y - p3x * p2y + p4x * p1y + p3x * p4y - p4x * p3y)); - - H.at<double>(0, 0) = detsInv * (p1x * p3x * p2y - p2x * p3x * p1y - p1x * p4x * p2y + p2x * p4x * p1y - p1x * p3x * p4y + p1x * p4x * p3y + p2x * p3x * p4y - p2x * p4x * p3y); - H.at<double>(0, 1) = detsInv * (p1x * p2x * p3y - p1x * p3x * p2y - p1x * p2x * p4y + p2x * p4x * p1y + p1x * p3x * p4y - p3x * p4x * p1y - p2x * p4x * p3y + p3x * p4x * p2y); - H.at<double>(0, 2) = detsInv * halfLength * (p1x * p2x * p3y - p2x * p3x * p1y - p1x * p2x * p4y + p1x * p4x * p2y - p1x * p4x * p3y + p3x * p4x * p1y + p2x * p3x * p4y - p3x * p4x * p2y); - H.at<double>(1, 0) = detsInv * (p1x * p2y * p3y - p2x * p1y * p3y - p1x * p2y * p4y + p2x * p1y * p4y - p3x * p1y * p4y + p4x * p1y * p3y + p3x * p2y * p4y - p4x * p2y * p3y); - H.at<double>(1, 1) = detsInv * (p2x * p1y * p3y - p3x * p1y * p2y - p1x * p2y * p4y + p4x * p1y * p2y + p1x * p3y * p4y - p4x * p1y * p3y - p2x * p3y * p4y + p3x * p2y * p4y); - H.at<double>(1, 2) = detsInv * halfLength * (p1x * p2y * p3y - p3x * p1y * p2y - p2x * p1y * p4y + p4x * p1y * p2y - p1x * p3y * p4y + p3x * p1y * p4y + p2x * p3y * p4y - p4x * p2y * p3y); - H.at<double>(2, 0) = -detsInv * (p1x * p3y - p3x * p1y - p1x * p4y - p2x * p3y + p3x * p2y + p4x * p1y + p2x * p4y - p4x * p2y); - H.at<double>(2, 1) = detsInv * (p1x * p2y - p2x * p1y - p1x * p3y + p3x * p1y + p2x * p4y - p4x * p2y - p3x * p4y + p4x * p3y); - H.at<double>(2, 2) = 1.0; -} - -void IPPE::PoseSolver::makeCanonicalObjectPoints(InputArray _objectPoints, OutputArray _canonicalObjPoints, OutputArray _MmodelPoints2Canonical) -{ - - int objType = _objectPoints.type(); - assert((objType == CV_64FC3) | (objType == CV_32FC3)); - - size_t n = _objectPoints.rows() * _objectPoints.cols(); - - _canonicalObjPoints.create(1, n, CV_64FC2); - _MmodelPoints2Canonical.create(4, 4, CV_64FC1); - - cv::Mat objectPoints = _objectPoints.getMat(); - cv::Mat canonicalObjPoints = _canonicalObjPoints.getMat(); - - cv::Mat UZero(3, n, CV_64FC1); - - double xBar = 0; - double yBar = 0; - double zBar = 0; - bool isOnZPlane = true; - for (size_t i = 0; i < n; i++) { - double x, y, z; - if (objType == CV_32FC3) { - x = static_cast<double>(objectPoints.at<Vec3f>(i)[0]); - y = static_cast<double>(objectPoints.at<Vec3f>(i)[1]); - z = static_cast<double>(objectPoints.at<Vec3f>(i)[2]); - } - else { - x = objectPoints.at<Vec3d>(i)[0]; - y = objectPoints.at<Vec3d>(i)[1]; - z = objectPoints.at<Vec3d>(i)[2]; - - if (abs(z) > IPPE_SMALL) { - isOnZPlane = false; - } - } - xBar += x; - yBar += y; - zBar += z; - - UZero.at<double>(0, i) = x; - UZero.at<double>(1, i) = y; - UZero.at<double>(2, i) = z; - } - xBar = xBar / (double)n; - yBar = yBar / (double)n; - zBar = zBar / (double)n; - - for (size_t i = 0; i < n; i++) { - UZero.at<double>(0, i) -= xBar; - UZero.at<double>(1, i) -= yBar; - UZero.at<double>(2, i) -= zBar; - } - - cv::Mat MCenter(4, 4, CV_64FC1); - MCenter.setTo(0); - MCenter.at<double>(0, 0) = 1; - MCenter.at<double>(1, 1) = 1; - MCenter.at<double>(2, 2) = 1; - MCenter.at<double>(3, 3) = 1; - - MCenter.at<double>(0, 3) = -xBar; - MCenter.at<double>(1, 3) = -yBar; - MCenter.at<double>(2, 3) = -zBar; - - if (isOnZPlane) { - //MmodelPoints2Canonical is given by MCenter - MCenter.copyTo(_MmodelPoints2Canonical); - for (size_t i = 0; i < n; i++) { - canonicalObjPoints.at<Vec2d>(i)[0] = UZero.at<double>(0, i); - canonicalObjPoints.at<Vec2d>(i)[1] = UZero.at<double>(1, i); - } - } - else { - cv::Mat UZeroAligned(3, n, CV_64FC1); - cv::Mat R; //rotation that rotates objectPoints to the plane z=0 - - if (!computeObjextSpaceR3Pts(objectPoints,R)) - { - //we could not compute R, problably because there is a duplicate point in {objectPoints(0),objectPoints(1),objectPoints(2)}. So we compute it with the SVD (which is slower): - computeObjextSpaceRSvD(UZero,R); - } - - UZeroAligned = R * UZero; - - for (size_t i = 0; i < n; i++) { - canonicalObjPoints.at<Vec2d>(i)[0] = UZeroAligned.at<double>(0, i); - canonicalObjPoints.at<Vec2d>(i)[1] = UZeroAligned.at<double>(1, i); - assert(abs(UZeroAligned.at<double>(2, i))<=IPPE_SMALL); - } - - cv::Mat MRot(4, 4, CV_64FC1); - MRot.setTo(0); - MRot.at<double>(3, 3) = 1; - - R.copyTo(MRot.colRange(0, 3).rowRange(0, 3)); - cv::Mat Mb = MRot * MCenter; - Mb.copyTo(_MmodelPoints2Canonical); - } -} - -void IPPE::PoseSolver::evalReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _M, float& err) -{ - cv::Mat projectedPoints; - cv::Mat imagePoints = _imagePoints.getMat(); - cv::Mat r; - rot2vec(_M.getMat().colRange(0, 3).rowRange(0, 3), r); - - if (_cameraMatrix.empty()) { - //there is no camera matrix and image points are in normalized pixel coordinates - cv::Mat K(3, 3, CV_64FC1); - K.setTo(0); - K.at<double>(0, 0) = 1; - K.at<double>(1, 1) = 1; - K.at<double>(2, 2) = 1; - cv::Mat kc; - cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), K, kc, projectedPoints); - } - else { - cv::projectPoints(_objectPoints, r, _M.getMat().colRange(3, 4).rowRange(0, 3), _cameraMatrix, _distCoeffs, projectedPoints); - } - - err = 0; - size_t n = _objectPoints.rows() * _objectPoints.cols(); - - float dx, dy; - for (size_t i = 0; i < n; i++) { - if (projectedPoints.depth() == CV_32FC1) { - dx = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0]; -// dy = projectedPoints.at<Vec2f>(i)[0] - imagePoints.at<Vec2f>(i)[0]; - dy = projectedPoints.at<Vec2f>(i)[1] - imagePoints.at<Vec2f>(i)[1]; - } - else { - dx = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0]; -// dy = projectedPoints.at<Vec2d>(i)[0] - imagePoints.at<Vec2d>(i)[0]; - dy = projectedPoints.at<Vec2d>(i)[1] - imagePoints.at<Vec2d>(i)[1]; - } - - err += dx * dx + dy * dy; - } - err = sqrt(err / (2.0f * n)); -} - -void IPPE::PoseSolver::sortPosesByReprojError(cv::InputArray _objectPoints, cv::InputArray _imagePoints, cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs, cv::InputArray _Ma, cv::InputArray _Mb, cv::OutputArray _M1, cv::OutputArray _M2, float& err1, float& err2) -{ - float erra, errb; - evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Ma, erra); - evalReprojError(_objectPoints, _imagePoints, _cameraMatrix, _distCoeffs, _Mb, errb); - if (erra < errb) { - err1 = erra; - _Ma.copyTo(_M1); - - err2 = errb; - _Mb.copyTo(_M2); - } - else { - err1 = errb; - _Mb.copyTo(_M1); - - err2 = erra; - _Ma.copyTo(_M2); - } -} - -void HomographyHO::normalizeDataIsotropic(cv::InputArray _Data, cv::OutputArray _DataN, cv::OutputArray _T, cv::OutputArray _Ti) -{ - cv::Mat Data = _Data.getMat(); - int numPoints = Data.rows * Data.cols; - assert((Data.rows == 1) | (Data.cols == 1)); - assert((Data.channels() == 2) | (Data.channels() == 3)); - assert(numPoints >= 4); - - int dataType = _Data.type(); - assert((dataType == CV_64FC2) | (dataType == CV_64FC3) | (dataType == CV_32FC2) | (dataType == CV_32FC3)); - - _DataN.create(2, numPoints, CV_64FC1); - - _T.create(3, 3, CV_64FC1); - _Ti.create(3, 3, CV_64FC1); - - cv::Mat DataN = _DataN.getMat(); - cv::Mat T = _T.getMat(); - cv::Mat Ti = _Ti.getMat(); - - _T.setTo(0); - _Ti.setTo(0); - - double xm, ym; - int numChannels = Data.channels(); - - xm = 0; - ym = 0; - for (int i = 0; i < numPoints; i++) { - if (numChannels == 2) { - if (dataType == CV_32FC2) { - xm = xm + Data.at<Vec2f>(i)[0]; - ym = ym + Data.at<Vec2f>(i)[1]; - } - else { - xm = xm + Data.at<Vec2d>(i)[0]; - ym = ym + Data.at<Vec2d>(i)[1]; - } - } - else { - if (dataType == CV_32FC3) { - xm = xm + Data.at<Vec3f>(i)[0]; - ym = ym + Data.at<Vec3f>(i)[1]; - } - else { - xm = xm + Data.at<Vec3d>(i)[0]; - ym = ym + Data.at<Vec3d>(i)[1]; - } - } - } - xm = xm / (double)numPoints; - ym = ym / (double)numPoints; - - double kappa = 0; - double xh, yh; - - for (int i = 0; i < numPoints; i++) { - - if (numChannels == 2) { - if (dataType == CV_32FC2) { - xh = Data.at<Vec2f>(i)[0] - xm; - yh = Data.at<Vec2f>(i)[1] - ym; - } - else { - xh = Data.at<Vec2d>(i)[0] - xm; - yh = Data.at<Vec2d>(i)[1] - ym; - } - } - else { - if (dataType == CV_32FC3) { - xh = Data.at<Vec3f>(i)[0] - xm; - yh = Data.at<Vec3f>(i)[1] - ym; - } - else { - xh = Data.at<Vec3d>(i)[0] - xm; - yh = Data.at<Vec3d>(i)[1] - ym; - } - } - - DataN.at<double>(0, i) = xh; - DataN.at<double>(1, i) = yh; - kappa = kappa + xh * xh + yh * yh; - } - double beta = sqrt(2 * numPoints / kappa); - DataN = DataN * beta; - - T.at<double>(0, 0) = 1.0 / beta; - T.at<double>(1, 1) = 1.0 / beta; - - T.at<double>(0, 2) = xm; - T.at<double>(1, 2) = ym; - - T.at<double>(2, 2) = 1; - - Ti.at<double>(0, 0) = beta; - Ti.at<double>(1, 1) = beta; - - Ti.at<double>(0, 2) = -beta * xm; - Ti.at<double>(1, 2) = -beta * ym; - - Ti.at<double>(2, 2) = 1; -} - -void HomographyHO::homographyHO(cv::InputArray _srcPoints, cv::InputArray _targPoints, cv::OutputArray _H) -{ - - _H.create(3, 3, CV_64FC1); - cv::Mat H = _H.getMat(); - - cv::Mat DataA, DataB, TA, TAi, TB, TBi; - - HomographyHO::normalizeDataIsotropic(_srcPoints, DataA, TA, TAi); - HomographyHO::normalizeDataIsotropic(_targPoints, DataB, TB, TBi); - - int n = DataA.cols; - assert(n == DataB.cols); - - cv::Mat C1(1, n, CV_64FC1); - cv::Mat C2(1, n, CV_64FC1); - cv::Mat C3(1, n, CV_64FC1); - cv::Mat C4(1, n, CV_64FC1); - - cv::Mat Mx(n, 3, CV_64FC1); - cv::Mat My(n, 3, CV_64FC1); - - double mC1, mC2, mC3, mC4; - mC1 = 0; - mC2 = 0; - mC3 = 0; - mC4 = 0; - - for (int i = 0; i < n; i++) { - C1.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(0, i); - C2.at<double>(0, i) = -DataB.at<double>(0, i) * DataA.at<double>(1, i); - C3.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(0, i); - C4.at<double>(0, i) = -DataB.at<double>(1, i) * DataA.at<double>(1, i); - - mC1 = mC1 + C1.at<double>(0, i); - mC2 = mC2 + C2.at<double>(0, i); - mC3 = mC3 + C3.at<double>(0, i); - mC4 = mC4 + C4.at<double>(0, i); - } - - mC1 = mC1 / n; - mC2 = mC2 / n; - mC3 = mC3 / n; - mC4 = mC4 / n; - - for (int i = 0; i < n; i++) { - Mx.at<double>(i, 0) = C1.at<double>(0, i) - mC1; - Mx.at<double>(i, 1) = C2.at<double>(0, i) - mC2; - Mx.at<double>(i, 2) = -DataB.at<double>(0, i); - - My.at<double>(i, 0) = C3.at<double>(0, i) - mC3; - My.at<double>(i, 1) = C4.at<double>(0, i) - mC4; - My.at<double>(i, 2) = -DataB.at<double>(1, i); - } - - cv::Mat DataAT, DataADataAT, DataADataATi, Pp, Bx, By, Ex, Ey, D; - - cv::transpose(DataA, DataAT); - DataADataAT = DataA * DataAT; - double dt = DataADataAT.at<double>(0, 0) * DataADataAT.at<double>(1, 1) - DataADataAT.at<double>(0, 1) * DataADataAT.at<double>(1, 0); - - DataADataATi = cv::Mat(2, 2, CV_64FC1); - DataADataATi.at<double>(0, 0) = DataADataAT.at<double>(1, 1) / dt; - DataADataATi.at<double>(0, 1) = -DataADataAT.at<double>(0, 1) / dt; - DataADataATi.at<double>(1, 0) = -DataADataAT.at<double>(1, 0) / dt; - DataADataATi.at<double>(1, 1) = DataADataAT.at<double>(0, 0) / dt; - - Pp = DataADataATi * DataA; - - Bx = Pp * Mx; - By = Pp * My; - - Ex = DataAT * Bx; - Ey = DataAT * By; - - D = cv::Mat(2 * n, 3, CV_64FC1); - cv::Mat DT, DDT; - - for (int i = 0; i < n; i++) { - D.at<double>(i, 0) = Mx.at<double>(i, 0) - Ex.at<double>(i, 0); - D.at<double>(i, 1) = Mx.at<double>(i, 1) - Ex.at<double>(i, 1); - D.at<double>(i, 2) = Mx.at<double>(i, 2) - Ex.at<double>(i, 2); - - D.at<double>(i + n, 0) = My.at<double>(i, 0) - Ey.at<double>(i, 0); - D.at<double>(i + n, 1) = My.at<double>(i, 1) - Ey.at<double>(i, 1); - D.at<double>(i + n, 2) = My.at<double>(i, 2) - Ey.at<double>(i, 2); - } - - cv::transpose(D, DT); - DDT = DT * D; - - cv::Mat S, U, V, h12, h45; - double h3, h6; - - cv::eigen(DDT, S, U); - - cv::Mat h789(3, 1, CV_64FC1); - h789.at<double>(0, 0) = U.at<double>(2, 0); - h789.at<double>(1, 0) = U.at<double>(2, 1); - h789.at<double>(2, 0) = U.at<double>(2, 2); - - h12 = -Bx * h789; - h45 = -By * h789; - - h3 = -(mC1 * h789.at<double>(0, 0) + mC2 * h789.at<double>(1, 0)); - h6 = -(mC3 * h789.at<double>(0, 0) + mC4 * h789.at<double>(1, 0)); - - H.at<double>(0, 0) = h12.at<double>(0, 0); - H.at<double>(0, 1) = h12.at<double>(1, 0); - H.at<double>(0, 2) = h3; - - H.at<double>(1, 0) = h45.at<double>(0, 0); - H.at<double>(1, 1) = h45.at<double>(1, 0); - H.at<double>(1, 2) = h6; - - H.at<double>(2, 0) = h789.at<double>(0, 0); - H.at<double>(2, 1) = h789.at<double>(1, 0); - H.at<double>(2, 2) = h789.at<double>(2, 0); - - H = TB * H * TAi; - H = H / H.at<double>(2, 2); -} - - -void IPPE::PoseSolver::rotateVec2ZAxis(InputArray _a, OutputArray _Ra) -{ - _Ra.create(3,3,CV_64FC1); - Mat Ra = _Ra.getMat(); - - double ax = _a.getMat().at<double>(0); - double ay = _a.getMat().at<double>(1); - double az = _a.getMat().at<double>(2); - - double nrm = sqrt(ax*ax + ay*ay + az*az); - ax = ax/nrm; - ay = ay/nrm; - az = az/nrm; - - double c = az; - - if (abs(1.0+c)< std::numeric_limits<float>::epsilon()) - { - Ra.setTo(0.0); - Ra.at<double>(0,0) = 1.0; - Ra.at<double>(1,1) = 1.0; - Ra.at<double>(2,2) = -1.0; - } - else - { - double d = 1.0/(1.0+c); - double ax2 = ax*ax; - double ay2 = ay*ay; - double axay = ax*ay; - - Ra.at<double>(0,0) = - ax2*d + 1.0; - Ra.at<double>(0,1) = -axay*d; - Ra.at<double>(0,2) = -ax; - - Ra.at<double>(1,0) = -axay*d; - Ra.at<double>(1,1) = - ay2*d + 1.0; - Ra.at<double>(1,2) = -ay; - - Ra.at<double>(2,0) = ax; - Ra.at<double>(2,1) = ay; - Ra.at<double>(2,2) = 1.0 - (ax2 + ay2)*d; - } - - -} - -bool IPPE::PoseSolver::computeObjextSpaceR3Pts(InputArray _objectPoints, OutputArray R) -{ - bool ret; //return argument - double p1x,p1y,p1z; - double p2x,p2y,p2z; - double p3x,p3y,p3z; - - cv::Mat objectPoints = _objectPoints.getMat(); -// size_t n = objectPoints.rows*objectPoints.cols; - if (objectPoints.type() == CV_32FC3) - { - p1x = objectPoints.at<Vec3f>(0)[0]; - p1y = objectPoints.at<Vec3f>(0)[1]; - p1z = objectPoints.at<Vec3f>(0)[2]; - - p2x = objectPoints.at<Vec3f>(1)[0]; - p2y = objectPoints.at<Vec3f>(1)[1]; - p2z = objectPoints.at<Vec3f>(1)[2]; - - p3x = objectPoints.at<Vec3f>(2)[0]; - p3y = objectPoints.at<Vec3f>(2)[1]; - p3z = objectPoints.at<Vec3f>(2)[2]; - } - else - { - p1x = objectPoints.at<Vec3d>(0)[0]; - p1y = objectPoints.at<Vec3d>(0)[1]; - p1z = objectPoints.at<Vec3d>(0)[2]; - - p2x = objectPoints.at<Vec3d>(1)[0]; - p2y = objectPoints.at<Vec3d>(1)[1]; - p2z = objectPoints.at<Vec3d>(1)[2]; - - p3x = objectPoints.at<Vec3d>(2)[0]; - p3y = objectPoints.at<Vec3d>(2)[1]; - p3z = objectPoints.at<Vec3d>(2)[2]; - } - - double nx = (p1y - p2y)*(p1z - p3z) - (p1y - p3y)*(p1z - p2z); - double ny = (p1x - p3x)*(p1z - p2z) - (p1x - p2x)*(p1z - p3z); - double nz = (p1x - p2x)*(p1y - p3y) - (p1x - p3x)*(p1y - p2y); - - double nrm = sqrt(nx*nx+ ny*ny + nz*nz); - if (nrm>IPPE_SMALL) - { - nx = nx/nrm; - ny = ny/nrm; - nz = nz/nrm; - cv::Mat v(3,1,CV_64FC1); - v.at<double>(0) = nx; - v.at<double>(1) = ny; - v.at<double>(2) = nz; - rotateVec2ZAxis(v,R); - ret = true; - } - else - { - ret = false; - } - return ret; -} - -bool IPPE::PoseSolver::computeObjextSpaceRSvD(InputArray _objectPointsZeroMean, OutputArray _R) -{ - bool ret; //return argument - _R.create(3,3,CV_64FC1); - cv::Mat R = _R.getMat(); - - //we could not compute R with the first three points, so lets use the SVD - cv::SVD s; - cv::Mat W, U, VT; - s.compute(_objectPointsZeroMean.getMat() * _objectPointsZeroMean.getMat().t(), W, U, VT); - double s3 = W.at<double>(2); - double s2 = W.at<double>(1); - - //check if points are coplanar: - assert(s3 / s2 < IPPE_SMALL); - - R = U.t(); - if (cv::determinant(R) < 0) { //this ensures R is a rotation matrix and not a general unitary matrix: - R.at<double>(2, 0) = -R.at<double>(2, 0); - R.at<double>(2, 1) = -R.at<double>(2, 1); - R.at<double>(2, 2) = -R.at<double>(2, 2); - } - ret = true; - return ret; -} -