ccTrace.cpp

Go to the documentation of this file.

// ----------------------------------------------------------------------------
// -                        CloudViewer: www.cloudViewer.org                  -
// ----------------------------------------------------------------------------
// Copyright (c) 2018-2024 www.cloudViewer.org
// SPDX-License-Identifier: MIT
// ----------------------------------------------------------------------------
 
#include "ccTrace.h"
 
#include <ecvDisplayTools.h>
 
#include <bitset>
#include <queue>
 
ccTrace::ccTrace(ccPointCloud* associatedCloud) : ccPolyline(associatedCloud) {
    init(associatedCloud);
}
 
ccTrace::ccTrace(ccPolyline* obj) : ccPolyline(obj->getAssociatedCloud()) {
    ccPointCloud* cld = dynamic_cast<ccPointCloud*>(obj->getAssociatedCloud());
    assert(cld != nullptr);  // should never be null
    init(cld);
 
    // load waypoints from metadata
    if (obj->hasMetaData("waypoints")) {
        QString waypoints = obj->getMetaData("waypoints").toString();
        for (QString str : waypoints.split(",")) {
            if (str != "") {
                int pID = str.toInt();
                m_waypoints.push_back(pID);  // add waypoint
            }
        }
 
        // store waypoints metadata
        QVariantMap* map = new QVariantMap();
        map->insert("waypoints", waypoints);
        setMetaData(*map, true);
    }
 
    // load cost function from metadata
    if (obj->hasMetaData("cost_function")) {
        ccTrace::COST_MODE = obj->getMetaData("cost_function").toInt();
    }
 
    setName(obj->getName());
 
    // copy polyline into trace points
    std::deque<int> seg;
    for (unsigned i = 0; i < obj->size(); i++) {
        // copy into "trace" object
        int pId = obj->getPointGlobalIndex(i);  // get global point ID
        seg.push_back(pId);
 
        // also copy into polyline object
        addPointIndex(pId);
    }
    m_trace.push_back(seg);
 
    // recalculate trace if polyline data somehow lost [redundancy thing..]
    if (obj->size() == 0) {
        m_trace.clear();
        optimizePath();  //[slooooow...!]
    }
 
    // load SNE data from metadata (TODO)
 
    invalidateBoundingBox();  // update bounding box (for picking)
}
 
void ccTrace::init(ccPointCloud* associatedCloud) {
    setAssociatedCloud(associatedCloud);  // the ccPolyline c'tor should do
                                          // this, but just to be sure...
    m_cloud = associatedCloud;            // store pointer ourselves also
    m_search_r = calculateOptimumSearchRadius();  // estimate the search radius
                                                  // we want to use
 
    // store these info as object attributes
    updateMetadata();
}
 
void ccTrace::updateMetadata() {
    QVariantMap* map = new QVariantMap();
    map->insert("ccCompassType", "Trace");
    map->insert("search_r", m_search_r);
    map->insert("cost_function", ccTrace::COST_MODE);
 
    // TODO - write metadata for structure normal estimates
 
    setMetaData(*map, true);
}
 
int ccTrace::insertWaypoint(int pointId) {
    if (m_waypoints.size() >= 2) {
        // get location of point to add
        // const CCVector3* Q = m_cloud->getPoint(pointId);
        CCVector3 Q = *m_cloud->getPoint(pointId);
        CCVector3 start, end;
        // check if point is "inside" any segments
        for (int i = 1; i < m_waypoints.size(); i++) {
            // get start and end points
            m_cloud->getPoint(m_waypoints[i - 1], start);
            m_cloud->getPoint(m_waypoints[i], end);
 
            // are we are "inside" this segment
            if (inCircle(&start, &end, &Q)) {
                // insert waypoint
                m_waypoints.insert(m_waypoints.begin() + i, pointId);
                m_previous.push_back(i);
                return i;
            }
        }
 
        // check if the point is closer to the start than the end -> i.e. the
        // point should be 'pre-pended'
        CCVector3 sp = Q - start;
        CCVector3 ep = Q - end;
        if (sp.norm2() < ep.norm2()) {
            m_waypoints.insert(m_waypoints.begin(), pointId);
            m_previous.push_back(0);
            return 0;
        }
    }
 
    // add point to end of the trace
    m_waypoints.push_back(pointId);
    int id = static_cast<int>(m_waypoints.size()) - 1;
    m_previous.push_back(id);  // store for undo options
    return id;
}
 
// test if the query point is within a circle bound by segStart & segEnd
bool ccTrace::inCircle(const CCVector3* segStart,
                       const CCVector3* segEnd,
                       const CCVector3* query) {
    // calculate vector Query->Start and Query->End
    CCVector3 QS(segStart->x - query->x, segStart->y - query->y,
                 segStart->z - query->z);
    CCVector3 QE(segEnd->x - query->x, segEnd->y - query->y,
                 segEnd->z - query->z);
 
    // is angle between these vectors obtuce (i.e. QS dot QE) < 0)? If so we are
    // inside a circle between start&end, otherwise we are not
    QS.normalize();
    QE.normalize();
 
    return QS.dot(QE) < 0;
}
 
