PinholeCalibrationEngine.cpp

Go to the documentation of this file.

#include "PinholeCalibrationEngine.h"
 
namespace
{
    constexpr std::size_t minimumObservations = 30;
    constexpr std::size_t minimumPointsPerObservation = 12;
 
    bool isFinite(const cv::Point2f& point)
    {
        return std::isfinite(point.x) && std::isfinite(point.y);
    }
 
    bool isFinite(const cv::Point3f& point)
    {
        return std::isfinite(point.x) && std::isfinite(point.y) && std::isfinite(point.z);
    }
} // namespace
 
namespace stereo_calib
{
    bool PinholeCalibrationEngine::calibrate(
        const std::vector<StereoObservation>& observations, 
        const PinholeCalibrationOptions& options, 
        CalibrationResult& result,
        std::string& errorMessage) const
    {
        result = CalibrationResult{};
        errorMessage.clear();
 
        CalibrationResult calculated;
        calculated.model = CameraModel::Pinhole;
        calculated.leftCamera.model = CameraModel::Pinhole;
        calculated.rightCamera.model = CameraModel::Pinhole;
        calculated.mode = options.common.calibrationMode;
 
        try
        {
            if(!options.isValid())
            {
                errorMessage = "Invalid pinhole calibration options.";
                return false;
            }
 
            if(!validateObservations(observations, options.common.imageSize, errorMessage))
            {
                return false;
            }
 
            switch (options.common.calibrationMode)
            {
            case CalibrationMode::MonocularLeft:
                if(!calibrateMonocular(observations, 
                    CameraSide::Left, 
                    options, 
                    calculated.leftCamera, 
                    errorMessage))
                {
                    return false;
                }
                break;
            case CalibrationMode::MonocularRight:
                if(!calibrateMonocular(
                    observations, 
                    CameraSide::Right, 
                    options, 
                    calculated.rightCamera, 
                    errorMessage))
                {
                    return false;
                }
                break;
            case CalibrationMode::MonocularBoth:
                if (!calibrateMonocular(observations, CameraSide::Left, options,
                                        calculated.leftCamera, errorMessage) ||
                    !calibrateMonocular(observations, CameraSide::Right, options,
                                        calculated.rightCamera, errorMessage))
                {
                    return false;
                }
                break;
 
            case CalibrationMode::StereoFull:
                if (!calibrateMonocular(observations, CameraSide::Left, options,
                                        calculated.leftCamera, errorMessage) ||
                    !calibrateMonocular(observations, CameraSide::Right, options,
                                        calculated.rightCamera, errorMessage) ||
                    !calibrateStereo(observations, options, calculated.leftCamera,
                                    calculated.rightCamera, calculated.stereo, errorMessage))
                {
                    return false;
                }
                break;
            default:
                errorMessage = "Unknown calibration mode.";
                return false;
                break;
            }
 
            if (!calculated.isValid())
            {
                errorMessage = "Calibration produced an invalid result.";
                return false;
            }
 
            result = std::move(calculated);
            return true;
        }
        catch(const cv::Exception& e)
        {
            errorMessage = std::string("OpenCV exception during calibration: ") + e.what();
        }
        catch(const std::exception& e)
        {
            errorMessage = std::string("Standard exception during calibration: ") + e.what();
        }
        catch(...)
        {
            errorMessage = "Unknown exception during calibration.";
        }
        
        result = CalibrationResult{};
        return false;
    }
 
