BundlerFilter.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "BundlerFilter.h"
 
// Local
#include "BinFilter.h"
#include "BundlerImportDlg.h"
 
// CV_DB_LIB
#include <CVLog.h>
#include <ecvCameraSensor.h>
#include <ecvGLMatrix.h>
#include <ecvHObjectCaster.h>
#include <ecvImage.h>
#include <ecvPointCloud.h>
#include <ecvProgressDialog.h>
 
// Qt
#include <QApplication>
#include <QDir>
#include <QFile>
#include <QFileDialog>
#include <QFileInfo>
#include <QInputDialog>
#include <QString>
 
// Qt5/Qt6 Compatibility
#include <QtCompat.h>
 
// cloudViewer (for DTM generation)
#include <MeshSamplingTools.h>
#include <PointProjectionTools.h>
#include <ecvMesh.h>
 
// System
#include <string>
 
struct BundlerCamera {
    BundlerCamera() : f_pix(0), k1(0), k2(0), trans(), isValid(true) {}
 
    float f_pix;
    float k1;
    float k2;
    ccGLMatrixd trans;
    bool isValid;
};
 
BundlerFilter::BundlerFilter()
    : FileIOFilter({"_Snavely Bundler Filter",
                    DEFAULT_PRIORITY,  // priority
                    QStringList{"out"}, "out",
                    QStringList{"Snavely's Bundler output (*.out)"},
                    QStringList(), Import}) {}
 
CC_FILE_ERROR BundlerFilter::loadFile(const QString& filename,
                                      ccHObject& container,
                                      LoadParameters& parameters) {
    return loadFileExtended(filename, container, parameters);
}
 
// ortho-rectified image related information
struct ORImageInfo {
    QString name;             // image name
    unsigned w, h;            // image dimensions
    double minC[2], maxC[2];  // local bounding box
};
 
