extrCalibration.cpp

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