CalibrationCreator2.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    VisionX::ArmarXObjects::CalibrationCreator2
 * @author     Christian R. G. Dreher <c.dreher@kit.edu>
 * @date       2020
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "CalibrationCreator2.h"
 
// VisionX
//#include <VisionX/tools/TypeMapping.h>
#include <VisionX/libraries/imrec/helper.h>
#include <VisionX/tools/ImageUtil.h>
 
// IVT
#include <cstdio>
#include <ctime>
#include <iostream>
#include <sstream>
#include <string>
 
#include <sys/time.h>
 
#include <opencv2/calib3d.hpp>
#include <opencv2/core.hpp>
#include <opencv2/core/utility.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
 
#include "Image/ByteImage.h"
#include "Image/ImageProcessor.h"
#include "Image/IplImageAdaptor.h"
 
//static inline void read(const FileNode& node, Settings& x, const Settings& default_value = Settings())
//{
//    if(node.empty())
//        x = default_value;
//    else
//        x.read(node);
//}
 
//static inline void write(FileStorage& fs, const String&, const Settings& s )
//{
//    s.write(fs);
//}
 
namespace
{
 
 
    static double
    computeReprojectionErrors(const std::vector<std::vector<cv::Point3f>>& objectPoints,
                              const std::vector<std::vector<cv::Point2f>>& imagePoints,
                              const std::vector<cv::Mat>& rvecs,
                              const std::vector<cv::Mat>& tvecs,
                              const cv::Mat& cameraMatrix,
                              const cv::Mat& distCoeffs,
                              std::vector<float>& perViewErrors)
    {
        std::vector<cv::Point2f> imagePoints2;
        size_t totalPoints = 0;
        double totalErr = 0, err;
        perViewErrors.resize(objectPoints.size());
 
        for (size_t i = 0; i < objectPoints.size(); ++i)
        {
            cv::projectPoints(
                objectPoints[i], rvecs[i], tvecs[i], cameraMatrix, distCoeffs, imagePoints2);
            err = cv::norm(imagePoints[i], imagePoints2, cv::NORM_L2);
 
            size_t n = objectPoints[i].size();
            perViewErrors[i] = (float)std::sqrt(err * err / n);
            totalErr += err * err;
            totalPoints += n;
        }
 
        return std::sqrt(totalErr / totalPoints);
    }
 
    static void
    calcBoardCornerPositions(cv::Size boardSize,
                             float squareSize,
                             std::vector<cv::Point3f>& corners,
                             visionx::Settings::Pattern patternType /*= Settings::CHESSBOARD*/)
    {
        corners.clear();
 
        switch (patternType)
        {
            case visionx::Settings::CHESSBOARD:
            case visionx::Settings::CIRCLES_GRID:
                for (int i = 0; i < boardSize.height; ++i)
                    for (int j = 0; j < boardSize.width; ++j)
                    {
                        corners.push_back(cv::Point3f(j * squareSize, i * squareSize, 0));
                    }
                break;
 
            case visionx::Settings::ASYMMETRIC_CIRCLES_GRID:
                for (int i = 0; i < boardSize.height; i++)
                    for (int j = 0; j < boardSize.width; j++)
                    {
                        corners.push_back(
                            cv::Point3f((2 * j + i % 2) * squareSize, i * squareSize, 0));
                    }
                break;
            default:
                break;
        }
    }
 
    static bool
    runCalibration(visionx::Settings& s,
                   cv::Size& imageSize,
                   cv::Mat& cameraMatrix,
                   cv::Mat& distCoeffs,
                   std::vector<std::vector<cv::Point2f>> imagePoints,
                   std::vector<cv::Mat>& rvecs,
                   std::vector<cv::Mat>& tvecs,
                   std::vector<float>& reprojErrs,
                   double& totalAvgErr)
    {
        //! [fixed_aspect]
        cameraMatrix = cv::Mat::eye(3, 3, CV_64F);
        if (s.flag & cv::CALIB_FIX_ASPECT_RATIO)
        {
            cameraMatrix.at<double>(0, 0) = s.aspectRatio;
        }
        //! [fixed_aspect]
        distCoeffs = cv::Mat::zeros(8, 1, CV_64F);
 
        std::vector<std::vector<cv::Point3f>> objectPoints(1);
        calcBoardCornerPositions(s.boardSize, s.squareSize, objectPoints[0], s.calibrationPattern);
 
        objectPoints.resize(imagePoints.size(), objectPoints[0]);
 
        //Find intrinsic and extrinsic camera parameters
        double rms;
 
        rms = cv::calibrateCamera(
            objectPoints, imagePoints, imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, s.flag);
 
        ARMARX_INFO << "Re-projection error reported by calibrateCamera: " << rms;
 
        bool ok = cv::checkRange(cameraMatrix) && cv::checkRange(distCoeffs);
 
        totalAvgErr = computeReprojectionErrors(
            objectPoints, imagePoints, rvecs, tvecs, cameraMatrix, distCoeffs, reprojErrs);
 
        return ok;
    }
 
