Subversion Repositories seema-scanner

#include "SMCalibrationParams.h"

#include <QFileInfo>
#include <QString>
#include <QDateTime>

#include <iostream>
#include <fstream>
#include <iomanip>
#include <opencv2/calib3d/calib3d.hpp>


bool SMCalibrationParams::load(const QString& filename){
    QFileInfo info(filename);
  //  QString type = info.suffix();

    if(info.exists() && info.suffix()=="yml")
        return loadYML(filename);
    else
    {
        std::cerr << "SMCalibrationParams error: no such .yml file: " << filename.toStdString() << std::endl;
        return false;
    }
}

bool SMCalibrationParams::save(const QString& filename){
    QFileInfo info(filename);
    QString type = info.suffix();

    if (type=="slcalib") {return saveSLCALIB(filename);}
    if (type=="yml") {return saveYML(filename);}
    if (type=="m") {return saveMatlab(filename);}

    return false;
}

bool SMCalibrationParams::loadYML(const QString& filename){
    cv::FileStorage fs(filename.toStdString(), cv::FileStorage::READ); //
    if (!fs.isOpened())
    {
        std::cerr << "SMCalibrationParams error: could not open file " << filename.toStdString() << std::endl;
        return false;
    }

    cv::Mat temp;
    fs["Kc"] >> temp; Kc = temp;
    fs["kc"] >> temp; kc = temp;
    fs["Kp"] >> temp; Kp = temp;
    fs["kp"] >> temp; kp = temp;
    fs["Rp"] >> temp; Rp = temp;
    fs["Tp"] >> temp; Tp = temp;

    fs["cam_error"] >> cam_error;
    fs["proj_error"] >> proj_error;
    fs["stereo_error"] >> stereo_error;

    fs.release();

    return true;
}

bool SMCalibrationParams::saveSLCALIB(const QString& filename){


    FILE * fp = fopen(qPrintable(filename), "w");
    if (!fp)
        return false;

    fprintf(fp, "#V1.0 SLStudio calibration\n");
    fprintf(fp, "#Calibration time: %s\n\n", calibrationDateTime.c_str());
    fprintf(fp, "Kc\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kc(0,0), Kc(0,1), Kc(0,2), Kc(1,0), Kc(1,1), Kc(1,2), Kc(2,0), Kc(2,1), Kc(2,2));
    fprintf(fp, "kc\n%f %f %f %f %f\n\n", kc(0), kc(1), kc(2), kc(3), kc(4));
    fprintf(fp, "Kp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Kp(0,0), Kp(0,1), Kp(0,2), Kp(1,0), Kp(1,1), Kp(1,2), Kp(2,0), Kp(2,1), Kp(2,2));
    fprintf(fp, "kp\n%f %f %f %f %f\n\n", kp(0), kp(1), kp(2), kp(3), kp(4));
    fprintf(fp, "Rp\n%f %f %f\n%f %f %f\n%f %f %f\n\n", Rp(0,0), Rp(0,1), Rp(0,2), Rp(1,0), Rp(1,1), Rp(1,2), Rp(2,0), Rp(2,1), Rp(2,2));
    fprintf(fp, "Tp\n%f %f %f\n\n", Tp(0), Tp(1), Tp(2));

    fprintf(fp, "cam_error: %f\n\n", cam_error);
    fprintf(fp, "proj_error: %f\n\n", proj_error);
    fprintf(fp, "stereo_error: %f\n\n", stereo_error);

    fclose(fp);

    return true;

}

bool SMCalibrationParams::saveYML(const QString& filename){
    cv::FileStorage fs(filename.toStdString(), cv::FileStorage::WRITE);
    if (!fs.isOpened())
        return false;

    fs << "Kc" << cv::Mat(Kc) << "kc" << cv::Mat(kc)
       << "Kp" << cv::Mat(Kp) << "kp" << cv::Mat(kp)
       << "Rp" << cv::Mat(Rp) << "Tp" << cv::Mat(Tp)
       << "cam_error" << cam_error
       << "proj_error" << proj_error
       << "stereo_error" << stereo_error;
    fs.release();

    return true;
}

bool SMCalibrationParams::saveMatlab(const QString& filename){

    std::ofstream file(qPrintable(filename));
    if (!file)
        return false;

    file << "%%SLStudio calibration"  << std::endl;
    file << "Kc = " << Kc << ";" << std::endl;
    file << "kc = " << kc << ";" << std::endl;
    file << "Kp = " << Kp << ";" << std::endl;
    file << "kp = " << kp << ";" << std::endl;
    file << "Rp = " << Rp << ";" << std::endl;
    file << "Tp = " << Tp << ";" << std::endl;

    file.close();

    return true;

}

void SMCalibrationParams::print(std::ostream &stream){

    stream  << std::setw(5) << std::setprecision(4)
            << "========================================\n"
            << "Camera Calibration: \n"
            << "- cam_error:\n" << cam_error << "\n"
            << "- Kc:\n" << Kc << "\n"
            << "- kc:\n" << kc << "\n"
            << "Projector Calibration: " << "\n"
            << "- proj_error: \n" << proj_error << "\n"
            << "- Kp: \n" << Kp << "\n"
            << "- kp: \n" << kp << "\n"
            << "Stereo Calibration: \n"
            << "- stereo_error:\n" << stereo_error << "\n"
            << "- Rp:\n" << Rp << "\n"
            << "- Tp:\n" << Tp << std::endl;
}