<commit_before>#include "ofxCv/Calibration.h"
#include "ofxCv/Helpers.h"
#include "ofFileUtils.h"
#include "ofGraphics.h"
#include "ofMesh.h"
#include "ofXml.h"

namespace ofxCv {
    
    using namespace cv;
    
    void Intrinsics::setup(float focalLength, cv::Size imageSize, cv::Size2f sensorSize, cv::Point2d principalPoint) {
        float focalPixels = (focalLength / sensorSize.width) * imageSize.width;
        float fx = focalPixels; // focal length in pixels on x
        float fy = focalPixels;  // focal length in pixels on y
        float cx = imageSize.width * principalPoint.x;  // image center in pixels on x
        float cy = imageSize.height * principalPoint.y;  // image center in pixels on y
        Mat cameraMatrix = (Mat1d(3, 3) <<
                          fx, 0, cx,
                          0, fy, cy,
                          0, 0, 1);
        setup(cameraMatrix, imageSize, sensorSize);
    }
    void Intrinsics::setup(Mat cameraMatrix, cv::Size imageSize, cv::Size2f sensorSize) {
        this->cameraMatrix = cameraMatrix;
        this->imageSize = imageSize;
        this->sensorSize = sensorSize;
        updateValues();
    }
    
    void Intrinsics::updateValues() {
        calibrationMatrixValues(cameraMatrix,
                                imageSize,
                                sensorSize.width, sensorSize.height,
                                fov.x, fov.y,
                                focalLength,
                                principalPoint, // sets principalPoint in mm
                                aspectRatio);
    }
    
    void Intrinsics::setImageSize(cv::Size imgSize) {
        imageSize = imgSize;
    }
    
    Mat Intrinsics::getCameraMatrix() const {
        return cameraMatrix;
    }
    
    cv::Size Intrinsics::getImageSize() const {
        return imageSize;
    }
    
    cv::Size2f Intrinsics::getSensorSize() const {
        return sensorSize;
    }
    
    cv::Point2d Intrinsics::getFov() const {
        return fov;
    }
    
    double Intrinsics::getFocalLength() const {
        return focalLength;
    }
    
    double Intrinsics::getAspectRatio() const {
        return aspectRatio;
    }
    
    Point2d Intrinsics::getPrincipalPoint() const {
        return principalPoint;
    }
    
    void Intrinsics::loadProjectionMatrix(float nearDist, float farDist, cv::Point2d viewportOffset) const {
        ofViewport(viewportOffset.x, viewportOffset.y, imageSize.width, imageSize.height);
        ofSetMatrixMode(OF_MATRIX_PROJECTION);
        ofLoadIdentityMatrix();
        float w = imageSize.width;
        float h = imageSize.height;
        float fx = cameraMatrix.at<double>(0, 0);
        float fy = cameraMatrix.at<double>(1, 1);
        float cx = principalPoint.x;
        float cy = principalPoint.y;
        
        ofMatrix4x4 frustum;
        frustum.makeFrustumMatrix(
                                  nearDist * (-cx) / fx, nearDist * (w - cx) / fx,
                                  nearDist * (cy) / fy, nearDist * (cy - h) / fy,
                                  nearDist, farDist);
        ofMultMatrix(frustum);
        
        ofSetMatrixMode(OF_MATRIX_MODELVIEW);
        ofLoadIdentityMatrix();
        
        ofMatrix4x4 lookAt;
        lookAt.makeLookAtViewMatrix(glm::vec3(0,0,0), glm::vec3(0,0,1), glm::vec3(0,-1,0));
        ofMultMatrix(lookAt);
    }
    
    Calibration::Calibration() :
    patternType(CHESSBOARD),
    patternSize(cv::Size(10, 7)), // based on Chessboard_A4.pdf, assuming world units are centimeters
    subpixelSize(cv::Size(11,11)),
    squareSize(2.5),
    reprojectionError(0),
    distCoeffs(Mat::zeros(8, 1, CV_64F)),
    fillFrame(true),
    ready(false) {
        
    }
    
    void Calibration::save(string filename, bool absolute) const {
        if(!ready){
            ofLog(OF_LOG_ERROR, "Calibration::save() failed, because your calibration isn't ready yet!");
        }
        FileStorage fs(ofToDataPath(filename, absolute), FileStorage::WRITE);
        cv::Size imageSize = distortedIntrinsics.getImageSize();
        cv::Size sensorSize = distortedIntrinsics.getSensorSize();
        Mat cameraMatrix = distortedIntrinsics.getCameraMatrix();
        fs << "cameraMatrix" << cameraMatrix;
        fs << "imageSize_width" << imageSize.width;
        fs << "imageSize_height" << imageSize.height;
        fs << "sensorSize_width" << sensorSize.width;
        fs << "sensorSize_height" << sensorSize.height;
        fs << "distCoeffs" << distCoeffs;
        fs << "reprojectionError" << reprojectionError;
        fs << "features" << "[";
        for(std::size_t i = 0; i < imagePoints.size(); i++) {
            fs << "[:" << imagePoints[i] << "]";
        }
        fs << "]";
    }
    
