Octree based Point Cloud Downsample
本算法基于哈希八叉树，并可进行按比例抽稀。
基本思路是迭代执行如下步骤直到目标数量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 l_lstX;
std::vector l_lstY;
std::vector 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 l_lstIndex;
l_oHashedOctree.DownSample(l_dRadio, l_lstIndex);
return 0;
} Header File
/*
Hashed Octree
Copyright (C) by Tony Gauss ([email protected]) 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
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& 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& 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 m_lstDepthPointNum; // number of points in each depth
std::unordered_map > m_oHashedOctree; // hash map to store the octree
};
};
#endif
Source File
/*
Hashed Octree
Copyright (C) by Tony Gauss ([email protected]) 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& 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& 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);
}
}
}