extrCalibration.cpp

        resultPoint.z = rot_inv.at<double>(2, 0) * (tmpPoint.x) + rot_inv.at<double>(2, 1) * (tmpPoint.y) +
                        rot_inv.at<double>(2, 2) * (tmpPoint.z);

        if(debug)
        {
            std::cout << "#resultPoint: (" << resultPoint.x << ", " << resultPoint.y << ", " << resultPoint.z << ")"
                      << std::endl;
        }
        if(debug)
        {
            std::cout << "Coord Translation: x: " << mControlWidget->getCalibCoord3DTransX()
                      << ", y: " << mControlWidget->getCalibCoord3DTransY()
                      << ", z: " << mControlWidget->getCalibCoord3DTransZ() << std::endl;
        }


        // Coordinate Transformations

        resultPoint.x -= mControlWidget->getCalibCoord3DTransX();
        resultPoint.y -= mControlWidget->getCalibCoord3DTransY();
        resultPoint.z -= mControlWidget->getCalibCoord3DTransZ();

        resultPoint.x *= mControlWidget->getCalibCoord3DSwapX() ? -1 : 1;
        resultPoint.y *= mControlWidget->getCalibCoord3DSwapY() ? -1 : 1;
        resultPoint.z *= mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1;
    }
    else //////////////// End new method
    {
        //////////////// Start old method

        cv::Point3f camInWorld = transformRT(cv::Point3f(0, 0, 0));

        // 3D-Punkt vor der Kamera mit Tiefe 5
        CvPoint3D32f pointBeforeCam;
        pointBeforeCam.x = (p2d.x - mControlWidget->cx->value()) / mControlWidget->fx->value() * 50;
        pointBeforeCam.y = (p2d.y - mControlWidget->cy->value()) / mControlWidget->fy->value() * 50;
        pointBeforeCam.z = 50;
        if(debug)
        {
            std::cout << "Point before Camera: [" << pointBeforeCam.x << ", " << pointBeforeCam.y << ", "
                      << pointBeforeCam.z << "]" << std::endl;
        }
        // 3D-Punkt vor Kamera in Weltkoordinaten
        cv::Point3f pBCInWorld = transformRT(pointBeforeCam);
        if(debug)
        {
            std::cout << "Point before Camera in World-Coordinatesystem: [" << pBCInWorld.x << ", " << pBCInWorld.y
                      << ", " << pBCInWorld.z << "]" << std::endl;
        }
        if(debug)
        {
            std::cout << "Camera in World-Coordinatesystem: [" << camInWorld.x << ", " << camInWorld.y << ", "
                      << camInWorld.z << "]" << std::endl;
        }
        // Berechnung des Richtungsvektors der Gerade von der Kamera durch den Pixel
        // Als Sttzvektor der Geraden wird die Position der Kamera gewhlt
        pBCInWorld.x -= camInWorld.x;
        pBCInWorld.y -= camInWorld.y;
        pBCInWorld.z -= camInWorld.z;
        if(debug)
        {
            std::cout << "G:x = (" << camInWorld.x << " / " << camInWorld.y << " / " << camInWorld.z << ") + lambda ("
                      << pBCInWorld.x << " / " << pBCInWorld.y << " / " << pBCInWorld.z << ")" << std::endl;
        }

        // Berechnung des Schnittpunktes: Hier lambda von der Geraden
        double lambda = (h - camInWorld.z) / (pBCInWorld.z);
        if(debug)
        {
            std::cout << "Lambda: " << lambda << std::endl;
        }

        // Lambda in Gerade einsetzen
        resultPoint.x = (mControlWidget->getCalibCoord3DSwapX() ? -1 : 1) * (camInWorld.x + lambda * pBCInWorld.x);
        resultPoint.y = (mControlWidget->getCalibCoord3DSwapY() ? -1 : 1) * (camInWorld.y + lambda * pBCInWorld.y);
        resultPoint.z = (mControlWidget->getCalibCoord3DSwapZ() ? -1 : 1) * (camInWorld.z + lambda * pBCInWorld.z);

    } //////////////// End old method

    return resultPoint;
}
/**
 * Transformiert den angegebenen 3D-Punkt mit der Rotation und Translation
 * um Umrechnungen zwischen verschiedenen Koordinatensystemen zu ermglichen
 */
