ecvCameraSensor.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "ecvCameraSensor.h"
 
#include <cmath>
 
// LOCAL
#include "ecvDisplayTools.h"
#include "ecvImage.h"
#include "ecvMesh.h"
#include "ecvPointCloud.h"
 
// CV_CORE_LIB
#include <ConjugateGradient.h>
 
// QT
#include <QDir>
 
// Qt5/Qt6 Compatibility
#include <QtCompat.h>
 
ccCameraSensor::IntrinsicParameters::IntrinsicParameters()
    : vertFocal_pix(1.0f),
      skew(0),
      vFOV_rad(0),
      zNear_mm(0.001f),
      zFar_mm(1000.0f),
      arrayWidth(0),
      arrayHeight(0) {
    pixelSize_mm[0] = 1.0f;
    pixelSize_mm[1] = 1.0f;
 
    principal_point[0] = arrayWidth / 2.0f;
    principal_point[1] = arrayHeight / 2.0f;
}
 
void ccCameraSensor::IntrinsicParameters::GetKinectDefaults(
        IntrinsicParameters& params) {
    // default Kinect parameters from:
    //  "Accuracy and Resolution of Kinect Depth Data for Indoor Mapping
    //  Applications" Kourosh Khoshelham and Sander Oude Elberink
    float focal_mm = static_cast<float>(
            5.45 * 1.0e-3);  // focal length (real distance in meter)
    float pixelSize_mm = static_cast<float>(
            9.3 * 1.0e-6);  // pixel size (real distance in meter)
    params.vertFocal_pix = ConvertFocalMMToPix(focal_mm, pixelSize_mm);
    params.pixelSize_mm[0] = pixelSize_mm;
    params.pixelSize_mm[1] = pixelSize_mm;
    params.skew = static_cast<float>(0.0);  // skew in image
    params.vFOV_rad = static_cast<float>(
            43.0 * M_PI / 180.0);  // vertical field of view (in rad)
    params.zNear_mm = static_cast<float>(
            0.5);  // distance to the closest recordable depth
    params.zFar_mm = static_cast<float>(
            5.0);              // distance to the furthest recordable depth
    params.arrayWidth = 640;   // image width
    params.arrayHeight = 480;  // image height
    params.principal_point[0] = params.arrayWidth / 2.0f;
    params.principal_point[1] = params.arrayHeight / 2.0f;
}
 
ccCameraSensor::BrownDistortionParameters::BrownDistortionParameters() {
    principalPointOffset[0] = 0;
    principalPointOffset[1] = 0;
    linearDisparityParams[0] = 0;
    linearDisparityParams[1] = 0;
    K_BrownParams[0] = 0;
    K_BrownParams[1] = 0;
    K_BrownParams[2] = 0;
    P_BrownParams[0] = 0;
    P_BrownParams[1] = 0;
}
 
void ccCameraSensor::BrownDistortionParameters::GetKinectDefaults(
        BrownDistortionParameters& params) {
    // default Kinect parameters from:
    //  "Accuracy and Resolution of Kinect Depth Data for Indoor Mapping
    //  Applications" Kourosh Khoshelham and Sander Oude Elberink
    params.principalPointOffset[0] = static_cast<float>(-0.063 * 1.0e-3);
    params.principalPointOffset[1] = static_cast<float>(-0.039 * 1.0e-3);
    params.linearDisparityParams[0] = static_cast<float>(-2.85 * 1.0e-3);
    params.linearDisparityParams[1] = static_cast<float>(3.0);
    params.K_BrownParams[0] = static_cast<float>(2.42 * 1.0e-3);
    params.K_BrownParams[1] = static_cast<float>(-1.7 * 1.0e-4);
    params.K_BrownParams[2] = static_cast<float>(0.0);
    params.P_BrownParams[0] = static_cast<float>(-3.3 * 1.0e-4);
    params.P_BrownParams[1] = static_cast<float>(5.25 * 1.0e-4);
}
 
ccCameraSensor::FrustumInformation::FrustumInformation()
    : isComputed(false),
      drawFrustum(false),
      drawSidePlanes(false),
      frustumCorners(nullptr),
      frustumHull(nullptr) {}
 
ccCameraSensor::FrustumInformation::~FrustumInformation() {
    // always delete the hull before the corners, are it depends on them!
    if (frustumHull) {
        delete frustumHull;
        frustumHull = nullptr;
    }
    if (frustumCorners) {
        delete frustumCorners;
        frustumCorners = nullptr;
    }
}
 
bool ccCameraSensor::FrustumInformation::initFrustumCorners() {
    if (!frustumCorners) {
        frustumCorners = new ccPointCloud("Frustum corners");
    } else {
        frustumCorners->clear();
    }
 
    if (!frustumCorners->reserve(8)) {
        // not enough memory to load frustum corners!
        delete frustumCorners;
        frustumCorners = nullptr;
        return false;
    }
    return true;
}
 
bool ccCameraSensor::FrustumInformation::initFrustumHull() {
    // we only need to do this once!
    if (frustumHull) return true;
 
    if (!frustumCorners || frustumCorners->size() < 8) {
        CVLog::Warning(
                "[ccCameraSensor::FrustumInformation::initFrustumHull] Corners "
                "are not initialized!");
        return false;
    }
 
    frustumHull = new ccMesh(frustumCorners);
    if (!frustumHull->reserve(6 * 2)) {
        CVLog::Warning(
                "[ccCameraSensor::FrustumInformation::initFrustumHull] Not "
                "enough memory!");
        delete frustumHull;
        frustumHull = nullptr;
        return false;
    }
 
    frustumHull->addTriangle(0, 2, 3);
    frustumHull->addTriangle(0, 3, 1);
 
    frustumHull->addTriangle(2, 4, 5);
    frustumHull->addTriangle(2, 5, 3);
 
    frustumHull->addTriangle(4, 6, 7);
    frustumHull->addTriangle(4, 7, 5);
 
    frustumHull->addTriangle(6, 0, 1);
    frustumHull->addTriangle(6, 1, 7);
 
    frustumHull->addTriangle(6, 4, 2);
    frustumHull->addTriangle(6, 2, 0);
 
    frustumHull->addTriangle(1, 3, 5);
    frustumHull->addTriangle(1, 5, 7);
 
    frustumHull->setVisible(true);
 
    return true;
}
 
ccCameraSensor::ccCameraSensor()
    : ccSensor("Camera Sensor"),
      m_plane_color(ecvColor::red),
      m_projectionMatrixIsValid(false) {
    // graphic representation
    lockVisibility(false);
    setSelectionBehavior(SELECTION_FIT_BBOX);
}
 
ccCameraSensor::ccCameraSensor(const IntrinsicParameters& iParams)
    : ccSensor("Camera Sensor"),
      m_plane_color(ecvColor::red),
      m_projectionMatrixIsValid(false) {
    // graphic representation
    lockVisibility(false);
    setSelectionBehavior(SELECTION_FIT_BBOX);
 
    // projection
    setIntrinsicParameters(iParams);
}
 
ccCameraSensor::ccCameraSensor(const ccCameraSensor& sensor)
    : ccSensor(sensor),
      m_plane_color(ecvColor::red),
      m_projectionMatrix(sensor.m_projectionMatrix),
      m_projectionMatrixIsValid(false) {
    setIntrinsicParameters(sensor.m_intrinsicParams);
 
    // distortion params
    if (m_distortionParams) {
        LensDistortionParameters::Shared clonedDistParams;
        switch (m_distortionParams->getModel()) {
            case SIMPLE_RADIAL_DISTORTION: {
                // simply duplicate the struct
                RadialDistortionParameters* clone =
                        new RadialDistortionParameters;
                *clone = *static_cast<const RadialDistortionParameters*>(
                        sensor.m_distortionParams.data());
                clonedDistParams = LensDistortionParameters::Shared(clone);
            } break;
 
            case EXTENDED_RADIAL_DISTORTION: {
                // simply duplicate the struct
                ExtendedRadialDistortionParameters* clone =
                        new ExtendedRadialDistortionParameters;
                *clone =
                        *static_cast<const ExtendedRadialDistortionParameters*>(
                                sensor.m_distortionParams.data());
                clonedDistParams = LensDistortionParameters::Shared(clone);
            } break;
 
            case BROWN_DISTORTION: {
                // simply duplicate the struct
                BrownDistortionParameters* clone =
                        new BrownDistortionParameters;
                *clone = *static_cast<const BrownDistortionParameters*>(
                        sensor.m_distortionParams.data());
                clonedDistParams = LensDistortionParameters::Shared(clone);
            } break;
 
            default:
                // unhandled type?!
                assert(false);
                break;
        }
        setDistortionParameters(clonedDistParams);
    }
}
 
ccCameraSensor::~ccCameraSensor() {}
 
ccBBox ccCameraSensor::getOwnBB(bool withGLFeatures /*=false*/) {
    if (!withGLFeatures) {
        return ccBBox();
    }
 
    // get current sensor position
    ccIndexedTransformation sensorPos;
    if (!getAbsoluteTransformation(sensorPos, m_activeIndex)) {
        return ccBBox();
    }
 
    CCVector3 upperLeftPoint = computeUpperLeftPoint();
 
    ccPointCloud cloud;
    if (!cloud.reserve(5)) {
        // not enough memory?!
        return ccBBox();
    }
 
    cloud.addPoint(CCVector3(0, 0, 0));
    cloud.addPoint(
            CCVector3(upperLeftPoint.x, upperLeftPoint.y, -upperLeftPoint.z));
    cloud.addPoint(
            CCVector3(-upperLeftPoint.x, upperLeftPoint.y, -upperLeftPoint.z));
    cloud.addPoint(
            CCVector3(-upperLeftPoint.x, -upperLeftPoint.y, -upperLeftPoint.z));
    cloud.addPoint(
            CCVector3(upperLeftPoint.x, -upperLeftPoint.y, -upperLeftPoint.z));
 
    // add frustum corners if necessary
    if (m_frustumInfos.isComputed &&
        (m_frustumInfos.drawFrustum || m_frustumInfos.drawSidePlanes) &&
        m_frustumInfos.frustumCorners) {
        unsigned cornerCount = m_frustumInfos.frustumCorners->size();
        if (cloud.reserve(cloud.size() + cornerCount)) {
            for (unsigned i = 0; i < cornerCount; ++i)
                cloud.addPoint(*m_frustumInfos.frustumCorners->getPoint(i));
        }
    }
 
    cloud.applyRigidTransformation(sensorPos);
    return cloud.getOwnBB(false);
}
 
ccBBox ccCameraSensor::getOwnFitBB(ccGLMatrix& trans) {
    // get current sensor position
    ccIndexedTransformation sensorPos;
    if (!getAbsoluteTransformation(sensorPos, m_activeIndex)) {
        return ccBBox();
    }
 
    trans = sensorPos;
 
    CCVector3 upperLeftPoint = computeUpperLeftPoint();
    if (upperLeftPoint.z > 0) {
        return ccBBox(-upperLeftPoint,
                      CCVector3(upperLeftPoint.x, upperLeftPoint.y, 0));
    } else {
        return ccBBox(CCVector3(upperLeftPoint.x, upperLeftPoint.y, 0),
                      -upperLeftPoint);
    }
}
 
void ccCameraSensor::setVertFocal_pix(float vertFocal_pix) {
    assert(vertFocal_pix > 0);
    m_intrinsicParams.vertFocal_pix = vertFocal_pix;
 
    // old frustum is not valid anymore!
    m_frustumInfos.isComputed = false;
    // same thing for the projection matrix
    m_projectionMatrixIsValid = false;
}
 
void ccCameraSensor::setVerticalFov_rad(float fov_rad) {
    assert(fov_rad > 0);
    m_intrinsicParams.vFOV_rad = fov_rad;
}
 
void ccCameraSensor::setIntrinsicParameters(const IntrinsicParameters& params) {
    m_intrinsicParams = params;
    // old frustum is not valid anymore!
    m_frustumInfos.isComputed = false;
    // same thing for the projection matrix
    m_projectionMatrixIsValid = false;
}
 
bool ccCameraSensor::applyViewport() {
    if (!ecvDisplayTools::GetCurrentScreen()) {
        CVLog::Warning(
                "[ccCameraSensor::applyViewport] No associated display!");
        return false;
    }
 
    ccIndexedTransformation trans;
    if (!getActiveAbsoluteTransformation(trans)) {
        return false;
    }
 
    if (m_intrinsicParams.arrayHeight <= 0) {
        CVLog::Warning("[ccCameraSensor::applyViewport] Sensor height is 0!");
        return false;
    }
 
    float fov_deg = cloudViewer::RadiansToDegrees(m_intrinsicParams.vFOV_rad);
 
    ecvViewportParameters viewParams = ecvDisplayTools::GetViewportParameters();
    viewParams.fov_deg = fov_deg;
    viewParams.zFar = static_cast<double>(m_intrinsicParams.zFar_mm);
    viewParams.zNear = static_cast<double>(m_intrinsicParams.zNear_mm);
 
    CCVector3 upperLeftPointd = computeUpperLeftPoint();
 
    // pos
    CCVector3 camC = trans.getTranslationAsVec3D();
    viewParams.setCameraCenter(CCVector3d::fromArray(camC));
 
    int flag = 1;
    if (upperLeftPointd.z > 0) {
        flag = 1;
    } else {
        flag = -1;
    }
 
    // up
    CCVector3 upDir = flag * trans.getColumnAsVec3D(1);
    upDir.normalize();
    if (cloudViewer::LessThanEpsilon(std::fabs(upDir.norm()))) {
        CVLog::Warning(
                "[ccCameraSensor::applyViewport] Viewing dir is parallel to "
                "the plane Y!");
        return false;
    }
    viewParams.up = CCVector3d::fromArray(upDir);
 
    // focal
    CCVector3 viewDir = flag * trans.getColumnAsVec3D(2);
    CCVector3 focal3D = camC - viewDir;
    viewParams.focal = CCVector3d::fromArray(focal3D);
    viewParams.setPivotPoint(viewParams.focal, true);
 
    ecvDisplayTools::SetViewportParameters(viewParams);
    ecvDisplayTools::UpdateScreen();
 
    return true;
}
 