    static void
    saveCameraParams(visionx::Settings& s,
                     cv::Size& imageSize,
                     cv::Mat& cameraMatrix,
                     cv::Mat& distCoeffs,
                     const std::vector<cv::Mat>& rvecs,
                     const std::vector<cv::Mat>& tvecs,
                     const std::vector<float>& reprojErrs,
                     const std::vector<std::vector<cv::Point2f>>& imagePoints,
                     double totalAvgErr)
    {
        cv::FileStorage fs(s.outputFileName, cv::FileStorage::WRITE);
 
        time_t tm;
        time(&tm);
        struct tm* t2 = localtime(&tm);
        char buf[1024];
        strftime(buf, sizeof(buf), "%c", t2);
 
        fs << "calibration_time" << buf;
 
        if (!rvecs.empty() || !reprojErrs.empty())
        {
            fs << "nr_of_frames" << (int)std::max(rvecs.size(), reprojErrs.size());
        }
        fs << "image_width" << imageSize.width;
        fs << "image_height" << imageSize.height;
        fs << "board_width" << s.boardSize.width;
        fs << "board_height" << s.boardSize.height;
        fs << "square_size" << s.squareSize;
 
        if (s.flag & cv::CALIB_FIX_ASPECT_RATIO)
        {
            fs << "fix_aspect_ratio" << s.aspectRatio;
        }
 
        if (s.flag)
        {
            std::stringstream flagsStringStream;
 
            flagsStringStream << "flags:"
                              << (s.flag & cv::CALIB_USE_INTRINSIC_GUESS ? " +use_intrinsic_guess"
                                                                         : "")
                              << (s.flag & cv::CALIB_FIX_ASPECT_RATIO ? " +fix_aspectRatio" : "")
                              << (s.flag & cv::CALIB_FIX_PRINCIPAL_POINT ? " +fix_principal_point"
                                                                         : "")
                              << (s.flag & cv::CALIB_ZERO_TANGENT_DIST ? " +zero_tangent_dist" : "")
                              << (s.flag & cv::CALIB_FIX_K1 ? " +fix_k1" : "")
                              << (s.flag & cv::CALIB_FIX_K2 ? " +fix_k2" : "")
                              << (s.flag & cv::CALIB_FIX_K3 ? " +fix_k3" : "")
                              << (s.flag & cv::CALIB_FIX_K4 ? " +fix_k4" : "")
                              << (s.flag & cv::CALIB_FIX_K5 ? " +fix_k5" : "")
                              << (s.flag & cv::CALIB_FIX_K6 ? " +fix_k6" : "");
 
            fs.writeComment(flagsStringStream.str());
        }
 
        fs << "flags" << s.flag;
 
        fs << "camera_matrix" << cameraMatrix;
        fs << "distortion_coefficients" << distCoeffs;
 
        fs << "avg_reprojection_error" << totalAvgErr;
        if (s.writeExtrinsics && !reprojErrs.empty())
        {
            fs << "per_view_reprojection_errors" << cv::Mat(reprojErrs);
        }
 
        if (s.writeExtrinsics && !rvecs.empty() && !tvecs.empty())
        {
            CV_Assert(rvecs[0].type() == tvecs[0].type());
            cv::Mat bigmat((int)rvecs.size(), 6, CV_MAKETYPE(rvecs[0].type(), 1));
            bool needReshapeR = rvecs[0].depth() != 1 ? true : false;
            bool needReshapeT = tvecs[0].depth() != 1 ? true : false;
 
            for (size_t i = 0; i < rvecs.size(); i++)
            {
                cv::Mat r = bigmat(cv::Range(int(i), int(i + 1)), cv::Range(0, 3));
                cv::Mat t = bigmat(cv::Range(int(i), int(i + 1)), cv::Range(3, 6));
 
                if (needReshapeR)
                {
                    rvecs[i].reshape(1, 1).copyTo(r);
                }
                else
                {
                    //*.t() is MatExpr (not Mat) so we can use assignment operator
                    CV_Assert(rvecs[i].rows == 3 && rvecs[i].cols == 1);
                    r = rvecs[i].t();
                }
 
                if (needReshapeT)
                {
                    tvecs[i].reshape(1, 1).copyTo(t);
                }
                else
                {
                    CV_Assert(tvecs[i].rows == 3 && tvecs[i].cols == 1);
                    t = tvecs[i].t();
                }
            }
            fs.writeComment(
                "a set of 6-tuples (rotation vector + translation vector) for each view");
            fs << "extrinsic_parameters" << bigmat;
        }
 
        if (s.writePoints && !imagePoints.empty())
        {
            cv::Mat imagePtMat((int)imagePoints.size(), (int)imagePoints[0].size(), CV_32FC2);
            for (size_t i = 0; i < imagePoints.size(); i++)
            {
                cv::Mat r = imagePtMat.row(int(i)).reshape(2, imagePtMat.cols);
                cv::Mat imgpti(imagePoints[i]);
                imgpti.copyTo(r);
            }
            fs << "image_points" << imagePtMat;
        }
    }
 