bool ccTrace::optimizePath(int maxIterations) {
    bool success = true;
 
    if (m_waypoints.size() < 2) {
        m_trace.clear();
        return false;  // no segments...
    }
 
#ifdef DEBUG_PATH
    int idx = m_cloud->getScalarFieldIndexByName(
            "Search");  // look for scalar field to write search to
    if (idx == -1)      // doesn't exist - create
        idx = m_cloud->addScalarField("Search");
    m_cloud->setCurrentScalarField(idx);
#endif
 
    // update stored cost function etc.
    updateMetadata();
 
    // update internal vars
    m_maxIterations = maxIterations;
 
    // loop through segments and build/rebuild trace
    int start, end, tID;  // declare vars
    for (unsigned i = 1; i < m_waypoints.size(); i++) {
        // calculate indices
        start = m_waypoints[i - 1];  // global point id for the start waypoint
        end = m_waypoints[i];        // global point id for the end waypoint
        tID = i - 1;  // id of the trace segment id (in m_trace vector)
 
        // are we adding to the end of the trace?
        if (tID >= m_trace.size()) {
            std::deque<int> segment = optimizeSegment(
                    start, end, m_search_r);  // calculate segment
            m_trace.push_back(segment);       // store segment
            success = success &&
                      !segment.empty();  // if the queue is empty, we failed
        } else  // no... we're somewhere in the middle - update segment if
                // necessary
        {
            if (!m_trace[tID].empty() && (m_trace[tID][0] == start) &&
                (m_trace[tID][m_trace[tID].size() - 1] ==
                 end))     // valid trace and start/end match
                continue;  // this trace has already been calculated - we can
                           // skip! :)
            else {
                // calculate segment
                std::deque<int> segment = optimizeSegment(
                        start, end, m_search_r);  // calculate segment
                success = success &&
                          !segment.empty();  // if the queue is empty, we failed
 
                // add trace
                if (m_trace[tID][m_trace[tID].size() - 1] ==
                    end)  // end matches - we can replace the current trace &
                          // things should be sweet (all prior traces will have
                          // been updated already)
                    m_trace[tID] = segment;  // end is correct - overwrite this
                                             // block, then hopefully we will
                                             // match in the next one
                else  // end doesn't match - we need to insert
                    m_trace.insert(m_trace.begin() + tID, segment);
            }
        }
    }
 
#ifdef DEBUG_PATH
    cloudViewer::ScalarField* f = m_cloud->getScalarField(idx);
    f->computeMinAndMax();
#endif
 
    // write control points to property (for reloading)
    QVariantMap* map = new QVariantMap();
    QString waypoints = "";
 
    for (unsigned i = 0; i < m_waypoints.size(); i++) {
        waypoints += QString::number(m_waypoints[i]) + ",";
    }
 
    map->insert("waypoints", waypoints);
    setMetaData(*map, true);
 
    // push points onto underlying polyline object (for picking & save/load)
    finalizePath();
 
    return success;
}
 
void ccTrace::finalizePath() {
    // clear existing points in background "polyline"
    clear();
 
    // push trace buffer to said polyline (for save/export etc.)
    for (std::deque<int> seg : m_trace) {
        for (int p : seg) {
            addPointIndex(p);
        }
    }
 
    // invalidate bb
    invalidateBoundingBox();
}
 
void ccTrace::recalculatePath() {
    m_trace.clear();
    optimizePath();
}
 
