short int is_bst_l(struct node *root) {
if (root->left == NULL)
return 1;
if (is_bst_l(root->left)) {
if (min_left(root->left) >= root->data) {
return 1;
}
}
return 0;
}
int min_left(struct node *root) {
if (root->right == NULL)
return root->data;
return min_left(root->right);
}
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;
}