    bool
    runCalibrationAndSave(visionx::Settings& s,
                          cv::Size imageSize,
                          cv::Mat& cameraMatrix,
                          cv::Mat& distCoeffs,
                          std::vector<std::vector<cv::Point2f>> imagePoints)
    {
        std::vector<cv::Mat> rvecs, tvecs;
        std::vector<float> reprojErrs;
        double totalAvgErr = 0;
 
        bool ok = runCalibration(s,
                                 imageSize,
                                 cameraMatrix,
                                 distCoeffs,
                                 imagePoints,
                                 rvecs,
                                 tvecs,
                                 reprojErrs,
                                 totalAvgErr);
        ARMARX_INFO << (ok ? "Calibration succeeded" : "Calibration failed")
                    << ". avg re projection error = " << totalAvgErr;
 
        if (ok)
            saveCameraParams(s,
                             imageSize,
                             cameraMatrix,
                             distCoeffs,
                             rvecs,
                             tvecs,
                             reprojErrs,
                             imagePoints,
                             totalAvgErr);
        return ok;
    }
 
} // namespace
 
namespace visionx
{
 
    const std::string CalibrationCreator2::default_name = "CalibrationCreator2";
 
    void
    CalibrationCreator2::onInitImageProcessor()
    {
        providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
        usingImageProvider(providerName);
 
        getProperty(m_outputFileName, "OutputFileName");
        waitingTimeBetweenCaptures =
            IceUtil::Time::milliSeconds(getProperty<int>("WaitingIntervalBetweenImages"));
        std::string image_side = getProperty<std::string>("ImageToUse").getValue();
        if (image_side == "left")
        {
            imageID = 0;
        }
        else if (image_side == "right")
        {
            imageID = 1;
        }
        else
        {
            throw std::runtime_error("ImageToUse must be either left or right");
        }
    }
 
    void
    CalibrationCreator2::onConnectImageProcessor()
    {
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
        StereoCalibrationInterfacePrx calibrationProvider =
            StereoCalibrationInterfacePrx::checkedCast(imageProviderInfo.proxy);
        auto cam = calibrationProvider->getStereoCalibration();
        auto calib = imageID == 0 ? cam.calibrationLeft : cam.calibrationRight;
        auto focal_lengths = calib.cameraParam.focalLength;
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        // cv::Size ; The size of the board -> Number of items by width and height
        getProperty(m_settings.boardSize.width, "NumberOfColumns");
        getProperty(m_settings.boardSize.height, "NumberOfRows");
        // Pattern ; One of the Chessboard, circles, or asymmetric circle pattern
        m_settings.calibrationPattern = Settings::Pattern::CHESSBOARD;
        // float ; The size of a square in your defined unit (point, millimeter,etc).
        getProperty(m_settings.squareSize, "PatternSquareSize");
        // int ; The number of frames to use from the input for calibration
        getProperty(m_settings.nrFrames, "NumberOfImages");
        //float ; The aspect ratio
        if (getProperty<bool>("EnableFixAspectRatio"))
        {
            m_settings.aspectRatio = focal_lengths.at(0) / focal_lengths.at(1);
        }
        else
        {
            m_settings.aspectRatio = 0;
        }
        //bool ; Write detected feature points
        m_settings.writePoints = true;
        //bool ; Write extrinsic parameters
        m_settings.writeExtrinsics = false;
        //bool ; Flip the captured images around the horizontal axis
        m_settings.flipVertical = false;
        //std::string ; The name of the file where to write
        m_settings.outputFileName = m_outputFileName;
        //bool ;
        getProperty(m_settings.useK3ToK6, "UseAdditionalParams");
 
        m_settings.validate();
        if (not m_settings.goodInput)
        {
            throw std::runtime_error("Invalid input detected. Application stopping.");
        }
 
        ARMARX_INFO << "Board info: " << m_settings.boardSize.width << "x"
                    << m_settings.boardSize.height << " squares with square size of "
                    << m_settings.squareSize;
    }
 
