Octree based Point Cloud Downsample

原创 2015年11月20日 15:47:11

本算法基于哈希八叉树,并可进行按比例抽稀。

基本思路是迭代执行如下步骤直到目标数量ReserveNum等于0:

1)抽取不大于目标数量ReserveNum的前N层点云,并实时调整八叉树,类似从二叉树中pop元素;

2)更新目标数量ReserveNum为原值减去已去除的点云数量;


Notice: this code is a demo. No optimization tech has been used.


#include "tgHashedOctree.h"

int main(int argc, char* argv[])
{
	//
	std::vector<double> l_lstX;
	std::vector<double> l_lstY;
	std::vector<double> l_lstZ;
	//
	// put point cloud into l_lstX, l_lstY, l_lstZ
	// 
	tg::mesh::HashedOctree l_oHashedOctree;
	for (int i = 0; i < l_lstX.size(); ++i)
		l_oHashedOctree.AddPoint(l_lstX[i], l_lstY[i], l_lstY[i], i);

	std::vector<size_t> l_lstIndex;
	l_oHashedOctree.DownSample(l_dRadio, l_lstIndex);

	return 0;
}

Header File

/*
Hashed Octree

Copyright (C) by Tony Gauss (gaoxing.mes@gmail.com) in 2015

License : MIT
http://opensource.org/licenses/MIT

Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

*/

#ifndef ___tgHashedOctree___
#define ___tgHashedOctree___

#include <unordered_map>

namespace tg
{

/*

HashedOctree
this class defines a hashed octree, only the indices of the input points will be stored in 
the octree, a downsample method is developed based on this octree
Notic: downsample will clear the octree

*/
class HashedOctree
{
public:
	///////////////////////////////////////
	// constructors and destructor

	HashedOctree(const int&	i_nMaxDepth = 20);

	~HashedOctree();

	///////////////////////////////////////
	// public member functions

	// Add a normalized point into the Octree, 
	void AddPoint(	const double	i_dX	,	// Range [-0.5, 0.5]
					const double	i_dY	,	// Range [-0.5, 0.5]
					const double	i_dZ	,	// Range [-0.5, 0.5]
					const size_t	i_sIndex);	// index of the point in the original point cloud

	// Downsample the point cloud based on Octree
	void DownSample(	const double				i_dRatio	,	// reserve ratio
								std::vector<size_t>&		i_lstIndex	);	// indices of points that are reserverd

	// show how many depth of the Octree have been filled
	int GetFilledDepth() const;

	// show how many points the Octree contains
	size_t Size() const;

private:
	///////////////////////////////////////
	// private member functions

	// Get the key of the child no deeper than a threshold
	size_t GetChildNoDeeperThan(size_t			i_sKey		,	// input key
								const int&		i_nDepthThre);	// threshold depth

	// Remove all points above a threshold depth
	void RemoveNodesAboveDepth(	size_t			i_sKey		,	// initial key, should be 1
								int				i_nDepthCur	,	// initial Depth, should be 1
								const int&		i_nDepthThre);	// threshold depth

	// Get all points below a threshold depth
	void GetPointsBelowDepth(	std::vector<size_t>&		i_lstIndex	,	// indices list to contain the result
								size_t						i_sKey		,	// initial key, should be 1
								int							i_nDepthCur	,	// initial Depth, should be 1
								const int&					i_nDepthThre);	// threshold depth

	///////////////////////////////////////
	// private variables

	int m_nMaxDepth		;		// max depth of the octree
	int m_nFilledDepth	;		// filled depth of the octree
	size_t m_sReserveNum;		// number of points that shall be reserved

	std::vector<size_t> m_lstDepthPointNum;		// number of points in each depth

	std::unordered_map<size_t, std::vector<size_t> > m_oHashedOctree;	// hash map to store the octree
	
};

};

#endif

Source File