bool ccCameraSensor::getProjectionMatrix(ccGLMatrix& matrix) {
    if (!m_projectionMatrixIsValid) computeProjectionMatrix();
 
    matrix = m_projectionMatrix;
 
    return m_projectionMatrixIsValid;  // even if we have computed the
                                       // projection matrix, it may still have
                                       // failed!
}
 
void ccCameraSensor::computeProjectionMatrix() {
    m_projectionMatrix.toZero();
    float* mat = m_projectionMatrix.data();
 
    // diagonal
    mat[0] = getHorizFocal_pix();
    mat[5] = getVertFocal_pix();
    mat[10] = 1.0f;
    mat[15] = 1.0f;
 
    // skew
    mat[4] = m_intrinsicParams.skew;
 
    // translation from image (0,0)
    mat[12] = m_intrinsicParams.principal_point[0];
    mat[13] = m_intrinsicParams.principal_point[1];
 
    m_projectionMatrixIsValid = true;
}
 
bool ccCameraSensor::toFile_MeOnly(QFile& out, short dataVersion) const {
    assert(out.isOpen() && (out.openMode() & QIODevice::WriteOnly));
    if (dataVersion < 38) {
        assert(false);
        return false;
    }
 
    if (!ccSensor::toFile_MeOnly(out, dataVersion)) return false;
 
    // projection matrix (35 <= dataVersion < 38)
    // if (!m_projectionMatrix.toFile(out, dataVersion))
    //  return WriteError();
 
    // IntrinsicParameters
    QDataStream outStream(&out);
    outStream << m_intrinsicParams.vertFocal_pix;
    outStream << m_intrinsicParams.arrayWidth;
    outStream << m_intrinsicParams.arrayHeight;
    outStream << m_intrinsicParams.pixelSize_mm[0];
    outStream << m_intrinsicParams.pixelSize_mm[1];
    outStream << m_intrinsicParams.skew;
    outStream << m_intrinsicParams.vFOV_rad;
    outStream << m_intrinsicParams.zNear_mm;
    outStream << m_intrinsicParams.zFar_mm;
    outStream << m_intrinsicParams.principal_point[0];
    outStream << m_intrinsicParams.principal_point[1];
 
    // distortion parameters (dataVersion>=38)
    DistortionModel distModel = m_distortionParams
                                        ? m_distortionParams->getModel()
                                        : NO_DISTORTION_MODEL;
    outStream << static_cast<uint32_t>(distModel);
 
    if (m_distortionParams) {
        switch (m_distortionParams->getModel()) {
            case SIMPLE_RADIAL_DISTORTION: {
                RadialDistortionParameters* params =
                        static_cast<RadialDistortionParameters*>(
                                m_distortionParams.data());
                outStream << params->k1;
                outStream << params->k2;
            } break;
 
            case EXTENDED_RADIAL_DISTORTION: {
                ExtendedRadialDistortionParameters* params =
                        static_cast<ExtendedRadialDistortionParameters*>(
                                m_distortionParams.data());
                outStream << params->k1;
                outStream << params->k2;
                outStream << params->k3;
            } break;
 
            case BROWN_DISTORTION: {
                BrownDistortionParameters* params =
                        static_cast<BrownDistortionParameters*>(
                                m_distortionParams.data());
                outStream << params->K_BrownParams[0];
                outStream << params->K_BrownParams[1];
                outStream << params->K_BrownParams[2];
                outStream << params->P_BrownParams[0];
                outStream << params->P_BrownParams[1];
                outStream << params->principalPointOffset[0];
                outStream << params->principalPointOffset[1];
                outStream << params->linearDisparityParams[0];
                outStream << params->linearDisparityParams[1];
            } break;
            default:
                assert(false);
                break;
        }
    }
 
    // FrustumInformation
    outStream << m_frustumInfos.drawFrustum;
    outStream << m_frustumInfos.drawSidePlanes;
    outStream << m_frustumInfos.center.x;
    outStream << m_frustumInfos.center.y;
    outStream << m_frustumInfos.center.z;
 
    return true;
}
 
short ccCameraSensor::minimumFileVersion_MeOnly() const {
    // Camera sensor with intrinsic/distortion params requires version 43+
    return std::max(static_cast<short>(43),
                    ccSensor::minimumFileVersion_MeOnly());
}
 
bool ccCameraSensor::fromFile_MeOnly(QFile& in,
                                     short dataVersion,
                                     int flags,
                                     LoadedIDMap& oldToNewIDMap) {
    if (!ccSensor::fromFile_MeOnly(in, dataVersion, flags, oldToNewIDMap))
        return false;
 
    // serialization wasn't possible before v3.5!
    if (dataVersion < 35) return false;
 
    // projection matrix (35 <= dataVersion < 38)
    if (dataVersion < 38) {
        // we don't need to save/load this matrix as it is dynamically computed!
        ccGLMatrix dummyMatrix;
        if (!dummyMatrix.fromFile(in, dataVersion, flags, oldToNewIDMap))
            return ReadError();
    }
    m_projectionMatrixIsValid = false;
 
    // IntrinsicParameters
    QDataStream inStream(&in);
    inStream >> m_intrinsicParams.vertFocal_pix;
    inStream >> m_intrinsicParams.arrayWidth;
    inStream >> m_intrinsicParams.arrayHeight;
    inStream >> m_intrinsicParams.pixelSize_mm[0];
    inStream >> m_intrinsicParams.pixelSize_mm[1];
    inStream >> m_intrinsicParams.skew;
    inStream >> m_intrinsicParams.vFOV_rad;
    inStream >> m_intrinsicParams.zNear_mm;
    inStream >> m_intrinsicParams.zFar_mm;
 
    if (dataVersion >= 43) {
        // since version 43, we added the principal point
        inStream >> m_intrinsicParams.principal_point[0];
        inStream >> m_intrinsicParams.principal_point[1];
    } else {
        m_intrinsicParams.principal_point[0] =
                m_intrinsicParams.arrayWidth / 2.0f;
        m_intrinsicParams.principal_point[1] =
                m_intrinsicParams.arrayHeight / 2.0f;
    }
 
    // distortion parameters
    DistortionModel distModel = NO_DISTORTION_MODEL;
    if (dataVersion < 38) {
        // before v38, only Brown's parameters were used (and always set)
        distModel = BROWN_DISTORTION;
    } else {
        uint32_t distModeli;
        inStream >> distModeli;
        distModel = static_cast<DistortionModel>(distModeli);
    }
 
    // load parameters (if any)
    switch (distModel) {
        case SIMPLE_RADIAL_DISTORTION: {
            RadialDistortionParameters* distParams =
                    new RadialDistortionParameters;
            inStream >> distParams->k1;
            inStream >> distParams->k2;
 
            setDistortionParameters(
                    LensDistortionParameters::Shared(distParams));
        } break;
 
        case EXTENDED_RADIAL_DISTORTION: {
            ExtendedRadialDistortionParameters* distParams =
                    new ExtendedRadialDistortionParameters;
            inStream >> distParams->k1;
            inStream >> distParams->k2;
            inStream >> distParams->k3;
 
            setDistortionParameters(
                    LensDistortionParameters::Shared(distParams));
        } break;
 
        case BROWN_DISTORTION: {
            BrownDistortionParameters* distParams =
                    new BrownDistortionParameters;
            inStream >> distParams->K_BrownParams[0];
            inStream >> distParams->K_BrownParams[1];
            inStream >> distParams->K_BrownParams[2];
            inStream >> distParams->P_BrownParams[0];
            inStream >> distParams->P_BrownParams[1];
            inStream >> distParams->principalPointOffset[0];
            inStream >> distParams->principalPointOffset[1];
            inStream >> distParams->linearDisparityParams[0];
            inStream >> distParams->linearDisparityParams[1];
 
            setDistortionParameters(
                    LensDistortionParameters::Shared(distParams));
        } break;
 
        default:
            // do nothing
            break;
    }
 
    // FrustumInformation
    if (dataVersion < 38) {
        bool dummyBool;  // formerly: m_frustumInfos.isComputed (no need to
                         // save/load it!)
        inStream >> dummyBool;
    }
    m_frustumInfos.isComputed = false;
    inStream >> m_frustumInfos.drawFrustum;
    inStream >> m_frustumInfos.drawSidePlanes;
    ccSerializationHelper::CoordsFromDataStream(inStream, flags,
                                                m_frustumInfos.center.u, 3);
 
    if (dataVersion < 38) {
        // frustum corners: no need to save/load them!
        for (unsigned i = 0; i < 8; ++i) {
            CCVector3 P;
            ccSerializationHelper::CoordsFromDataStream(inStream, flags, P.u,
                                                        3);
        }
    }
 
    return true;
}
 
bool ccCameraSensor::fromLocalCoordToGlobalCoord(const CCVector3& localCoord,
                                                 CCVector3& globalCoord) const {
    ccIndexedTransformation trans;
 
    if (!getActiveAbsoluteTransformation(trans)) return false;
 
    globalCoord = localCoord;
    trans.apply(globalCoord);
 
    return true;
}
 
bool ccCameraSensor::fromGlobalCoordToLocalCoord(const CCVector3& globalCoord,
                                                 CCVector3& localCoord) const {
    ccIndexedTransformation trans;
 
    if (!getActiveAbsoluteTransformation(trans)) return false;
 
    localCoord = globalCoord;
    trans.inverse().apply(localCoord);
 
    return true;
}
 
bool ccCameraSensor::fromLocalCoordToImageCoord(
        const CCVector3& localCoord,
        CCVector2& imageCoord,
        bool withLensError /*=true*/) const {
#ifdef CHECK_THIS_AFTERWARDS
 
    // Change in 3D image coordinates system for good projection
    CCVector3 imageCoordSystem(localCoord.x, localCoord.y, -localCoord.z);
 
    // We test if the point is in front or behind the sensor ? If it is behind
    // (or in the center of the sensor i.e. z=0.0), we can't project!
    if (imageCoordSystem.z < FLT_EPSILON) return false;
 
    // projection
    ccGLMatrix mat;
    if (!getProjectionMatrix(mat)) return false;
    CCVector3 projCoord =
            mat *
            imageCoordSystem;  // at this stage, coordinates are homogeneous
    projCoord = projCoord / projCoord.z;  // coordinates are now in pixels
    CCVector2 initial(projCoord.x, projCoord.y);
    CCVector2 coord = initial;
 
    // apply lens correction if necessary
    // if (withLensError)
    //  fromIdealImCoordToRealImCoord(initial, coord);
 
    // test if the projected point is into the image boundaries (width,height)
    if (coord.x < 0 || coord.x >= m_intrinsicParams.arrayWidth || coord.y < 0 ||
        coord.y >= m_intrinsicParams.arrayHeight) {
        return false;
    }
 
    // Change in 3D image coordinates system
    imageCoord = coord;
 
#else
 
    // We test if the point is in front or behind the sensor ? If it is behind
    // (or in the center of the sensor i.e. depth = 0), we can't project!
    double depth = -static_cast<double>(
            localCoord.z);  // warning: the camera looks backward!
#define BACK_POINTS_CULLING
#ifdef BACK_POINTS_CULLING
    if (depth < FLT_EPSILON) return false;
#endif
 
    // perspective division
    CCVector2d p(localCoord.x / depth, localCoord.y / depth);
 
    // conversion to pixel coordinates
    double factor = m_intrinsicParams.vertFocal_pix;
 
    // apply radial distortion (if any)
    if (withLensError && m_distortionParams) {
        if (m_distortionParams->getModel() == SIMPLE_RADIAL_DISTORTION) {
            const RadialDistortionParameters* params =
                    static_cast<RadialDistortionParameters*>(
                            m_distortionParams.data());
            double norm2 = p.norm2();
            double rp = 1.0 +
                        norm2 * (params->k1 +
                                 norm2 * params->k2);  // scaling factor to undo
                                                       // the radial distortion
            factor *= rp;
        } else if (m_distortionParams->getModel() ==
                   EXTENDED_RADIAL_DISTORTION) {
            const ExtendedRadialDistortionParameters* params =
                    static_cast<ExtendedRadialDistortionParameters*>(
                            m_distortionParams.data());
            double norm2 = p.norm2();
            double rp =
                    1.0 +
                    norm2 * (params->k1 +
                             norm2 * (params->k2 +
                                      norm2 * params->k3));  // scaling factor
                                                             // to undo the
                                                             // radial
                                                             // distortion
            factor *= rp;
        }
    }
    //*/
 
    CCVector2d p2 = p * factor;
 
    p2.x += m_intrinsicParams.principal_point[0];
    p2.y = m_intrinsicParams.principal_point[1] - p2.y;
 
    imageCoord.x = static_cast<PointCoordinateType>(p2.x);
    imageCoord.y = static_cast<PointCoordinateType>(p2.y);
 
#endif
 
    return true;
}
 
