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
  */
Point3f ExtrCalibration::transformRT(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();

    Mat rvec(3,1,CV_64F, rvec_array);
    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);

    Point3f point3D;

    point3D.x = rotation_matrix[0] * p.x +
            rotation_matrix[3] * p.y +
            rotation_matrix[6] * p.z -
            mControlWidget->trans1->value();//translation_vector2[0];
    point3D.y = rotation_matrix[1] * p.x +
            rotation_matrix[4] * p.y +
            rotation_matrix[7] * p.z -
            mControlWidget->trans2->value();//translation_vector2[1];
    point3D.z = rotation_matrix[2] * p.x +
            rotation_matrix[5] * p.y +
            rotation_matrix[8] * p.z -
            mControlWidget->trans3->value();//translation_vector2[2];
    return point3D;
}
bool ExtrCalibration::isOutsideImage(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 < 0 || p2d.x > mMainWindow->getImage()->width() || p2d.y < 0 || p2d.y > mMainWindow->getImage()->height();
        return p2d.x < -bS || p2d.x > mMainWindow->getImage()->width()-bS || p2d.y < -bS || p2d.y > mMainWindow->getImage()->height()-bS;
    }else
    {
        return false;
    }
}