关闭

Octree based Point Cloud Downsample

222人阅读 评论(0) 收藏 举报
分类:

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

基本思路是迭代执行如下步骤直到目标数量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);
	}
}

}




0
0

查看评论
* 以上用户言论只代表其个人观点,不代表CSDN网站的观点或立场
    个人资料
    • 访问:1345次
    • 积分:107
    • 等级:
    • 排名:千里之外
    • 原创:10篇
    • 转载:0篇
    • 译文:0篇
    • 评论:0条
    文章分类