bool ccCameraSensor::fromImageCoordToLocalCoord(
        const CCVector2& imageCoord,
        CCVector3& localCoord,
        PointCoordinateType depth,
        bool withLensCorrection /*=true*/) const {
    CCVector3d p2(imageCoord.x, imageCoord.y, 0.0);
 
    p2.x -= m_intrinsicParams.principal_point[0];
    p2.y = m_intrinsicParams.principal_point[1] - p2.y;
 
    // apply inverse radial distortion (if any)
    // TODO
 
    double factor = static_cast<double>(m_intrinsicParams.vertFocal_pix);
    CCVector3d p = p2 / factor;
 
    // perspective
    localCoord =
            CCVector3(static_cast<PointCoordinateType>(p.x * depth),
                      static_cast<PointCoordinateType>(p.y * depth), -depth);
 
    return true;
}
 
bool ccCameraSensor::fromGlobalCoordToImageCoord(
        const CCVector3& globalCoord,
        CCVector2& imageCoord,
        bool withLensError /*=true*/) const {
    CCVector3 localCoord;
    if (!fromGlobalCoordToLocalCoord(globalCoord, localCoord)) return false;
 
    return fromLocalCoordToImageCoord(localCoord, imageCoord, withLensError);
}
 
bool ccCameraSensor::fromImageCoordToGlobalCoord(
        const CCVector2& imageCoord,
        CCVector3& globalCoord,
        PointCoordinateType z0,
        bool withLensCorrection /*=true*/) const {
    ccIndexedTransformation trans;
 
    if (!getActiveAbsoluteTransformation(trans)) return false;
 
    CCVector3 localCoord;
    if (!fromImageCoordToLocalCoord(imageCoord, localCoord, PC_ONE,
                                    withLensCorrection))
        return false;
 
    // update altitude: we must compute the intersection between the plane Z =
    // Z0 (world) and the camera (input pixel) viewing direction
    CCVector3 viewDir = localCoord;
    trans.applyRotation(viewDir);
    viewDir.normalize();
 
    if (cloudViewer::LessThanEpsilon(fabs(viewDir.z))) {
        // viewing dir is parallel to the plane Z = Z0!
        return false;
    }
 
    CCVector3 camC = trans.getTranslationAsVec3D();
    PointCoordinateType dZ = z0 - camC.z;
 
    PointCoordinateType u = dZ / viewDir.z;
#ifdef BACK_POINTS_CULLING
    if (u < 0) return false;  // wrong direction!
#endif
 
    globalCoord = camC + u * viewDir;
 
    return true;
}
 
bool ccCameraSensor::fromRealImCoordToIdealImCoord(const CCVector2& real,
                                                   CCVector2& ideal) const {
    // no distortion parameters?
    if (!m_distortionParams) {
        ideal = real;
        return true;
    }
 
    switch (m_distortionParams->getModel()) {
        case SIMPLE_RADIAL_DISTORTION:
        case EXTENDED_RADIAL_DISTORTION: {
            // TODO: we need a pre-computed distortion map to do this!
        } break;
 
        case BROWN_DISTORTION: {
            const BrownDistortionParameters* distParams =
                    static_cast<BrownDistortionParameters*>(
                            m_distortionParams.data());
            const float& sX = m_intrinsicParams.pixelSize_mm[0];
            const float& sY = m_intrinsicParams.pixelSize_mm[1];
 
            // 1st correction : principal point correction
            float cx = m_intrinsicParams.principal_point[0] +
                       distParams->principalPointOffset[0] / sX;  // in pixels
            float cy = m_intrinsicParams.principal_point[1] +
                       distParams->principalPointOffset[1] / sY;  // in pixels
 
            // 2nd correction : Brown's lens distortion correction
            float dx = (static_cast<float>(real.x) - cx) *
                       m_intrinsicParams.pixelSize_mm[0];  // real distance
            float dy = (static_cast<float>(real.y) - cy) *
                       m_intrinsicParams.pixelSize_mm[1];  // real distance
            float dx2 = dx * dx;
            float dy2 = dy * dy;
            float r = sqrt(dx2 + dy2);
            float r2 = r * r;
            float r4 = r2 * r2;
            float r6 = r4 * r2;
            const float& K1 = distParams->K_BrownParams[0];
            const float& K2 = distParams->K_BrownParams[1];
            const float& K3 = distParams->K_BrownParams[2];
            const float& P1 = distParams->P_BrownParams[0];
            const float& P2 = distParams->P_BrownParams[1];
 
            // compute new value
            float correctedX = (dx * (1 + K1 * r2 + K2 * r4 + K3 * r6) +
                                P1 * (r2 + 2 * dx2) + 2 * P2 * dx * dy);
            float correctedY = (dy * (1 + K1 * r2 + K2 * r4 + K3 * r6) +
                                P2 * (r2 + 2 * dy2) + 2 * P1 * dx * dy);
            ideal.x = static_cast<PointCoordinateType>(correctedX / sX);
            ideal.y = static_cast<PointCoordinateType>(correctedY / sY);
 
            // We test if the new pixel falls inside the image boundaries
            // return ( ideal.x >= 0 && ideal.x < m_intrinsicParams.arrayWidth
            //      &&  ideal.y >= 0 && ideal.y <
            // m_intrinsicParams.arrayHeight
            //); DGM: the ideal pixel can be outside of the original image of
            // course!!!
            return true;
        }
 
        default:
            // not handled?
            assert(false);
            break;
    }
 
    return false;
}
 
bool ccCameraSensor::computeUncertainty(const CCVector2& pixel,
                                        const float depth,
                                        Vector3Tpl<ScalarType>& sigma) const {
    // no distortion parameters?
    if (!m_distortionParams) {
        return false;
    }
 
    switch (m_distortionParams->getModel()) {
        case SIMPLE_RADIAL_DISTORTION:
        case EXTENDED_RADIAL_DISTORTION: {
            // TODO
            return false;
        } break;
 
        case BROWN_DISTORTION: {
            const BrownDistortionParameters* distParams =
                    static_cast<BrownDistortionParameters*>(
                            m_distortionParams.data());
            // TODO ==> check if the input pixel coordinate must be the real or
            // ideal projection
 
            const int& width = m_intrinsicParams.arrayWidth;
            const int& height = m_intrinsicParams.arrayHeight;
            const float* c = m_intrinsicParams.principal_point;
 
            // check validity
            if (pixel.x < 0 || pixel.x > width || pixel.y < 0 ||
                pixel.y > height || depth < FLT_EPSILON)
                return false;
 
            // init parameters
            const float& A = distParams->linearDisparityParams[0];
            float z2 = depth * depth;
            float invSigmaD = 8.0f;
            float factor = A * z2 / invSigmaD;
 
            const float& mu = m_intrinsicParams.pixelSize_mm[0];
            const float verFocal_pix = getVertFocal_pix();
            const float horizFocal_pix = getHorizFocal_pix();
 
            // computes uncertainty
            sigma.x = static_cast<ScalarType>(
                    std::abs(factor * (pixel.x - c[0]) / horizFocal_pix));
            sigma.y = static_cast<ScalarType>(
                    std::abs(factor * (pixel.y - c[1]) / verFocal_pix));
            sigma.z = static_cast<ScalarType>(std::abs(factor * mu));
 
            return true;
        }
 
        default: {
            // not handled?
            assert(false);
        } break;
    }
 
    return false;
}
 
bool ccCameraSensor::computeUncertainty(
        cloudViewer::ReferenceCloud* points,
        std::vector<Vector3Tpl<ScalarType>>&
                accuracy /*, bool lensCorrection*/) {
    if (!points || points->size() == 0) {
        CVLog::Warning(
                "[ccCameraSensor::computeUncertainty] Internal error: invalid "
                "input cloud");
        return false;
    }
 
    if (!m_distortionParams ||
        m_distortionParams->getModel() != BROWN_DISTORTION) {
        CVLog::Warning(
                "[ccCameraSensor::computeUncertainty] Sensor has no associated "
                "uncertainty model! (Brown, etc.)");
        return false;
    }
 
    unsigned count = points->size();
    accuracy.clear();
    try {
        accuracy.resize(count);
    } catch (const std::bad_alloc&) {
        CVLog::Warning(
                "[ccCameraSensor::computeUncertainty] Not enough memory!");
        return false;
    }
 
    for (unsigned i = 0; i < count; i++) {
        const CCVector3* coordGlobal = points->getPoint(i);
        CCVector3 coordLocal;
        CCVector2 coordImage;
 
        if (fromGlobalCoordToLocalCoord(*coordGlobal, coordLocal) &&
            fromLocalCoordToImageCoord(coordLocal, coordImage)) {
            computeUncertainty(coordImage, std::abs(coordLocal.z), accuracy[i]);
        } else {
            accuracy[i].x = accuracy[i].y = accuracy[i].z = NAN_VALUE;
        }
    }
 
    return true;
}
 
// see
// http://opencv.willowgarage.com/documentation/cpp/camera_calibration_and_3d_reconstruction.html
QImage ccCameraSensor::undistort(const QImage& image) const {
    if (image.isNull()) {
        CVLog::Warning("[ccCameraSensor::undistort] Invalid input image!");
        return QImage();
    }
 
    // nothing to do
    // no distortion parameters?
    if (!m_distortionParams) {
        CVLog::Warning("[ccCameraSensor::undistort] No distortion model set!");
        return QImage();
    }
 
    switch (m_distortionParams->getModel()) {
        case SIMPLE_RADIAL_DISTORTION:
        case EXTENDED_RADIAL_DISTORTION: {
            const RadialDistortionParameters* params =
                    static_cast<RadialDistortionParameters*>(
                            m_distortionParams.data());
            float k1 = params->k1;
            float k2 = params->k2;
            if (k1 == 0 && k2 == 0) {
                CVLog::Warning(
                        "[ccCameraSensor::undistort] Invalid radial distortion "
                        "coefficients!");
                return QImage();
            }
            float k3 = 0;
            if (m_distortionParams->getModel() == EXTENDED_RADIAL_DISTORTION) {
                k3 = static_cast<ExtendedRadialDistortionParameters*>(
                             m_distortionParams.data())
                             ->k3;
            }
 
            int width = image.width();
            int height = image.height();
 
            float xScale = image.width() /
                           static_cast<float>(m_intrinsicParams.arrayWidth);
            float yScale = image.height() /
                           static_cast<float>(m_intrinsicParams.arrayHeight);
            float rScale = sqrt(xScale * xScale + yScale * yScale);
 
            // try to reserve memory for new image
            QImage newImage(QSize(width, height), image.format());
            if (newImage.isNull()) {
                CVLog::Warning(
                        "[ccCameraSensor::undistort] Not enough memory!");
                return QImage();
            }
            newImage.fill(0);
 
            float vertFocal_pix = getVertFocal_pix() * xScale;
            float horizFocal_pix = getHorizFocal_pix() * yScale;
            float vf2 = vertFocal_pix * vertFocal_pix;
            float hf2 = horizFocal_pix * horizFocal_pix;
            float cx = m_intrinsicParams.principal_point[0] * xScale;
            float cy = m_intrinsicParams.principal_point[1] * yScale;
            k1 *= rScale;
            k2 *= rScale;
            k3 *= rScale;
 
            assert((image.depth() % 8) == 0);
            int depth = image.depth() / 8;
            int bytesPerLine = image.bytesPerLine();
            const uchar* iImageBits = image.bits();
            uchar* oImageBits = newImage.bits();
 
            // image undistortion
            {
                for (int i = 0; i < width; ++i) {
                    float x = i - cx;
                    float x2 = x * x;
                    for (int j = 0; j < height; ++j) {
                        float y = j - cy;
                        float y2 = y * y;
 
                        float p2 = x2 / hf2 + y2 / vf2;  // p = pix/f
                        float rp =
                                1.0f +
                                p2 * (k1 +
                                      p2 * (k2 + p2 * k3));  // r(p) = 1.0 + k1
                                                             // * ||p||^2 + k2 *
                                                             // ||p||^4 + k3 *
                                                             // ||p||^6
                        float eqx = rp * x + cx;
                        float eqy = rp * y + cy;
 
                        int pixx = static_cast<int>(eqx);
                        int pixy = static_cast<int>(eqy);
                        if (pixx >= 0 && pixx < width && pixy >= 0 &&
                            pixy < height) {
                            const uchar* iPixel =
                                    iImageBits + j * bytesPerLine + i * depth;
                            uchar* oPixel = oImageBits + pixy * bytesPerLine +
                                            pixx * depth;
                            memcpy(oPixel, iPixel, depth);
                            // newImage.setPixel(i, j, image.pixel(pixx, pixy));
                        }
                    }
                }
            }
 
            return newImage;
        } break;
 
        case BROWN_DISTORTION:
            // TODO
            break;
 
        default:
            // not handled?
            assert(false);
            break;
    }
 
    CVLog::Warning(
            "[ccCameraSensor::undistort] Can't undistort the image with the "
            "current distortion model!");
 
    return QImage();
}
 
