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;
-}
-