Мне нужно выяснить, какие точки облака точек видны с датчика RGBD, расположенного в начале координат (0,0,0). Я попытался использовать класс voxelgridOcclusionEstimation из pcl, чтобы определить видимую область в облаке с точки зрения датчика. Он использует технику трассировки лучей.
В качестве эксперимента я попытался получить видимую область в сфере, центр которой удовлетворяет одному из следующих условий:
- центр вдоль х
- центр вдоль y
- центр вдоль z
- центр находится в плоскости xz
- центр находится вдоль плоскости y z
- центр находится вдоль плоскости x y.
Во всех случаях датчик находится в исходной точке с нулевым вращением.
Оценка voxelgridOcclusionEstimation дает странные результаты. Зеленая область обозначает видимую область, а красная — закрытую область.
Мой код:
int main(int argc, char * argv[])
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_occluded(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_visible(new pcl::PointCloud<pcl::PointXYZ>);
pcl::io::loadPCDFile(argv[1],*cloud_in);
Eigen::Quaternionf quat(1,0,0,0);
cloud_in->sensor_origin_ = Eigen::Vector4f(0,0,0,0);
cloud_in->sensor_orientation_= quat;
pcl::VoxelGridOcclusionEstimation<pcl::PointXYZ> voxelFilter;
voxelFilter.setInputCloud (cloud_in);
float leaf_size=atof(argv[2]);
voxelFilter.setLeafSize (leaf_size, leaf_size, leaf_size);
voxelFilter.initializeVoxelGrid();
std::vector<Eigen::Vector3i,
Eigen::aligned_allocator<Eigen::Vector3i> > occluded_voxels;
for (size_t i=0;i<cloud_in->size();i++)
{
PointT pt=cloud_in->points[i];
Eigen::Vector3i grid_cordinates=voxelFilter.getGridCoordinates (pt.x, pt.y, pt.z);
int grid_state;
int ret=voxelFilter.occlusionEstimation( grid_state, grid_cordinates );
if (grid_state==1)
{
cloud_occluded->push_back(cloud_in->points[i]);
}
else
{
cloud_visible->push_back(cloud_in->points[i]);
}
}
pcl::io::savePCDFile(argv[3],*cloud_occluded);
pcl::io::savePCDFile(argv[4],*cloud_visible);
return 0;
}