ccImage* ccCameraSensor::undistort(ccImage* image,
                                   bool inplace /*=true*/) const {
    if (!image || image->data().isNull()) {
        CVLog::Warning(
                "[ccCameraSensor::undistort] Invalid/empty input image!");
        return nullptr;
    }
 
    QImage newImage = undistort(image->data());
    if (newImage.isNull()) {
        // warning message should have been already issued
        return nullptr;
    }
 
    // update image parameters
    if (inplace) {
        image->setData(newImage);
        return image;
    } else {
        return new ccImage(newImage, image->getName() + QString(".undistort"));
    }
}
 
bool ccCameraSensor::isGlobalCoordInFrustum(
        const CCVector3& globalCoord /*, bool withLensCorrection*/) const {
    CCVector3 localCoord;
 
    // Tests if the projection is in the field of view
    if (!fromGlobalCoordToLocalCoord(globalCoord,
                                     localCoord /*, withLensCorrection*/))
        return false;
 
    // Tests if the projected point is between zNear and zFar
    const float& z = localCoord.z;
    const float& n = m_intrinsicParams.zNear_mm;
    const float& f = m_intrinsicParams.zFar_mm;
 
    return (-z <= f && -z > n && std::abs(f + z) >= FLT_EPSILON &&
            std::abs(n + z) >= FLT_EPSILON);
}
 
CCVector3 ccCameraSensor::computeUpperLeftPoint() const {
    if (m_intrinsicParams.arrayHeight == 0) return CCVector3(0, 0, 0);
 
    float ar = m_intrinsicParams.arrayHeight != 0
                       ? static_cast<float>(m_intrinsicParams.arrayWidth) /
                                 m_intrinsicParams.arrayHeight
                       : 1.0f;
    float halfFov = m_intrinsicParams.vFOV_rad / 2;
 
    CCVector3 upperLeftPoint;
    upperLeftPoint.z =
            m_scale * ConvertFocalPixToMM(m_intrinsicParams.vertFocal_pix,
                                          m_intrinsicParams.pixelSize_mm[1]);
    upperLeftPoint.y = upperLeftPoint.z * tanf(halfFov);
    upperLeftPoint.x = upperLeftPoint.z * tanf(halfFov * ar);
 
    return upperLeftPoint;
}
 
bool ccCameraSensor::computeFrustumCorners() {
    if (m_intrinsicParams.arrayHeight == 0) {
        CVLog::Warning(
                "[ccCameraSensor::computeFrustumCorners] Sensor height is 0!");
        return false;
    }
 
    float ar = static_cast<float>(m_intrinsicParams.arrayWidth) /
               m_intrinsicParams.arrayHeight;
    float halfFov = m_intrinsicParams.vFOV_rad / 2;
 
    float xIn = std::abs(tan(halfFov * ar));
    float yIn = std::abs(tan(halfFov));
    const float& zNear = m_intrinsicParams.zNear_mm;
    const float& zFar = m_intrinsicParams.zFar_mm;
 
    // compute points of frustum in image coordinate system (warning: in the
    // system, z=-z)
    if (!m_frustumInfos.initFrustumCorners()) {
        CVLog::Warning(
                "[ccCameraSensor::computeFrustumCorners] Not enough memory!");
        return false;
    }
 
    PointCoordinateType orientation = -PC_ONE * m_scale / std::abs(m_scale);
 
    // DO NOT MODIFY THE ORDER OF THE CORNERS!! A LOT OF CODE DEPENDS OF THIS
    // ORDER!!
    m_frustumInfos.frustumCorners->addPoint(CCVector3(xIn, yIn, orientation) *
                                            zNear);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(xIn, yIn, orientation) *
                                            zFar);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(xIn, -yIn, orientation) *
                                            zNear);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(xIn, -yIn, orientation) *
                                            zFar);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(-xIn, -yIn, orientation) *
                                            zNear);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(-xIn, -yIn, orientation) *
                                            zFar);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(-xIn, yIn, orientation) *
                                            zNear);
    m_frustumInfos.frustumCorners->addPoint(CCVector3(-xIn, yIn, orientation) *
                                            zFar);
 
    // compute center of the circumscribed sphere
    const CCVector3* P0 = m_frustumInfos.frustumCorners->getPoint(0);
    const CCVector3* P5 = m_frustumInfos.frustumCorners->getPoint(5);
 
    float dz = P0->z - P5->z;
    float z = (std::abs(dz) < FLT_EPSILON
                       ? P0->z
                       : (P0->norm2() - P5->norm2()) / (2 * dz));
 
    m_frustumInfos.center = CCVector3(0, 0, z);
 
    // frustum corners are now computed
    m_frustumInfos.isComputed = true;
 
    return true;
}
 
bool ccCameraSensor::computeGlobalPlaneCoefficients(
        float planeCoefficients[6][4],
        CCVector3 frustumCorners[8],
        CCVector3 edges[6],
        CCVector3& center) {
    if (!m_frustumInfos.isComputed)
        if (!computeFrustumCorners()) return false;
 
    assert(m_frustumInfos.frustumCorners &&
           m_frustumInfos.frustumCorners->size() == 8);
 
    // compute frustum corners in the global coordinates system
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(0),
                                frustumCorners[0]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(1),
                                frustumCorners[1]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(2),
                                frustumCorners[2]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(3),
                                frustumCorners[3]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(4),
                                frustumCorners[4]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(5),
                                frustumCorners[5]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(6),
                                frustumCorners[6]);
    fromLocalCoordToGlobalCoord(*m_frustumInfos.frustumCorners->getPoint(7),
                                frustumCorners[7]);
 
    /*
    //-- METHOD 1 --//
    // See "Fast Extraction of Viewing Frustum Planes from the
    World-View-Projection Matrix" of Gil Gribb and Klaus Hartmann
    // Attention !! With this method, plane equations are not normalized ! You
    should add normalization if you need it :
    // It means that if you have your plane equation in the form (ax + by + cz +
    d = 0), then --> k = sqrt(a*a + b*b + c*c) and your new coefficients are -->
    a=a/k, b=b/k, c=c/k, d=d/k ccGLMatrix projectionMatrix; if
    (!getProjectionMatrix(projectionMatrix)) return false;
 
    ccGLMatrix mat = projectionMatrix * m_orientMatrix;
    float* coeffs = mat.data();
    // right
    planeCoefficients[0][0] = coeffs[3] - coeffs[0];
    planeCoefficients[0][1] = coeffs[7] - coeffs[4];
    planeCoefficients[0][2] = coeffs[11] - coeffs[8];
    planeCoefficients[0][3] = coeffs[15] - coeffs[12];
    // bottom
    planeCoefficients[1][0] = coeffs[3] + coeffs[1];
    planeCoefficients[1][1] = coeffs[7] + coeffs[5];
    planeCoefficients[1][2] = coeffs[11] + coeffs[9];
    planeCoefficients[1][3] = coeffs[15] + coeffs[12];
    // left
    planeCoefficients[2][0] = coeffs[3] + coeffs[0];
    planeCoefficients[2][1] = coeffs[7] + coeffs[4];
    planeCoefficients[2][2] = coeffs[11] + coeffs[8];
    planeCoefficients[2][3] = coeffs[15] + coeffs[12];
    // top
    planeCoefficients[3][0] = coeffs[3] - coeffs[1];
    planeCoefficients[3][1] = coeffs[7] - coeffs[5];
    planeCoefficients[3][2] = coeffs[11] - coeffs[9];
    planeCoefficients[3][3] = coeffs[15] - coeffs[12];
    // near
    planeCoefficients[4][0] = coeffs[3] + coeffs[2];
    planeCoefficients[4][1] = coeffs[7] + coeffs[6];
    planeCoefficients[4][2] = coeffs[11] + coeffs[10];
    planeCoefficients[4][3] = coeffs[15] + coeffs[14];
    // far
    planeCoefficients[5][0] = coeffs[3] - coeffs[2];
    planeCoefficients[5][1] = coeffs[7] - coeffs[6];
    planeCoefficients[5][2] = coeffs[11] - coeffs[10];
    planeCoefficients[5][3] = coeffs[15] - coeffs[14];
    // normalization --> temporary because it is quite long ; could be done
    before... for (int i=0 ; i<6 ; i++)
    {
            float a = planeCoefficients[i][0];
            float b = planeCoefficients[i][1];
            float c = planeCoefficients[i][2];
            float d = planeCoefficients[i][3];
            float k = sqrt(pow(a,2)+pow(b,2)+pow(c,2));
            planeCoefficients[i][0] = a/k;
            planeCoefficients[i][1] = b/k;
            planeCoefficients[i][2] = c/k;
            planeCoefficients[i][3] = d/k;
    }
    */
 
    //-- METHOD 2 --//
    // If you do not like method 1, use this standard method!
    // compute equations for side planes
    for (int i = 0; i < 4; i++) {
        CCVector3 v1 = frustumCorners[i * 2 + 1] - frustumCorners[i * 2];
        CCVector3 v2 =
                frustumCorners[((i + 1) * 2) % 8] - frustumCorners[i * 2];
        CCVector3 n = v1.cross(v2);
        n.normalize();
        planeCoefficients[i][0] = n.x;
        planeCoefficients[i][1] = n.y;
        planeCoefficients[i][2] = n.z;
        planeCoefficients[i][3] = -frustumCorners[i * 2].dot(n);
    }
    // compute equations for near and far planes
    {
        CCVector3 v1 = frustumCorners[0] - frustumCorners[6];
        CCVector3 v2 = frustumCorners[4] - frustumCorners[6];
        CCVector3 n = v1.cross(v2);
        n.normalize();
        planeCoefficients[4][0] = n.x;
        planeCoefficients[4][1] = n.y;
        planeCoefficients[4][2] = n.z;
        planeCoefficients[4][3] = -frustumCorners[6].dot(n);
        planeCoefficients[5][0] = -n.x;
        planeCoefficients[5][1] = -n.y;
        planeCoefficients[5][2] = -n.z;
        planeCoefficients[5][3] = -frustumCorners[7].dot(-n);
    }
 
    // compute frustum edges
    {
        edges[0] = frustumCorners[1] - frustumCorners[0];
        edges[1] = frustumCorners[3] - frustumCorners[2];
        edges[2] = frustumCorners[5] - frustumCorners[4];
        edges[3] = frustumCorners[7] - frustumCorners[6];
        edges[4] = frustumCorners[6] - frustumCorners[0];
        edges[5] = frustumCorners[2] - frustumCorners[0];
        for (unsigned i = 0; i < 6; i++) {
            edges[i].normalize();
        }
    }
 
    // compute frustum center in the global coordinates system
    fromLocalCoordToGlobalCoord(m_frustumInfos.center, center);
 
    return true;
}
 
