CloudCompare内部八叉树集成与最邻近搜索

目的

想使用一下CloudCompare自带的八叉树，替换掉nanoflann。

nanoflann返回值出现异常

​ 用于最邻近搜索，在radiussearch下，如果能找到最近点，那么确实找到的是最近点，如果找不到最近点，那么就会出现问题，如下：

nanoflann::KdTreeFLANN<PB::pmPointXYZ>::Ptr kdtree = std::make_shared<nanoflann::KdTreeFLANN<PB::pmPointXYZ>>();
if (!kdtree) {
    std::cerr << "KD tree build failed!" << std::endl;
    return;
}
kdtree->setInputCloud(cloud);
for (int i = 0; i < checkcorrs.size(); ++i) {
    std::vector<int> searchIndexes;
    std::vector<SCALAR_TYPE> distances;
    SCALAR_TYPE *search_pt = new SCALAR_TYPE[3];
    search_pt[0] = checkcorrs[i].x;
    search_pt[1] = checkcorrs[i].y;
    search_pt[2] = checkcorrs[i].z;
    kdtree->radiusSearch(search_pt, 2.0, searchIndexes, distances);
}

​ 在for循环中，每次更新query的点坐标，然后试图从点云中返回其最近的2.0m范围内的点，如果query点周围没有符合条件的点，那么就会返回上一次for循环的值。详见issue。

使用CloudCompare自带的Octree

• 核心为直接接入cc内部的octree实现最近点的索引，位于#include <DgmOctree.h>，这样检索速度大大加快。过程如下：

1. 首先找到当前选中哪些entity，此时需要引用主窗口的头文件并获取全局静态变量#include <mainwindow.h>；

const ccHObject::Container &selectedEntities = MainWindow::TheInstance()->getSelectedEntities();

2. 遍历每个选中的entity，并将其转换为点云类型，需要#include <ccHObjectCaster.h>头文件；

for (ccHObject* ent : selectedEntities)
{
	ccPointCloud* pc = ccHObjectCaster::ToPointCloud(ent);
}

3. 获取ccPointCloud点云类中的八叉树索引，如果未构建，则重新构建；

 ccOctree::Shared octree = pc->getOctree();
 if (!octree)
 {
     octree = pc->computeOctree();
     if (!octree)
     {
     	ccLog::LogMessage(QString::fromLocal8Bit("索引构建失败，") + ent->getName(), ccLog::LOG_ERROR);
     	continue;
     }
 }

4. 开始索引，测试了有两种方式，一种是使用DgmOctree的findNeighborsInASphereStartingFromCell，另一种是使用DgmOctree的getPointsInSphericalNeighbourhood。这里的PointCoordinateType是cc内部定义的数值变量的类型，默认为float型。检索出来的邻域点存放在CCLib::DgmOctree::NeighboursSet中，通过源码可以看出就是描述点信息的vector向量。需要注意的是，这两个方式都需要提前知道八叉树的层级，通过octree->findBestLevelForAGivenNeighbourhoodSizeExtraction得到当前半径下需要在哪一层level做检索。NearestNeighboursSphericalSearchStruct中的参数设置没搞明白，就先不用了。

PointCoordinateType search_radius = 1.0;
// @1
//CCLib::DgmOctree::NearestNeighboursSphericalSearchStruct nnsss;
//nnsss.queryPoint = search_pt;
//CCLib::DgmOctree::NeighboursSet neighbour_pts = nnsss.pointsInNeighbourhood;
//int ret_pt_num = octree->findNeighborsInASphereStartingFromCell(nnsss, search_radius, true);
// @2
unsigned char level = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(search_radius);
CCVector3 search_pt = CCVector3(checkcorrs[i].ctrl_pt.x + global_offset.x,checkcorrs[i].ctrl_pt.y + global_offset.y, checkcorrs[i].ctrl_pt.z + global_offset.z);
CCLib::DgmOctree::NeighboursSet neighbour_pts;         
int ret_pt_num = octree->getPointsInSphericalNeighbourhood(search_pt, search_radius, neighbour_pts, level);

​ 使用cc自带的八叉树之后，发现检索速度非常快（由于八叉树cell分区的原因，虽然返回的值可能不准确，但足够用了）。部分核心代码如下：