    void Calibration::load(string filename, bool absolute) {
        imagePoints.clear();
        FileStorage fs(ofToDataPath(filename, absolute), FileStorage::READ);
        cv::Size imageSize;
        cv::Size2f sensorSize;
        Mat cameraMatrix;
        fs["cameraMatrix"] >> cameraMatrix;
        fs["imageSize_width"] >> imageSize.width;<commit_msg>Fix for issue #175 - yml parser was not writing features correctly<commit_after>#include "ofxCv/Calibration.h"
#include "ofxCv/Helpers.h"
#include "ofFileUtils.h"
#include "ofGraphics.h"
#include "ofMesh.h"
#include "ofXml.h"

namespace ofxCv {
    
    using namespace cv;
    
    void Intrinsics::setup(float focalLength, cv::Size imageSize, cv::Size2f sensorSize, cv::Point2d principalPoint) {
        float focalPixels = (focalLength / sensorSize.width) * imageSize.width;
        float fx = focalPixels; // focal length in pixels on x
        float fy = focalPixels;  // focal length in pixels on y
        float cx = imageSize.width * principalPoint.x;  // image center in pixels on x
        float cy = imageSize.height * principalPoint.y;  // image center in pixels on y
        Mat cameraMatrix = (Mat1d(3, 3) <<
                          fx, 0, cx,
                          0, fy, cy,
                          0, 0, 1);
        setup(cameraMatrix, imageSize, sensorSize);
    }
    void Intrinsics::setup(Mat cameraMatrix, cv::Size imageSize, cv::Size2f sensorSize) {
        this->cameraMatrix = cameraMatrix;
        this->imageSize = imageSize;
        this->sensorSize = sensorSize;
        updateValues();
    }
    
    void Intrinsics::updateValues() {
        calibrationMatrixValues(cameraMatrix,
                                imageSize,
                                sensorSize.width, sensorSize.height,
                                fov.x, fov.y,
                                focalLength,
                                principalPoint, // sets principalPoint in mm
                                aspectRatio);
    }
    
    void Intrinsics::setImageSize(cv::Size imgSize) {
        imageSize = imgSize;
    }
    
    Mat Intrinsics::getCameraMatrix() const {
        return cameraMatrix;
    }
    
    cv::Size Intrinsics::getImageSize() const {
        return imageSize;
    }
    
    cv::Size2f Intrinsics::getSensorSize() const {
        return sensorSize;
    }
    
    cv::Point2d Intrinsics::getFov() const {
        return fov;
    }
    
    double Intrinsics::getFocalLength() const {
        return focalLength;
    }
    
    double Intrinsics::getAspectRatio() const {
        return aspectRatio;
    }
    
    Point2d Intrinsics::getPrincipalPoint() const {
        return principalPoint;
    }
    
    void Intrinsics::loadProjectionMatrix(float nearDist, float farDist, cv::Point2d viewportOffset) const {
        ofViewport(viewportOffset.x, viewportOffset.y, imageSize.width, imageSize.height);
        ofSetMatrixMode(OF_MATRIX_PROJECTION);
        ofLoadIdentityMatrix();
        float w = imageSize.width;
        float h = imageSize.height;
        float fx = cameraMatrix.at<double>(0, 0);
        float fy = cameraMatrix.at<double>(1, 1);
        float cx = principalPoint.x;
        float cy = principalPoint.y;
        
        ofMatrix4x4 frustum;
        frustum.makeFrustumMatrix(
                                  nearDist * (-cx) / fx, nearDist * (w - cx) / fx,
                                  nearDist * (cy) / fy, nearDist * (cy - h) / fy,
                                  nearDist, farDist);
        ofMultMatrix(frustum);
        
        ofSetMatrixMode(OF_MATRIX_MODELVIEW);
        ofLoadIdentityMatrix();
        
        ofMatrix4x4 lookAt;
        lookAt.makeLookAtViewMatrix(glm::vec3(0,0,0), glm::vec3(0,0,1), glm::vec3(0,-1,0));
        ofMultMatrix(lookAt);
    }
    
    Calibration::Calibration() :
    patternType(CHESSBOARD),
    patternSize(cv::Size(10, 7)), // based on Chessboard_A4.pdf, assuming world units are centimeters
    subpixelSize(cv::Size(11,11)),
    squareSize(2.5),
    reprojectionError(0),
    distCoeffs(Mat::zeros(8, 1, CV_64F)),
    fillFrame(true),
    ready(false) {
        
    }
    
    void Calibration::save(string filename, bool absolute) const {
        if(!ready){
            ofLog(OF_LOG_ERROR, "Calibration::save() failed, because your calibration isn't ready yet!");
        }
        FileStorage fs(ofToDataPath(filename, absolute), FileStorage::WRITE);
        cv::Size imageSize = distortedIntrinsics.getImageSize();
        cv::Size sensorSize = distortedIntrinsics.getSensorSize();
        Mat cameraMatrix = distortedIntrinsics.getCameraMatrix();
        fs << "cameraMatrix" << cameraMatrix;
        fs << "imageSize_width" << imageSize.width;
        fs << "imageSize_height" << imageSize.height;
        fs << "sensorSize_width" << sensorSize.width;
        fs << "sensorSize_height" << sensorSize.height;
        fs << "distCoeffs" << distCoeffs;
        fs << "reprojectionError" << reprojectionError;
        fs << "features" << "[";
        for(std::size_t i = 0; i < imagePoints.size(); i++) {
            fs << imagePoints[i];
        }
        fs << "]";
    }
    
    void Calibration::load(string filename, bool absolute) {
        imagePoints.clear();
        FileStorage fs(ofToDataPath(filename, absolute), FileStorage::READ);
        cv::Size imageSize;
        cv::Size2f sensorSize;
        Mat cameraMatrix;
        fs["cameraMatrix"] >> cameraMatrix;
        fs["imageSize_width"] >> imageSize.width;
{'fraction_non_alphanumeric': 0.11177042801556421, 'fraction_numerical': 0.010408560311284046, 'mean_word_length': 2.1354071363220495, 'pattern_counts': {'":': 0, '<': 89, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 8, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5500817', 'n_tokens_mistral': 3141, 'n_tokens_neox': 3035, 'n_words': 643}