void ccCameraSensor::updateData() {
    CCVector3d upperLeftPointd =
            CCVector3d::fromArray(computeUpperLeftPoint().data());
 
    // near plane
    m_nearPlane.clear();
    m_nearPlane.points_.push_back(Eigen::Vector3d(
            upperLeftPointd.x, upperLeftPointd.y, -upperLeftPointd.z));
    m_nearPlane.points_.push_back(Eigen::Vector3d(
            -upperLeftPointd.x, upperLeftPointd.y, -upperLeftPointd.z));
    m_nearPlane.points_.push_back(Eigen::Vector3d(
            -upperLeftPointd.x, -upperLeftPointd.y, -upperLeftPointd.z));
    m_nearPlane.points_.push_back(Eigen::Vector3d(
            upperLeftPointd.x, -upperLeftPointd.y, -upperLeftPointd.z));
    m_nearPlane.lines_.push_back(Eigen::Vector2i(0, 1));
    m_nearPlane.lines_.push_back(Eigen::Vector2i(1, 2));
    m_nearPlane.lines_.push_back(Eigen::Vector2i(2, 3));
    m_nearPlane.lines_.push_back(Eigen::Vector2i(3, 0));
 
    // side lines
    m_sideLines.clear();
    m_sideLines.points_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
    m_sideLines.points_.push_back(Eigen::Vector3d(
            upperLeftPointd.x, upperLeftPointd.y, -upperLeftPointd.z));
    m_sideLines.points_.push_back(Eigen::Vector3d(
            -upperLeftPointd.x, upperLeftPointd.y, -upperLeftPointd.z));
    m_sideLines.points_.push_back(Eigen::Vector3d(
            -upperLeftPointd.x, -upperLeftPointd.y, -upperLeftPointd.z));
    m_sideLines.points_.push_back(Eigen::Vector3d(
            upperLeftPointd.x, -upperLeftPointd.y, -upperLeftPointd.z));
    m_sideLines.lines_.push_back(Eigen::Vector2i(0, 1));
    m_sideLines.lines_.push_back(Eigen::Vector2i(0, 2));
    m_sideLines.lines_.push_back(Eigen::Vector2i(0, 3));
    m_sideLines.lines_.push_back(Eigen::Vector2i(0, 4));
 
    // up arrow
    const double arrowHeight = 3 * upperLeftPointd.y / 2;
    const double baseHeight = 6 * upperLeftPointd.y / 5;
    const double arrowHalfWidth = 2 * upperLeftPointd.x / 5;
    const double baseHalfWidth = 1 * upperLeftPointd.x / 5;
 
    // arrow
    m_arrow.clear();
    m_arrow.points_.push_back(
            Eigen::Vector3d(-baseHalfWidth, baseHeight, -upperLeftPointd.z));
    m_arrow.points_.push_back(
            Eigen::Vector3d(baseHalfWidth, baseHeight, -upperLeftPointd.z));
    m_arrow.points_.push_back(Eigen::Vector3d(baseHalfWidth, upperLeftPointd.y,
                                              -upperLeftPointd.z));
    m_arrow.points_.push_back(Eigen::Vector3d(-baseHalfWidth, upperLeftPointd.y,
                                              -upperLeftPointd.z));
    m_arrow.lines_.push_back(Eigen::Vector2i(0, 1));
    m_arrow.lines_.push_back(Eigen::Vector2i(1, 2));
    m_arrow.lines_.push_back(Eigen::Vector2i(2, 3));
    m_arrow.lines_.push_back(Eigen::Vector2i(3, 0));
    m_arrow.points_.push_back(
            Eigen::Vector3d(-arrowHalfWidth, baseHeight, -upperLeftPointd.z));
    m_arrow.points_.push_back(
            Eigen::Vector3d(arrowHalfWidth, baseHeight, -upperLeftPointd.z));
    m_arrow.points_.push_back(
            Eigen::Vector3d(0, arrowHeight, -upperLeftPointd.z));
    m_arrow.lines_.push_back(Eigen::Vector2i(4, 5));
    m_arrow.lines_.push_back(Eigen::Vector2i(5, 6));
    m_arrow.lines_.push_back(Eigen::Vector2i(6, 4));
 
    // axis (for test)
    {
        double l = std::abs(upperLeftPointd.z) / 2.0;
        m_axis.clear();
        m_axis.points_.push_back(Eigen::Vector3d(0.0, 0.0, 0.0));
        m_axis.points_.push_back(Eigen::Vector3d(l, 0.0, 0.0));
        m_axis.points_.push_back(Eigen::Vector3d(0.0, l, 0.0));
        m_axis.points_.push_back(Eigen::Vector3d(0.0, 0.0, -l));
 
        // right vector
        m_axis.lines_.push_back(Eigen::Vector2i(0, 1));
        m_axis.colors_.push_back(ecvColor::Rgb::ToEigen(ecvColor::red));
 
        // up vector
        m_axis.lines_.push_back(Eigen::Vector2i(0, 2));
        m_axis.colors_.push_back(ecvColor::Rgb::ToEigen(ecvColor::yellow));
 
        // view vector
        m_axis.lines_.push_back(Eigen::Vector2i(0, 3));
        m_axis.colors_.push_back(ecvColor::Rgb::ToEigen(ecvColor::green));
    }
}
 
void ccCameraSensor::drawMeOnly(CC_DRAW_CONTEXT& context) {
    if (!MACRO_Draw3D(context)) return;
 
    // we draw a little 3d representation of the sensor and some of its
    // attributes
    if (!ecvDisplayTools::GetCurrentScreen()) {
        return;
    }
 
    // build-up the normal representation own 'context'
    CC_DRAW_CONTEXT cameraContext = context;
    // we must remove the 'push name flag' so that the primitives don't push
    // their own!
    cameraContext.drawingFlags &= (~CC_ENTITY_PICKING);
    cameraContext.currentLineWidth = 1;
    cameraContext.defaultPolylineColor = getFrameColor();
    cameraContext.defaultMeshColor = getPlaneColor();
    cameraContext.opacity = 0.2f;
    ccIndexedTransformation sensorPos;
    if (!getAbsoluteTransformation(sensorPos, getActiveIndex())) {
        // no visible position for this index!
        return;
    }
 
    // update drawing data
    updateData();
 
    // frustum
    if (m_frustumInfos.drawFrustum || m_frustumInfos.drawSidePlanes) {
        // always compute corners
        computeFrustumCorners();
 
        if (m_frustumInfos.frustumCorners &&
            m_frustumInfos.frustumCorners->size() >= 8) {
            // frustum area (lines)
            if (m_frustumInfos.drawFrustum) {
                const CCVector3* P0 =
                        m_frustumInfos.frustumCorners->getPoint(0);
                const CCVector3* P1 =
                        m_frustumInfos.frustumCorners->getPoint(1);
                const CCVector3* P2 =
                        m_frustumInfos.frustumCorners->getPoint(2);
                const CCVector3* P3 =
                        m_frustumInfos.frustumCorners->getPoint(3);
                const CCVector3* P4 =
                        m_frustumInfos.frustumCorners->getPoint(4);
                const CCVector3* P5 =
                        m_frustumInfos.frustumCorners->getPoint(5);
                const CCVector3* P6 =
                        m_frustumInfos.frustumCorners->getPoint(6);
                const CCVector3* P7 =
                        m_frustumInfos.frustumCorners->getPoint(7);
 
                cameraContext.currentLineWidth = 2;
 
                m_sideLines.points_.push_back(CCVector3d::fromArray(*P0));  // 5
                m_sideLines.points_.push_back(CCVector3d::fromArray(*P1));  // 6
                m_sideLines.points_.push_back(CCVector3d::fromArray(*P2));  // 7
                m_sideLines.points_.push_back(CCVector3d::fromArray(*P3));  // 8
                m_sideLines.points_.push_back(CCVector3d::fromArray(*P4));  // 9
                m_sideLines.points_.push_back(
                        CCVector3d::fromArray(*P5));  // 10
                m_sideLines.points_.push_back(
                        CCVector3d::fromArray(*P6));  // 11
                m_sideLines.points_.push_back(
                        CCVector3d::fromArray(*P7));  // 12
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(5, 6));
                m_sideLines.lines_.push_back(Eigen::Vector2i(6, 8));
                m_sideLines.lines_.push_back(Eigen::Vector2i(8, 7));
                m_sideLines.lines_.push_back(Eigen::Vector2i(7, 5));
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(7, 8));
                m_sideLines.lines_.push_back(Eigen::Vector2i(8, 10));
                m_sideLines.lines_.push_back(Eigen::Vector2i(10, 9));
                m_sideLines.lines_.push_back(Eigen::Vector2i(9, 7));
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(9, 10));
                m_sideLines.lines_.push_back(Eigen::Vector2i(10, 12));
                m_sideLines.lines_.push_back(Eigen::Vector2i(12, 11));
                m_sideLines.lines_.push_back(Eigen::Vector2i(11, 9));
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(11, 12));
                m_sideLines.lines_.push_back(Eigen::Vector2i(12, 6));
                m_sideLines.lines_.push_back(Eigen::Vector2i(6, 5));
                m_sideLines.lines_.push_back(Eigen::Vector2i(5, 11));
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(11, 5));
                m_sideLines.lines_.push_back(Eigen::Vector2i(5, 7));
                m_sideLines.lines_.push_back(Eigen::Vector2i(7, 9));
                m_sideLines.lines_.push_back(Eigen::Vector2i(9, 11));
 
                m_sideLines.lines_.push_back(Eigen::Vector2i(6, 12));
                m_sideLines.lines_.push_back(Eigen::Vector2i(12, 10));
                m_sideLines.lines_.push_back(Eigen::Vector2i(10, 8));
                m_sideLines.lines_.push_back(Eigen::Vector2i(8, 6));
            }
 
            // frustum area (planes)
            if (m_frustumInfos.drawSidePlanes) {
                if (!m_frustumInfos.frustumHull &&
                    m_frustumInfos.initFrustumHull()) {
                    if (m_frustumInfos.isComputed) {
                        m_frustumInfos.frustumCorners->applyRigidTransformation(
                                sensorPos);
                    }
 
                    // set the rigth display (just to be sure)
                    m_frustumInfos.frustumHull->setTempColor(m_color);
                    m_frustumInfos.frustumHull->showWired(false);
                    m_frustumInfos.frustumHull->enableStippling(true);
                    m_frustumInfos.frustumHull->setOpacity(
                            cameraContext.opacity);
                    cameraContext.defaultMeshColor = m_color;
                    cameraContext.meshRenderingMode =
                            MESH_RENDERING_MODE::ECV_SURFACE_MODE;
                    cameraContext.viewID =
                            m_frustumInfos.frustumHull->getViewId();
                    ecvDisplayTools::Draw(cameraContext,
                                          m_frustumInfos.frustumHull);
                }
            }
        }
    }
 
    // frustum area (planes)
    if (m_frustumInfos.frustumHull) {
        cameraContext.visible =
                context.visible && m_frustumInfos.drawSidePlanes;
        cameraContext.viewID = m_frustumInfos.frustumHull->getViewId();
        ecvDisplayTools::HideShowEntities(cameraContext);
    }
 
    // tranformation
    {
        Eigen::Matrix4d transformation = ccGLMatrixd::ToEigenMatrix4(sensorPos);
        m_nearPlane.Transform(transformation);
        m_nearPlane.setTempColor(getPlaneColor());
        m_sideLines.Transform(transformation);
        m_sideLines.setTempColor(getFrameColor());
        m_arrow.Transform(transformation);
        m_arrow.setTempColor(getFrameColor());
        m_axis.Transform(transformation);
    }
 
    cameraContext.visible = context.visible;
    cameraContext.viewID = context.viewID;
    cameraContext.defaultMeshColor = getPlaneColor();
    ecvDisplayTools::Draw(cameraContext, this);
}
 
void ccCameraSensor::clearDrawings() {
    if (m_frustumInfos.frustumHull) {
        CC_DRAW_CONTEXT context;
        context.removeEntityType = ENTITY_TYPE::ECV_MESH;
        context.removeViewID = m_frustumInfos.frustumHull->getViewId();
        ecvDisplayTools::RemoveEntities(context);
    }
    ecvDisplayTools::RemoveEntities(this);
}
 
void ccCameraSensor::hideShowDrawings(CC_DRAW_CONTEXT& context) {
    context.viewID = this->getViewId();
    ecvDisplayTools::HideShowEntities(context);
 
    if (m_frustumInfos.frustumHull) {
        ecvDisplayTools::HideShowEntities(
                m_frustumInfos.frustumHull,
                context.visible && m_frustumInfos.drawSidePlanes);
    }
}
 
float ccCameraSensor::ConvertFocalPixToMM(float focal_pix,
                                          float ccdPixelSize_mm) {
    if (ccdPixelSize_mm < FLT_EPSILON) {
        CVLog::Warning(
                "[ccCameraSensor::convertFocalPixToMM] Invalid CCD pixel size! "
                "(<= 0)");
        return -1.0f;
    }
 
    return focal_pix * ccdPixelSize_mm;
}
 
float ccCameraSensor::ConvertFocalMMToPix(float focal_mm,
                                          float ccdPixelSize_mm) {
    if (ccdPixelSize_mm < FLT_EPSILON) {
        CVLog::Warning(
                "[ccCameraSensor::convertFocalMMToPix] Invalid CCD pixel size! "
                "(<= 0)");
        return -1.0f;
    }
 
    return focal_mm / ccdPixelSize_mm;
}
 
float ccCameraSensor::ComputeFovRadFromFocalPix(float focal_pix,
                                                int imageSize_pix) {
    if (imageSize_pix <= 0) {
        // invalid image size
        return -1.0f;
    }
 
    return 2 * atanf(imageSize_pix / (2 * focal_pix));
}
 
float ccCameraSensor::ComputeFovRadFromFocalMm(float focal_mm,
                                               float ccdSize_mm) {
    if (ccdSize_mm < FLT_EPSILON) {
        // invalid CDD size
        return -1.0f;
    }
 
    return 2 * atanf(ccdSize_mm / (2 * focal_mm));
}
 
