CloudCompare源代码学习
开源库
10 分钟 读完 (大约 1462 个字)

CloudCompare源代码学习

目的

想使用一下CloudCompare自带的八叉树，替换掉nanoflann。nanoflann在某些特殊场景下返回的结果是有问题的，导致程序出现异常。

nanoflann返回值出现异常

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
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>；
1
const ccHObject::Container &selectedEntities = MainWindow::TheInstance()->getSelectedEntities();
  1. 遍历每个选中的entity，并将其转换为点云类型，需要#include <ccHObjectCaster.h>头文件；
1
2
3
4
for (ccHObject* ent : selectedEntities)
{
	ccPointCloud* pc = ccHObjectCaster::ToPointCloud(ent);
}
  1. 获取ccPointCloud点云类中的八叉树索引，如果未构建，则重新构建；
1
2
3
4
5
6
7
8
9
10
ccOctree::Shared octree = pc->getOctree();
if (!octree)
{
    octree = pc->computeOctree();
    if (!octree)
    {
    	ccLog::LogMessage(QString::fromLocal8Bit("索引构建失败，") + ent->getName(), ccLog::LOG_ERROR);
    	continue;
    }
}
  1. 开始索引，测试了有两种方式，一种是使用DgmOctree的findNeighborsInASphereStartingFromCell，另一种是使用DgmOctree的getPointsInSphericalNeighbourhood。这里的PointCoordinateType是cc内部定义的数值变量的类型，默认为float型。检索出来的邻域点存放在CCLib::DgmOctree::NeighboursSet中，通过源码可以看出就是描述点信息的vector向量。需要注意的是，这两个方式都需要提前知道八叉树的层级，通过octree->findBestLevelForAGivenNeighbourhoodSizeExtraction得到当前半径下需要在哪一层level做检索。NearestNeighboursSphericalSearchStruct中的参数设置没搞明白，就先不用了。
1
2
3
4
5
6
7
8
9
10
11
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分区的原因，虽然返回的值可能不准确，但足够用了）。部分核心代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
// 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;
}
------ copyright ------

本博客所有文章除特别声明外，均采用 许可协议

title:CloudCompare源代码学习

author:AmazingHao

link:http://whu-lyh.github.io/blogs/2023/03/13/CloudCompare%E6%BA%90%E4%BB%A3%E7%A0%81%E5%AD%A6%E4%B9%A0/

publish time:2023-03-13

update time:2023-09-24

# ,
ALS数据采集成果制作要求
关于控制点检核的一些思考
| visits
关注我

链接

分类

标签云

B-Spline CloudCompare Docker FFMPEG Geometics Geometry Hexo LiDAR_SLAM Linux MarkDown Mathemetics OSG PCL PaperReview Point Cloud Pytorch ROS VS WindowsScript Zotero 服务器 鱼眼图像

最新文章

related posts

归档

Your browser is out-of-date!

Update your browser to view this website correctly. Update my browser now

×