int ccTrace::COST_MODE = ccTrace::MODE::DARK;  // set default cost mode
std::deque<int> ccTrace::optimizeSegment(int start, int end, int offset) {
    // check handle to point cloud
    if (!m_cloud) {
        return std::deque<int>();  // error -> no cloud
    }
 
    // retrieve and store start & end rgb
    if (m_cloud->hasColors()) {
        const ecvColor::Rgb& s = m_cloud->getPointColor(start);
        const ecvColor::Rgb& e = m_cloud->getPointColor(end);
        m_start_rgb[0] = s.r;
        m_start_rgb[1] = s.g;
        m_start_rgb[2] = s.b;
        m_end_rgb[0] = e.r;
        m_end_rgb[1] = e.g;
        m_end_rgb[2] = e.b;
    } else {  // no colour... set to 0 just in case something tries to use these
              // vars
        m_start_rgb[0] = 0;
        m_start_rgb[1] = 0;
        m_start_rgb[2] = 0;
        m_end_rgb[0] = 0;
        m_end_rgb[1] = 0;
        m_end_rgb[2] = 0;
    }
 
    // get location of target node - used to optimise algorithm to stop
    // searching paths leading away from the target
    const CCVector3* end_v = m_cloud->getPoint(end);
 
    // code essentially taken from wikipedia page for Djikstra:
    // https://en.wikipedia.org/wiki/Dijkstra%27s_algorithm
    std::vector<bool>
            visited;  // an array of bits to check if node has been visited
    std::priority_queue<Node*, std::vector<Node*>, Compare>
            openQueue;  // priority queue that stores nodes that haven't yet
                        // been explored/opened
    std::vector<Node*> nodes;  // list of visited nodes. Used to cleanup memory
                               // after re-constructing shortest path.
 
    // set size of visited such that there is one bit per point in the cloud
    visited.resize(m_cloud->size(), false);  // n.b. for 400 million points,
                                             // this will still only be ~50Mb =)
 
    // declare variables used in the loop
    Node* current = nullptr;
    int current_idx = 0;
    int cost = 0;
    int smallest_cost = 999999;
    int iter_count = 0;
    float cur_d2, next_d2;
 
    // setup buffer for storing nodes
    int bufferSize = 500000;  // 500k node buffer (~1.5-2Mb). This should be
                              // enough for most small-medium size traces. More
                              // buffers will be created for bigger ones.
    Node* node_buffer = new Node[bufferSize];
    nodes.push_back(node_buffer);  // store buffer in nodes list (for cleanup)
    int nodeCount = 1;             // start node will be added shortly
 
    // setup octree & values for nearest neighbour searches
    ccOctree::Shared oct = m_cloud->getOctree();
    if (!oct) {
        oct = m_cloud->computeOctree();  // if the user clicked "no" when asked
                                         // to compute the octree then tough....
    }
    unsigned char level =
            oct->findBestLevelForAGivenNeighbourhoodSizeExtraction(m_search_r);
 
    // initialize start node on node_buffer and add to openQueue
    node_buffer[0].set(start, 0, nullptr);
    openQueue.push(&node_buffer[0]);
 
    // mark start node as visited
    visited[start] = true;
 
    while (openQueue.size() > 0)  // while unvisited nodes exist
    {
        // check if we excede max iterations
        if (iter_count > m_maxIterations) {
            // cleanup buffers
            for (Node* n : nodes) {
                delete[] n;
            }
 
            // bail
            return std::deque<int>();  // bail
        }
 
        iter_count++;
 
        // get lowest cost node for expansion
        current = openQueue.top();
        current_idx = current->index;
 
        // remove node from open set
        openQueue.pop();  // remove node from open set (queue)
 
        if (current_idx == end)  // we've found it!
        {
            std::deque<int> path;
            path.push_back(end);  // add end node
 
            // traverse backwards to reconstruct path
            while (current->index != start) {
                current = current->previous;
                path.push_front(current->index);
            }
 
            path.push_front(start);
 
            // cleanup node buffers
            for (Node* n : nodes) {
                delete[] n;
            }
 
            // return
            return path;
        }
 
        // calculate distance from current nodes parent to end -> avoid going
        // backwards (in euclidean space) [essentially stops fracture turning >
        // 90 degrees)
        const CCVector3* cur = m_cloud->getPoint(current_idx);
        cur_d2 = (cur->x - end_v->x) * (cur->x - end_v->x) +
                 (cur->y - end_v->y) * (cur->y - end_v->y) +
                 (cur->z - end_v->z) * (cur->z - end_v->z);
 
        // fill "neighbours" with nodes - essentially get results of a "sphere"
        // search around active current point
        m_neighbours.clear();
 
        oct->getPointsInSphericalNeighbourhood(
                *cur, PointCoordinateType(m_search_r), m_neighbours, level);
 
        // loop through neighbours
        for (size_t i = 0; i < m_neighbours.size(); i++) {
            m_p = m_neighbours[i];
 
            if (visited[m_p.pointIndex])  // Has this node been visited before?
                                          // If so then bail.
                continue;
 
            // calculate (squared) distance from this neighbour to the end
            next_d2 = (m_p.point->x - end_v->x) * (m_p.point->x - end_v->x) +
                      (m_p.point->y - end_v->y) * (m_p.point->y - end_v->y) +
                      (m_p.point->z - end_v->z) * (m_p.point->z - end_v->z);
 
            if (next_d2 >=
                cur_d2)  // Bigger than the original distance? If so then bail.
                continue;
 
            // calculate cost to this neighbour
            cost = getSegmentCost(current_idx, m_p.pointIndex);
 
#ifdef DEBUG_PATH
            m_cloud->setPointScalarValue(
                    m_p.pointIndex,
                    static_cast<ScalarType>(
                            cost));  // STORE VISITED NODES (AND COST) FOR DEBUG
                                     // VISUALISATIONS
#endif
 
            // transform into cost from start node
            cost += current->total_cost;
 
            // check that the node buffer isn't full
            if (nodeCount == bufferSize) {
                // buffer is full - create a new one
                node_buffer = new Node[bufferSize];
                nodes.push_back(node_buffer);
                nodeCount = 0;
            }
 
            // initialize node on node buffer
            node_buffer[nodeCount].set(m_p.pointIndex, cost, current);
 
            // push node to open set
            openQueue.push(&node_buffer[nodeCount]);
 
            // we now have one more node
            nodeCount++;
 
            // mark node as visited
            visited[m_p.pointIndex] = true;
        }
    }
 
    assert(false);
    return std::deque<int>();  // shouldn't come here?
}
 