bool ccCameraSensor::computeOrthoRectificationParams(
        const ccImage* image,
        cloudViewer::GenericIndexedCloud* keypoints3D,
        std::vector<KeyPoint>& keypointsImage,
        double a_out[3],
        double b_out[3],
        double c_out[3]) const {
    if (!image || !keypoints3D) return false;
 
    unsigned count = static_cast<unsigned>(keypointsImage.size());
    if (count < 4) return false;
 
    // first guess for X (a0 a1 a2 b0 b1 b2 c1 c2)
    double norm = static_cast<double>(std::max(image->getW(), image->getH()));
    double X0[8] = {1.0 / sqrt(norm), 1.0 / norm, 1.0 / norm, 1.0 / sqrt(norm),
                    1.0 / norm,       1.0 / norm, 1.0 / norm, 1.0 / norm};
 
    // compute the A matrix and b vector
    unsigned Neq = 2 * count;         // number of equations
    double* A = new double[8 * Neq];  // 8 coefficients: a0 a1 a2 b0 b1 b2 c1 c2
    double* b = new double[Neq];
    if (!A || !b) {
        // not enough memory
        if (A) delete[] A;
        if (b) delete[] b;
        return false;
    }
 
    // for all points
    {
        double* _A = A;
        double* _b = b;
        for (unsigned i = 0; i < count; ++i) {
            const KeyPoint& kp = keypointsImage[i];
            double kpx = static_cast<double>(kp.x);
            double kpy = static_cast<double>(kp.y);
            const CCVector3* P = keypoints3D->getPoint(kp.index);
 
            *_A++ = 1.0;
            *_A++ = kpx;
            *_A++ = kpy;
            *_A++ = 0.0;
            *_A++ = 0.0;
            *_A++ = 0.0;
            *_A++ = -kpx * static_cast<double>(P->x);
            *_A++ = -kpy * static_cast<double>(P->x);
            *_b++ = static_cast<double>(P->x);
 
            *_A++ = 0.0;
            *_A++ = 0.0;
            *_A++ = 0.0;
            *_A++ = 1.0;
            *_A++ = kpx;
            *_A++ = kpy;
            *_A++ = -kpx * static_cast<double>(P->y);
            *_A++ = -kpy * static_cast<double>(P->y);
            *_b++ = static_cast<double>(P->y);
        }
    }
 
    // conjugate gradient initialization
    // we solve tA.A.X = tA.b
    cloudViewer::ConjugateGradient<8, double> cg;
    cloudViewer::SquareMatrixd& tAA = cg.A();
    double* tAb = cg.b();
 
    // compute tA.A and tA.b
    {
        for (unsigned i = 0; i < 8; ++i) {
            // tA.A part
            for (unsigned j = i; j < 8; ++j) {
                double sum_prod = 0;
                const double* _Ai = A + i;
                const double* _Aj = A + j;
                for (unsigned k = 0; k < Neq; ++k) {
                    // sum_prod += A[(8*2*k)+i]*A[(8*2*k)+j];
                    sum_prod += (*_Ai) * (*_Aj);
                    _Ai += 8;
                    _Aj += 8;
                }
                tAA.m_values[j][i] = tAA.m_values[i][j] = sum_prod;
            }
 
            // tA.b part
            {
                double sum_prod = 0;
                const double* _Ai = A + i;
                const double* _b = b;
                for (unsigned k = 0; k < Neq; ++k) {
                    // sum_prod += A[(8*2*k)+i]*b[k];
                    sum_prod += (*_Ai) * (*_b++);
                    _Ai += 8;
                }
                tAb[i] = sum_prod;
            }
        }
    }
 
    // init. conjugate gradient
    cg.initConjugateGradient(X0);
 
    // conjugate gradient iterations
    {
        double convergenceThreshold =
                1.0e-8 /* * norm*/;  // max. error for convergence
        for (unsigned i = 0; i < 1500; ++i) {
            double lastError = cg.iterConjugateGradient(X0);
            if (lastError < convergenceThreshold)  // converged
            {
                CVLog::PrintDebug(
                        QString("[computeOrthoRectificationParams] Convergence "
                                "reached in %1 iteration(s) (error: %2)")
                                .arg(i + 1)
                                .arg(lastError));
                break;
            }
        }
    }
 
    delete[] A;
    A = nullptr;
    delete[] b;
    b = nullptr;
 
    a_out[0] = X0[0];
    a_out[1] = X0[1];
    a_out[2] = X0[2];
    b_out[0] = X0[3];
    b_out[1] = X0[4];
    b_out[2] = X0[5];
    c_out[0] = 1.0;
    c_out[1] = X0[6];
    c_out[2] = X0[7];
 
    return true;
}
 
ccImage* ccCameraSensor::orthoRectifyAsImageDirect(
        const ccImage* image,
        PointCoordinateType Z0,
        double& pixelSize,
        bool undistortImages /*=true*/,
        double* minCorner /*=0*/,
        double* maxCorner /*=0*/,
        double* realCorners /*=0*/) const {
    // first, we compute the ortho-rectified image corners
    double corners[8];
 
    int width = static_cast<int>(image->getW());
    int height = static_cast<int>(image->getH());
 
    // top-left
    {
        CCVector2 xTopLeft(0, 0);
        CCVector3 P3D;
        if (!fromImageCoordToGlobalCoord(xTopLeft, P3D, Z0)) return nullptr;
#ifdef QT_DEBUG
        // internal check
        CCVector2 check(0, 0);
        fromGlobalCoordToImageCoord(P3D, check, false);
        assert((xTopLeft - check).norm2() <
               std::max(width, height) * FLT_EPSILON);
#endif
        corners[0] = P3D.x;
        corners[1] = P3D.y;
    }
 
    // top-right
    {
        CCVector2 xTopRight(static_cast<PointCoordinateType>(width), 0);
        CCVector3 P3D;
        if (!fromImageCoordToGlobalCoord(xTopRight, P3D, Z0)) return nullptr;
#ifdef QT_DEBUG
        // internal check
        CCVector2 check(0, 0);
        fromGlobalCoordToImageCoord(P3D, check, false);
        assert((xTopRight - check).norm2() <
               std::max(width, height) * FLT_EPSILON);
#endif
        corners[2] = P3D.x;
        corners[3] = P3D.y;
    }
 
    // bottom-right
    {
        CCVector2 xBottomRight(static_cast<PointCoordinateType>(width),
                               static_cast<PointCoordinateType>(height));
        CCVector3 P3D;
        if (!fromImageCoordToGlobalCoord(xBottomRight, P3D, Z0)) return nullptr;
#ifdef QT_DEBUG
        // internal check
        CCVector2 check(0, 0);
        fromGlobalCoordToImageCoord(P3D, check, false);
        assert((xBottomRight - check).norm2() <
               std::max(width, height) * FLT_EPSILON);
#endif
        corners[4] = P3D.x;
        corners[5] = P3D.y;
    }
 
    // bottom-left
    {
        CCVector2 xBottomLeft(0, static_cast<PointCoordinateType>(height));
        CCVector3 P3D;
        if (!fromImageCoordToGlobalCoord(xBottomLeft, P3D, Z0)) return nullptr;
#ifdef QT_DEBUG
        // internal check
        CCVector2 check(0, 0);
        fromGlobalCoordToImageCoord(P3D, check, false);
        assert((xBottomLeft - check).norm2() <
               std::max(width, height) * FLT_EPSILON);
#endif
        corners[6] = P3D.x;
        corners[7] = P3D.y;
    }
 
    if (realCorners) memcpy(realCorners, corners, 8 * sizeof(double));
 
    // we look for min and max bounding box
    double minC[2] = {corners[0], corners[1]};
    double maxC[2] = {corners[0], corners[1]};
 
    for (unsigned k = 1; k < 4; ++k) {
        const double* C = corners + 2 * k;
        if (minC[0] > C[0])
            minC[0] = C[0];
        else if (maxC[0] < C[0])
            maxC[0] = C[0];
 
        if (minC[1] > C[1])
            minC[1] = C[1];
        else if (maxC[1] < C[1])
            maxC[1] = C[1];
    }
 
    // output 3D boundaries (optional)
    if (minCorner) {
        minCorner[0] = minC[0];
        minCorner[1] = minC[1];
    }
    if (maxCorner) {
        maxCorner[0] = maxC[0];
        maxCorner[1] = maxC[1];
    }
 
    double dx = maxC[0] - minC[0];
    double dy = maxC[1] - minC[1];
 
    double _pixelSize = pixelSize;
    if (_pixelSize <= 0) {
        int maxSize = std::max(width, height);
        _pixelSize = std::max(dx, dy) / maxSize;
    }
    unsigned w = static_cast<unsigned>(dx / _pixelSize);
    unsigned h = static_cast<unsigned>(dy / _pixelSize);
 
    QImage orthoImage(w, h, QImage::Format_ARGB32);
    if (orthoImage.isNull())  // not enough memory!
        return nullptr;
 
    const QRgb blackValue = qRgb(0, 0, 0);
    const QRgb blackAlphaZero = qRgba(0, 0, 0, 0);
 
    for (unsigned i = 0; i < w; ++i) {
        PointCoordinateType xip =
                static_cast<PointCoordinateType>(minC[0] + i * _pixelSize);
        for (unsigned j = 0; j < h; ++j) {
            PointCoordinateType yip =
                    static_cast<PointCoordinateType>(minC[1] + j * _pixelSize);
 
            QRgb rgb = blackValue;  // output pixel is (transparent) black by
                                    // default
 
            CCVector3 P3D(xip, yip, Z0);
            CCVector2 imageCoord;
            if (fromGlobalCoordToImageCoord(P3D, imageCoord, undistortImages)) {
                int x = static_cast<int>(imageCoord.x);
                int y = static_cast<int>(imageCoord.y);
                if (x >= 0 && x < width && y >= 0 && y < height) {
                    rgb = image->data().pixel(x, y);
                }
            }
 
            // pure black pixels are treated as transparent ones!
            orthoImage.setPixel(i, h - 1 - j,
                                rgb != blackValue ? rgb : blackAlphaZero);
        }
    }
 
    // output pixel size (auto)
    pixelSize = _pixelSize;
 
    return new ccImage(orthoImage, getName());
}
 
ccImage* ccCameraSensor::orthoRectifyAsImage(
        const ccImage* image,
        cloudViewer::GenericIndexedCloud* keypoints3D,
        std::vector<KeyPoint>& keypointsImage,
        double& pixelSize,
        double* minCorner /*=0*/,
        double* maxCorner /*=0*/,
        double* realCorners /*=0*/) const {
    double a[3], b[3], c[3];
 
    if (!computeOrthoRectificationParams(image, keypoints3D, keypointsImage, a,
                                         b, c)) {
        return nullptr;
    }
 
    const double& a0 = a[0];
    const double& a1 = a[1];
    const double& a2 = a[2];
    const double& b0 = b[0];
    const double& b1 = b[1];
    const double& b2 = b[2];
    // const double& c0 = c[0];
    const double& c1 = c[1];
    const double& c2 = c[2];
 
    // first, we compute the ortho-rectified image corners
    double corners[8];
    double xi, yi, qi;
 
    int width = static_cast<int>(image->getW());
    int height = static_cast<int>(image->getH());
    double halfWidth = width / 2.0;
    double halfHeight = height / 2.0;
 
    // top-left
    xi = -halfWidth;
    yi = -halfHeight;
    qi = 1.0 + c1 * xi + c2 * yi;
    corners[0] = (a0 + a1 * xi + a2 * yi) / qi;
    corners[1] = (b0 + b1 * xi + b2 * yi) / qi;
 
    // top-right
    xi = halfWidth;
    yi = -halfHeight;
    qi = 1.0 + c1 * xi + c2 * yi;
    corners[2] = (a0 + a1 * xi + a2 * yi) / qi;
    corners[3] = (b0 + b1 * xi + b2 * yi) / qi;
 
    // bottom-right
    xi = halfWidth;
    yi = halfHeight;
    qi = 1.0 + c1 * xi + c2 * yi;
    corners[4] = (a0 + a1 * xi + a2 * yi) / qi;
    corners[5] = (b0 + b1 * xi + b2 * yi) / qi;
 
    // bottom-left
    xi = -halfWidth;
    yi = halfHeight;
    qi = 1.0 + c1 * xi + c2 * yi;
    corners[6] = (a0 + a1 * xi + a2 * yi) / qi;
    corners[7] = (b0 + b1 * xi + b2 * yi) / qi;
 
    if (realCorners) {
        memcpy(realCorners, corners, 8 * sizeof(double));
    }
 
    // we look for min and max bounding box
    double minC[2] = {corners[0], corners[1]};
    double maxC[2] = {corners[0], corners[1]};
 
    for (unsigned k = 1; k < 4; ++k) {
        const double* C = corners + 2 * k;
        if (minC[0] > C[0])
            minC[0] = C[0];
        else if (maxC[0] < C[0])
            maxC[0] = C[0];
 
        if (minC[1] > C[1])
            minC[1] = C[1];
        else if (maxC[1] < C[1])
            maxC[1] = C[1];
    }
 
    // output 3D boundaries (optional)
    if (minCorner) {
        minCorner[0] = minC[0];
        minCorner[1] = minC[1];
    }
    if (maxCorner) {
        maxCorner[0] = maxC[0];
        maxCorner[1] = maxC[1];
    }
 
    double dx = maxC[0] - minC[0];
    double dy = maxC[1] - minC[1];
 
    double _pixelSize = pixelSize;
    if (_pixelSize <= 0) {
        int maxSize = std::max(width, height);
        _pixelSize = std::max(dx, dy) / maxSize;
    }
    unsigned w = static_cast<unsigned>(dx / _pixelSize);
    unsigned h = static_cast<unsigned>(dy / _pixelSize);
 
    QImage orthoImage(w, h, QImage::Format_ARGB32);
    if (orthoImage.isNull())  // not enough memory!
        return nullptr;
 
    const QRgb blackValue = qRgb(0, 0, 0);
    const QRgb blackAlphaZero = qRgba(0, 0, 0, 0);
 
    for (unsigned i = 0; i < w; ++i) {
        double xip = minC[0] + static_cast<double>(i) * _pixelSize;
        for (unsigned j = 0; j < h; ++j) {
            QRgb rgb = blackValue;  // output pixel is (transparent) black by
                                    // default
 
            double yip = minC[1] + static_cast<double>(j) * _pixelSize;
            double q = (c2 * xip - a2) * (c1 * yip - b1) -
                       (c2 * yip - b2) * (c1 * xip - a1);
            double p =
                    (a0 - xip) * (c1 * yip - b1) - (b0 - yip) * (c1 * xip - a1);
            double yi = p / q;
            yi += halfHeight;
            int y = static_cast<int>(yi);
 
            if (y >= 0 && y < height) {
                q = (c1 * xip - a1) * (c2 * yip - b2) -
                    (c1 * yip - b1) * (c2 * xip - a2);
                p = (a0 - xip) * (c2 * yip - b2) - (b0 - yip) * (c2 * xip - a2);
                double xi = p / q;
                xi += halfWidth;
                int x = static_cast<int>(xi);
 
                if (x >= 0 && x < width) {
                    rgb = image->data().pixel(x, y);
                }
            }
 
            // pure black pixels are treated as transparent ones!
            orthoImage.setPixel(i, h - 1 - j,
                                rgb != blackValue ? rgb : blackAlphaZero);
        }
    }
 
    // output pixel size (auto)
    pixelSize = _pixelSize;
 
    return new ccImage(orthoImage, getName());
}
 