CC_FILE_ERROR BundlerFilter::loadFileExtended(
        const QString& filename,
        ccHObject& container,
        LoadParameters& parameters,
        const QString& _altKeypointsFilename /*=QString()*/,
        bool _undistortImages /*=false*/,
        bool _generateColoredDTM /*=false*/,
        unsigned _coloredDTMVerticesCount /*=1000000*/,
        float _scaleFactor /*=1.0f*/) {
    // opening file (ASCII)
    QFile f(filename);
    if (!f.open(QIODevice::ReadOnly)) return CC_FERR_READING;
 
    QTextStream stream(&f);
 
    // read header (should be "# Bundle file vX.Y")
    QString currentLine = stream.readLine();
    if (!currentLine.startsWith("# Bundle file", Qt::CaseInsensitive)) {
        CVLog::Error("File should start by '# Bundle file vX.Y'!");
        return CC_FERR_MALFORMED_FILE;
    }
    unsigned majorVer = 0, minorVer = 0;
    sscanf(qPrintable(currentLine), "# Bundle file v%u.%u", &majorVer,
           &minorVer);
    if (majorVer != 0 || (minorVer != 3 && minorVer != 4)) {
        CVLog::Error(
                "Only version 0.3 and 0.4 of Bundler files are supported!");
        return CC_FERR_WRONG_FILE_TYPE;
    }
 
    // second header line (should be <num_cameras> <num_points>)
    currentLine = stream.readLine();
    QStringList list =
            qtCompatSplitRegex(currentLine, "\\s+", QtCompat::SkipEmptyParts);
    if (list.size() != 2) {
        CVLog::Error(
                "[Bundler] Second line should be <num_cameras> <num_points>!");
        return CC_FERR_MALFORMED_FILE;
    }
    unsigned camCount = list[0].toInt();
    if (camCount == 0)
        CVLog::Warning("[Bundler] No camera defined in Bundler file!");
 
    unsigned ptsCount = list[1].toInt();
    if (ptsCount == 0)
        CVLog::Warning("[Bundler] No keypoints defined in Bundler file!");
 
    // parameters
    bool importKeypoints = false;
    bool useAltKeypoints = false;
    bool importImages = false;
    bool undistortImages = false;
    bool orthoRectifyImagesAsClouds = false;
    bool orthoRectifyImagesAsImages = false;
    bool orthoRectifyImages = false;
    bool generateColoredDTM = false;
    unsigned coloredDTMVerticesCount = 1000000;
    float scaleFactor = 1.0f;
    bool keepImagesInMemory = false;
    bool applyOptMatrix = false;
    ccGLMatrix orthoOptMatrix;
    orthoOptMatrix.toIdentity();
    BundlerImportDlg::OrthoRectMethod orthoRectMethod =
            BundlerImportDlg::OPTIMIZED;
 
    // default paths
    QString imageListFilename = QFileInfo(f).dir().absoluteFilePath("list.txt");
    QString altKeypointsFilename =
            QFileInfo(f).dir().absoluteFilePath("pmvs.ply");
 
    if (parameters.alwaysDisplayLoadDialog) {
        // open dialog
        BundlerImportDlg biDlg;
        biDlg.setKeypointsCount(ptsCount);
        biDlg.setCamerasCount(camCount);
        biDlg.setVer(majorVer, minorVer);
        biDlg.setImageListFilename(imageListFilename);
        biDlg.setAltKeypointsFilename(altKeypointsFilename);
 
        if (!biDlg.exec()) return CC_FERR_CANCELED_BY_USER;
 
        importKeypoints = biDlg.importKeypoints();
        useAltKeypoints = biDlg.useAlternativeKeypoints();
        if (useAltKeypoints)
            altKeypointsFilename = biDlg.getAltKeypointsFilename();
        importImages = biDlg.importImages();
        undistortImages = biDlg.undistortImages();
        orthoRectifyImagesAsClouds = biDlg.orthoRectifyImagesAsClouds();
        orthoRectifyImagesAsImages = biDlg.orthoRectifyImagesAsImages();
        generateColoredDTM = biDlg.generateColoredDTM();
        coloredDTMVerticesCount = biDlg.getDTMVerticesCount();
        scaleFactor = static_cast<float>(biDlg.getScaleFactor());
        keepImagesInMemory = biDlg.keepImagesInMemory();
        imageListFilename = biDlg.getImageListFilename();
        applyOptMatrix = biDlg.getOptionalTransfoMatrix(orthoOptMatrix);
        orthoRectMethod = biDlg.getOrthorectificationMethod();
    } else {
        importImages = true;
        orthoRectifyImagesAsImages = true;
        useAltKeypoints = !_altKeypointsFilename.isEmpty();
        if (useAltKeypoints) altKeypointsFilename = _altKeypointsFilename;
        undistortImages = _undistortImages;
        generateColoredDTM = _generateColoredDTM;
        if (generateColoredDTM)
            coloredDTMVerticesCount = _coloredDTMVerticesCount;
        scaleFactor = _scaleFactor;
    }
 
    if (!importKeypoints && !importImages) return CC_FERR_NO_LOAD;
 
    orthoRectifyImages =
            orthoRectifyImagesAsClouds || orthoRectifyImagesAsImages;
 
    // data
    std::vector<BundlerCamera> cameras;
    ccPointCloud* keypointsCloud = 0;
    ccHObject* altEntity = 0;
    typedef std::pair<unsigned, ccCameraSensor::KeyPoint> KeypointAndCamIndex;
    std::vector<KeypointAndCamIndex> keypointsDescriptors;
 
    // Read Bundler '.out' file
    {
        // progress dialog
        QScopedPointer<ecvProgressDialog> pDlg(0);
        if (parameters.parentWidget) {
            pDlg.reset(new ecvProgressDialog(
                    true, parameters.parentWidget));  // cancel available
            pDlg->setMethodTitle(QObject::tr("Open Bundler file"));
            pDlg->setInfo(QObject::tr("Cameras: %1\nPoints: %2")
                                  .arg(camCount)
                                  .arg(ptsCount));
            pDlg->start();
        }
        cloudViewer::NormalizedProgress nprogress(
                pDlg.data(),
                camCount + (importKeypoints || orthoRectifyImages ||
                                            generateColoredDTM
                                    ? ptsCount
                                    : 0));
 
        // read cameras info (whatever the case!)
        cameras.resize(camCount);
        unsigned camIndex = 0;
        for (std::vector<BundlerCamera>::iterator it = cameras.begin();
             it != cameras.end(); ++it, ++camIndex) {
            // f, k1 and k2
            currentLine = stream.readLine();
            if (currentLine.isEmpty()) return CC_FERR_READING;
            if (importImages) {
                QStringList tokens = qtCompatSplitRegex(
                        currentLine, "\\s+", QtCompat::SkipEmptyParts);
                if (tokens.size() < 3) return CC_FERR_MALFORMED_FILE;
                bool ok[3] = {true, true, true};
                it->f_pix = tokens[0].toFloat(ok);
                it->k1 = tokens[1].toFloat(ok + 1);
                it->k2 = tokens[2].toFloat(ok + 2);
                if (!ok[0] || !ok[1] || !ok[2]) return CC_FERR_MALFORMED_FILE;
            }
            // Rotation matrix
            double* mat = (importImages ? it->trans.data() : 0);
            double sum = 0;
            for (unsigned l = 0; l < 3; ++l) {
                currentLine = stream.readLine();
                if (currentLine.isEmpty()) return CC_FERR_READING;
                if (importImages) {
                    QStringList tokens =
                            currentLine.split("\\s+", QtCompat::SkipEmptyParts);
                    if (tokens.size() < 3) return CC_FERR_MALFORMED_FILE;
                    bool ok[3] = {true, true, true};
                    mat[l] = tokens[0].toDouble(ok);
                    mat[4 + l] = tokens[1].toDouble(ok + 1);
                    mat[8 + l] = tokens[2].toDouble(ok + 2);
                    if (!ok[0] || !ok[1] || !ok[2])
                        return CC_FERR_MALFORMED_FILE;
                    sum += fabs(mat[l]) + fabs(mat[4 + l]) + fabs(mat[8 + l]);
                }
            }
            if (importImages && cloudViewer::LessThanEpsilon(sum)) {
                CVLog::Warning("[Bundler] Camera #%i is invalid!",
                               camIndex + 1);
                it->isValid = false;
            }
 
            // Translation
            currentLine = stream.readLine();
            if (currentLine.isEmpty()) return CC_FERR_READING;
            if (importImages) {
                QStringList tokens = qtCompatSplitRegex(
                        currentLine, "\\s+", QtCompat::SkipEmptyParts);
                if (tokens.size() < 3) return CC_FERR_MALFORMED_FILE;
                bool ok[3] = {true, true, true};
                mat[12] = tokens[0].toDouble(ok);
                mat[13] = tokens[1].toDouble(ok + 1);
                mat[14] = tokens[2].toDouble(ok + 2);
                if (!ok[0] || !ok[1] || !ok[2]) return CC_FERR_MALFORMED_FILE;
            }
 
            if (pDlg && !nprogress.oneStep())  // cancel requested?
            {
                return CC_FERR_CANCELED_BY_USER;
            }
        }
 
        // read points
        if (!useAltKeypoints &&
            (importKeypoints || orthoRectifyImages || generateColoredDTM)) {
            keypointsCloud = new ccPointCloud("Keypoints");
            if (!keypointsCloud->reserve(ptsCount)) {
                delete keypointsCloud;
                return CC_FERR_NOT_ENOUGH_MEMORY;
            }
 
            bool hasColors = false;
            if (importKeypoints) {
                hasColors = keypointsCloud->reserveTheRGBTable();
                if (!hasColors)
                    CVLog::Warning(
                            "[Bundler] Not enough memory to load colors!");
                else
                    keypointsCloud->showColors(true);
            }
 
            bool storeKeypoints = orthoRectifyImages /* && !useAltKeypoints*/;
            // we'll check if all cameras are used or not!
            std::vector<bool> camUsage;
            // if (!useAltKeypoints)
            {
                try {
                    camUsage.resize(cameras.size(), false);
                } catch (const std::bad_alloc&) {
                    // nothing serious here
                }
            }
 
            CCVector3d Pshift(0, 0, 0);
            for (unsigned i = 0; i < ptsCount; ++i) {
                // Point (X,Y,Z)
                currentLine = stream.readLine();
                if (currentLine.startsWith(
                            "--"))  // skip lines starting with '--' (yes it
                                    // happens in some weird version of
                                    // Bundler?!)
                    currentLine = stream.readLine();
                if (currentLine.isEmpty()) {
                    delete keypointsCloud;
                    return CC_FERR_READING;
                }
 
                // read point coordinates (as strings)
                CCVector3d Pd(0, 0, 0);
                {
                    QStringList tokens =
                            currentLine.split("\\s+", QtCompat::SkipEmptyParts);
                    if (tokens.size() < 3) {
                        delete keypointsCloud;
                        return CC_FERR_MALFORMED_FILE;
                    }
                    // decode coordinates
                    bool ok[3] = {true, true, true};
                    Pd.x = tokens[0].toDouble(ok);
                    Pd.y = tokens[1].toDouble(ok + 1);
                    Pd.z = tokens[2].toDouble(ok + 2);
                    if (!ok[0] || !ok[1] || !ok[2]) {
                        delete keypointsCloud;
                        return CC_FERR_MALFORMED_FILE;
                    }
                }
 
                // first point: check for 'big' coordinates
                if (i == 0) {
                    bool preserveCoordinateShift = true;
                    if (HandleGlobalShift(Pd, Pshift, preserveCoordinateShift,
                                          parameters)) {
                        if (preserveCoordinateShift) {
                            keypointsCloud->setGlobalShift(Pshift);
                            // we must apply the shift to the cameras as well!!!
                            for (size_t j = 0; j < cameras.size(); ++j) {
                                ccGLMatrixd& trans = cameras[j].trans;
                                trans.invert();
                                trans.setTranslation(
                                        trans.getTranslationAsVec3D() + Pshift);
                                trans.invert();
                            }
                        }
                        CVLog::Warning(
                                "[Bundler] Cloud has been recentered! "
                                "Translation: (%.2f ; %.2f ; %.2f)",
                                Pshift.x, Pshift.y, Pshift.z);
                    }
                }
                keypointsCloud->addPoint(CCVector3::fromArray((Pd + Pshift).u));
 
                // RGB
                currentLine = stream.readLine();
                if (currentLine.isEmpty()) {
                    delete keypointsCloud;
                    return CC_FERR_READING;
                }
                QStringList colorParts =
                        currentLine.split(" ", QtCompat::SkipEmptyParts);
                if (colorParts.size() == 3) {
                    if (hasColors) {
                        QStringList tokens = currentLine.split(
                                "\\s+", QtCompat::SkipEmptyParts);
                        if (tokens.size() < 3) {
                            delete keypointsCloud;
                            return CC_FERR_MALFORMED_FILE;
                        }
                        int R = tokens[0].toInt();
                        int G = tokens[1].toInt();
                        int B = tokens[2].toInt();
                        keypointsCloud->addRGBColor(
                                static_cast<ColorCompType>(
                                        std::min<int>(R, ecvColor::MAX)),
                                static_cast<ColorCompType>(
                                        std::min<int>(G, ecvColor::MAX)),
                                static_cast<ColorCompType>(
                                        std::min<int>(B, ecvColor::MAX)));
                    }
 
                    currentLine = stream.readLine();
                } else if (colorParts.size() > 3) {
                    // sometimes, it appears that keypoints has no associated
                    // color! so we skip the line and assume it's in fact the
                    // keypoint description...
                    CVLog::Warning(
                            "[Bundler] Keypoint #%i has no associated color!",
                            i);
                    if (hasColors)
                        keypointsCloud->addRGBColor(0, 0,
                                                    0);  // black by default
                } else {
                    delete keypointsCloud;
                    return CC_FERR_MALFORMED_FILE;
                }
 
                // view list (currentLine should already be read, see above)
                if (currentLine.isEmpty()) {
                    delete keypointsCloud;
                    return CC_FERR_READING;
                }
 
                if (storeKeypoints || !camUsage.empty()) {
                    QStringList parts =
                            currentLine.split(" ", QtCompat::SkipEmptyParts);
                    if (!parts.isEmpty()) {
                        bool ok = false;
                        unsigned nviews = parts[0].toInt(&ok);
                        if (!ok || nviews * 4 + 1 > static_cast<unsigned>(
                                                            parts.size())) {
                            CVLog::Warning(
                                    "[Bundler] View list for point #%i is "
                                    "invalid!",
                                    i);
                        } else {
                            unsigned pos = 1;
                            for (unsigned n = 0; n < nviews; ++n) {
                                int cam = parts[pos++].toInt();  // camera index
                                ++pos;  // int key = parts[pos++].toInt();
                                        // //index of the SIFT keypoint where
                                        // the point was detected in that camera
                                        // (not used)
                                if (cam < 0 ||
                                    static_cast<unsigned>(cam) >= camCount) {
                                    pos += 2;
                                    continue;
                                }
                                if (!camUsage.empty()) {
                                    camUsage[cam] = true;
                                }
                                if (storeKeypoints) {
                                    float x =
                                            parts[pos++]
                                                    .toFloat();  // detected
                                                                 // positions of
                                                                 // that
                                                                 // keypoint (x)
                                    float y =
                                            parts[pos++]
                                                    .toFloat();  // detected
                                                                 // positions of
                                                                 // that
                                                                 // keypoint (y)
                                    // add key point
                                    KeypointAndCamIndex lastKeyPoint;
                                    lastKeyPoint.first =
                                            static_cast<unsigned>(cam);
                                    lastKeyPoint.second.index = i;
                                    lastKeyPoint.second.x =
                                            x *
                                            scaleFactor;  // the origin is the
                                                          // center of the
                                                          // image, the x-axis
                                                          // increases to the
                                                          // right
                                    lastKeyPoint.second.y =
                                            -y *
                                            scaleFactor;  // and the y-axis
                                                          // increases towards
                                                          // the top of the
                                                          // image
                                    try {
                                        keypointsDescriptors.push_back(
                                                lastKeyPoint);
                                    } catch (const std::bad_alloc&) {
                                        CVLog::Warning(
                                                "[Bundler] Not enough memory "
                                                "to store keypoints!");
                                        keypointsDescriptors.clear();
                                        orthoRectifyImages = false;
                                        storeKeypoints = false;
                                    }
                                }
                            }
                        }
                    }
                }
 
                if (pDlg && !nprogress.oneStep())  // cancel requested?
                {
                    delete keypointsCloud;
                    return CC_FERR_CANCELED_BY_USER;
                }
            }
 
            if (!camUsage.empty()) {
                for (size_t i = 0; i < camUsage.size(); ++i) {
                    if (!camUsage[i])
                        CVLog::Warning(QString("[Bundler] Camera #%1 has no "
                                               "associated keypoints!")
                                               .arg(i + 1));
                }
            }
 
            // apply optional matrix (if any)
            if (applyOptMatrix) {
                keypointsCloud->applyGLTransformation_recursive(
                        &orthoOptMatrix);
                CVLog::Print(
                        "[Bundler] Keypoints cloud has been transformed with "
                        "input matrix!");
                // this transformation is of no interest for the user
                // keypointsCloud->resetGLTransformationHistory_recursive();
            }
 
            if (importKeypoints) container.addChild(keypointsCloud);
        }
 
        if (pDlg) {
            pDlg->stop();
            QApplication::processEvents();
        }
    }
 
    // use alternative cloud/mesh as keypoints
    if (useAltKeypoints) {
        FileIOFilter::LoadParameters altKeypointsParams;
        CC_FILE_ERROR result = CC_FERR_NO_ERROR;
        ccHObject* altKeypointsContainer = FileIOFilter::LoadFromFile(
                altKeypointsFilename, altKeypointsParams, result);
        if (!altKeypointsContainer ||
            altKeypointsContainer->getChildrenNumber() != 1 ||
            (!altKeypointsContainer->getChild(0)->isKindOf(
                     CV_TYPES::POINT_CLOUD) &&
             !altKeypointsContainer->getChild(0)->isKindOf(CV_TYPES::MESH))) {
            if (!altKeypointsContainer)
                CVLog::Error(QString("[Bundler] Failed to load alternative "
                                     "keypoints file:\n'%1'")
                                     .arg(altKeypointsFilename));
            else
                CVLog::Error(
                        "[Bundler] Can't use this kind of entities as "
                        "keypoints (need one and only one cloud or mesh)");
 
            return CC_FERR_WRONG_FILE_TYPE;
        } else {
            altEntity = altKeypointsContainer->getChild(0);
            if (importKeypoints) container.addChild(altEntity);
        }
    }
 
    if (!importImages) return CC_FERR_NO_ERROR;
 
    assert(camCount > 0);
 
    // load images
    QDir imageDir =
            QFileInfo(f)
                    .dir();  // by default we look in the Bundler file folder
 
    // let's try to open the images list file (if necessary)
    QStringList imageFilenames;
    {
        imageFilenames.clear();
        QFile imageListFile(imageListFilename);
        if (!imageListFile.exists() ||
            !imageListFile.open(QIODevice::ReadOnly)) {
            CVLog::Error(
                    QString("[Bundler] Failed to open image list file! (%1)")
                            .arg(imageListFilename));
            if (!importKeypoints && keypointsCloud) delete keypointsCloud;
            if (!importKeypoints && altEntity) delete altEntity;
            return CC_FERR_UNKNOWN_FILE;
        }
 
        // we look for images in same directory
        imageDir = QFileInfo(imageListFile).dir();
        QTextStream imageListStream(&imageListFile);
 
        for (unsigned lineIndex = 0; lineIndex < camCount; ++lineIndex) {
            QString nextLine = imageListStream.readLine();
            if (nextLine.isEmpty()) break;
 
            QStringList parts = qtCompatSplitRegex(nextLine, "\\s+",
                                                   QtCompat::SkipEmptyParts);
            if (parts.size() > 0)
                imageFilenames << parts[0];
            else {
                CVLog::Error(QString("[Bundler] Couldn't extract image name "
                                     "from line %1 in file '%2'!")
                                     .arg(lineIndex)
                                     .arg(imageListFilename));
                break;
            }
        }
    }
 
    if (imageFilenames.size() <
        static_cast<int>(camCount))  // not enough images!
    {
        if (imageFilenames.isEmpty())
            CVLog::Error(QString("[Bundler] No filename could be extracted "
                                 "from file '%1'!")
                                 .arg(imageListFilename));
        else
            CVLog::Error(QString("[Bundler] Only %1 filenames (out of %2) "
                                 "could be extracted\nfrom file '%3'!")
                                 .arg(imageFilenames.size())
                                 .arg(camCount)
                                 .arg(imageListFilename));
        if (!importKeypoints && keypointsCloud) delete keypointsCloud;
        if (!importKeypoints && altEntity) delete altEntity;
        return CC_FERR_MALFORMED_FILE;
    }
 
    // let's try to open the image corresponding to each camera
    QScopedPointer<ecvProgressDialog> ipDlg(0);
    if (parameters.parentWidget) {
        ipDlg.reset(new ecvProgressDialog(
                true, parameters.parentWidget));  // cancel available
        ipDlg->setMethodTitle(QObject::tr("Open & process images"));
        ipDlg->setInfo(QObject::tr("Images: %1").arg(camCount));
        ipDlg->start();
        QApplication::processEvents();
    }
    cloudViewer::NormalizedProgress inprogress(ipDlg.data(), camCount);
 
    assert(imageFilenames.size() >= static_cast<int>(camCount));
 
    /*** pre-processing steps (colored MNT computation, etc.) ***/
 
    // for colored DTM generation
    int* mntColors = nullptr;
    cloudViewer::PointCloud* mntSamples = nullptr;
    if (generateColoredDTM) {
        QScopedPointer<ecvProgressDialog> toDlg(0);
        if (parameters.parentWidget) {
            toDlg.reset(new ecvProgressDialog(
                    true, parameters.parentWidget));  // cancel available
            toDlg->setMethodTitle(QObject::tr("Preparing colored DTM"));
            toDlg->start();
            QApplication::processEvents();
        }
 
        // 1st step: triangulate keypoints (or use existing one)
        ccGenericMesh* baseDTMMesh =
                (altEntity ? ccHObjectCaster::ToGenericMesh(altEntity) : 0);
        cloudViewer::GenericIndexedMesh* dummyMesh = baseDTMMesh;
        if (!baseDTMMesh) {
            // alternative keypoints?
            ccGenericPointCloud* altKeypoints =
                    (altEntity ? ccHObjectCaster::ToGenericPointCloud(altEntity)
                               : 0);
            std::string errorStr;
            dummyMesh = cloudViewer::PointProjectionTools::computeTriangulation(
                    altKeypoints ? altKeypoints : keypointsCloud,
                    cloudViewer::DELAUNAY_2D_BEST_LS_PLANE, 0, 0, errorStr);
            if (!dummyMesh) {
                CVLog::Warning(QString("[Bundler] Failed to generate DTM! (%1)")
                                       .arg(QString::fromStdString(errorStr)));
            }
        }
 
        if (dummyMesh) {
            // 2nd step: samples points on resulting mesh
            mntSamples = cloudViewer::MeshSamplingTools::samplePointsOnMesh(
                    (cloudViewer::GenericMesh*)dummyMesh,
                    coloredDTMVerticesCount);
            if (!baseDTMMesh) delete dummyMesh;
            dummyMesh = 0;
 
            if (mntSamples) {
                // 3rd step: project each point in all images and get average
                // color
                unsigned count = mntSamples->size();
                mntColors = new int[4 * count];  // R + G + B + accum count
                if (!mntColors) {
                    // not enough memory
                    CVLog::Error(
                            "Not enough memory to store DTM colors! DTM "
                            "generation cancelled");
                    delete mntSamples;
                    mntSamples = 0;
                    generateColoredDTM = false;
                } else {
                    memset(mntColors, 0, sizeof(int) * 4 * count);
                }
            }
        }
    }
 
    std::vector<ORImageInfo> OR_infos;
    double OR_pixelSize = -1.0;                 // auto for first image
    double OR_globalCorners[4] = {0, 0, 0, 0};  // corners for the global set
 
    // alternative keypoints? (for ortho-rectification only)
    ccGenericPointCloud *altKeypoints = 0, *_keypointsCloud = 0;
    if (orthoRectifyImages) {
        altKeypoints =
                (altEntity ? ccHObjectCaster::ToGenericPointCloud(altEntity)
                           : 0);
        _keypointsCloud = (altKeypoints ? altKeypoints : keypointsCloud);
    }
 
    /*** process each image ***/
 
    bool cancelledByUser = false;
    for (unsigned i = 0; i < camCount; ++i) {
        const BundlerCamera& cam = cameras[i];
        if (!cam.isValid) continue;
 
        ccImage* image = new ccImage();
        QString errorStr;
        if (!image->load(imageDir.absoluteFilePath(imageFilenames[i]),
                         errorStr)) {
            CVLog::Error(QString("[Bundler] %1 (image '%2')")
                                 .arg(errorStr, imageFilenames[i]));
            delete image;
            image = 0;
            break;
        }
 
        image->setName(imageFilenames[i]);
        image->setEnabled(false);
        image->setAlpha(0.75f);  // semi transparent by default
 
        // associate image with calibration information
        ccCameraSensor* sensor = 0;
        {
            ccCameraSensor::IntrinsicParameters params;
            params.arrayWidth = static_cast<int>(image->getW());
            params.arrayHeight = static_cast<int>(image->getH());
            // we define an arbitrary principal point
            params.principal_point[0] = params.arrayWidth / 2.0f;
            params.principal_point[1] = params.arrayHeight / 2.0f;
            // we use an arbitrary 'pixel size'
            params.pixelSize_mm[0] = params.pixelSize_mm[1] =
                    1.0f / std::max(params.arrayWidth, params.arrayHeight);
            params.vertFocal_pix = cam.f_pix * scaleFactor;
            params.vFOV_rad = ccCameraSensor::ComputeFovRadFromFocalPix(
                    cam.f_pix, params.arrayHeight);
 
            // camera position/orientation
            ccGLMatrix transf(cameras[i].trans.inverse().data());
 
            // dist to cloud
            PointCoordinateType dist =
                    keypointsCloud ? (transf.getTranslationAsVec3D() -
                                      keypointsCloud->getOwnBB().getCenter())
                                             .norm()
                                   : PC_ONE;
            params.zFar_mm = dist;
            params.zNear_mm = 0.001f;
 
            sensor = new ccCameraSensor(params);
            sensor->setName(QString("Camera #%1").arg(i + 1));
            sensor->setEnabled(true);
            sensor->setVisible(true /*false*/);
            sensor->setGraphicScale(
                    keypointsCloud
                            ? keypointsCloud->getOwnBB().getDiagNorm() / 10
                            : PC_ONE);
            sensor->setRigidTransformation(transf);
 
            // distortion parameters
            if (cameras[i].k1 != 0 || cameras[i].k2 != 0) {
                ccCameraSensor::RadialDistortionParameters* distParams =
                        new ccCameraSensor::RadialDistortionParameters;
                distParams->k1 = cameras[i].k1;
                distParams->k2 = cameras[i].k2;
                sensor->setDistortionParameters(
                        ccCameraSensor::LensDistortionParameters::Shared(
                                distParams));
            }
 
            // apply optional matrix (if any)
            if (applyOptMatrix) {
                sensor->applyGLTransformation_recursive(&orthoOptMatrix);
                // CVLog::Print("[Bundler] Camera cloud has been transformed
                // with input matrix!"); this transformation is of no interest
                // for the user
                // sensor->resetGLTransformationHistory_recursive();
            }
        }
        // the image is a child of the sensor!
        image->setAssociatedSensor(sensor);
        sensor->addChild(image);
 
        // ortho-rectification
        if (orthoRectifyImages) {
            assert(sensor && _keypointsCloud);
 
            // select image keypoints
            std::vector<ccCameraSensor::KeyPoint> keypointsImage;
            ccBBox keypointsImageBB;
            if (_keypointsCloud ==
                keypointsCloud)  // keypoints from Bundler file
            {
                for (std::vector<KeypointAndCamIndex>::const_iterator key =
                             keypointsDescriptors.begin();
                     key != keypointsDescriptors.end(); ++key) {
                    if (key->first == i) {
                        keypointsImage.push_back(key->second);
                        keypointsImageBB.add(
                                CCVector3(key->second.x, key->second.y, 0));
                    }
                }
            } else {
                // project alternative cloud in image!
                _keypointsCloud->placeIteratorAtBeginning();
                int half_w = (image->getW() >> 1);
                int half_h = (image->getH() >> 1);
                ccCameraSensor::KeyPoint kp;
                unsigned keyptsCount = _keypointsCloud->size();
                for (unsigned k = 0; k < keyptsCount; ++k) {
                    CCVector3 P(*_keypointsCloud->getPointPersistentPtr(k));
                    // apply bundler equation
                    cam.trans.apply(P);
                    // convert to keypoint
                    kp.x = -cam.f_pix * static_cast<float>(P.x / P.z);
                    kp.y = cam.f_pix * static_cast<float>(P.y / P.z);
                    if (static_cast<int>(kp.x) > -half_w &&
                        static_cast<int>(kp.x < half_w) &&
                        static_cast<int>(kp.y) > -half_h &&
                        static_cast<int>(kp.y < half_h)) {
                        kp.index = k;
                        keypointsImage.push_back(kp);
                        keypointsImageBB.add(CCVector3(kp.x, kp.y, 0));
                    }
                }
            }
 
            if (keypointsImage.size() < 4) {
                CVLog::Warning(QString("[Bundler] Not enough keypoints "
                                       "descriptors for image '%1'!")
                                       .arg(image->getName()));
            } else if (!keypointsImageBB.isValid() ||
                       keypointsImageBB.getDiagNorm() < PC_ONE) {
                CVLog::Warning(
                        QString("[Bundler] Keypoints descriptors for image "
                                "'%1' are invalid (= all the same)")
                                .arg(image->getName()));
            } else {
                if (orthoRectifyImagesAsImages) {
                    // for ortho-rectification log
                    ORImageInfo info;
                    double corners[8];
                    ccImage* orthoImage = 0;
 
                    //"standard" ortho-rectification method
                    if (orthoRectMethod == BundlerImportDlg::OPTIMIZED) {
                        orthoImage = sensor->orthoRectifyAsImage(
                                image, _keypointsCloud, keypointsImage,
                                OR_pixelSize, info.minC, info.maxC, corners);
                    }
                    //"direct" ortho-rectification method
                    else {
                        assert(orthoRectMethod == BundlerImportDlg::DIRECT ||
                               orthoRectMethod ==
                                       BundlerImportDlg::DIRECT_UNDISTORTED);
 
                        // we take the keypoints 'middle altitude' by default
                        CCVector3 bbMin, bbMax;
                        _keypointsCloud->getBoundingBox(bbMin, bbMax);
                        PointCoordinateType Z0 = (bbMin.z + bbMax.z) / 2;
 
                        orthoImage = sensor->orthoRectifyAsImageDirect(
                                image, Z0, OR_pixelSize,
                                orthoRectMethod ==
                                        BundlerImportDlg::DIRECT_UNDISTORTED,
                                info.minC, info.maxC, corners);
                    }
 
                    if (orthoImage) {
                        assert(!orthoImage->data().isNull());
                        info.name = QString("ortho_%1.png")
                                            .arg(QFileInfo(imageFilenames[i])
                                                         .baseName());
                        info.w = orthoImage->getW();
                        info.h = orthoImage->getH();
                        orthoImage->data().save(
                                imageDir.absoluteFilePath(info.name));
                        CVLog::Print(
                                QString("[Bundler] Ortho-rectified version of "
                                        "image '%1' (%2 x %3) saved to '%4'")
                                        .arg(imageFilenames[i])
                                        .arg(info.w)
                                        .arg(info.h)
                                        .arg(imageDir.absoluteFilePath(
                                                info.name)));
 
#ifdef TEST_TEXTURED_BUNDLER_IMPORT
 
                        // we tile the original image to avoid any OpenGL
                        // limitation on texture size
#define TBI_DEFAULT_TILE_POW 10  // 2^10 = 1024
                        const unsigned tileDim = (1 << TBI_DEFAULT_TILE_POW);
                        unsigned horiTile = (info.w >> TBI_DEFAULT_TILE_POW);
                        if (info.w - horiTile * tileDim != 0) ++horiTile;
                        unsigned vertTile = (info.h >> TBI_DEFAULT_TILE_POW);
                        if (info.h - vertTile * tileDim != 0) ++vertTile;
                        unsigned tiles = horiTile * vertTile;
                        unsigned verts = (horiTile + 1) * (vertTile + 1);
 
                        // vertices
                        ccPointCloud* rectVertices =
                                new ccPointCloud("vertices");
                        rectVertices->reserve(verts);
 
                        // mesh
                        ccMesh* rectMesh = new ccMesh(rectVertices);
                        rectMesh->reserve(2 * tiles);
                        rectMesh->addChild(rectVertices);
 
                        // materials (=textures)
                        ccMaterialSet* matSet = new ccMaterialSet("Texture");
 
                        // texture coordinates table
                        TextureCoordsContainer* texCoords =
                                new TextureCoordsContainer();
                        // texCoords->reserve(verts);
                        texCoords->reserve(4);
                        {
                            // float minu = 1.0f/(float)(2*tileDim);
                            // float maxu = 1.0-1.0f/(float)(2*tileDim);
                            float minu = 0.0f;
                            float maxu = 1.0f;
                            float TA[2] = {minu, minu};
                            float TB[2] = {maxu, minu};
                            float TC[2] = {maxu, maxu};
                            float TD[2] = {minu, maxu};
                            texCoords->addElement(TA);
                            texCoords->addElement(TB);
                            texCoords->addElement(TC);
                            texCoords->addElement(TD);
                        }
 
                        // per triangle material indexes
                        rectMesh->reservePerTriangleMtlIndexes();
                        // per triangle texture coordinates indexes
                        rectMesh->reservePerTriangleTexCoordIndexes();
 
                        double dcx = info.maxC[0] - info.minC[0];
                        double dcy = info.maxC[1] - info.minC[1];
 
                        // process all tiles
                        for (unsigned ti = 0; ti <= horiTile; ++ti) {
                            unsigned x = std::min(ti * tileDim, info.w);
                            double xRel = static_cast<double>(x) / info.w;
                            for (unsigned tj = 0; tj <= vertTile; ++tj) {
                                unsigned y = std::min(tj * tileDim, info.h);
                                double yRel = static_cast<double>(y) / info.h;
 
                                // add vertices
                                CCVector3 P(info.minC[0] + dcx * xRel,
                                            info.maxC[1] - dcy * yRel, 0);
                                rectVertices->addPoint(P);
 
                                // add texture coordinates
                                // float T0[2]={xRel,1.0-yRel};
                                // texCoords->addElement(T0);
 
                                if (ti < horiTile && tj < vertTile) {
                                    unsigned w = std::min(info.w - x, tileDim);
                                    unsigned h = std::min(info.h - y, tileDim);
 
                                    // create corresponding texture
                                    unsigned tileIndex = matSet->size();
                                    matSet->push_back(ccMaterial(info.name));
                                    matSet->back().texture =
                                            orthoImage->data().copy(x, y, w, h);
                                    // matSet->back().texture.save(imageDir.absoluteFilePath(QString("tile_%1_").arg(tileIndex)+info.name));
 
                                    unsigned iA = ti * (vertTile + 1) + tj;
                                    unsigned iB =
                                            (ti + 1) * (vertTile + 1) + tj;
                                    unsigned iC = iB + 1;
                                    unsigned iD = iA + 1;
 
                                    rectMesh->addTriangle(iA, iB, iD);
                                    // rectMesh->addTriangleTexCoordIndexes(iA,iB,iD);
                                    rectMesh->addTriangleTexCoordIndexes(0, 1,
                                                                         3);
                                    rectMesh->addTriangleMtlIndex(tileIndex);
                                    rectMesh->addTriangle(iB, iC, iD);
                                    // rectMesh->addTriangleTexCoordIndexes(iB,iC,iD);
                                    rectMesh->addTriangleTexCoordIndexes(1, 2,
                                                                         3);
                                    rectMesh->addTriangleMtlIndex(tileIndex);
                                }
                            }
                        }
 
                        rectMesh->showMaterials(true);
                        rectMesh->setName(info.name);
 
                        // associate texture coordinates table
                        rectMesh->setTexCoordinatesTable(texCoords);
                        // associate material set
                        rectMesh->setMaterialSet(matSet);
                        container.addChild(rectMesh);
#endif
 
                        delete orthoImage;
                        orthoImage = 0;
 
                        OR_infos.push_back(info);
 
                        // update global boundaries
                        if (OR_globalCorners[0] > info.minC[0])
                            OR_globalCorners[0] = info.minC[0];
                        if (OR_globalCorners[1] > info.minC[1])
                            OR_globalCorners[1] = info.minC[1];
                        if (OR_globalCorners[2] < info.maxC[0])
                            OR_globalCorners[2] = info.maxC[0];
                        if (OR_globalCorners[3] < info.maxC[1])
                            OR_globalCorners[3] = info.maxC[1];
                    } else {
                        CVLog::Warning(QString("[Bundler] Failed to "
                                               "ortho-rectify image '%1'!")
                                               .arg(image->getName()));
                    }
                }
 
                if (orthoRectifyImagesAsClouds) {
                    ccPointCloud* orthoCloud = sensor->orthoRectifyAsCloud(
                            image, _keypointsCloud, keypointsImage);
                    if (orthoCloud) {
                        orthoCloud->setGlobalScale(
                                _keypointsCloud->getGlobalScale());
                        orthoCloud->setGlobalShift(
                                _keypointsCloud->getGlobalShift());
                        container.addChild(orthoCloud);
                    } else {
                        CVLog::Warning(
                                QString("[Bundler] Failed to ortho-rectify "
                                        "image '%1' as a cloud!")
                                        .arg(image->getName()));
                    }
                }
            }
        }
 
        // undistortion
        if (sensor && undistortImages)
            if (!sensor->undistort(image, true))
                CVLog::Warning(
                        QString("[Bundler] Failed to undistort image '%1'!")
                                .arg(image->getName()));
 
        // DTM color 'blending'
        if (sensor && generateColoredDTM) {
            assert(mntSamples && mntColors);
            unsigned sampleCount = mntSamples->size();
            const QRgb blackValue = qRgb(0, 0, 0);
 
            ccGLMatrix sensorMatrix =
                    sensor->getRigidTransformation().inverse();
 
            // back project each MNT samples in this image to get color
            for (unsigned k = 0; k < sampleCount; ++k) {
                CCVector3 P = *mntSamples->getPointPersistentPtr(k);
 
                // apply bundler equation
                sensorMatrix.apply(P);
                if (cloudViewer::GreaterThanEpsilon(fabs(P.z))) {
                    CCVector3 p(-P.x / P.z, -P.y / P.z, 0.0);
                    // float norm_p2 = p.norm2();
                    // float rp = 1.0+norm_p2*(cam.k1+cam.k2*norm_p2); //images
                    // are already undistorted
                    float rp = 1.0f;
                    CCVector3 pprime = cam.f_pix * rp * p;
 
                    int px = static_cast<int>(image->getW() / 2.0f + pprime.x);
                    if (px >= 0 && px < static_cast<int>(image->getW())) {
                        int py = static_cast<int>(image->getH() / 2.0f -
                                                  pprime.y);
                        if (py >= 0 && py < static_cast<int>(image->getH())) {
                            QRgb rgb = image->data().pixel(px, py);
                            if (qAlpha(rgb) != 0 &&
                                rgb != blackValue)  // black pixels are ignored
                            {
                                int* col = mntColors + 4 * k;
                                col[0] += qRed(rgb);
                                col[1] += qGreen(rgb);
                                col[2] += qBlue(rgb);
                                col[3]++;  // accum
                            }
                        }
                    }
                }
            }
        }
 
        if (keepImagesInMemory) {
            container.addChild(sensor);
        } else {
            delete sensor;
            sensor = 0;
        }
 
        QApplication::processEvents();
 
        if (ipDlg && !inprogress.oneStep()) {
            cancelledByUser = true;
            break;
        }
    }
 
    if (!importKeypoints && keypointsCloud) delete keypointsCloud;
    keypointsCloud = 0;
 
    if (!importKeypoints && altEntity) delete altEntity;
    altEntity = 0;
 
    /*** post-processing steps ***/
 
    if (orthoRectifyImages && OR_pixelSize > 0) {
        //'close' log
        QFile f(imageDir.absoluteFilePath("ortho_rectification_log.txt"));
        if (f.open(QIODevice::WriteOnly | QIODevice::Text)) {
            QTextStream stream(&f);
            stream.setRealNumberNotation(QTextStream::FixedNotation);
            stream.setRealNumberPrecision(12);
            stream << "PixelSize" << ' ' << OR_pixelSize << QtCompat::endl;
            stream << "Global3DBBox" << ' ' << OR_globalCorners[0] << ' '
                   << OR_globalCorners[1] << ' ' << OR_globalCorners[2] << ' '
                   << OR_globalCorners[3] << QtCompat::endl;
            int globalWidth = static_cast<int>(
                    (OR_globalCorners[2] - OR_globalCorners[0]) / OR_pixelSize);
            int globalHeight = static_cast<int>(
                    (OR_globalCorners[3] - OR_globalCorners[1]) / OR_pixelSize);
            stream << "Global2DBBox" << ' ' << 0 << ' ' << 0 << ' '
                   << globalWidth - 1 << ' ' << globalHeight - 1
                   << QtCompat::endl;
 
            for (unsigned i = 0; i < OR_infos.size(); ++i) {
                stream << "Image" << ' ' << OR_infos[i].name << ' ';
                stream << "Local3DBBox" << ' ' << OR_infos[i].minC[0] << ' '
                       << OR_infos[i].minC[1] << ' ' << OR_infos[i].maxC[0]
                       << ' ' << OR_infos[i].maxC[1] << ' ';
                int xShiftGlobal = static_cast<int>(
                        (OR_infos[i].minC[0] - OR_globalCorners[0]) /
                        OR_pixelSize);
                int yShiftGlobal = static_cast<int>(
                        (OR_globalCorners[3] - OR_infos[i].maxC[1]) /
                        OR_pixelSize);
                stream << "Local2DBBox" << ' ' << xShiftGlobal << ' '
                       << yShiftGlobal << ' '
                       << xShiftGlobal + (static_cast<int>(OR_infos[i].w) - 1)
                       << ' '
                       << yShiftGlobal + (static_cast<int>(OR_infos[i].h) - 1)
                       << QtCompat::endl;
            }
        } else {
            CVLog::Warning(
                    "Failed to save orthorectification log file! "
                    "(ortho_rectification_log.txt)");
        }
    }
 
    if (generateColoredDTM) {
        assert(mntSamples && mntColors);
 
        if (!cancelledByUser) {
            // 3rd step: project each point in all images and get average color
            unsigned sampleCount = mntSamples->size();
 
            ccPointCloud* mntCloud = new ccPointCloud("colored DTM");
            if (mntCloud->reserve(sampleCount) &&
                mntCloud->reserveTheRGBTable()) {
                // for each point
                unsigned realCount = 0;
                const int* col = mntColors;
                for (unsigned i = 0; i < sampleCount; ++i, col += 4) {
                    if (col[3] > 0)  // accum
                    {
                        const CCVector3* X =
                                mntSamples->getPointPersistentPtr(i);
                        ecvColor::Rgb avgCol(
                                static_cast<ColorCompType>(col[0] / col[3]),
                                static_cast<ColorCompType>(col[1] / col[3]),
                                static_cast<ColorCompType>(col[2] / col[3]));
                        mntCloud->addPoint(*X);
                        mntCloud->addRGBColor(avgCol);
                        ++realCount;
                    }
                }
 
                if (realCount != 0) {
                    if (realCount < sampleCount) mntCloud->resize(realCount);
 
                    mntCloud->showColors(true);
                    container.addChild(mntCloud);
                    CVLog::Warning(
                            "[Bundler] DTM vertices successfully generated: "
                            "clean it if necessary then use 'Edit > Mesh > "
                            "Compute Delaunay 2D (Best LS plane)' then "
                            "'Smooth' to get a proper mesh");
 
                    if (!parameters.alwaysDisplayLoadDialog) {
                        // auto save DTM vertices
                        BinFilter bf;
                        QString outputFile = imageDir.absoluteFilePath(
                                "colored_dtm_vertices.bin");
                        BinFilter::SaveParameters parameters;
                        { parameters.alwaysDisplaySaveDialog = false; }
                        if (bf.saveToFile(mntCloud, outputFile, parameters) ==
                            CC_FERR_NO_ERROR)
                            CVLog::Print(QString("[Bundler] Color DTM vertices "
                                                 "automatically saved to '%2'")
                                                 .arg(outputFile));
                        else
                            CVLog::Warning(QString("[Bundler] Failed to save "
                                                   "DTM vertices to '%2'")
                                                   .arg(outputFile));
                    }
                } else {
                    CVLog::Warning(
                            "[Bundler] Failed to generate DTM! (no point "
                            "viewed in images?)");
                }
            } else {
                CVLog::Warning(
                        "[Bundler] Failed to generate DTM vertices cloud! (not "
                        "enough memory?)");
                delete mntCloud;
                mntCloud = 0;
            }
        }
 
        delete mntSamples;
        mntSamples = 0;
        delete[] mntColors;
        mntColors = 0;
    }
 
    return cancelledByUser ? CC_FERR_CANCELED_BY_USER : CC_FERR_NO_ERROR;
}