cv::Point3f ExtrCalibration::transformRT(cv::Point3f p)
{
    // ToDo: use projectPoints();

    double rvec_array[3], rotation_matrix[9];
    rvec_array[0] = mControlWidget->getCalibExtrRot1();
    rvec_array[1] = mControlWidget->getCalibExtrRot2();
    rvec_array[2] = mControlWidget->getCalibExtrRot3();

    cv::Mat rvec(3, 1, CV_64F, rvec_array);
    cv::Mat rot_mat(3, 3, CV_64F);
    // Transform the rotation vector into a rotation matrix with opencvs rodrigues method
    Rodrigues(rvec, rot_mat);

    rotation_matrix[0] = rot_mat.at<double>(0, 0);
    rotation_matrix[1] = rot_mat.at<double>(0, 1);
    rotation_matrix[2] = rot_mat.at<double>(0, 2);
    rotation_matrix[3] = rot_mat.at<double>(1, 0);
    rotation_matrix[4] = rot_mat.at<double>(1, 1);
    rotation_matrix[5] = rot_mat.at<double>(1, 2);
    rotation_matrix[6] = rot_mat.at<double>(2, 0);
    rotation_matrix[7] = rot_mat.at<double>(2, 1);
    rotation_matrix[8] = rot_mat.at<double>(2, 2);

    cv::Point3f point3D;

    point3D.x = rotation_matrix[0] * p.x + rotation_matrix[3] * p.y + rotation_matrix[6] * p.z -
                mControlWidget->trans1->value();
    point3D.y = rotation_matrix[1] * p.x + rotation_matrix[4] * p.y + rotation_matrix[7] * p.z -
                mControlWidget->trans2->value();
    point3D.z = rotation_matrix[2] * p.x + rotation_matrix[5] * p.y + rotation_matrix[8] * p.z -
                mControlWidget->trans3->value();
    return point3D;
}
bool ExtrCalibration::isOutsideImage(cv::Point2f p2d)
{
    int bS = mMainWindow->getImage() ? mMainWindow->getImageBorderSize() : 0;
    if(mMainWindow->getImage())
    {
        if(!isnormal(p2d.x) || !isnormal(p2d.y) || !isnormal(p2d.x) || !isnormal(p2d.y))
        {
            return true;
        }
        if(isnan(p2d.x) || isnan(p2d.y) || isinf(p2d.x) || isinf(p2d.y))
        {
            return true;
        }
        return p2d.x < -bS || p2d.x > mMainWindow->getImage()->width() - bS || p2d.y < -bS ||
               p2d.y > mMainWindow->getImage()->height() - bS;
    }
    else
    {
        return false;
    }
}


void ExtrCalibration::setXml(QDomElement &elem)
{
    reprojectionError.setXml(elem);
}

void ExtrCalibration::getXml(QDomElement &elem)
{
    QDomElement subElem;
    QString     styleString;

    for(subElem = elem.firstChildElement(); !subElem.isNull(); subElem = subElem.nextSiblingElement())
    {
        if(subElem.tagName() == "REPROJECTION_ERROR")
        {
            reprojectionError.getXml(subElem);
        }
    }
}