int ccTrace::getSegmentCost(int p1, int p2) {
    int cost = 1;  // n.b. default value is 1 so that if no cost functions are
                   // used, the function doesn't crash (and returns the
                   // unweighted shortest path)
    if (m_cloud->hasColors())  // check cloud has colour data
    {
        if (COST_MODE & MODE::RGB) cost += getSegmentCostRGB(p1, p2);
        if (COST_MODE & MODE::DARK) cost += getSegmentCostDark(p1, p2);
        if (COST_MODE & MODE::LIGHT) cost += getSegmentCostLight(p1, p2);
        if (COST_MODE & MODE::GRADIENT)
            cost += getSegmentCostGrad(p1, p2, m_search_r);
    }
    if (m_cloud->hasDisplayedScalarField())  // check cloud has scalar field
                                             // data
    {
        if (COST_MODE & MODE::SCALAR) cost += getSegmentCostScalar(p1, p2);
        if (COST_MODE & MODE::INV_SCALAR)
            cost += getSegmentCostScalarInv(p1, p2);
    }
 
    // these cost functions can be used regardless
    if (COST_MODE & MODE::CURVE) cost += getSegmentCostCurve(p1, p2);
    if (COST_MODE & MODE::DISTANCE) cost += getSegmentCostDist(p1, p2);
 
    return cost;
}
 
int ccTrace::getSegmentCostRGB(int p1, int p2) {
    // get colors
    const ecvColor::Rgb& p1_rgb = m_cloud->getPointColor(p1);
    const ecvColor::Rgb& p2_rgb = m_cloud->getPointColor(p2);
 
    // cost function: cost = |c1-c2| + 0.25 ( |c1-start| + |c1-end| + |c2-start|
    // + |c2-end| ) IDEA: can we somehow optimize all the square roots here (for
    // speed)?
    return sqrt(
                   //|c1-c2|
                   (p1_rgb.r - p2_rgb.r) * (p1_rgb.r - p2_rgb.r) +
                   (p1_rgb.g - p2_rgb.g) * (p1_rgb.g - p2_rgb.g) +
                   (p1_rgb.b - p2_rgb.b) * (p1_rgb.b - p2_rgb.b)) +
           0.25 *
                   (
                           //|c1-start|
                           sqrt((p1_rgb.r - m_start_rgb[0]) *
                                        (p1_rgb.r - m_start_rgb[0]) +
                                (p1_rgb.g - m_start_rgb[1]) *
                                        (p1_rgb.g - m_start_rgb[1]) +
                                (p1_rgb.b - m_start_rgb[2]) *
                                        (p1_rgb.b - m_start_rgb[2])) +
                           //|c1-end|
                           sqrt((p1_rgb.r - m_end_rgb[0]) *
                                        (p1_rgb.r - m_end_rgb[0]) +
                                (p1_rgb.g - m_end_rgb[1]) *
                                        (p1_rgb.g - m_end_rgb[1]) +
                                (p1_rgb.b - m_end_rgb[2]) *
                                        (p1_rgb.b - m_end_rgb[2])) +
                           //|c2-start|
                           sqrt((p2_rgb.r - m_start_rgb[0]) *
                                        (p2_rgb.r - m_start_rgb[0]) +
                                (p2_rgb.g - m_start_rgb[1]) *
                                        (p2_rgb.g - m_start_rgb[1]) +
                                (p2_rgb.b - m_start_rgb[2]) *
                                        (p2_rgb.b - m_start_rgb[2])) +
                           //|c2-end|
                           sqrt((p2_rgb.r - m_end_rgb[0]) *
                                        (p2_rgb.r - m_end_rgb[0]) +
                                (p2_rgb.g - m_end_rgb[1]) *
                                        (p2_rgb.g - m_end_rgb[1]) +
                                (p2_rgb.b - m_end_rgb[2]) *
                                        (p2_rgb.b - m_end_rgb[2]))) /
                   3.5;  // N.B. the divide by 3.5 scales this cost function to
                         // range between 0 & 255
}
 
int ccTrace::getSegmentCostDark(int p1, int p2) {
    // return magnitude of the point p2
    // const ColorCompType* p1_rgb = m_cloud->getPointColor(p1);
    const ecvColor::Rgb& p2_rgb = m_cloud->getPointColor(p2);
 
    // return "taxi-cab" length
    return (p2_rgb.r + p2_rgb.g +
            p2_rgb.b);  // note: this will naturally give a maximum of 765 (=255
                        // + 255 + 255)
}
 
int ccTrace::getSegmentCostLight(int p1, int p2) {
    // return the opposite of getCostDark
    return 765 - getSegmentCostDark(p1, p2);
}
 
