double shot_detector::computeCloudResolution(const pcl::PointCloud<PointType>::Ptr cloud)
{
double resolution = 0.0;
int numberOfPoints = 0;
int nres;
std::vector<int> indices(2);
std::vector<float> squaredDistances(2);
pcl::search::KdTree<PointType> tree;
tree.setInputCloud(cloud);
for (size_t i = 0; i < cloud->size(); ++i) {
if (! pcl_isfinite((*cloud)[i].x))
continue;
// Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch(i, 2, indices, squaredDistances);
if (nres == 2) {
resolution += sqrt(squaredDistances[1]);
++numberOfPoints;
}
}
if (numberOfPoints != 0)
resolution /= numberOfPoints;
return resolution;
}
double
dist_nn_3d_model(pcl::PointCloud<pcl::PointXYZ>::Ptr model_cloud, pcl::PointCloud<pcl::PointXYZ> obj_cloud,
carmen_point_t particle_pose, carmen_vector_3D_t car_global_position)
{
double theta = -particle_pose.theta;
pcl::PointXYZ point;
double dist = 0.0;
int k = 1; //k-nearest neighbor
// kd-tree object
pcl::search::KdTree<pcl::PointXYZ> kdtree;
kdtree.setInputCloud(model_cloud);
// This vector will store the output neighbors.
std::vector<int> pointIndices(k);
// This vector will store their squared distances to the search point.
std::vector<float> squaredDistances(k);
for (pcl::PointCloud<pcl::PointXYZ>::iterator it = obj_cloud.begin(); it != obj_cloud.end(); ++it)
{
// Necessary transformations (translation and rotation):
float x = car_global_position.x + it->x - particle_pose.x;
float y = car_global_position.y + it->y - particle_pose.y;
point.z = it->z;
point.x = x*cos(theta) - y*sin(theta);
point.y = x*sin(theta) + y*cos(theta);
if (kdtree.nearestKSearch(point, k, pointIndices, squaredDistances) > 0)
{
// dist += sqrt(double(squaredDistances[0]));
dist += double(squaredDistances[0]);
}
}
return dist;
}