/*
Hashed Octree

Copyright (C) by Tony Gauss (gaoxing.mes@gmail.com) in 2015

License : MIT
http://opensource.org/licenses/MIT

Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT
LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.

*/

#include "tgHashedOctree.h"

namespace tg
{

HashedOctree::HashedOctree(const int&	i_nMaxDepth)
{
	this->m_nMaxDepth = i_nMaxDepth;
	this->m_lstDepthPointNum.resize(this->m_nMaxDepth + 1, 0);
	this->m_nFilledDepth = 0;
}

HashedOctree::~HashedOctree()
{

}

size_t HashedOctree::Size() const
{
	size_t l_sCurNum = 0;
	for (int i = 0; i <= this->m_nMaxDepth; ++i)
	{
		l_sCurNum += this->m_lstDepthPointNum[i];
	}
	return l_sCurNum;
}

int HashedOctree::GetFilledDepth() const
{
	return this->m_nFilledDepth;
}

void HashedOctree::AddPoint(const double	i_dX		,
							const double	i_dY		,
							const double	i_dZ		,
							const size_t	i_sIndex	)
{
	//assert(i_dX >= -0.5 && i_dX <= 0.5);
	//assert(i_dY >= -0.5 && i_dY <= 0.5);
	//assert(i_dZ >= -0.5 && i_dZ <= 0.5);

	double l_dCentroid[3] = { 0 };
	size_t l_sLabelAll = 1;
	if (this->m_oHashedOctree.find(l_sLabelAll) == this->m_oHashedOctree.end())
	{
		this->m_oHashedOctree[l_sLabelAll].push_back(i_sIndex);
		this->m_nFilledDepth = 1;
		this->m_lstDepthPointNum[0]++;
		return;
	}

	int i;
	const size_t l_s1 = 1;
	for (i = 1; i < this->m_nMaxDepth; ++i)
	{
		size_t l_sLabel = 0;
		double l_dStep = 0.5 / (l_s1 << i);
		if (i_dX > l_dCentroid[0])
		{
			l_sLabel += 1;
			l_dCentroid[0] += l_dStep;
		}
		else
		{
			l_dCentroid[0] -= l_dStep;
		}
		if (i_dY > l_dCentroid[1])
		{
			l_sLabel += 2;
			l_dCentroid[1] += l_dStep;
		}
		else
		{
			l_dCentroid[1] -= l_dStep;
		}
		if (i_dZ > l_dCentroid[2])
		{
			l_sLabel += 4;
			l_dCentroid[2] += l_dStep;
		}
		else
		{
			l_dCentroid[2] -= l_dStep;
		}
		l_sLabelAll = (l_sLabelAll << 3) + l_sLabel;

		if (this->m_oHashedOctree.find(l_sLabelAll) == this->m_oHashedOctree.end())
		{
			++i;
			break;
		}
	}
	this->m_nFilledDepth = this->m_nFilledDepth > i ? this->m_nFilledDepth: i;
	this->m_lstDepthPointNum[i-1]++;

	this->m_oHashedOctree[l_sLabelAll].push_back(i_sIndex);
}

void HashedOctree::DownSample(	const double			i_dRatio	,
								std::vector<size_t>&	i_lstIndex	)
{
	assert(i_dRatio > 0);
	assert(i_dRatio <= 1.0);

	this->m_sReserveNum = this->Size() * i_dRatio;
	if(this->m_sReserveNum == 0)
		return;

	i_lstIndex.reserve(this->m_sReserveNum);
	while (i_lstIndex.size() < this->m_sReserveNum)
	{
		/////////////////////////////////////////
		// Find depth threshold
		size_t l_sCurNum = i_lstIndex.size();
		int l_nDepthThre;
		for (l_nDepthThre = 0; l_nDepthThre <= this->m_nMaxDepth; ++l_nDepthThre)
		{
			l_sCurNum += this->m_lstDepthPointNum[l_nDepthThre];
			if (l_sCurNum > this->m_sReserveNum)
				break;
		}

		/////////////////////////////////////////
		// Get points in depth below l_nDepthThre
		this->GetPointsBelowDepth(i_lstIndex, 1, 0, l_nDepthThre);

		printf("[%I64u/%I64u], Threshold: %d\n", i_lstIndex.size(), this->m_sReserveNum, l_nDepthThre);
	}
	this->m_oHashedOctree.clear();
}

void HashedOctree::GetPointsBelowDepth(	std::vector<size_t>&	i_lstIndex	,
										size_t					i_sKey		,
										int						i_nDepthCur	,
										const int&				i_nDepthThre)
{
	// return if threshold is reached
	if (i_nDepthCur >= i_nDepthThre)
		return;

	// return if this node does Not exits, which implies it has No child
	auto iter = this->m_oHashedOctree.find(i_sKey);
	if (iter == this->m_oHashedOctree.end())
		return;

	// Get a point from this node
	i_lstIndex.push_back(iter->second[0]);
	// Find the deepest (no deeper than the threshold) child of this node
	size_t l_sKeyTmp = GetChildNoDeeperThan(i_sKey, i_nDepthThre);
	// if the child's depth is equal to the threshold, move one point from child to this node 
	if ((l_sKeyTmp >> (3 * i_nDepthThre)) > 0)
	{
		auto iterTmp = this->m_oHashedOctree.find(l_sKeyTmp);
		iter->second[0] = iterTmp->second.back();
		iterTmp->second.pop_back();
		if (!iterTmp->second.size())
			this->m_oHashedOctree.erase(iterTmp);

		--this->m_lstDepthPointNum[i_nDepthThre];
	}
	// otherwise remove this node
	else
	{
		this->m_oHashedOctree.erase(iter);
		--this->m_lstDepthPointNum[i_nDepthCur];
	}
	// recursively ... until the threshold depth is reached
	i_sKey = (i_sKey << 3);
	for (size_t i = 0; i < 8; ++i)
	{
		this->GetPointsBelowDepth(i_lstIndex, i_sKey + i, i_nDepthCur + 1, i_nDepthThre);
	}
}

size_t HashedOctree::GetChildNoDeeperThan(	size_t		i_sKey		,
											const int&	i_nDepthThre)
{
	size_t l_sKeyMax = i_sKey;
	i_sKey = (i_sKey << 3);
	for (size_t i = 0; i < 8; ++i)
	{
		auto iter = this->m_oHashedOctree.find(i_sKey + i);
		if (iter != this->m_oHashedOctree.end())
		{
			if (((i_sKey + i) >> (3 * i_nDepthThre)) > 0)
				return i_sKey + i;

			size_t l_sKeyTmp = GetChildNoDeeperThan(i_sKey + i, i_nDepthThre);
			l_sKeyMax = l_sKeyMax > l_sKeyTmp ? l_sKeyMax : l_sKeyTmp;
		}
	}
	return l_sKeyMax;
}

void HashedOctree::RemoveNodesAboveDepth(	size_t		i_sKey		,	// initial key, should be 1
											int			i_nDepthCur	,	// initial Depth, should be 1
											const int&	i_nDepthThre)	// threshold depth
{
	// return if reached max depth
	if (i_nDepthCur > this->m_nMaxDepth)
		return;

	// return if this node does Not exits, which implies it has No child
	auto iter = this->m_oHashedOctree.find(i_sKey);
	if (iter == this->m_oHashedOctree.end())
		return;

	// remove this node if its depth is larger than threshold
	if (i_nDepthCur > i_nDepthThre)
	{
		this->m_oHashedOctree.erase(iter);
		--this->m_lstDepthPointNum[i_nDepthCur];
	}

	// recursively ... 
	i_sKey = (i_sKey << 3);
	for (size_t i = 0; i < 8; ++i)
	{
		this->RemoveNodesAboveDepth(i_sKey + i, i_nDepthCur + 1, i_nDepthThre);
	}
}

}