int ccTrace::getSegmentCostCurve(int p1, int p2) {
    int idx = m_cloud->getScalarFieldIndexByName(
            "Curvature");          // look for pre-existing gradient SF
    if (isCurvaturePrecomputed())  // scalar field found - return from
                                   // precomputed cost]
    {
        // activate SF
        m_cloud->setCurrentScalarField(idx);
 
        // return inverse of p2 value
        return m_cloud->getScalarField(idx)->getMax() -
               m_cloud->getPointScalarValue(p2);
    } else  // scalar field not found - do slow calculation...
    {
        // put neighbourhood in a cloudViewer::Neighbourhood structure
        if (m_neighbours.size() >
            4)  // need at least 4 points to calculate curvature....
        {
            m_neighbours.push_back(
                    m_p);  // add center point onto end of neighbourhood
 
            // compute curvature
            cloudViewer::DgmOctreeReferenceCloud nCloud(
                    &m_neighbours, static_cast<unsigned>(m_neighbours.size()));
            cloudViewer::Neighbourhood Z(&nCloud);
            float c = Z.computeCurvature(
                    *nCloud.getPoint(0),
                    cloudViewer::Neighbourhood::CurvatureType::MEAN_CURV);
 
            m_neighbours.erase(m_neighbours.end() -
                               1);  // remove center point from neighbourhood
                                    // (so as not to screw up loops)
 
            // curvature tends to range between 0 (high cost) and 10 (low cost),
            // though it can be greater than 10 in extreme cases hence we need
            // to map to domain 0 - 10 and then transform that to the (integer)
            // domain 0 - 884 to meet the cost function spec
            if (c > 10) c = 10;
 
            // scale curvature to range 0, 765
            c *= 76.5;
 
            // note that high curvature = low cost, hence subtract curvature
            // from 765
            return 765 - c;
        }
        return 765;  // unknown curvature - this point is high cost.
    }
}
 
int ccTrace::getSegmentCostGrad(int p1, int p2, float search_r) {
    int idx = m_cloud->getScalarFieldIndexByName(
            "Gradient");  // look for pre-existing gradient SF
    if (idx != -1)        // found precomputed gradient
    {
        // activate SF
        m_cloud->setCurrentScalarField(idx);
 
        // return inverse of p2 value
        return m_cloud->getScalarField(idx)->getMax() -
               m_cloud->getPointScalarValue(p2);
    } else  // not found... do expensive calculation
    {
        CCVector3 p = *m_cloud->getPoint(p2);
        const ecvColor::Rgb p2_rgb = m_cloud->getPointColor(p2);
        int p_value = p2_rgb.r + p2_rgb.g + p2_rgb.b;
 
        if (m_neighbours.size() >
            2)  // need at least 2 points to calculate gradient....
        {
            // N.B. The following code is mostly stolen from the computeGradient
            // function in CLOUDVIEWER
            CCVector3d sum(0, 0, 0);
            cloudViewer::DgmOctree::PointDescriptor n;
            for (int i = 0; i < m_neighbours.size(); i++) {
                n = m_neighbours[i];
 
                // vector from p1 to m_p
                CCVector3 deltaPos = *n.point - p;
                double norm2 = deltaPos.norm2d();
 
                // colour
                const ecvColor::Rgb& c = m_cloud->getPointColor(n.pointIndex);
                int c_value = (static_cast<int>(c.r) + c.g) + c.b;
 
                // calculate gradient weighted by distance to the point (i.e.
                // divide by distance^2)
                if (cloudViewer::GreaterThanEpsilon(norm2)) {
                    // color magnitude difference
                    int deltaValue = p_value - c_value;
                    // divide by norm^2 to get distance-weighted gradient
                    deltaValue /=
                            norm2;  // we divide by 'norm' to get the normalized
                                    // direction, and by 'norm' again to get the
                                    // gradient (hence we use the squared norm)
                    // sum stuff
                    sum.x += deltaPos.x *
                             deltaValue;  // warning: here 'deltaValue'=
                                          // deltaValue / squaredNorm(deltaPos)
                                          // ;)
                    sum.y += deltaPos.y * deltaValue;
                    sum.z += deltaPos.z * deltaValue;
                }
            }
 
            float gradient = sum.norm() / m_neighbours.size();
 
            // ensure gradient is lass than a case-specific maximum gradient
            // (colour change from white to black across a distance or search_r,
            //                                                                                   giving a gradient of (255+255+255) / search_r)
            gradient = std::min(gradient, 765 / search_r);
            gradient *= search_r;   // scale between 0 and 765
            return 765 - gradient;  // return inverse gradient (high gradient =
                                    // low cost)
        }
        return 765;  // no gradient = high cost
    }
}
 
int ccTrace::getSegmentCostDist(int p1, int p2) { return 255; }
 
int ccTrace::getSegmentCostScalar(int p1, int p2) {
    // m_cloud->getCurrentDisplayedScalarFieldIndex();
    ccScalarField* sf = static_cast<ccScalarField*>(
            m_cloud->getCurrentDisplayedScalarField());
    return (sf->getValue(p2) - sf->getMin()) *
           (765 / (sf->getMax() - sf->getMin()));  // return scalar field value
                                                   // mapped to range 0 - 765
}
 
int ccTrace::getSegmentCostScalarInv(int p1, int p2) {
    ccScalarField* sf = static_cast<ccScalarField*>(
            m_cloud->getCurrentDisplayedScalarField());
    return (sf->getMax() - sf->getValue(p2)) *
           (765 /
            (sf->getMax() - sf->getMin()));  // return inverted scalar field
                                             // value mapped to range 0 - 765
}
 
