// System includes
|
#include <stdio.h>
|
#include <assert.h>
|
|
// CUDA runtime
|
#include <cuda_runtime.h>
|
#include <nvrtc_helper.h>
|
|
// helper functions and utilities to work with CUDA
|
#include <helper_functions.h>
|
|
// ¾ØÕóÔËËã¿â
|
#include <Eigen/Core>
|
#include <Eigen/Dense>
|
#include <Eigen/SVD>
|
|
// pcl
|
#include <pcl/point_types.h>
|
#include <pcl/common/common.h>
|
#include <pcl/io/pcd_io.h>
|
#include <pcl/io/vtk_io.h>
|
#include <pcl/io/ply_io.h>
|
#include <pcl/registration/icp.h>
|
#include <pcl/visualization/pcl_visualizer.h>
|
#include <pcl/search/search.h>
|
#include <pcl/search/kdtree.h>
|
#include <pcl/features/normal_3d.h>
|
#include <pcl/visualization/cloud_viewer.h>
|
#include <pcl/filters/passthrough.h>
|
#include <pcl/filters/extract_indices.h>
|
#include <pcl/filters/crop_box.h>
|
#include <pcl/filters/radius_outlier_removal.h>
|
#include <pcl/segmentation/region_growing.h>
|
#include <pcl/surface/gp3.h>
|
#include <pcl/console/time.h>
|
#include <pcl/filters/random_sample.h>
|
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
|
#include <pcl/gpu/containers/initialization.h>
|
#include <pcl/gpu/octree/octree.hpp>
|
#include <pcl/gpu/features/features.hpp>
|
#include <pcl/gpu/segmentation/gpu_extract_clusters.h>
|
#include <pcl/gpu/segmentation/impl/gpu_extract_clusters.hpp>
|
|
// custom
|
#include "kdTree.h"
|
//#include "kernel.h"
|
|
using namespace Eigen;
|
using namespace Eigen::internal;
|
using namespace Eigen::Architecture;
|
|
#define SourcePointSize_1 20000
|
#define SourcePointSize_2 200000
|
|
extern "C" bool get_distance(
|
TreeElement *tree_array,
|
const int *size,
|
const coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance);
|
|
extern "C" bool get_distance_icp(
|
TreeElement *tree_array,
|
const int *size,
|
coordinate *points,
|
const coordinate *normals,
|
coordinate *nearest,
|
double *distance);
|
|
extern "C" bool rotate_points_host(
|
coordinate *points,
|
double *rotate_matrix);
|
|
extern "C" bool transfer_points_host(
|
coordinate *points,
|
coordinate *dev_transfer_vector);
|
|
bool next_iteration = true;
|
|
coordinate getBarycenter(coordinate *point,int size)
|
{
|
coordinate barycenter;
|
|
double sumX = 0;
|
double sumY = 0;
|
double sumZ = 0;
|
|
for (int i = 0; i < size; i++)
|
{
|
sumX += point[i].x;
|
sumY += point[i].y;
|
sumZ += point[i].z;
|
}
|
|
barycenter.x = sumX / size;
|
barycenter.y = sumY / size;
|
barycenter.z = sumZ / size;
|
|
return barycenter;
|
}
|
|
void keyboardEventOccurred(const pcl::visualization::KeyboardEvent& event,
|
void* nothing)
|
{
|
if (event.getKeySym() == "space" && event.keyDown())
|
next_iteration = true;
|
}
|
|
// icp
|
void step1()
|
{
|
TreeNode *tree = new TreeNode;
|
cudaError_t cudaStatus;
|
|
coordinate sourceBarycenter;
|
coordinate targetBarycenter;
|
|
#pragma region µãÔÆÔØÈë
|
|
//===============================µãÔÆÔØÈë======================================//
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_trans(new pcl::PointCloud<pcl::PointXYZ>());
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>());
|
|
pcl::io::loadPLYFile("D:\\test\\Mesh.ply", *cloud_in);
|
pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target);
|
|
pcl::PointXYZ min_target;
|
pcl::PointXYZ max_target;
|
|
pcl::PointXYZ min_in;
|
pcl::PointXYZ max_in;
|
|
pcl::getMinMax3D(*cloud_in, min_in, max_in);
|
pcl::getMinMax3D(*cloud_target, min_target, max_target);
|
|
|
pcl::RadiusOutlierRemoval<pcl::PointXYZ> radius_filter;
|
|
//for (int i = 0; i < cloud_in->points.size(); i++)
|
//{
|
// cloud_in->points[i].z *= 100;
|
//}
|
|
//radius_filter.setInputCloud(cloud_in);
|
//radius_filter.setRadiusSearch(7);
|
//radius_filter.setMinNeighborsInRadius(30); //ÉèÖòéѯµãµÄÁÚÓòµã¼¯ÊýСÓÚ2µÄɾ³ý
|
//radius_filter.filter(*cloud_in);
|
|
//for (int i = 0; i < cloud_in->points.size(); i++)
|
//{
|
// cloud_in->points[i].z *= 0.01;
|
//}
|
|
//pcl::CropBox<pcl::PointXYZ> cropper;
|
//Vector4f min_point;
|
//Vector4f max_point;
|
//min_point(0) = min_in.x + 0.5;// ((max_in.x - min_in.x) - (max_target.x - min_target.x)) / 2;
|
//min_point(1) = min_in.y + 0.5;// ((max_in.y - min_in.y) - (max_target.y - min_target.y)) / 2;
|
//min_point(2) = min_in.z;
|
//min_point(3) = 1.0;
|
//max_point(0) = max_in.x - 0.5;// ((max_in.x - min_in.x) - (max_target.x - min_target.x)) / 2;
|
//max_point(1) = max_in.y - 0.5;// ((max_in.y - min_in.y) - (max_target.y - min_target.y)) / 2;
|
//max_point(2) = max_in.z;
|
//max_point(3) = 1.0;
|
//cropper.setMin(min_point);
|
//cropper.setMax(max_point);
|
//cropper.setInputCloud(cloud_in);
|
//cropper.filter(*cloud_in);
|
|
pcl::RandomSample<pcl::PointXYZ> rs;
|
rs.setSample(SourcePointSize_1);
|
|
rs.setInputCloud(cloud_in);
|
rs.filter(*cloud);
|
|
rs.setInputCloud(cloud_target);
|
rs.filter(*cloud_target);
|
|
#pragma endregion
|
|
#pragma region ·¨Ïß¹ÀËã
|
|
//===============================·¨Ïß¹ÀËã====================================//
|
pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud));
|
pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
|
pcl::gpu::Octree::PointCloud cloud_device;
|
cloud_device.upload(cloud->points);
|
normals_device.upload(normals_p->points);
|
|
pcl::gpu::NormalEstimation normal_estimator;
|
normal_estimator.setRadiusSearch(2.0, 50);
|
normal_estimator.setInputCloud(cloud_device);
|
normal_estimator.compute(normals_device);
|
normals_device.download(normals_p->points);
|
|
for (int i = 0; i < normals_p->size(); i++)
|
{
|
pcl::Normal normal;
|
|
normal.normal_x = normals_p->points[i].x;
|
normal.normal_y = normals_p->points[i].y;
|
normal.normal_z = normals_p->points[i].z;
|
|
source_normals->push_back(normal);
|
}
|
|
//=======================================================================//
|
|
#pragma endregion
|
|
clock_t start, end;
|
start = clock();
|
|
#pragma region kdTree½¨Á¢
|
//==============================kdTree½¨Á¢===========================//
|
|
coordinate *points_target = new coordinate[cloud_target->points.size()];
|
|
for (int i = 0; i < cloud_target->points.size(); i++)
|
{
|
points_target[i].x = cloud_target->points[i].x;
|
points_target[i].y = cloud_target->points[i].y;
|
points_target[i].z = cloud_target->points[i].z;
|
}
|
|
tree = build_kdtree(points_target, cloud_target->points.size(), tree);
|
|
|
|
int height = getHeight(tree);
|
int arraySize = pow(2, height) - 1;
|
|
TreeElement *tree_array = new TreeElement[arraySize];
|
coordinate *points = new coordinate[SourcePointSize_1];
|
coordinate *point_normals = new coordinate[SourcePointSize_1];
|
coordinate *nearest = new coordinate[SourcePointSize_1];
|
double *distance = new double[SourcePointSize_1];
|
|
for (int i = 0; i < arraySize; i++)
|
{
|
tree_array[i].dom_elt = *(new coordinate);
|
tree_array[i].split = 0;
|
tree_array[i].isEmpty = true;
|
}
|
|
TreeToArray(tree, tree_array);
|
|
for (int i = 0; i < SourcePointSize_1; i++)
|
{
|
points[i].x = cloud->points[i].x;
|
points[i].y = cloud->points[i].y;
|
points[i].z = cloud->points[i].z;
|
|
point_normals[i].x = normals_p->points[i].x;
|
point_normals[i].y = normals_p->points[i].y;
|
point_normals[i].z = normals_p->points[i].z;
|
}
|
|
targetBarycenter = getBarycenter(points_target, cloud_target->points.size());
|
sourceBarycenter = getBarycenter(points, cloud->points.size());
|
|
delete[] points_target;
|
|
coordinate delta;
|
delta.x = targetBarycenter.x - sourceBarycenter.x;
|
delta.y = targetBarycenter.y - sourceBarycenter.y;
|
delta.z = targetBarycenter.z - sourceBarycenter.z;
|
|
#pragma endregion
|
|
#pragma region GPU×î½üÁÚ
|
|
double rotate_matrix_array[9] = { 1,0,0,0,1,0,0,0,1 };
|
Eigen::Matrix<double, 3, 3> matrix;
|
Eigen::Matrix3d matrix_u;
|
Eigen::Matrix3d matrix_s;
|
Eigen::Matrix3d matrix_v;
|
Eigen::Matrix4d matrix_temp = Matrix4d::Identity(4, 4);
|
Eigen::Matrix4d matrix_final = Matrix4d::Identity(4, 4);
|
|
Matrix<double, SourcePointSize_1, 3> source_m;
|
Matrix<double, SourcePointSize_1, 3> target_m;
|
|
double distance_sum;
|
double distance_avg;
|
|
TreeElement *dev_tree_array;
|
coordinate *dev_points;
|
coordinate *dev_point_normals;
|
coordinate *dev_nearest;
|
double *dev_rotate_matrix;
|
coordinate *dev_transfer_vector;
|
double *dev_distance;
|
int *dev_arraySize;
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr s_points(new pcl::PointCloud<pcl::PointXYZ>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr t_points(new pcl::PointCloud<pcl::PointXYZ>);
|
|
for (int i = 0; i < SourcePointSize_1; i++)
|
{
|
pcl::PointXYZ point;
|
point.x = 0;
|
point.y = 0;
|
point.z = 0;
|
s_points->points.push_back(point);
|
t_points->points.push_back(point);
|
}
|
|
/*pcl::visualization::PCLVisualizer viewer;
|
viewer.setBackgroundColor(0, 0, 0);
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_s_color_h(s_points, 20, 180, 20);
|
viewer.addPointCloud<pcl::PointXYZ>(s_points, cloud_s_color_h, "s_cloud");
|
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_t_color_h(t_points, 180, 180, 180);
|
viewer.addPointCloud<pcl::PointXYZ>(t_points, cloud_t_color_h, "t_cloud");
|
viewer.addCoordinateSystem(20);
|
viewer.registerKeyboardCallback(&keyboardEventOccurred, (void*)NULL);*/
|
|
cudaMalloc((void**)&dev_tree_array, sizeof(TreeElement) * arraySize);
|
cudaMalloc((void**)&dev_points, sizeof(coordinate) * SourcePointSize_1);
|
cudaMalloc((void**)&dev_point_normals, sizeof(coordinate) * SourcePointSize_1);
|
cudaMalloc((void**)&dev_rotate_matrix, sizeof(double) * 9);
|
cudaMalloc((void**)&dev_transfer_vector, sizeof(coordinate));
|
cudaMalloc((void**)&dev_nearest, sizeof(coordinate) * SourcePointSize_1);
|
cudaMalloc((void**)&dev_distance, sizeof(double) * SourcePointSize_1);
|
cudaMalloc((void**)&dev_arraySize, sizeof(int));
|
|
cudaStatus = cudaMemcpy(dev_transfer_vector, &delta, sizeof(coordinate), cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
transfer_points_host(dev_points, dev_transfer_vector);
|
cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
|
matrix_temp(0, 3) = delta.x;
|
matrix_temp(1, 3) = delta.y;
|
matrix_temp(2, 3) = delta.z;
|
matrix_final = matrix_final * matrix_temp;
|
|
for (int i = 0; i < 20; i++)
|
{
|
//viewer.spinOnce();
|
|
if (next_iteration)
|
{
|
|
|
#pragma region ×î½üÁÚ
|
|
cudaStatus = cudaMemcpy(dev_tree_array, tree_array, sizeof(TreeElement) * arraySize, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_point_normals, point_normals, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_nearest, nearest, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_distance, distance, sizeof(double) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_arraySize, &arraySize, sizeof(int), cudaMemcpyHostToDevice);
|
|
get_distance_icp(
|
dev_tree_array,
|
dev_arraySize,
|
dev_points,
|
dev_point_normals,
|
dev_nearest,
|
dev_distance);
|
|
cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
cudaMemcpy(nearest, dev_nearest, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
cudaMemcpy(distance, dev_distance, sizeof(double) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
|
#pragma endregion
|
|
#pragma region ¼ÆËãÐýת¾ØÕó
|
|
distance_sum = 0;
|
|
//sourceBarycenter = getBarycenter(points, SourcePointSize_1);
|
//targetBarycenter = getBarycenter(nearest, SourcePointSize_1);
|
|
/*int size = SourcePointSize_1;
|
int threshold_index = (int)(size*0.1);
|
double *distance_temp = new double[SourcePointSize_1];
|
for (int i = 0; i < SourcePointSize_1; i++)
|
{
|
distance_temp[i] = distance[i] * distance[i];
|
}
|
std::nth_element(distance_temp, distance_temp + threshold_index, distance_temp + SourcePointSize_1);
|
double threshold = sqrt(distance_temp[threshold_index - 1]);
|
delete[] distance_temp;*/
|
|
matrix = Eigen::Matrix3d::Zero();
|
for (int i = 0; i < SourcePointSize_1; i++)
|
{
|
distance_sum += distance[i] * distance[i];
|
|
/*matrix+= Eigen::Vector3d(points[i].x, points[i].y, points[i].z) *
|
Eigen::Vector3d(nearest[i].x, nearest[i].y, nearest[i].z).transpose();*/
|
|
source_m(i, 0) = points[i].x - sourceBarycenter.x;
|
source_m(i, 1) = points[i].y - sourceBarycenter.y;
|
source_m(i, 2) = points[i].z - sourceBarycenter.z;
|
target_m(i, 0) = nearest[i].x - targetBarycenter.x;
|
target_m(i, 1) = nearest[i].y - targetBarycenter.y;
|
target_m(i, 2) = nearest[i].z - targetBarycenter.z;
|
|
}
|
distance_avg = distance_sum / SourcePointSize_1;
|
|
printf("Distance square sum average:%f\n", distance_avg);
|
|
matrix = source_m.transpose() * target_m;
|
Eigen::JacobiSVD<Eigen::Matrix3d> svd(matrix, Eigen::ComputeFullV | Eigen::ComputeFullU);
|
matrix_u = svd.matrixU();
|
matrix_v = svd.matrixV();
|
Matrix3d rotate = matrix_v * matrix_u.transpose();
|
|
if (rotate.determinant() < 0)
|
{
|
matrix_v(2, 0) = -matrix_v(2, 0);
|
matrix_v(2, 1) = -matrix_v(2, 1);
|
matrix_v(2, 2) = -matrix_v(2, 2);
|
|
Matrix3d rotate = matrix_v * matrix_u.transpose();
|
}
|
|
cout << "Rotate=\n" << rotate << endl;
|
|
rotate_matrix_array[0] = rotate(0, 0);
|
rotate_matrix_array[1] = rotate(0, 1);
|
rotate_matrix_array[2] = rotate(0, 2);
|
|
rotate_matrix_array[3] = rotate(1, 0);
|
rotate_matrix_array[4] = rotate(1, 1);
|
rotate_matrix_array[5] = rotate(1, 2);
|
|
rotate_matrix_array[6] = rotate(2, 0);
|
rotate_matrix_array[7] = rotate(2, 1);
|
rotate_matrix_array[8] = rotate(2, 2);
|
|
cudaStatus = cudaMemcpy(dev_rotate_matrix, rotate_matrix_array, sizeof(double) * 9, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
rotate_points_host(dev_points, dev_rotate_matrix);
|
cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
|
|
#pragma endregion
|
|
#pragma region ¼ÆËãƽÒƾØÕó
|
|
sourceBarycenter = getBarycenter(points, SourcePointSize_1);
|
//targetBarycenter = getBarycenter(nearest, SourcePointSize_1);
|
|
delta.x = targetBarycenter.x - sourceBarycenter.x;
|
delta.y = targetBarycenter.y - sourceBarycenter.y;
|
delta.z = targetBarycenter.z - sourceBarycenter.z;
|
|
printf("transfer=[%f,%f,%f]\n", delta.x, delta.y, delta.z);
|
|
cudaStatus = cudaMemcpy(dev_transfer_vector, &delta, sizeof(coordinate), cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyHostToDevice);
|
transfer_points_host(dev_points, dev_transfer_vector);
|
cudaMemcpy(points, dev_points, sizeof(coordinate) * SourcePointSize_1, cudaMemcpyDeviceToHost);
|
|
#pragma endregion
|
|
matrix_temp(0, 0) = rotate(0, 0);
|
matrix_temp(0, 1) = rotate(0, 1);
|
matrix_temp(0, 2) = rotate(0, 2);
|
matrix_temp(1, 0) = rotate(1, 0);
|
matrix_temp(1, 1) = rotate(1, 1);
|
matrix_temp(1, 2) = rotate(1, 2);
|
matrix_temp(2, 0) = rotate(2, 0);
|
matrix_temp(2, 1) = rotate(2, 1);
|
matrix_temp(2, 2) = rotate(2, 2);
|
matrix_temp(0, 3) = delta.x;
|
matrix_temp(1, 3) = delta.y;
|
matrix_temp(2, 3) = delta.z;
|
|
matrix_final = matrix_final * matrix_temp;
|
|
/*for (int index = 0; index < SourcePointSize; index++)
|
{
|
t_points->points[index].x = nearest[index].x;
|
t_points->points[index].y = nearest[index].y;
|
t_points->points[index].z = nearest[index].z;
|
s_points->points[index].x = points[index].x;
|
s_points->points[index].y = points[index].y;
|
s_points->points[index].z = points[index].z;
|
}
|
viewer.updatePointCloud(s_points, cloud_s_color_h, "s_cloud");
|
viewer.updatePointCloud(t_points, cloud_t_color_h, "t_cloud");*/
|
|
//next_iteration = false;
|
}
|
}
|
|
#pragma endregion
|
|
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
|
icp.setMaximumIterations(20);
|
pcl::transformPointCloud(*cloud, *cloud, matrix_final);
|
icp.setInputSource(cloud);
|
icp.setInputTarget(cloud_target);
|
icp.align(*cloud);
|
|
if (icp.hasConverged())
|
{
|
matrix_final = icp.getFinalTransformation().cast<double>() * matrix_final;
|
icp.setMaximumIterations(1);
|
icp.align(*cloud);
|
std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
|
}
|
|
end = clock();
|
double costTime = (double)(end - start) / CLOCKS_PER_SEC;
|
printf("Use Time:%f\n", costTime);
|
|
|
delete[] tree_array;
|
delete[] points;
|
delete[] point_normals;
|
delete[] nearest;
|
delete[] distance;
|
|
cudaFree(dev_tree_array);
|
cudaFree(dev_points);
|
cudaFree(dev_point_normals);
|
cudaFree(dev_rotate_matrix);
|
cudaFree(dev_transfer_vector);
|
cudaFree(dev_nearest);
|
cudaFree(dev_distance);
|
cudaFree(dev_arraySize);
|
|
pcl::transformPointCloud(*cloud_in, *cloud_trans, matrix_final);
|
|
pcl::io::savePLYFile("D:\\test\\mesh_trans_gpu_2.ply", *cloud_trans);
|
}
|
|
// ¼ÆËãµãÔƵ½±ê×¼µãÔƵľàÀë
|
void step2()
|
{
|
TreeNode *tree = new TreeNode;
|
cudaError_t cudaStatus;
|
|
#pragma region µãÔÆÔØÈë
|
|
//===============================µãÔÆÔØÈë======================================//
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
|
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_target(new pcl::PointCloud<pcl::PointXYZ>());
|
|
pcl::io::loadPLYFile("D:\\test\\mesh_trans_gpu_1.ply", *cloud);
|
pcl::io::loadPLYFile("D:\\test\\stand2.ply", *cloud_target);
|
|
pcl::RandomSample<pcl::PointXYZ> rs;
|
rs.setSample(SourcePointSize_2);
|
|
rs.setInputCloud(cloud);
|
rs.filter(*cloud);
|
|
/*rs.setInputCloud(cloud_target);
|
rs.filter(*cloud_target);*/
|
|
#pragma endregion
|
|
clock_t start, end;
|
start = clock();
|
|
#pragma region ·¨Ïß¹ÀËã
|
|
//===============================·¨Ïß¹ÀËã====================================//
|
pcl::PointCloud <pcl::Normal>::Ptr source_normals(new pcl::PointCloud <pcl::Normal>);
|
pcl::PointCloud<pcl::PointXYZ>::Ptr normals_p(new pcl::PointCloud <pcl::PointXYZ>(*cloud));
|
pcl::gpu::DeviceArray<pcl::PointXYZ> normals_device;
|
pcl::gpu::Octree::PointCloud cloud_device;
|
cloud_device.upload(cloud->points);
|
normals_device.upload(normals_p->points);
|
|
pcl::gpu::NormalEstimation normal_estimator;
|
normal_estimator.setRadiusSearch(2.0, 50);
|
normal_estimator.setInputCloud(cloud_device);
|
normal_estimator.compute(normals_device);
|
normals_device.download(normals_p->points);
|
|
for (int i = 0; i < normals_p->size(); i++)
|
{
|
pcl::Normal normal;
|
|
normal.normal_x = normals_p->points[i].x;
|
normal.normal_y = normals_p->points[i].y;
|
normal.normal_z = normals_p->points[i].z;
|
|
source_normals->push_back(normal);
|
}
|
|
//=======================================================================//
|
|
#pragma endregion
|
|
#pragma region kdTree½¨Á¢
|
//===============================kdTree½¨Á¢===============================//
|
coordinate *points_target = new coordinate[cloud_target->points.size()];
|
|
for (int i = 0; i < cloud_target->points.size(); i++)
|
{
|
points_target[i].x = cloud_target->points[i].x;
|
points_target[i].y = cloud_target->points[i].y;
|
points_target[i].z = cloud_target->points[i].z;
|
}
|
|
tree = build_kdtree(points_target, cloud_target->points.size(), tree);
|
|
delete[] points_target;
|
|
//====================================================================//
|
#pragma endregion
|
|
#pragma region ת»»³ÉÊý×é
|
//==============================ת»»³ÉÊý×é===========================//
|
|
int height = getHeight(tree);
|
int arraySize = pow(2, height) - 1;
|
|
TreeElement *tree_array = new TreeElement[arraySize];
|
coordinate *points = new coordinate[SourcePointSize_2];
|
coordinate *point_normals = new coordinate[SourcePointSize_2];
|
coordinate *nearest = new coordinate[SourcePointSize_2];
|
double *distance = new double[SourcePointSize_2];
|
|
for (int i = 0; i < arraySize; i++)
|
{
|
tree_array[i].dom_elt = *(new coordinate);
|
tree_array[i].split = 0;
|
tree_array[i].isEmpty = true;
|
}
|
|
TreeToArray(tree, tree_array);
|
|
for (int i = 0; i < SourcePointSize_2; i++)
|
{
|
points[i].x = cloud->points[i].x;
|
points[i].y = cloud->points[i].y;
|
points[i].z = cloud->points[i].z;
|
|
point_normals[i].x = normals_p->points[i].x;
|
point_normals[i].y = normals_p->points[i].y;
|
point_normals[i].z = normals_p->points[i].z;
|
}
|
#pragma endregion
|
|
#pragma region CPU×î½üÁÚ
|
//==================================================================//
|
//clock_t start, end;
|
//start = clock();
|
//coordinate target;
|
//ofstream outFile;
|
//outFile.open("D:\\data2.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
|
//for (int i = 0; i < cloud->points.size(); i++)
|
//{
|
// target.x = cloud->points[i].x;
|
// target.y = cloud->points[i].y;
|
// target.z = cloud->points[i].z;
|
// searchNearest(tree_array, cloud_target->points.size(), target, *nearest, *distance);
|
// float delta_x = target.x - nearest->x;
|
// float delta_y = target.y - nearest->y;
|
// float delta_z = target.z - nearest->z;
|
// *distance =
|
// source_normals->points[i].normal_x * delta_x +
|
// source_normals->points[i].normal_y * delta_y +
|
// source_normals->points[i].normal_z * delta_z;
|
// outFile << target.x << ',' << target.y << ',' << target.z << "," << distance << endl;
|
//}
|
//outFile.close();
|
//end = clock();
|
//double costTime = (double)(end - start) / CLOCKS_PER_SEC;
|
//printf("Use Time:%f\n", costTime);
|
#pragma endregion
|
|
#pragma region GPU×î½üÁÚ
|
|
TreeElement *dev_tree_array;
|
coordinate *dev_points;
|
coordinate *dev_point_normals;
|
coordinate *dev_nearest;
|
double *dev_distance;
|
int *dev_arraySize;
|
|
cudaMalloc((void**)&dev_tree_array, sizeof(TreeElement) * arraySize);
|
cudaMalloc((void**)&dev_points, sizeof(coordinate) * SourcePointSize_2);
|
cudaMalloc((void**)&dev_point_normals, sizeof(coordinate) * SourcePointSize_2);
|
cudaMalloc((void**)&dev_nearest, sizeof(coordinate) * SourcePointSize_2);
|
cudaMalloc((void**)&dev_distance, sizeof(double) * SourcePointSize_2);
|
cudaMalloc((void**)&dev_arraySize, sizeof(int));
|
|
cudaStatus = cudaMemcpy(dev_tree_array, tree_array, sizeof(TreeElement) * arraySize, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_points, points, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_point_normals, point_normals, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_nearest, nearest, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_distance, distance, sizeof(double) * SourcePointSize_2, cudaMemcpyHostToDevice);
|
cudaStatus = cudaMemcpy(dev_arraySize, &arraySize, sizeof(int), cudaMemcpyHostToDevice);
|
|
get_distance(dev_tree_array, dev_arraySize, dev_points, dev_point_normals, dev_nearest, dev_distance);
|
|
cudaMemcpy(nearest, dev_nearest, sizeof(coordinate) * SourcePointSize_2, cudaMemcpyDeviceToHost);
|
cudaMemcpy(distance, dev_distance, sizeof(double) * SourcePointSize_2, cudaMemcpyDeviceToHost);
|
|
#pragma endregion
|
|
end = clock();
|
double costTime = (double)(end - start) / CLOCKS_PER_SEC;
|
printf("Use Time:%f\n", costTime);
|
|
#pragma region Êý¾Ý±£´æ¼°Õ¹Ê¾
|
|
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_result(new pcl::PointCloud<pcl::PointXYZI>);
|
|
for (int i = 0; i < SourcePointSize_2; i++)
|
{
|
pcl::PointXYZI point;
|
|
point.x = points[i].x;
|
point.y = points[i].y;
|
//point.z = points[i].z;
|
point.z = distance[i] * 100;
|
point.intensity = distance[i];
|
|
cloud_result->push_back(point);
|
|
}
|
|
|
pcl::visualization::PCLVisualizer viewer;
|
viewer.setBackgroundColor(0, 0, 0);
|
pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointXYZI> fildColor(cloud_result, "intensity");//°´ÕÕintensity×ֶνøÐÐäÖȾ
|
viewer.addPointCloud<pcl::PointXYZI>(cloud_result, fildColor, "s_cloud");
|
viewer.addCoordinateSystem(20);
|
|
while (!viewer.wasStopped())
|
{
|
viewer.spinOnce();
|
}
|
|
ofstream outFile;
|
outFile.open("D:\\data4.csv", ios::out); // ´ò¿ªÄ£Ê½¿ÉÊ¡ÂÔ
|
for (int i = 0; i < SourcePointSize_2; i++)
|
{
|
outFile << points[i].x << ','
|
<< points[i].y << ','
|
<< points[i].z << ","
|
<< nearest[i].x << ","
|
<< nearest[i].y << ","
|
<< nearest[i].z << ","
|
<< distance[i] << endl;
|
}
|
outFile.close();
|
|
#pragma endregion
|
|
delete[] tree_array;
|
delete[] points;
|
delete[] point_normals;
|
delete[] nearest;
|
delete[] distance;
|
|
cudaFree(dev_tree_array);
|
cudaFree(dev_points);
|
cudaFree(dev_point_normals);
|
cudaFree(dev_nearest);
|
cudaFree(dev_distance);
|
}
|
|
int main(int argc, char **argv)
|
{
|
//step1();
|
|
step2();
|
|
return 0;
|
}
|
