AbstractKDNode *KDTree::Build(__in std::vector<Triangle const *> const &set,
__in float3_t const &min,
__in float3_t const &max,
__in std::size_t depth) {
if ((set.size() <= max_bucket_size_) || (depth > max_hierarchy_depth_)) {
// std::tcerr << set.size();
if (set.empty()) {
return new KDNull();
} else {
return new KDLeaf(set);
}
}
depth++;
float3_t const range(max - min);
std::size_t axis = ~0U; // 分割次元
float_t value = std::numeric_limits<float_t>::signaling_NaN(); // 分割位置
// surface area heuristic
float_t min_cost = std::numeric_limits<float_t>::max();
// event
struct open_close_event {
bool operator<(open_close_event const &e) const {
// value が等しい場合 open Event < close Event とする
return (value < e.value) ? true : (value > e.value) ? false
: type < e.type;
}
bool type; // event type: {false: open, true: close}
float_t value;
};
#define EVENT_UPDATE(v) \
{ \
float_t const r = v - min[k]; \
float_t const sa_left = a + b * r; \
float_t const sa_right = a + b * (range[k] - r); \
float_t const cost = n_left * sa_left + n_right * sa_right; \
if (min_cost > cost) { \
min_cost = cost; \
axis = k; \
value = v; \
} \
}
std::vector<open_close_event> event_list(set.size() * 2);
for (std::size_t k = 0; k < 3; ++k) {
// init event list
for (std::size_t i = 0, size = set.size(); i < size; ++i) {
event_list[i].type = false;
event_list[i].value = set[i]->min(k); // 最小値
event_list[i + size].type = true;
event_list[i + size].value = set[i]->max(k) + EPSILON; // 最大値
}
// sort event list
std::sort(event_list.begin(), event_list.end());
// compute min cost
float_t const a = range[(k + 1) % 3] * range[(k + 2) % 3];
float_t const b = range[(k + 1) % 3] + range[(k + 2) % 3];
std::size_t n_left = 0;
std::size_t n_right = set.size();
{
float_t const v = event_list[0].value - EPSILON;
EVENT_UPDATE(v);
}
for (std::size_t i = 0, size = event_list.size(); i < size; ++i) {
if (event_list[i].type) {
n_right--; // close event
} else {
n_left++; // open event
}
float_t v = event_list[i].value;
if ((i + 1) < size) {
if (v == event_list[i + 1].value) {
continue; // do not evalute
}
// v += (event_list[i+1].value - v) * float_t(0.5);
} else {
v += EPSILON;
}
EVENT_UPDATE(v);
}
}
#undef EVENT_UPDATE
// 左右に分類
std::vector<Triangle const *> set_left;
std::vector<Triangle const *> set_right;
for (std::size_t i = 0, size = set.size(); i < size; ++i) {
switch (set[i]->Overlap(axis, value)) {
case 1:
set_left.push_back(set[i]);
break; // 左
case 2:
set_right.push_back(set[i]);
break; // 右
case 3:
set_left.push_back(set[i]);
set_right.push_back(set[i]);
break; // 両方
default:
assert(!"bad Triangle::Overlap");
break;
}
}
// set.swap(std::vector<Triangle const *>()); // clear & trim
// create nodes
KDNode *node = new KDNode(axis, value);
{
float3_t max_left(max);
max_left[axis] = value;
node->SetChild(0, Build(set_left, min, max_left, depth));
}
{
float3_t min_right(min);
min_right[axis] = value;
node->SetChild(1, Build(set_right, min_right, max, depth));
}
return node;
}
void Localization::voxelizeCloud(const PointCloud::Ptr& cloud_in, const Eigen::VectorXi& pts_cam_source_in,
PointCloud::Ptr& cloud_out, Eigen::VectorXi& pts_cam_source_out, double cell_size)
{
Eigen::Vector3d min_left, min_right;
min_left << 10000, 10000, 10000;
min_right << 10000, 10000, 10000;
Eigen::Matrix3Xd pts(3, cloud_in->points.size());
int num_left = 0;
int num_right = 0;
for (int i = 0; i < cloud_in->points.size(); i++)
{
if (pts_cam_source_in(i) == 0)
{
if (cloud_in->points[i].x < min_left(0))
min_left(0) = cloud_in->points[i].x;
if (cloud_in->points[i].y < min_left(1))
min_left(1) = cloud_in->points[i].y;
if (cloud_in->points[i].z < min_left(2))
min_left(2) = cloud_in->points[i].z;
num_left++;
}
else if (pts_cam_source_in(i) == 1)
{
if (cloud_in->points[i].x < min_right(0))
min_right(0) = cloud_in->points[i].x;
if (cloud_in->points[i].y < min_right(1))
min_right(1) = cloud_in->points[i].y;
if (cloud_in->points[i].z < min_right(2))
min_right(2) = cloud_in->points[i].z;
num_right++;
}
pts.col(i) = cloud_in->points[i].getVector3fMap().cast<double>();
}
// find the cell that each point falls into
std::set<Eigen::Vector3i, Localization::UniqueVectorComparator> bins_left;
std::set<Eigen::Vector3i, Localization::UniqueVectorComparator> bins_right;
int prev;
for (int i = 0; i < pts.cols(); i++)
{
if (pts_cam_source_in(i) == 0)
{
Eigen::Vector3i v = floorVector((pts.col(i) - min_left) / cell_size);
bins_left.insert(v);
prev = bins_left.size();
}
else if (pts_cam_source_in(i) == 1)
{
Eigen::Vector3i v = floorVector((pts.col(i) - min_right) / cell_size);
bins_right.insert(v);
}
}
// calculate the cell values
Eigen::Matrix3Xd voxels_left(3, bins_left.size());
Eigen::Matrix3Xd voxels_right(3, bins_right.size());
int i = 0;
for (std::set<Eigen::Vector3i, Localization::UniqueVectorComparator>::iterator it = bins_left.begin();
it != bins_left.end(); it++)
{
voxels_left.col(i) = (*it).cast<double>();
i++;
}
i = 0;
for (std::set<Eigen::Vector3i, Localization::UniqueVectorComparator>::iterator it = bins_right.begin();
it != bins_right.end(); it++)
{
voxels_right.col(i) = (*it).cast<double>();
i++;
}
voxels_left.row(0) = voxels_left.row(0) * cell_size
+ Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(0);
voxels_left.row(1) = voxels_left.row(1) * cell_size
+ Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(1);
voxels_left.row(2) = voxels_left.row(2) * cell_size
+ Eigen::VectorXd::Ones(voxels_left.cols()) * min_left(2);
voxels_right.row(0) = voxels_right.row(0) * cell_size
+ Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(0);
voxels_right.row(1) = voxels_right.row(1) * cell_size
+ Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(1);
voxels_right.row(2) = voxels_right.row(2) * cell_size
+ Eigen::VectorXd::Ones(voxels_right.cols()) * min_right(2);
PointCloud::Ptr cloud(new PointCloud);
cloud->resize(bins_left.size() + bins_right.size());
Eigen::VectorXi pts_cam_source(bins_left.size() + bins_right.size());
for (int i = 0; i < bins_left.size(); i++)
{
pcl::PointXYZ p;
p.x = voxels_left(0, i);
p.y = voxels_left(1, i);
p.z = voxels_left(2, i);
cloud->points[i] = p;
pts_cam_source(i) = 0;
}
for (int i = 0; i < bins_right.size(); i++)
{
pcl::PointXYZ p;
p.x = voxels_right(0, i);
p.y = voxels_right(1, i);
p.z = voxels_right(2, i);
cloud->points[bins_left.size() + i] = p;
pts_cam_source(bins_left.size() + i) = 1;
}
cloud_out = cloud;
pts_cam_source_out = pts_cam_source;
}