// functions for calculating cost SFs
void ccTrace::buildGradientCost(QWidget* parent) {
    // is there already a gradient SF?
    if (isGradientPrecomputed())  // already done :)
        return;
 
    // create new SF for greyscale
    int idx = m_cloud->addScalarField("Greyscale");
    m_cloud->setCurrentScalarField(idx);
 
    // make colours greyscale and push to SF (otherwise copy active SF)
    for (unsigned i = 0; i < m_cloud->size(); i++) {
        const ecvColor::Rgb& col = m_cloud->getPointColor(i);
        m_cloud->setPointScalarValue(
                i, static_cast<ScalarType>((static_cast<int>(col.r) + col.g) +
                                           col.b));
    }
    // compute min/max
    m_cloud->getScalarField(idx)->computeMinAndMax();
 
    // calculate new SF with gradient of the old one
    float roughnessKernelSize = m_search_r;
    m_cloud->setCurrentOutScalarField(idx);  // set out gradient field - values
                                             // are read from here for calc
 
    // make gradient sf
    int gIdx = m_cloud->addScalarField("Gradient");
    m_cloud->setCurrentInScalarField(
            gIdx);  // set in scalar field - gradient is written here
 
    // get/build octree
    ecvProgressDialog pDlg(true, parent);
    pDlg.show();
 
    ccOctree::Shared octree = m_cloud->getOctree();
    if (!octree) {
        octree = m_cloud->computeOctree(&pDlg);
        if (!octree) {
            CVLog::Error("Failed to compute octree");
            return;
        }
    }
 
    // calculate gradient
    int result = cloudViewer::ScalarFieldTools::computeScalarFieldGradient(
            m_cloud,
            m_search_r,  // auto --> FIXME: should be properly set by the user!
            false, false, &pDlg, octree.data());
 
    pDlg.close();
 
    if (result != 0) {
        m_cloud->deleteScalarField(gIdx);
        CVLog::Warning("Failed to compute the scalar field gradient");
        return;
    }
 
    // calculate bounds
    m_cloud->getScalarField(gIdx)->computeMinAndMax();
 
    // normalize and log-transform
    m_cloud->setCurrentScalarField(gIdx);
    float logMax = log(m_cloud->getScalarField(gIdx)->getMax() + 10);
    for (unsigned i = 0; i < m_cloud->size(); i++) {
        int nVal = 765 * log(m_cloud->getPointScalarValue(i) + 10) / logMax;
        if (nVal < 0)  // this is caused by isolated points that were assigned
                       // "null" value gradients
            nVal = 1;  // set to low gradient by default
        m_cloud->setPointScalarValue(i, nVal);
    }
 
    // recompute min-max...
    m_cloud->getScalarField(gIdx)->computeMinAndMax();
}
 
void ccTrace::buildCurvatureCost(QWidget* parent) {
    if (isCurvaturePrecomputed())  // already done
        return;
 
    // create curvature SF
    // make gradient sf
    int idx = m_cloud->addScalarField("Curvature");
    m_cloud->setCurrentInScalarField(
            idx);  // set in scalar field - curvature is written here
    m_cloud->setCurrentScalarField(idx);
 
    // get/build octree
    ecvProgressDialog pDlg(true, parent);
    pDlg.show();
 
    ccOctree::Shared octree = m_cloud->getOctree();
    if (!octree) {
        octree = m_cloud->computeOctree(&pDlg);
    }
 
    // calculate curvature
    cloudViewer::GeometricalAnalysisTools::ErrorCode result =
            cloudViewer::GeometricalAnalysisTools::ComputeCharactersitic(
                    cloudViewer::GeometricalAnalysisTools::Curvature,
                    cloudViewer::Neighbourhood::CurvatureType::MEAN_CURV,
                    m_cloud, m_search_r, nullptr, &pDlg, octree.data());
 
    pDlg.close();
 
    if (result != cloudViewer::GeometricalAnalysisTools::NoError) {
        m_cloud->deleteScalarField(idx);
        CVLog::Warning("Failed to compute the curvature");
        return;
    }
 
    // calculate minmax
    m_cloud->getScalarField(idx)->computeMinAndMax();
 
    // normalize and log-transform
    float logMax = log(m_cloud->getScalarField(idx)->getMax() + 10);
    for (unsigned i = 0; i < m_cloud->size(); i++) {
        int nVal = 765 * log(m_cloud->getPointScalarValue(i) + 10) / logMax;
        if (nVal < 0)  // this is caused by isolated points that were assigned
                       // "null" value curvatures
            nVal = 1;  // set to low gradient by default
        m_cloud->setPointScalarValue(i, nVal);
    }
 
    // recompute min-max...
    m_cloud->getScalarField(idx)->computeMinAndMax();
}
 
bool ccTrace::isGradientPrecomputed() {
    int idx = m_cloud->getScalarFieldIndexByName(
            "Gradient");  // look for pre-existing gradient SF
    return idx != -1;     // was something found?
}
bool ccTrace::isCurvaturePrecomputed() {
    int idx = m_cloud->getScalarFieldIndexByName(
            "Curvature");  // look for pre-existing gradient SF
    return idx != -1;      // was something found?
}
 
