extrCalibration.cpp

/*
 * PeTrack - Software for tracking pedestrians movement in videos
 * Copyright (C) 2010-2022 Forschungszentrum Jülich GmbH,
 * Maik Boltes, Juliane Adrian, Ricardo Martin Brualla, Arne Graf, Paul Häger, Daniel Hillebrand,
 * Deniz Kilic, Paul Lieberenz, Daniel Salden, Tobias Schrödter, Ann Katrin Seemann
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program 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 <https://www.gnu.org/licenses/>.
 */

#include "extrCalibration.h"

#include "control.h"
#include "pMessageBox.h"
#include "petrack.h"

#include <QFileDialog>
#include <QtWidgets>

#define MAX_AV_ERROR 20

ExtrCalibration::ExtrCalibration(PersonStorage &storage) : mPersonStorage(storage)
{
    mMainWindow    = nullptr;
    mControlWidget = nullptr;
}

ExtrCalibration::~ExtrCalibration() {}

void ExtrCalibration::setMainWindow(Petrack *mw)
{
    mMainWindow    = mw;
    mControlWidget = mw->getControlWidget();
    init();
}
void ExtrCalibration::init()
{
    rotation_matrix     = new double[9];
    translation_vector  = new double[3];
    translation_vector2 = new double[3];

    camValues  = new double[9];
    distValues = new double[8];

    isExtCalib = false;
}
bool ExtrCalibration::isEmptyExtrCalibFile()
{
    return mExtrCalibFile.isEmpty();
}

void ExtrCalibration::setExtrCalibFile(const QString &f)
{
    mExtrCalibFile = f;
}

QString ExtrCalibration::getExtrCalibFile()
{
    if(!this->isEmptyExtrCalibFile())
    {
        return mExtrCalibFile;
    }
    else
    {
        return QString();
    }
}

bool ExtrCalibration::openExtrCalibFile()
{
    if(mMainWindow)
    {
        static QString lastDir;

        if(!mExtrCalibFile.isEmpty())
        {
            lastDir = QFileInfo(mExtrCalibFile).path();
        }

        QString extrCalibFile = QFileDialog::getOpenFileName(
            mMainWindow,
            Petrack::tr("Open extrinisc calibration file with point correspondences"),
            lastDir,
            "3D-Calibration-File (*.3dc);;Text (*.txt);;All supported types (*.3dc *.txt);;All files (*.*)");
        if(!extrCalibFile.isEmpty())
        {
            mExtrCalibFile = extrCalibFile;
            return loadExtrCalibFile();
        }
    }
    return false;
}

/**
 * @brief Loads the extrinsic calibration from mExtrCalibFile
 *
 * This methods reads an extrinsic calibration in one of two formats:
 * First: 3D coordinates followed by corresponding 2D coordinates
 *
 *     x y z px py
 *
 * Second: Just 3D coordinates
 *
 *     x y z
 *
 * It is possible to optionally start the file with the number of lines:
 *
 *     2
 *     x1 y1 z1
 *     x2 y2 z2
 *
 * This is just going to be ignored. Comments start with "#".
 *
 * @return
 */
bool ExtrCalibration::loadExtrCalibFile()
{
    bool all_ok = true;

    if(!mExtrCalibFile.isEmpty())
    {
        if(mExtrCalibFile.right(4) == ".3dc" || mExtrCalibFile.right(4) == ".txt")
        {
            QFile file(mExtrCalibFile);
            if(!file.open(QIODevice::ReadOnly | QIODevice::Text))
            {
                PCritical(
                    mMainWindow,
                    QObject::tr("Petrack"),
                    QObject::tr("Error: Cannot open %1:\n%2.").arg(mExtrCalibFile, file.errorString()));
                return false;
            }

            debout << "Reading 3D calibration data from " << mExtrCalibFile << "..." << std::endl;


            std::vector<cv::Point3f> points3D_tmp;
            std::vector<cv::Point2f> points2D_tmp;

            QTextStream in(&file);
            QString     line;
            int         line_counter = 0, counter;
            float       x, y, z, px, py;
            float       zahl;
            bool        with_2D_data = false, with_3D_data = false, end_loop = false;

            // Exit loop when reaching the end of the file
            while(!in.atEnd())
            {
                // Neue Zeile einlesen
                line = in.readLine();
                ++line_counter;

                // Kommentare ueberlesen
                if(line.startsWith("#", Qt::CaseInsensitive) || line.startsWith(";;", Qt::CaseInsensitive) ||
                   line.startsWith("//", Qt::CaseInsensitive) || line.startsWith("!", Qt::CaseInsensitive))
                {
                    continue;
                }

                QTextStream stream(&line);
                counter  = 0;
                end_loop = false;

                while(!stream.atEnd() && !end_loop)
                {
                    stream >> zahl;
                    ++counter;

                    switch(counter)
                    {
                        case 1:
                            x = zahl;
                            if(!with_3D_data)
                            {
                                points3D_tmp.clear();
                                with_3D_data = true;
                            }
                            break;
                        case 2:
                            y = zahl;
                            break;
                        case 3:
                            z = zahl;
                            break;
                        case 4:
                            px = zahl;
                            if(!with_2D_data)
                            {
                                points2D_tmp.clear();