bool ccCameraSensor::OrthoRectifyAsImages(
        std::vector<ccImage*> images,
        double a[],
        double b[],
        double c[],
        unsigned maxSize,
        QDir* outputDir /*=0*/,
        std::vector<ccImage*>* result /*=0*/,
        std::vector<std::pair<double, double>>* relativePos /*=0*/) {
    size_t count = images.size();
    if (count == 0) {
        CVLog::Warning("[OrthoRectifyAsImages] No image to process?!");
        return false;
    }
 
    // min & max corners for each images
    std::vector<double> minCorners;
    std::vector<double> maxCorners;
    try {
        minCorners.resize(2 * count);
        maxCorners.resize(2 * count);
    } catch (const std::bad_alloc&) {
        // not enough memory
        CVLog::Warning("[OrthoRectifyAsImages] Not enough memory!");
        return false;
    }
    // max dimension of all (ortho-rectified) images, horizontally or vertically
    double maxDimAllImages = 0;
    // corners for the global set
    double globalCorners[4] = {0, 0, 0, 0};
 
    // compute output corners and max dimension for all images
    for (size_t k = 0; k < count; ++k) {
        const double& a0 = a[k * 3];
        const double& a1 = a[k * 3 + 1];
        const double& a2 = a[k * 3 + 2];
        const double& b0 = b[k * 3];
        const double& b1 = b[k * 3 + 1];
        const double& b2 = b[k * 3 + 2];
        // const double& c0 = c[k*3];
        const double& c1 = c[k * 3 + 1];
        const double& c2 = c[k * 3 + 2];
 
        // first, we compute the ortho-rectified image corners
        double corners[8];
        double xi, yi, qi;
 
        unsigned width = images[k]->getW();
        unsigned height = images[k]->getH();
 
        // top-left
        xi = -0.5 * width;
        yi = -0.5 * height;
        qi = 1.0 + c1 * xi + c2 * yi;
        corners[0] = (a0 + a1 * xi + a2 * yi) / qi;
        corners[1] = (b0 + b1 * xi + b2 * yi) / qi;
 
        // top-right
        xi = 0.5 * width;
        // yi = -0.5*height;
        qi = 1.0 + c1 * xi + c2 * yi;
        corners[2] = (a0 + a1 * xi + a2 * yi) / qi;
        corners[3] = (b0 + b1 * xi + b2 * yi) / qi;
 
        // bottom-right
        // xi =  0.5*width;
        yi = 0.5 * height;
        qi = 1.0 + c1 * xi + c2 * yi;
        corners[4] = (a0 + a1 * xi + a2 * yi) / qi;
        corners[5] = (b0 + b1 * xi + b2 * yi) / qi;
 
        // bottom-left
        xi = -0.5 * width;
        // yi = 0.5*height;
        qi = 1.0 + c1 * xi + c2 * yi;
        corners[6] = (a0 + a1 * xi + a2 * yi) / qi;
        corners[7] = (b0 + b1 * xi + b2 * yi) / qi;
 
        // we look for min and max bounding box
        double* minC = &minCorners[2 * k];
        double* maxC = &maxCorners[2 * k];
        maxC[0] = minC[0] = corners[0];
        maxC[1] = minC[1] = corners[1];
        for (unsigned k = 1; k < 4; ++k) {
            const double* C = corners + 2 * k;
            // dimension: X
            if (minC[0] > C[0])
                minC[0] = C[0];
            else if (maxC[0] < C[0])
                maxC[0] = C[0];
 
            if (globalCorners[0] > minC[0]) globalCorners[0] = minC[0];
            if (globalCorners[2] < maxC[0]) globalCorners[2] = maxC[0];
 
            // dimension: Y
            if (minC[1] > C[1])
                minC[1] = C[1];
            else if (maxC[1] < C[1])
                maxC[1] = C[1];
 
            if (globalCorners[1] > minC[1]) globalCorners[1] = minC[1];
            if (globalCorners[3] < maxC[1]) globalCorners[3] = maxC[1];
        }
 
        double dx = maxC[0] - minC[0];
        double dy = maxC[1] - minC[1];
        double maxd = std::max(dx, dy);
        if (maxd > maxDimAllImages) maxDimAllImages = maxd;
    }
 
    // deduce pixel size
    double pixelSize = maxDimAllImages / maxSize;
 
    if (outputDir) {
        // write header
        QFile f(outputDir->absoluteFilePath("ortho_rectification_log.txt"));
        if (f.open(QIODevice::WriteOnly | QIODevice::Text)) {
            QTextStream stream(&f);
            stream.setRealNumberNotation(QTextStream::FixedNotation);
            stream.setRealNumberPrecision(6);
            stream << "PixelSize" << ' ' << pixelSize << QtCompat::endl;
            stream << "Global3DBBox" << ' ' << globalCorners[0] << ' '
                   << globalCorners[1] << ' ' << globalCorners[2] << ' '
                   << globalCorners[3] << QtCompat::endl;
            int globalWidth = static_cast<int>(
                    ceil((globalCorners[2] - globalCorners[0]) / pixelSize));
            int globalHeight = static_cast<int>(
                    ceil((globalCorners[3] - globalCorners[1]) / pixelSize));
            stream << "Global2DBBox" << ' ' << 0 << ' ' << 0 << ' '
                   << globalWidth - 1 << ' ' << globalHeight - 1
                   << QtCompat::endl;
        }
    }
 
    // projet each image accordingly
    for (size_t k = 0; k < count; ++k) {
        double* minC = &minCorners[2 * k];
        double* maxC = &maxCorners[2 * k];
        double dx = maxC[0] - minC[0];
        double dy = maxC[1] - minC[1];
 
        ccImage* image = images[k];
        unsigned width = images[k]->getW();
        unsigned height = images[k]->getH();
        unsigned w = static_cast<unsigned>(ceil(dx / pixelSize));
        unsigned h = static_cast<unsigned>(ceil(dy / pixelSize));
 
        QImage orthoImage(w, h, QImage::Format_ARGB32);
        if (orthoImage.isNull())  // not enough memory!
        {
            // clear mem.
            if (result) {
                while (!result->empty()) {
                    delete result->back();
                    result->pop_back();
                }
            }
            CVLog::Warning("[OrthoRectifyAsImages] Not enough memory!");
            return false;
        }
 
        // ortho rectification parameters
        const double& a0 = a[k * 3];
        const double& a1 = a[k * 3 + 1];
        const double& a2 = a[k * 3 + 2];
        const double& b0 = b[k * 3];
        const double& b1 = b[k * 3 + 1];
        const double& b2 = b[k * 3 + 2];
        // const double& c0 = c[k*3];
        const double& c1 = c[k * 3 + 1];
        const double& c2 = c[k * 3 + 2];
 
        for (unsigned i = 0; i < w; ++i) {
            double xip = minC[0] + static_cast<double>(i) * pixelSize;
            for (unsigned j = 0; j < h; ++j) {
                double yip = minC[1] + static_cast<double>(j) * pixelSize;
                double q = (c2 * xip - a2) * (c1 * yip - b1) -
                           (c2 * yip - b2) * (c1 * xip - a1);
                double p = (a0 - xip) * (c1 * yip - b1) -
                           (b0 - yip) * (c1 * xip - a1);
                double yi = p / q;
 
                q = (c1 * xip - a1) * (c2 * yip - b2) -
                    (c1 * yip - b1) * (c2 * xip - a2);
                p = (a0 - xip) * (c2 * yip - b2) - (b0 - yip) * (c2 * xip - a2);
                double xi = p / q;
 
                xi += 0.5 * width;
                yi += 0.5 * height;
 
                int x = static_cast<int>(xi);
                int y = static_cast<int>(yi);
                if (x >= 0 && x < static_cast<int>(width) && y >= 0 &&
                    y < static_cast<int>(height)) {
                    QRgb rgb = image->data().pixel(x, y);
                    // pure black pixels are treated as transparent ones!
                    if (qRed(rgb) + qGreen(rgb) + qBlue(rgb) > 0)
                        orthoImage.setPixel(i, h - 1 - j, rgb);
                    else
                        orthoImage.setPixel(
                                i, h - 1 - j,
                                qRgba(qRed(rgb), qGreen(rgb), qBlue(rgb), 0));
                } else
                    orthoImage.setPixel(i, h - 1 - j, qRgba(255, 0, 255, 0));
            }
        }
 
        // eventually compute relative pos
        if (relativePos) {
            double xShift = (minC[0] - minCorners[0]) / pixelSize;
            double yShift = (minC[1] - minCorners[1]) / pixelSize;
            relativePos->emplace_back(xShift, yShift);
        }
 
        if (outputDir) {
            // export image
            QString exportFilename =
                    QString("ortho_rectified_%1.png").arg(image->getName());
            orthoImage.save(outputDir->absoluteFilePath(exportFilename));
 
            // export meta-data
            QFile f(outputDir->absoluteFilePath("ortho_rectification_log.txt"));
            if (f.open(QIODevice::WriteOnly | QIODevice::Append |
                       QIODevice::Text))  // always append
            {
                double xShiftGlobal = (minC[0] - globalCorners[0]) / pixelSize;
                double yShiftGlobal = (minC[1] - globalCorners[1]) / pixelSize;
                QTextStream stream(&f);
                stream.setRealNumberNotation(QTextStream::FixedNotation);
                stream.setRealNumberPrecision(6);
                stream << "Image" << ' ' << exportFilename << ' ';
                stream << "Local3DBBox" << ' ' << minC[0] << ' ' << minC[1]
                       << ' ' << maxC[0] << ' ' << maxC[1] << ' ';
                stream << "Local2DBBox" << ' ' << xShiftGlobal << ' '
                       << yShiftGlobal << ' ' << xShiftGlobal + (double)(w - 1)
                       << ' ' << yShiftGlobal + (double)(h - 1)
                       << QtCompat::endl;
                f.close();
            }
        }
 
        if (result)
            result->push_back(new ccImage(orthoImage, image->getName()));
    }
 
    return true;
}
 
ccPointCloud* ccCameraSensor::orthoRectifyAsCloud(
        const ccImage* image,
        cloudViewer::GenericIndexedCloud* keypoints3D,
        std::vector<KeyPoint>& keypointsImage) const {
    double a[3], b[3], c[3];
 
    if (!computeOrthoRectificationParams(image, keypoints3D, keypointsImage, a,
                                         b, c))
        return nullptr;
 
    const double& a0 = a[0];
    const double& a1 = a[1];
    const double& a2 = a[2];
    const double& b0 = b[0];
    const double& b1 = b[1];
    const double& b2 = b[2];
    // const double& c0 = c[0];
    const double& c1 = c[1];
    const double& c2 = c[2];
 
    PointCoordinateType defaultZ = 0;
 
    unsigned width = image->getW();
    unsigned height = image->getH();
 
    ccPointCloud* proj =
            new ccPointCloud(getName() + QString(".ortho-rectified"));
    if (!proj->reserve(width * height) || !proj->reserveTheRGBTable()) {
        CVLog::Warning("[orthoRectifyAsCloud] Not enough memory!");
        delete proj;
        return nullptr;
    }
    proj->showColors(true);
 
    unsigned realCount = 0;
 
    // ortho rectification
    {
        for (unsigned pi = 0; pi < width; ++pi) {
            double xi = static_cast<double>(pi) - 0.5 * width;
            for (unsigned pj = 0; pj < height; ++pj) {
                double yi = static_cast<double>(pj) - 0.5 * height;
                double qi = 1.0 + c1 * xi + c2 * yi;
                CCVector3 P(static_cast<PointCoordinateType>(
                                    (a0 + a1 * xi + a2 * yi) / qi),
                            static_cast<PointCoordinateType>(
                                    (b0 + b1 * xi + b2 * yi) / qi),
                            defaultZ);
 
                // and color?
                QRgb rgb = image->data().pixel(pi, pj);
                int r = qRed(rgb);
                int g = qGreen(rgb);
                int b = qBlue(rgb);
                if (r + g + b > 0) {
                    // add point
                    proj->addPoint(P);
                    // and color
                    ecvColor::Rgb C(static_cast<ColorCompType>(r),
                                    static_cast<ColorCompType>(g),
                                    static_cast<ColorCompType>(b));
                    proj->addRGBColor(C);
                    ++realCount;
                }
            }
        }
    }
 
    if (realCount == 0) {
        CVLog::Warning(QString("[orthoRectifyAsCloud] Image '%1' was black, "
                               "nothing to project!")
                               .arg(image->getName()));
        delete proj;
        proj = nullptr;
    } else {
        proj->resize(realCount);
    }
 
    return proj;
}
 