ccFitPlane* ccTrace::fitPlane(int surface_effect_tolerance,
                              float min_planarity) {
    // put all "trace" points into the cloud
    finalizePath();
 
    if (size() < 3) return 0;  // need three points to fit a plane
 
    // check we are not trying to fit a plane to a line
    cloudViewer::Neighbourhood Z(this);
 
    // calculate eigenvalues of neighbourhood
    cloudViewer::SquareMatrixd cov = Z.computeCovarianceMatrix();
    cloudViewer::SquareMatrixd eigVectors;
    std::vector<double> eigValues;
    if (Jacobi<double>::ComputeEigenValuesAndVectors(cov, eigVectors, eigValues,
                                                     true)) {
        // sort eigenvalues into decending order
        std::sort(eigValues.rbegin(), eigValues.rend());
 
        float y = eigValues[1];  // middle eigen
        float z = eigValues[2];  // smallest eigen (parallel to plane normal)
 
        // calculate planarity (0 = line or random, 1 = plane)
        float planarity = 1.0f - z / y;
        if (planarity < min_planarity) {
            return nullptr;
        }
    }
 
    // fit plane
    double rms = 0.0;  // output for rms
    ccFitPlane* p = ccFitPlane::Fit(this, &rms);
 
    if (!p) {
        return nullptr;  // return null for invalid planes
    }
 
    p->updateAttributes(rms, m_search_r);
 
    // test for 'surface effect'
    if (m_cloud->hasNormals()) {
        // calculate average normal of points on trace
        CCVector3 n_avg;
        for (unsigned i = 0; i < size(); i++) {
            // get normal vector
            CCVector3 n = ccNormalVectors::GetNormal(
                    m_cloud->getPointNormalIndex(this->getPointGlobalIndex(i)));
            n_avg += n;
        }
        n_avg *= (PC_ONE / size());  // turn sum into average
 
        // compare to plane normal
        CCVector3 n = p->getNormal();
        if (acos(n_avg.dot(n)) <
            0.01745329251 * surface_effect_tolerance)  // 0.01745329251 converts
                                                       // deg to rad
        {
            // this is a false (surface) plane - reject
            return 0;  // don't return plane
        }
    }
 
    // all is good! Return the plane :)
    return p;
}
 
void ccTrace::bakePathToScalarField() {
    // bake points
    int vertexCount = static_cast<int>(m_cloud->size());
    for (const std::deque<int>& seg : m_trace) {
        for (int p : seg) {
            if (p >= 0 && p < vertexCount) {
                m_cloud->setPointScalarValue(static_cast<unsigned>(p),
                                             getUniqueID());
            }
        }
    }
}
 
float ccTrace::calculateOptimumSearchRadius() {
    cloudViewer::DgmOctree::NeighboursSet neighbours;
 
    // setup octree & values for nearest neighbour searches
    ccOctree::Shared oct = m_cloud->getOctree();
    if (!oct) {
        oct = m_cloud->computeOctree();  // if the user clicked "no" when asked
                                         // to compute the octree then tough....
    }
 
    // init vars needed for nearest neighbour search
    unsigned char level = oct->findBestLevelForAGivenPopulationPerCell(2);
    cloudViewer::ReferenceCloud* nCloud =
            new cloudViewer::ReferenceCloud(m_cloud);
 
    // pick 15 random points
    unsigned int npoints = m_cloud->size();
    double dsum = 0;
    srand(npoints);  // set seed as n for repeatability
    for (unsigned int i = 0; i < 30; i++) {
        // int rn = rand() * rand(); //need to make bigger than rand max...
        int r = (rand() * rand()) %
                npoints;  // random(ish) number between 0 and n
 
        // find nearest neighbour for point
        nCloud->clear(false);
        double d = -1.0;
        oct->findPointNeighbourhood(m_cloud->getPoint(r), nCloud, 2, level, d);
 
        if (d != -1.0)  // if a point was found
        {
            dsum += sqrt(d);
        }
    }
 
    // average nearest-neighbour distances
    double d = dsum / 30;
 
    // return a number slightly larger than the average distance
    return d * 1.5;
}
 