    bool PinholeCalibrationEngine::validateObservations(
        const std::vector<StereoObservation>& observations, 
        const cv::Size& expectedImageSize, 
        std::string& errorMessage) const
    {
        if (observations.size() < minimumObservations)
        {
            errorMessage = "At least " + std::to_string(minimumObservations) +
                        " stereo observations are required.";
            return false;
        }
 
        std::size_t expectedPointCount = 0;
        std::vector<cv::Point3f> referenceObjectPoints;
        for (std::size_t index = 0; index < observations.size(); ++index)
        {
            const StereoObservation& observation = observations[index];
            const std::string prefix = "Observation #" + std::to_string(index + 1) + ": ";
 
            if (observation.imageSize != expectedImageSize)
            {
                std::ostringstream message;
                message << prefix << "image size is " << observation.imageSize.width << "x"
                        << observation.imageSize.height << ", expected " << expectedImageSize.width
                        << "x" << expectedImageSize.height << ".";
                errorMessage = message.str();
                return false;
            }
 
            if (!observation.isValid())
            {
                errorMessage = prefix + "has inconsistent point lists or an invalid timestamp delta.";
                return false;
            }
 
            if (observation.objectPoints.size() < minimumPointsPerObservation)
            {
                errorMessage = prefix + "contains fewer than " +
                            std::to_string(minimumPointsPerObservation) + " calibration points.";
                return false;
            }
 
            if (!std::isfinite(observation.leftTimestampSeconds) ||
                !std::isfinite(observation.rightTimestampSeconds) ||
                !std::isfinite(observation.timestampDeltaSeconds))
            {
                errorMessage = prefix + "contains a non-finite timestamp.";
                return false;
            }
 
            for (std::size_t pointIndex = 0; pointIndex < observation.objectPoints.size(); ++pointIndex)
            {
                if (!isFinite(observation.objectPoints[pointIndex]) ||
                    !isFinite(observation.leftImagePoints[pointIndex]) ||
                    !isFinite(observation.rightImagePoints[pointIndex]))
                {
                    errorMessage = prefix + "point #" + std::to_string(pointIndex + 1) +
                                " contains a non-finite coordinate.";
                    return false;
                }
            }
 
            if (index == 0)
            {
                expectedPointCount = observation.objectPoints.size();
                referenceObjectPoints = observation.objectPoints;
            }
            else if (observation.objectPoints.size() != expectedPointCount)
            {
                errorMessage = prefix + "has " + std::to_string(observation.objectPoints.size()) +
                            " object points, expected " + std::to_string(expectedPointCount) + ".";
                return false;
            }
            else
            {
                for (std::size_t pointIndex = 0; pointIndex < expectedPointCount; ++pointIndex)
                {
                    if (cv::norm(observation.objectPoints[pointIndex] - referenceObjectPoints[pointIndex]) > 1e-6)
                    {
                        errorMessage = prefix + "uses object points different from observation #1.";
                        return false;
                    }
                }
            }
        }
 
        return true;
    }
 
    bool PinholeCalibrationEngine::calibrateMonocular(
        const std::vector<StereoObservation>& observations, 
        CameraSide cameraSide, 
        const PinholeCalibrationOptions& options,
        CameraCalibrationResult& result, 
        std::string& errorMessage) const
    {
        std::vector<std::vector<cv::Point3f>> objectPoints;
        std::vector<std::vector<cv::Point2f>> leftImagePoints;
        std::vector<std::vector<cv::Point2f>> rightImagePoints;
        if (!extractCalibrationPoints(observations, objectPoints, leftImagePoints, rightImagePoints, errorMessage))
        {
            return false;
        }
 
        const std::vector<std::vector<cv::Point2f>>& imagePoints =
            cameraSide == CameraSide::Left ? leftImagePoints : rightImagePoints;
        cv::Mat intrinsic = cv::Mat::eye(3, 3, CV_64F);
        if(options.cameraFocalLengthGuess > 0.0 && (options.monocularFlags & cv::CALIB_USE_INTRINSIC_GUESS))
        {
            intrinsic.at<double>(0, 0) = options.cameraFocalLengthGuess;
            intrinsic.at<double>(1, 1) = options.cameraFocalLengthGuess;
            intrinsic.at<double>(0, 2) = options.common.imageSize.width * 0.5;
            intrinsic.at<double>(1, 2) = options.common.imageSize.height * 0.5;
        }
        // OpenCV's standard pinhole calibration vector contains k3 as its
        // fifth entry. Keep it fixed to zero during optimization, then expose
        // only the four coefficients consumed by camCalib.
        cv::Mat distortion = cv::Mat::zeros(5, 1, CV_64F);
        std::vector<cv::Mat> rotationVectors;
        std::vector<cv::Mat> translationVectors;
 
        const double rms = cv::calibrateCamera(objectPoints, imagePoints, options.common.imageSize,
                                                    intrinsic, distortion, rotationVectors,
                                                    translationVectors, options.monocularFlags,
                                                    options.criteria);
 
        result = CameraCalibrationResult{};
        result.model = CameraModel::Pinhole;
        result.imageSize = options.common.imageSize;
        result.K = intrinsic.clone();
        result.D = distortion.reshape(1, 5).rowRange(0, 4).clone();
        result.rotationVectors = std::move(rotationVectors);
        result.translationVectors = std::move(translationVectors);
        result.rms = rms;
 
        if (result.rotationVectors.size() != observations.size() ||
            result.translationVectors.size() != observations.size())
        {
            errorMessage = "OpenCV returned an incomplete set of monocular poses.";
            return false;
        }
 
        result.perViewRms.reserve(observations.size());
        for (std::size_t index = 0; index < observations.size(); ++index)
        {
            std::vector<cv::Point2f> projectedPoints;
            cv::projectPoints(objectPoints[index],
                                result.rotationVectors[index], 
                                result.translationVectors[index],
                                result.K, result.D,
                                projectedPoints);
            if (projectedPoints.size() != imagePoints[index].size())
            {
                errorMessage = "OpenCV returned an incomplete projected point set.";
                return false;
            }
            const double viewRms = cv::norm(projectedPoints, imagePoints[index], cv::NORM_L2) /
                                std::sqrt(static_cast<double>(projectedPoints.size()));
            result.perViewRms.push_back(viewRms);
        }
 
        if (!result.isValid())
        {
            errorMessage = "Monocular calibration produced invalid camera parameters.";
            return false;
        }
        return true;
    }
 
