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; }