static QSharedPointer<ccSphere> c_unitPointMarker(0);
void ccTrace::drawMeOnly(CC_DRAW_CONTEXT& context) {
    if (!MACRO_Foreground(context))  // 2D foreground only
        return;                      // do nothing
 
    if (MACRO_Draw3D(context)) {
        if (m_waypoints.empty())  // no points -> bail!
            return;
 
        // get the set of OpenGL functions (version 2.1)
        if (ecvDisplayTools::GetCurrentScreen() == nullptr) {
            assert(false);
            return;
        }
 
        // check sphere exists
        if (!c_unitPointMarker) {
            c_unitPointMarker = QSharedPointer<ccSphere>(
                    new ccSphere(1.0f, 0, "PointMarker", 6));
 
            c_unitPointMarker->showColors(true);
            c_unitPointMarker->setVisible(true);
            c_unitPointMarker->setEnabled(true);
            c_unitPointMarker->showNormals(false);
        }
 
        glDrawParams glParams;
        getDrawingParameters(glParams);
 
        // not sure what this does, but it looks like fun
        CC_DRAW_CONTEXT markerContext =
                context;  // build-up point maker own 'context'
        // markerContext.display = 0;
        markerContext.drawingFlags &=
                (~CC_ENTITY_PICKING);  // we must remove the 'push name flag'
                                       // so that the sphere doesn't push its
                                       // own!
 
        // get camera info
        ccGLCameraParameters camera;
        ecvDisplayTools::GetGLCameraParameters(camera);
        const ecvViewportParameters& viewportParams =
                ecvDisplayTools::GetViewportParameters();
 
        // push name for picking
        bool entityPickingMode = MACRO_EntityPicking(context);
        if (entityPickingMode) {
            // glFunc->glPushName(getUniqueIDForDisplay());
            // minimal display for picking mode!
            glParams.showNorms = false;
            glParams.showColors = false;
        }
 
        // set draw colour
        ecvColor::Rgb color = getMeasurementColour();
        c_unitPointMarker->setTempColor(color);
 
        // get point size for drawing
        float pSize = markerContext.defaultPointSize;
        // glFunc->glGetFloatv(GL_POINT_SIZE, &pSize);
 
        // draw key-points and structure normals (if assigned)
        if (m_isActive) {
            for (size_t i = 0; i < m_waypoints.size(); i++) {
                // glFunc->glMatrixMode(GL_MODELVIEW);
                // glFunc->glPushMatrix();
 
                const CCVector3* P = m_cloud->getPoint(m_waypoints[i]);
                // ccGL::Translate(glFunc, P->x, P->y, P->z);
                markerContext.transformInfo.setTranslationStart(
                        CCVector3(P->x, P->y, P->z));
                float scale = context.labelMarkerSize * m_relMarkerScale * 0.3 *
                              fmin(pSize, 4);
                if (viewportParams.perspectiveView && viewportParams.zFar > 0) {
                    // in perspective view, the actual scale depends on the
                    // distance to the camera!
                    double d =
                            (camera.modelViewMat * CCVector3d::fromArray(P->u))
                                    .norm();
                    double unitD = viewportParams.zFar /
                                   2;  // we consider that the 'standard' scale
                                       // is at half the depth
                    scale = static_cast<float>(
                            scale *
                            sqrt(d /
                                 unitD));  // sqrt = empirical (probably because
                                           // the marker size is already partly
                                           // compensated by
                                           // ecvDisplayTools::computeActualPixelSize())
                }
                // glFunc->glScalef(scale, scale, scale);
                markerContext.transformInfo.setScale(
                        CCVector3(scale, scale, scale));
                c_unitPointMarker->draw(markerContext);
                // glFunc->glPopMatrix();
            }
        } else  // just draw lines
        {
            // draw lines
            for (const std::deque<int>& seg : m_trace) {
                if (m_width != 0) {
                    // glFunc->glPushAttrib(GL_LINE_BIT);
                    // glFunc->glLineWidth(static_cast<GLfloat>(m_width));
                    markerContext.defaultLineWidth = m_width;
                }
                // glFunc->glBegin(GL_LINE_STRIP);
                // glFunc->glColor3f(color.r, color.g, color.b);
                // for (int p : seg)
                //{
                //  ccGL::Vertex3v(glFunc, m_cloud->getPoint(p)->u);
                // }
                // glFunc->glEnd();
                // if (m_width != 0)
                //{
                //  glFunc->glPopAttrib();
                // }
            }
        }
 
        // draw trace points if trace is active OR point size is large
        // (otherwise line gets hidden)
        if (m_isActive || pSize > 8) {
            for (const std::deque<int>& seg : m_trace) {
                for (int p : seg) {
                    // glFunc->glMatrixMode(GL_MODELVIEW);
                    // glFunc->glPushMatrix();
 
                    const CCVector3* P = m_cloud->getPoint(p);
                    // ccGL::Translate(glFunc, P->x, P->y, P->z);
                    markerContext.transformInfo.setTranslationStart(
                            CCVector3(P->x, P->y, P->z));
                    float scale = context.labelMarkerSize * m_relMarkerScale *
                                  fmin(pSize, 4) * 0.2;
                    if (viewportParams.perspectiveView &&
                        viewportParams.zFar > 0) {
                        // in perspective view, the actual scale depends on the
                        // distance to the camera!
                        const double* M = camera.modelViewMat.data();
                        double d = (camera.modelViewMat *
                                    CCVector3d::fromArray(P->u))
                                           .norm();
                        double unitD = viewportParams.zFar /
                                       2;  // we consider that the 'standard'
                                           // scale is at half the depth
                        scale = static_cast<float>(
                                scale *
                                sqrt(d /
                                     unitD));  // sqrt = empirical (probably
                                               // because the marker size is
                                               // already partly compensated by
                                               // ecvDisplayTools::computeActualPixelSize())
                    }
 
                    // glFunc->glScalef(scale, scale, scale);
                    markerContext.transformInfo.setScale(
                            CCVector3(scale, scale, scale));
                    c_unitPointMarker->draw(markerContext);
                    // glFunc->glPopMatrix();
                }
            }
        }
    }
}
 
bool ccTrace::isTrace(
        ccHObject* object)  // return true if object is a valid trace
                            // [regardless of it's class type]
{
    if (object->hasMetaData("ccCompassType")) {
        return object->getMetaData("ccCompassType")
                .toString()
                .contains("Trace");
    }
    return false;
}