void ReprojectionError::getXml(QDomElement &subElem)
{
    if(subElem.hasAttribute("SUM_PH"))
    {
        mPointHeightAvg = subElem.attribute("AVG_PH").toDouble();
    }
    if(subElem.hasAttribute("SD_PH"))
    {
        mPointHeightStdDev = subElem.attribute("SD_PH").toDouble();
        if(mPointHeightStdDev < 0)
        {
            mPointHeightVariance = -1;
        }
        else
        {
            mPointHeightVariance = pow(mPointHeightStdDev, 2);
        }
    }
    if(subElem.hasAttribute("MAX_PH"))
    {
        mPointHeightMax = subElem.attribute("MAX_PH").toDouble();
    }
    if(subElem.hasAttribute("SUM_DH"))
    {
        mDefaultHeightAvg = subElem.attribute("AVG_DH").toDouble();
    }
    if(subElem.hasAttribute("SD_DH"))
    {
        mDefaultHeightStdDev = subElem.attribute("SD_DH").toDouble();
        if(mDefaultHeightStdDev < 0)
        {
            mDefaultHeightVariance = -1;
        }
        else
        {
            mDefaultHeightVariance = pow(mDefaultHeightStdDev, 2);
        }
    }
    if(subElem.hasAttribute("MAX_DH"))
    {
        mDefaultHeightMax = subElem.attribute("MAX_DH").toDouble();
    }
    if(subElem.hasAttribute("SUM_PX"))
    {
        mPixelAvg = subElem.attribute("AVG_PX").toDouble();
    }
    if(subElem.hasAttribute("SD_PX"))
    {
        mPixelStdDev = subElem.attribute("SD_PX").toDouble();
        if(mPixelStdDev < 0)
        {
            mPixelVariance = -1;
        }
        else
        {
            mPixelVariance = pow(mPixelStdDev, 2);
        }
    }
    if(subElem.hasAttribute("MAX_PX"))
    {
        mPixelMax = subElem.attribute("MAX_PX").toDouble();
    }
    if(subElem.hasAttribute("USED_HEIGHT"))
    {
        mUsedDefaultHeight = subElem.attribute("USED_HEIGHT").toDouble();
    }

    auto data = getData();
    mValid    = !std::any_of(data.begin(), data.end(), [](double a) { return !std::isfinite(a) || a < 0; });
}

void ReprojectionError::setXml(QDomElement &elem) const
{
    QDomElement subElem = elem.ownerDocument().createElement("REPROJECTION_ERROR");

    subElem.setAttribute("AVG_PH", mPointHeightAvg);
    subElem.setAttribute("SD_PH", mPointHeightStdDev);
    subElem.setAttribute("MAX_PH", mPointHeightMax);
    subElem.setAttribute("AVG_DH", mDefaultHeightAvg);
    subElem.setAttribute("SD_DH", mDefaultHeightStdDev);
    subElem.setAttribute("MAX_DH", mDefaultHeightMax);
    subElem.setAttribute("AVG_PX", mPixelAvg);
    subElem.setAttribute("SD_PX", mPixelStdDev);
    subElem.setAttribute("MAX_PX", mPixelMax);
    subElem.setAttribute("USED_HEIGHT", mUsedDefaultHeight);

    elem.appendChild(subElem);
}

double ReprojectionError::pointHeightAvg() const
{
    return mPointHeightAvg;
}

double ReprojectionError::pointHeightStdDev() const
{
    return mPointHeightStdDev;
}

double ReprojectionError::pointHeightVariance() const
{
    return mPointHeightVariance;
}

double ReprojectionError::pointHeightMax() const
{
    return mPointHeightMax;
}

double ReprojectionError::defaultHeightAvg() const
{
    return mDefaultHeightAvg;
}

double ReprojectionError::defaultHeightStdDev() const
{
    return mDefaultHeightStdDev;
}

double ReprojectionError::defaultHeightVariance() const
{
    return mDefaultHeightVariance;
}

double ReprojectionError::defaultHeightMax() const
{
    return mDefaultHeightMax;
}

double ReprojectionError::pixelAvg() const
{
    return mPixelAvg;
}

double ReprojectionError::pixelStdDev() const
{
    return mPixelStdDev;
}

double ReprojectionError::pixelVariance() const
{
    return mPixelVariance;
}

double ReprojectionError::pixelMax() const
{
    return mPixelMax;
}

double ReprojectionError::usedDefaultHeight() const
{
    return mUsedDefaultHeight;
}