|
//#include <pcl/point_types.h>
|
//#include <pcl/gpu/containers/device_array.h>
|
|
// CUDA runtime
|
#include <cuda_runtime.h>
|
|
// helper functions and utilities to work with CUDA
|
#include <helper_cuda.h>
|
#include <helper_functions.h>
|
|
#include "kdTree.h"
|
|
#include <stack>
|
#include <stdio.h>
|
#include <iostream>
|
#include <vector>
|
|
|
__device__ double dev_distance(coordinate a, coordinate b) {
|
double tmp = (a.x - b.x) * (a.x - b.x)
|
+ (a.y - b.y) * (a.y - b.y)
|
+ (a.z - b.z) * (a.z - b.z);
|
return sqrt(tmp);
|
}
|
|
__device__ void dev_search_nearest(
|
const TreeElement* treeArray,
|
const int size,
|
coordinate target,
|
coordinate &nearestpoint,
|
double & distance)
|
{
|
|
//std::stack<int> search_path;
|
|
int search_path[40];
|
|
int path_index = -1;
|
int index = 0;
|
TreeElement pSearch = treeArray[index];
|
|
coordinate nearest;
|
double dist;
|
|
while (!pSearch.isEmpty)
|
{
|
path_index += 1;
|
search_path[path_index] = index;
|
|
if ((2 * index + 1) >= size)
|
{
|
break;
|
}
|
|
switch (pSearch.split)
|
{
|
case 0:
|
if (target.x <= pSearch.dom_elt.x) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
case 1:
|
if (target.y <= pSearch.dom_elt.y) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
default:
|
if (target.z <= pSearch.dom_elt.z) /* Èç¹ûСÓھͽøÈë×ó×ÓÊ÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
else
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
break;
|
}
|
}
|
//È¡³ösearch_path×îºóÒ»¸ö¸³¸ønearest
|
index = search_path[path_index];
|
nearest.x = treeArray[index].dom_elt.x;
|
nearest.y = treeArray[index].dom_elt.y;
|
nearest.z = treeArray[index].dom_elt.z;
|
path_index -= 1;
|
dist = dev_distance(nearest, target);
|
|
//3. »ØËÝËÑË÷·¾¶
|
TreeElement pBack;
|
|
while (path_index != -1)
|
{
|
//È¡³ösearch_path×îºóÒ»¸ö½áµã¸³¸øpBack
|
index = search_path[path_index];
|
pBack = treeArray[index];
|
path_index -= 1;
|
|
if ((2 * index + 1) >= size) /* Èç¹ûpBackΪҶ×Ó½áµã */
|
{
|
if (dev_distance(nearest, target) > dev_distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = dev_distance(pBack.dom_elt, target);
|
}
|
}
|
else if (treeArray[2 * index + 1].isEmpty == true && treeArray[2 * index + 2].isEmpty == true) /* Èç¹ûpBackΪҶ×Ó½áµã */
|
{
|
if (dev_distance(nearest, target) > dev_distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = dev_distance(pBack.dom_elt, target);
|
}
|
}
|
else
|
{
|
UL s = pBack.split;
|
switch (s)
|
{
|
case 0:
|
if (fabs(pBack.dom_elt.x - target.x) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (dev_distance(nearest, target) > dev_distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = dev_distance(pBack.dom_elt, target);
|
}
|
|
if (target.x <= pBack.dom_elt.x) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty) //pSearch¼ÓÈëµ½search_pathÖÐ
|
{
|
path_index += 1;
|
search_path[path_index] = index;
|
}
|
}
|
break;
|
case 1:
|
if (fabs(pBack.dom_elt.y - target.y) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (dev_distance(nearest, target) > dev_distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = dev_distance(pBack.dom_elt, target);
|
}
|
if (target.y <= pBack.dom_elt.y) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty) //pSearch¼ÓÈëµ½search_pathÖÐ
|
{
|
path_index += 1;
|
search_path[path_index] = index;
|
}
|
}
|
break;
|
default:
|
if (fabs(pBack.dom_elt.z - target.z) < dist) /* Èç¹ûÒÔtargetΪÖÐÐĵÄÔ²£¨Çò»ò³¬Çò£©£¬°ë¾¶ÎªdistµÄÔ²Óë·Ö¸î³¬Æ½ÃæÏཻ£¬ ÄÇô¾ÍÒªÌøµ½ÁíÒ»±ßµÄ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
if (dev_distance(nearest, target) > dev_distance(pBack.dom_elt, target))
|
{
|
nearest = pBack.dom_elt;
|
dist = dev_distance(pBack.dom_elt, target);
|
}
|
if (target.z <= pBack.dom_elt.z) /* Èç¹ûtargetλÓÚpBackµÄ×ó×ӿռ䣬ÄÇô¾ÍÒªÌøµ½ÓÒ×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 2;
|
pSearch = treeArray[index];
|
}
|
else /* Èç¹ûtargetλÓÚpBackµÄÓÒ×ӿռ䣬ÄÇô¾ÍÒªÌøµ½×ó×Ó¿Õ¼äÈ¥ËÑË÷ */
|
{
|
index = 2 * index + 1;
|
pSearch = treeArray[index];
|
}
|
if (!pSearch.isEmpty) //pSearch¼ÓÈëµ½search_pathÖÐ
|
{
|
path_index += 1;
|
search_path[path_index] = index;
|
}
|
}
|
break;
|
}
|
}
|
|
}
|
|
nearestpoint.x = nearest.x;
|
nearestpoint.y = nearest.y;
|
nearestpoint.z = nearest.z;
|
distance = dist;
|
}
|
|
__global__ void search_all_nearest(
|
TreeElement *tree_array,
|
const int *size,
|
const coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance)
|
{
|
int Id = blockIdx.x * 4 + threadIdx.x;
|
//printf("Block id:%d,Thread id:%d,points:%f \n", blockIdx.x, threadIdx.x, points[Id].x);
|
|
dev_search_nearest(tree_array, *size, points[Id], nearest[Id], distance[Id]);
|
double delta_x = points[Id].x - nearest[Id].x;
|
double delta_y = points[Id].y - nearest[Id].y;
|
double delta_z = points[Id].z - nearest[Id].z;
|
distance[Id] =
|
normals[Id].x * delta_x +
|
normals[Id].y * delta_y +
|
normals[Id].z * delta_z;
|
}
|
|
__global__ void search_all_nearest_icp(
|
TreeElement *tree_array,
|
const int *size,
|
coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance)
|
{
|
int Id = blockIdx.x * 1000 + threadIdx.x;
|
|
dev_search_nearest(tree_array, *size, points[Id], nearest[Id], distance[Id]);
|
/*double delta_x = points[Id].x - nearest[Id].x;
|
double delta_y = points[Id].y - nearest[Id].y;
|
double delta_z = points[Id].z - nearest[Id].z;
|
distance[Id] =
|
normals[Id].x * delta_x +
|
normals[Id].y * delta_y +
|
normals[Id].z * delta_z;*/
|
|
}
|
|
__global__ void rotate_points(
|
coordinate *points,
|
double *rotate_matrix)
|
{
|
int Id = blockIdx.x * 1000 + threadIdx.x;
|
//printf("Block id:%d,Thread id:%d,points:%f \n", blockIdx.x, threadIdx.x, points[Id].x);
|
|
points[Id].x = points[Id].x * rotate_matrix[0] + points[Id].y * rotate_matrix[1] + points[Id].z * rotate_matrix[2];
|
points[Id].y = points[Id].x * rotate_matrix[3] + points[Id].y * rotate_matrix[4] + points[Id].z * rotate_matrix[5];
|
points[Id].z = points[Id].x * rotate_matrix[6] + points[Id].y * rotate_matrix[7] + points[Id].z * rotate_matrix[8];
|
}
|
|
__global__ void transfer_points(
|
coordinate *points,
|
coordinate *dev_transfer_vector)
|
{
|
int Id = blockIdx.x * 1000 + threadIdx.x;
|
|
points[Id].x += dev_transfer_vector->x;
|
points[Id].y += dev_transfer_vector->y;
|
points[Id].z += dev_transfer_vector->z;
|
}
|
|
extern "C" bool get_distance(
|
TreeElement *tree_array,
|
const int *size,
|
const coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance)
|
{
|
//dim3 cudaBlockSize(256, 1, 1);
|
//dim3 cudaGridSize((1000 + cudaBlockSize.x - 1) / cudaBlockSize.x, 1, 1);
|
search_all_nearest << <50000, 4 >> > (
|
tree_array,
|
size,
|
points,
|
normals,
|
nearest,
|
distance);
|
return true;
|
}
|
|
extern "C" bool get_distance_icp(
|
TreeElement *tree_array,
|
const int *size,
|
coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance)
|
{
|
search_all_nearest_icp << <20, 1000 >> > (
|
tree_array,
|
size,
|
points,
|
normals,
|
nearest,
|
distance);
|
return true;
|
}
|
|
extern "C" bool rotate_points_host(
|
coordinate *points,
|
double *rotate_matrix)
|
{
|
rotate_points << <20, 1000 >> > (points, rotate_matrix);
|
return true;
|
}
|
|
extern "C" bool transfer_points_host(
|
coordinate *points,
|
coordinate *dev_transfer_vector)
|
{
|
transfer_points << <20, 1000 >> > (points, dev_transfer_vector);
|
return true;
|
}
|