Point Cloud Library 1.7.2编译及使用详细教程

PCL主页:http://pointclouds.org/about/ 首先,简单介绍下PCL是什么。 PCL是Point Cloud Library的简称,是一个大型的开源项目,用于2D/3D图...
  • sdochengxu1993
  • sdochengxu1993
  • 2015年09月02日 09:27
  • 1260

3D开源点库(PointCloud Library)介绍

Point Clouds Library (PCL) 是由Radu Bogdan等人开发的一个开源项目,其最初的动机是专注于点云感知的应用,并集成大量高级的用语3D感知的方法,适用于3 Buildin...
  • kezunhai
  • kezunhai
  • 2013年08月07日 12:02
  • 1802

苹果曝光无人车新进展,这名华人工程师是主要贡献者

苹果进军自动驾驶汽车的传闻由来已久,最新的传闻是苹果已经搁置了整车研发的计划,转而开发自动驾驶汽车的软件平台。最近,也有不少路人在苹果总部附近看到过苹果的雷克萨斯路测车。 近日,向来...
  • dQCFKyQDXYm3F8rB0
  • dQCFKyQDXYm3F8rB0
  • 2017年11月23日 00:00
  • 9184

编写shader模拟水波扩散效果

这次分析了shadertoy上的一个例子 先上效果图。 中心是一个大的波纹,我们可以用鼠标控制小的波纹,产生干涉。 用opengl模拟水纹是一个复杂的过程,这里我们采用的方法是。。。欺骗法。 将水...
  • chenxizhibi
  • chenxizhibi
  • 2016年05月01日 21:46
  • 1042