/********************************************************************/
/*******************                              *******************/
/*******************  ccOctreeFrustumIntersector  *******************/
/*******************                              *******************/
/********************************************************************/
 
bool ccOctreeFrustumIntersector::build(cloudViewer::DgmOctree* octree) {
    if (!octree) return false;
 
    for (int i = 0; i < cloudViewer::DgmOctree::MAX_OCTREE_LEVEL + 1; i++)
        m_cellsBuilt[i].clear();
 
    const cloudViewer::DgmOctree::cellsContainer& thePointsAndTheirCellCodes =
            octree->pointsAndTheirCellCodes();
    cloudViewer::DgmOctree::cellsContainer::const_iterator it =
            thePointsAndTheirCellCodes.begin();
 
    try {
        for (it = thePointsAndTheirCellCodes.begin();
             it != thePointsAndTheirCellCodes.end(); ++it) {
            cloudViewer::DgmOctree::CellCode completeCode = it->theCode;
            for (unsigned char level = 1;
                 level <= cloudViewer::DgmOctree::MAX_OCTREE_LEVEL; level++) {
                unsigned char bitDec =
                        cloudViewer::DgmOctree::GET_BIT_SHIFT(level);
                m_cellsBuilt[level].insert(completeCode >> bitDec);
            }
        }
    } catch (const std::bad_alloc&) {
        CVLog::Warning("[ccCameraSensor::prepareOctree] Not enough memory!");
        for (int i = 0; i <= cloudViewer::DgmOctree::MAX_OCTREE_LEVEL; i++) {
            m_cellsBuilt[i].clear();
        }
        return false;
    }
 
    m_associatedOctree = octree;
 
    return true;
}
 
ccOctreeFrustumIntersector::OctreeCellVisibility
ccOctreeFrustumIntersector::separatingAxisTest(
        const CCVector3& bbMin,
        const CCVector3& bbMax,
        const float planesCoefficients[6][4],
        const CCVector3 frustumCorners[8],
        const CCVector3 frustumEdges[6],
        const CCVector3& frustumCenter) {
    // first test : if the box is too far from the frustum, there is no
    // intersection
    CCVector3 boxCenter = (bbMax + bbMin) / 2;
    PointCoordinateType dCenter = (boxCenter - frustumCenter).norm();
    PointCoordinateType boxRadius = (bbMax - bbMin).norm();
    PointCoordinateType frustumRadius =
            (frustumCorners[0] - frustumCenter).norm();
    if (dCenter > boxRadius + frustumRadius) return CELL_OUTSIDE_FRUSTUM;
 
    // We could add a test : is the cell circumscribed circle in the frustum
    // incircle frustum ?
    // --> if we are lucky, it could save a lot of time !...
 
    // box corners
    CCVector3 boxCorners[8];
    {
        for (unsigned i = 0; i < 8; i++)
            boxCorners[i] = CCVector3((i & 4) ? bbMin.x : bbMax.x,
                                      (i & 2) ? bbMin.y : bbMax.y,
                                      (i & 1) ? bbMin.z : bbMax.z);
    }
 
    // There are 28 tests to perform:
    //  nbFacesCube = n1 = 3;
    //  nbFacesFrustum = n2 = 5;
    //  nbEdgesCube = n3 = 3;
    //  nbEdgesFrustum = n4 = 6;
    //  nbOtherFrustumCombinations = n5 = 2
    //  nbVecToTest = n1 + n2 + n3*n4 = 3 + 5 + 3*6 + n5 = 28;
    static const unsigned nbVecToTest = 28;
    CCVector3 VecToTest[nbVecToTest];
    // vectors orthogonals to box planes
    VecToTest[0] = CCVector3(1, 0, 0);
    VecToTest[1] = CCVector3(0, 1, 0);
    VecToTest[2] = CCVector3(0, 0, 1);
    // vectors orthogonals to frustum planes
    VecToTest[3] = CCVector3(planesCoefficients[0][0], planesCoefficients[0][1],
                             planesCoefficients[0][2]);
    VecToTest[4] = CCVector3(planesCoefficients[1][0], planesCoefficients[1][1],
                             planesCoefficients[1][2]);
    VecToTest[5] = CCVector3(planesCoefficients[2][0], planesCoefficients[2][1],
                             planesCoefficients[2][2]);
    VecToTest[6] = CCVector3(planesCoefficients[3][0], planesCoefficients[3][1],
                             planesCoefficients[3][2]);
    VecToTest[7] = CCVector3(planesCoefficients[4][0], planesCoefficients[4][1],
                             planesCoefficients[4][2]);
    // combinations box and frustum
    VecToTest[8] = VecToTest[0].cross(frustumEdges[0]);
    VecToTest[9] = VecToTest[0].cross(frustumEdges[1]);
    VecToTest[10] = VecToTest[0].cross(frustumEdges[2]);
    VecToTest[11] = VecToTest[0].cross(frustumEdges[3]);
    VecToTest[12] = VecToTest[0].cross(frustumEdges[4]);
    VecToTest[13] = VecToTest[0].cross(frustumEdges[5]);
    VecToTest[14] = VecToTest[1].cross(frustumEdges[0]);
    VecToTest[15] = VecToTest[1].cross(frustumEdges[1]);
    VecToTest[16] = VecToTest[1].cross(frustumEdges[2]);
    VecToTest[17] = VecToTest[1].cross(frustumEdges[3]);
    VecToTest[18] = VecToTest[1].cross(frustumEdges[4]);
    VecToTest[19] = VecToTest[1].cross(frustumEdges[5]);
    VecToTest[20] = VecToTest[2].cross(frustumEdges[0]);
    VecToTest[21] = VecToTest[2].cross(frustumEdges[1]);
    VecToTest[22] = VecToTest[2].cross(frustumEdges[2]);
    VecToTest[23] = VecToTest[2].cross(frustumEdges[3]);
    VecToTest[24] = VecToTest[2].cross(frustumEdges[4]);
    VecToTest[25] = VecToTest[2].cross(frustumEdges[5]);
    // combinations frustum
    VecToTest[26] = frustumEdges[0].cross(frustumEdges[2]);
    VecToTest[27] = frustumEdges[1].cross(frustumEdges[3]);
 
    // normalization
    {
        for (unsigned i = 0; i < nbVecToTest; i++) VecToTest[i].normalize();
    }
 
    bool boxInside = true;
 
    // project volume corners
    {
        for (unsigned i = 0; i < nbVecToTest; i++) {
            CCVector3 testVec = VecToTest[i];
 
            // box points
            float dMinBox = testVec.dot(boxCorners[0]);
            float dMaxBox = dMinBox;
            {
                for (unsigned j = 1; j < 8; j++) {
                    float d = testVec.dot(boxCorners[j]);
                    if (d > dMaxBox) dMaxBox = d;
                    if (d < dMinBox) dMinBox = d;
                }
            }
 
            // frustum points
            float dMinFru = testVec.dot(frustumCorners[0]);
            float dMaxFru = dMinFru;
            {
                for (unsigned j = 1; j < 8; j++) {
                    float d = testVec.dot(frustumCorners[j]);
                    if (d > dMaxFru) dMaxFru = d;
                    if (d < dMinFru) dMinFru = d;
                }
            }
 
            // if this plane is a separating plane, the cell is outside the
            // frustum
            if (dMaxBox < dMinFru || dMaxFru < dMinBox)
                return CELL_OUTSIDE_FRUSTUM;
 
            // if this plane is NOT a separating plane, the cell is at least
            // intersecting the frustum
            else {
                // moreover, the cell can be completely inside the frustum...
                if (dMaxBox > dMaxFru || dMinBox < dMinFru) boxInside = false;
            }
        }
    }
 
    return boxInside ? CELL_INSIDE_FRUSTUM : CELL_INTERSECT_FRUSTUM;
}
 
void ccOctreeFrustumIntersector::computeFrustumIntersectionByLevel(
        unsigned char level,
        cloudViewer::DgmOctree::CellCode parentTruncatedCode,
        OctreeCellVisibility parentResult,
        const float planesCoefficients[6][4],
        const CCVector3 ptsFrustum[8],
        const CCVector3 edges[6],
        const CCVector3& center) {
    if (parentResult == CELL_OUTSIDE_FRUSTUM) return;
 
    // move code to the left
    cloudViewer::DgmOctree::CellCode baseTruncatedCode =
            (parentTruncatedCode << 3);
 
    // test to do on the 8 child cells
    for (unsigned i = 0; i < 8; i++) {
        // set truncated code of the current cell
        cloudViewer::DgmOctree::CellCode truncatedCode = baseTruncatedCode + i;
 
        // if the current cell has not been built (contains no 3D points), we
        // skip it
        std::unordered_set<cloudViewer::DgmOctree::CellCode>::const_iterator
                got = m_cellsBuilt[level].find(truncatedCode);
        if (got != m_cellsBuilt[level].end()) {
            // get extrema of the current cell
            CCVector3 bbMin, bbMax;
            m_associatedOctree->computeCellLimits(truncatedCode, level, bbMin,
                                                  bbMax, true);
 
            // look if there is a separating plane
            OctreeCellVisibility result =
                    (parentResult == CELL_INSIDE_FRUSTUM
                             ? CELL_INSIDE_FRUSTUM
                             : separatingAxisTest(bbMin, bbMax,
                                                  planesCoefficients,
                                                  ptsFrustum, edges, center));
 
            // if the cell is not outside the frustum, there is a kind of
            // intersection (inside or intesecting)
            if (result != CELL_OUTSIDE_FRUSTUM) {
                if (result == CELL_INSIDE_FRUSTUM)
                    m_cellsInFrustum[level].insert(truncatedCode);
                else
                    m_cellsIntersectFrustum[level].insert(truncatedCode);
 
                // we do the same for the children (if we have not already
                // reached the end of the tree)
                if (level < cloudViewer::DgmOctree::MAX_OCTREE_LEVEL)
                    computeFrustumIntersectionByLevel(
                            level + 1, truncatedCode, result,
                            planesCoefficients, ptsFrustum, edges, center);
            }
        }
    }
}
 
void ccOctreeFrustumIntersector::computeFrustumIntersectionWithOctree(
        std::vector<std::pair<unsigned, CCVector3>>& pointsToTest,
        std::vector<unsigned>& inCameraFrustum,
        const float planesCoefficients[6][4],
        const CCVector3 ptsFrustum[8],
        const CCVector3 edges[6],
        const CCVector3& center) {
    // clear old result
    {
        for (int i = 0; i <= cloudViewer::DgmOctree::MAX_OCTREE_LEVEL; i++) {
            m_cellsInFrustum[i].clear();
            m_cellsIntersectFrustum[i].clear();
        }
    }
 
    // find intersecting cells
    computeFrustumIntersectionByLevel(1, 0, CELL_INTERSECT_FRUSTUM,
                                      planesCoefficients, ptsFrustum, edges,
                                      center);
 
    // get points
    unsigned char level = static_cast<unsigned char>(
            cloudViewer::DgmOctree::MAX_OCTREE_LEVEL);
 
    // dealing with cells completely inside the frustum
    std::unordered_set<cloudViewer::DgmOctree::CellCode>::const_iterator it;
    cloudViewer::ReferenceCloud pointsInCell(
            m_associatedOctree->associatedCloud());
    for (it = m_cellsInFrustum[level].begin();
         it != m_cellsInFrustum[level].end(); ++it) {
        // get all points in cell
        if (m_associatedOctree->getPointsInCell(*it, level, &pointsInCell,
                                                true)) {
            // all points are inside the frustum since the cell itself is
            // completely inside
            for (size_t i = 0; i < pointsInCell.size(); i++)
                inCameraFrustum.push_back(pointsInCell.getPointGlobalIndex(
                        static_cast<unsigned>(i)));
        }
    }
 
    // dealing with cells intersecting the frustum (not completely inside)
    for (it = m_cellsIntersectFrustum[level].begin();
         it != m_cellsIntersectFrustum[level].end(); ++it) {
        // get all points in cell
        if (m_associatedOctree->getPointsInCell(*it, level, &pointsInCell,
                                                true)) {
            // all points may not be inside the frustum since the cell itself is
            // not completely inside
            size_t pointCount = pointsInCell.size();
            size_t sizeBefore = pointsToTest.size();
            pointsToTest.resize(pointCount + sizeBefore);
            for (size_t i = 0; i < pointCount; i++) {
                unsigned currentIndice = pointsInCell.getPointGlobalIndex(
                        static_cast<unsigned>(i));
                const CCVector3* vec =
                        pointsInCell.getPoint(static_cast<unsigned>(i));
                pointsToTest[sizeBefore + i] =
                        std::pair<unsigned, CCVector3>(currentIndice, *vec);
            }
        }
    }
}