// qCC_db
#include <ccHObjectCaster.h>
#include <ccPointCloud.h>
#include <DgmOctree.h>
#include <mainwindow.h>
// 控制点类
struct ControlPoint : public CCVector3d
{
    std::string gcp_name; // gcp name
    int id; // gcp index
    // operator << overwrite
    inline friend std::ostream& operator<<(std::ostream &os, const ControlPoint &pt) {
        os << std::setprecision(10) << "gcp_name: " << pt.gcp_name
            << ", x = " << pt.x << " , y = " << pt.y << " , z = " << pt.z;
        return os;
    }
};
typedef std::vector<ControlPoint> ControlPointVec;
// 同名点类
struct HomonymPoint : public CCVector3d
{
    double gps_time; // gpstime from origin las point cloud file
    HomonymPoint() :CCVector3d(0.0, 0.0, 0.0), gps_time(0.0) {}
    // container stores the nearest points located inside a certain radius for coordinate interpolation
    std::vector<P2M::BaseType::Point3D> nearest_pts;
    HomonymPoint(const double x_, const double y_, const double z_)
        :CCVector3d(x_, y_, z_), gps_time(0.0)
    {}
    // operator << overwrite
    inline friend std::ostream& operator<<(std::ostream &os, const HomonymPoint &pt)
    {
        os << std::setprecision(10) << "Correspondence point gps time stamp: " << pt.gps_time
            << ", x = " << pt.x << " , y = " << pt.y << " , z = " << pt.z;
        return os;
    }
};
typedef std::vector<HomonymPoint> HomonymPointVec;
// 对应关系类
struct checkCorrespondence
{
    int id;
    ControlPoint ctrl_pt;
    HomonymPoint corr_pt;
    bool isChecked;
    // for point cloud surface fitting, Coordinates of data points.
    std::vector<mba::point<2>> coo;
    // Data values.
    std::vector<double> val;
    // statistic info
    float dist;
    float dist_x;
    float dist_y;
    float dist_z;
};
typedef std::vector<checkCorrespondence> checkCorrespondences;
// get local part of ALS point cloud
bool getCorrespondPoints(checkCorrespondences &checkcorrs, CCVector3d &global_offset)
{
    const ccHObject::Container &selectedEntities = MainWindow::TheInstance()->getSelectedEntities();
    if (selectedEntities.empty())
        return false;
    // global shift check, note that offset may be incorrected
    const ccShiftedObject* shifted = ccHObjectCaster::ToShifted(selectedEntities[0]);
    if (shifted)
    {
        global_offset = shifted->getGlobalShift();
    }
    for (ccHObject* ent : selectedEntities)
    {
        ccPointCloud* pc = ccHObjectCaster::ToPointCloud(ent);
        if (!pc)
        {
            ccLog::LogMessage(QString::fromLocal8Bit("未选中有效点云，") + ent->getName(), ccLog::LOG_ERROR);
            continue;
        }
        ccOctree::Shared octree = pc->getOctree();
        if (!octree)
        {
            octree = pc->computeOctree();
            if (!octree)
            {
                ccLog::LogMessage(QString::fromLocal8Bit("索引构建失败，") + ent->getName(), ccLog::LOG_ERROR);
                continue;
            }
        }
        Q_ASSERT(octree != nullptr);
        //CCLib::DgmOctree::NearestNeighboursSphericalSearchStruct nnsss;
        PointCoordinateType search_radius = 1.0;
        unsigned char level = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(search_radius);
        // find correspondence and calculate
        for (int i = 0; i < checkcorrs.size(); ++i) 
        {
            CCVector3 search_pt = CCVector3(checkcorrs[i].ctrl_pt.x + global_offset.x,
                checkcorrs[i].ctrl_pt.y + global_offset.y, checkcorrs[i].ctrl_pt.z + global_offset.z);
            //nnsss.queryPoint = search_pt;
            //int ret_pt_num = octree->findNeighborsInASphereStartingFromCell(nnsss, search_radius, true);
            //CCLib::DgmOctree::NeighboursSet neighbour_pts = nnsss.pointsInNeighbourhood;
            CCLib::DgmOctree::NeighboursSet neighbour_pts;         
            int ret_pt_num = octree->getPointsInSphericalNeighbourhood(search_pt, search_radius, neighbour_pts, level);
            if (ret_pt_num < 1)
                continue;
            // get the nearest point
            if (neighbour_pts[0].squareDistd < checkcorrs[i].dist)
            {
                const CCVector3 *ret_pt0 = neighbour_pts[0].point;
                checkcorrs[i].corr_pt = HomonymPoint(ret_pt0->x - global_offset.x, ret_pt0->y - global_offset.y, ret_pt0->z - global_offset.z);
            }
            // get the set of neighbour pts
            for (auto pt: neighbour_pts)
            {
                checkcorrs[i].isChecked = true;
                const CCVector3 *ret_pt = pt.point;              
                // copy to fitting data container
                checkcorrs[i].coo.push_back(mba::point<2>{ret_pt->x, ret_pt->y});
                checkcorrs[i].val.emplace_back(ret_pt->z);
            } 
        }
    }
    return true;
}
// interpolate the height value based on nearest points
void interpolateCoordinate(checkCorrespondence &corr, const CCVector3d &global_offset)
{
    // get coarse boundary
    double minx = 0.0, maxx = 0.0, miny = 0.0, maxy = 0.0;
    for (auto pt : corr.coo)
    {
        minx = (std::min)(minx, pt[0]);
        maxx = (std::max)(maxx, pt[0]);
        miny = (std::min)(miny, pt[1]);
        maxy = (std::max)(maxy, pt[1]);
    }
    mba::point<2> lo = { minx - 3.0, miny - 3.0 };
    mba::point<2> hi = { maxx + 3.0, maxy + 3.0 };
    // Initial grid size.
    mba::index<2> grid = { 5, 5 };
    mba::MBA<2> interp(lo, hi, grid, corr.coo, corr.val);
    // residual between gt and measurement
    corr.corr_pt.z = interp(mba::point<2>{corr.ctrl_pt.x + global_offset.x, corr.ctrl_pt.y + global_offset.y});
    corr.dist_z = corr.ctrl_pt.z - corr.corr_pt.z;
}
// for interpolating the position of target gcp points
bool qualityCheck(const std::string &gcp_path, const std::string &output_path)
{
    namespace PB = P2M::BaseType;
    // load gcps
    if (!P2M::Util::FileUtility::FileExist(output_path)) {
        std::cerr << "Output Path: " << output_path << " doesn't exist!" << std::endl;
        return false;
    }
    checkCorrespondences checkcorrs;
    CCVector3d global_offset(0.0,0.0,0.0);
    // load check gcp points
    loadGcpFile(gcp_path, checkcorrs, global_offset);
    // get loca part of ALS point clouds
    getCorrespondPoints(checkcorrs, global_offset);
    // get height difference based on surface fitting
    for (auto &checkcorr : checkcorrs)
    {
        if (checkcorr.isChecked)
        {
            interpolateCoordinate(checkcorr, global_offset);
        }
    }
    return true;
}