基于octree的空间划分及搜索操作

(1)  octree是一种用于管理稀疏3D数据的树形数据结构,每个内部节点都正好有八个子节点,介绍如何用octree在点云数据中进行空间划分及近邻搜索,实现“体素内近邻搜索(Neighbors wi...
  • u013019296
  • u013019296
  • 2017年04月11日 13:53
  • 356

PCL的学习必要性、重要性、意义及最初——持续修改中

点云是无压缩三维位图。点云库是一个大规模,开放工程的2D,3D处理库。骨架包括......(更多信息请参考中文网站.....) 点云库是机器人学领域一个重要的基础库,定义了新的数据格式,并发展为成熟的...
  • wishchin
  • wishchin
  • 2013年07月25日 13:22
  • 1596

(十一)OcTree教程三--OcTree在PCL中的应用-无序点云在空间中的动态检测

无序点云在空间中的动态检测在这篇教程中我们会学到如何用八叉树实现对多幅无序点云在空间变化上的检测。这里说的空间变化包括了点云的大小,分辨率,密度和点序。通过递归得比较八叉树的树结构,空间上的变化会从体...
  • jiaojialulu
  • jiaojialulu
  • 2017年04月05日 21:02
  • 1694

谁是我邻居--kdTree&OcTree

由于分割工作需要对点云的邻近点进行操作,不断对比和访问某个点的邻居,所以决定点云的相邻关系是非常重要的。对于Scan来说,邻居关系是天然的。但对于很多杂乱点云,或者滤波,分割后的点云来说,邻居关系就已...
  • fandq1223
  • fandq1223
  • 2016年11月15日 19:59
  • 1239

叉树Octree原理及简单实现(C++版)

1、对Octree的描述 Octree的定义是:若不为空树的话,树中任一节点的子节点恰好只会有八个,或零个,也就是子节点不会有0与8以外的数目。那么,这要用来做什么?想象一个立方体,我们最少可以切成...
  • Chinamming
  • Chinamming
  • 2013年11月24日 13:22
  • 2043

OcTree 八叉树

八叉树实现算法
  • zhuyingqingfen
  • zhuyingqingfen
  • 2013年08月02日 20:18
  • 5209
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:Octree based Point Cloud Downsample
举报原因:
原因补充:

(最多只允许输入30个字)