    void
    CalibrationCreator2::onDisconnectImageProcessor()
    {
        delete cameraImages[0];
        delete cameraImages[1];
        delete[] cameraImages;
    }
 
    void
    CalibrationCreator2::onExitImageProcessor()
    {
    }
 
    void
    CalibrationCreator2::process()
    {
        cv::Mat view;
 
        if (not waitForImages(1000))
        {
            return;
        }
 
        if (IceUtil::Time::now() - timeOfLastCapture < waitingTimeBetweenCaptures)
        {
            return;
        }
 
        ARMARX_INFO << "Capturing image for calibration #" << imagePoints.size();
        getImages(cameraImages);
        timeOfLastCapture = IceUtil::Time::now();
        visionx::imrec::convert(*cameraImages[imageID], view);
 
        //-----  If no more image, or got enough, then stop calibration and show result -------------
        if (m_mode == CAPTURING && imagePoints.size() >= (size_t)m_settings.nrFrames)
        {
            ARMARX_IMPORTANT << "Running calibration now...";
            if (runCalibrationAndSave(m_settings, imageSize, cameraMatrix, distCoeffs, imagePoints))
            {
                m_mode = CALIBRATED;
            }
            ARMARX_IMPORTANT << "Done writing calibration file.";
        }
 
        imageSize = view.size(); // Format input image.
        if (m_settings.flipVertical)
        {
            cv::flip(view, view, 0);
        }
 
        //! [find_pattern]
        std::vector<cv::Point2f> pointBuf;
 
        bool found;
 
        int chessBoardFlags =
            cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_NORMALIZE_IMAGE | cv::CALIB_CB_FAST_CHECK;
 
        switch (m_settings.calibrationPattern) // Find feature points on the input format
        {
            case Settings::CHESSBOARD:
                ARMARX_DEBUG << "Searching chessboard corners...";
                found = cv::findChessboardCorners(
                    view, m_settings.boardSize, pointBuf, chessBoardFlags);
                break;
            case Settings::CIRCLES_GRID:
                found = cv::findCirclesGrid(view, m_settings.boardSize, pointBuf);
                break;
            case Settings::ASYMMETRIC_CIRCLES_GRID:
                found = cv::findCirclesGrid(
                    view, m_settings.boardSize, pointBuf, cv::CALIB_CB_ASYMMETRIC_GRID);
                break;
            default:
                found = false;
                break;
        }
        //! [find_pattern]
 
        //! [pattern_found]
        if (found) // If done with success,
        {
            // improve the found corners' coordinate accuracy for chessboard
            if (m_settings.calibrationPattern == Settings::CHESSBOARD)
            {
                cv::Mat viewGray;
                cv::cvtColor(view, viewGray, cv::COLOR_BGR2GRAY);
                cv::cornerSubPix(
                    viewGray,
                    pointBuf,
                    cv::Size(11, 11),
                    cv::Size(-1, -1),
                    cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
            }
 
            if (m_mode == CAPTURING)
            {
                imagePoints.push_back(pointBuf);
            }
 
            // Draw the corners.
            cv::drawChessboardCorners(view, m_settings.boardSize, cv::Mat(pointBuf), found);
        }
        //! [pattern_found]
 
        //----------------------------- Output Text ------------------------------------------------
        //! [output_text]
 
        //        //------------------------- Video capture  output  undistorted ------------------------------
        //        //! [output_undistorted]
        //        if (m_mode == CALIBRATED && m_settings.showUndistorsed)
        //        {
        //            cv::Mat temp = view.clone();
        //            cv::undistort(temp, view, cameraMatrix, distCoeffs);
        //        }
        //        //! [output_undistorted]
 
        //------------------------------ Show image and check for input commands -------------------
        //! [await_input]
    }
 
} // namespace visionx