目的
想使用一下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>
,这样检索速度大大加快。过程如下:
- 首先找到当前选中哪些entity,此时需要引用主窗口的头文件并获取全局静态变量
#include <mainwindow.h>
;
const ccHObject::Container &selectedEntities = MainWindow::TheInstance()->getSelectedEntities();
- 遍历每个选中的entity,并将其转换为点云类型,需要
#include <ccHObjectCaster.h>
头文件;
for (ccHObject* ent : selectedEntities)
{
ccPointCloud* pc = ccHObjectCaster::ToPointCloud(ent);
}
- 获取ccPointCloud点云类中的八叉树索引,如果未构建,则重新构建;
ccOctree::Shared octree = pc->getOctree();
if (!octree)
{
octree = pc->computeOctree();
if (!octree)
{
ccLog::LogMessage(QString::fromLocal8Bit("索引构建失败,") + ent->getName(), ccLog::LOG_ERROR);
continue;
}
}
- 开始索引,测试了有两种方式,一种是使用
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;
}