    bool PinholeCalibrationEngine::calibrateStereo(
        const std::vector<StereoObservation>& observations,
        const PinholeCalibrationOptions& options, 
        const CameraCalibrationResult& leftCamera,
        const CameraCalibrationResult& rightCamera, 
        StereoCalibrationResult& result, 
        std::string& errorMessage) const
    {
        std::vector<std::vector<cv::Point3f>> objectPoints;
        std::vector<std::vector<cv::Point2f>> leftImagePoints;
        std::vector<std::vector<cv::Point2f>> rightImagePoints;
        if (!extractCalibrationPoints(observations, objectPoints, leftImagePoints, rightImagePoints, errorMessage))
        {
            return false;
        }
 
        cv::Mat leftK = leftCamera.K.clone();
        cv::Mat rightK = rightCamera.K.clone();
        cv::Mat leftD = leftCamera.D.clone();
        cv::Mat rightD = rightCamera.D.clone();
        cv::Mat Rotation, Translation, Essential, Fundamental;
 
        const double rms = cv::stereoCalibrate(
            objectPoints, leftImagePoints, rightImagePoints,
            leftK, leftD, rightK, rightD,
            options.common.imageSize, Rotation, Translation, Essential, Fundamental,
            options.stereoFlags, options.criteria);
 
        result = StereoCalibrationResult{};
        result.R = Rotation.clone();
        result.T = Translation.reshape(1, 3).clone();
        result.rms = rms;
 
        if (!result.isValid())
        {
            errorMessage = "Stereo calibration produced an invalid result.";
            return false;
        }
        return true;
    }
 
    bool PinholeCalibrationEngine::extractCalibrationPoints(
        const std::vector<StereoObservation>& observations, 
        std::vector<std::vector<cv::Point3f>>& objectPoints,
        std::vector<std::vector<cv::Point2f>>& leftImagePoints, 
        std::vector<std::vector<cv::Point2f>>& rightImagePoints,
        std::string& errorMessage) const
    {
        
        objectPoints.clear();
        leftImagePoints.clear();
        rightImagePoints.clear();
        objectPoints.reserve(observations.size());
        leftImagePoints.reserve(observations.size());
        rightImagePoints.reserve(observations.size());
 
        for (const StereoObservation& observation : observations)
        {
            objectPoints.push_back(observation.objectPoints);
            leftImagePoints.push_back(observation.leftImagePoints);
            rightImagePoints.push_back(observation.rightImagePoints);
        }
        if (objectPoints.empty())
        {
            errorMessage = "No calibration points are available.";
            return false;
        }
        return true;
    }
}