PCL应用及CMakeLists编译示例

企业开发 2023-05-17 03:28:14 阅读次数: 0 
/*
 * @Date: 2022-10-28 08:32:53
 * @Author: yang_haitao
 * @LastEditors: yanghaitao [email protected]
 * @LastEditTime: 2022-12-28 08:52:01
 * @FilePath: /K-Lane/cpp_lane_det/lane_det/main.cpp
 */
#include <iostream>
#include <sstream>
#include <fstream>
#include <random>
#include <algorithm>
#include <cuda_runtime.h>
#include <math.h>

#include "params.h"
#include "laneDet.h"

#include <Eigen/Dense>
#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>



// creates a new point type that contains XYZ data (SSE padded)
struct PointXYZIRT {
    
    
 PCL_ADD_POINT4D;
 uint8_t intensity;
 uint16_t ring;
 double timestamp;
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 }EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,
 (float, x, x)
 (float, y, y)
 (float, z, z)
 (uint8_t, intensity, intensity)
 (uint16_t, ring, ring)
 (double, timestamp, timestamp) 
) 

void ConvertPointXYZIRTToPointXYZI(const pcl::PointCloud<PointXYZIRT>::Ptr in_cloud_xyzirt,
pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud)
{
    
            
  in_cloud->header = in_cloud_xyzirt->header;
  in_cloud->width = in_cloud_xyzirt->width;
  in_cloud->height = in_cloud_xyzirt->height;
  in_cloud->is_dense = in_cloud_xyzirt->is_dense;
  for(size_t i=0;i<in_cloud_xyzirt->points.size();i++)
  {
    
             
    pcl::PointXYZI p;
    p.x = in_cloud_xyzirt->points[i].x;
    p.y = in_cloud_xyzirt->points[i].y;
    p.z = in_cloud_xyzirt->points[i].z;
    p.intensity = in_cloud_xyzirt->points[i].intensity;
    in_cloud->points.push_back(p);
  }
  
}


std::string Data_File = "../data/";
std::string Save_Dir = "../eval/kitti/object/pred_velo/";
std::string Model_File = "/home/work_dir/K-Lane/cpp_lane_det/model/best_1215_98.onnx";


ros::Publisher pub_cloud;
ros::Publisher pub_lane_points;
ros::Publisher pub_lane_markers;
std_msgs::Header mHeader;

#define checkCudaErrors(status)                                   \
{
      
                                                                       \
  if (status != 0)                                                \
  {
      
                                                                     \
    std::cout << "Cuda failure: " << cudaGetErrorString(status)   \
              << " at line " << __LINE__                          \
              << " in file " << __FILE__                          \
              << " error status: " << status                      \
              << std::endl;                                       \
              abort();                                            \
    }                                                             \
}



void Getinfo(void)
{
    
    
  cudaDeviceProp prop;

  int count = 0;
  cudaGetDeviceCount(&count);
  printf("\nGPU has cuda devices: %d\n", count);
  for (int i = 0; i < count; ++i) {
    
    
    cudaGetDeviceProperties(&prop, i);
    printf("----device id: %d info----\n", i);
    printf("  GPU : %s \n", prop.name);
    printf("  Capbility: %d.%d\n", prop.major, prop.minor);
    printf("  Global memory: %luMB\n", prop.totalGlobalMem >> 20);
    printf("  Const memory: %luKB\n", prop.totalConstMem  >> 10);
    printf("  SM in a block: %luKB\n", prop.sharedMemPerBlock >> 10);
    printf("  warp size: %d\n", prop.warpSize);
    printf("  threads in a block: %d\n", prop.maxThreadsPerBlock);
    printf("  block dim: (%d,%d,%d)\n", prop.maxThreadsDim[0], prop.maxThreadsDim[1], prop.maxThreadsDim[2]);
    printf("  grid dim: (%d,%d,%d)\n", prop.maxGridSize[0], prop.maxGridSize[1], prop.maxGridSize[2]);
  }
  printf("\n");
}

int loadData(const char *file, void **data, unsigned int *length)
{
    
    
  std::fstream dataFile(file, std::ifstream::in);

  if (!dataFile.is_open())
  {
    
    
	  std::cout << "Can't open files: "<< file << std::endl;
	  return -1;
  }

  //get length of file:
  unsigned int len = 0;
  dataFile.seekg (0, dataFile.end);
  len = dataFile.tellg();
  dataFile.seekg (0, dataFile.beg);

  //allocate memory:
  char *buffer = new char[len];
  if(buffer==NULL) {
    
    
	  std::cout << "Can't malloc buffer."<<std::endl;
    dataFile.close();
	  exit(-1);
  }

  //read data as a block:
  dataFile.read(buffer, len);
  dataFile.close();

  *data = (void*)buffer;
  *length = len;
  return 0;  
}

// template <typename T, typename RGB, typename PointT>
void points2Msg(std::vector<float4> &points, std::vector<float3> &color, std::vector<std::vector<float4> > &each_lane, 
                pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud)
{
    
    
  for (size_t i = 0; i < points.size(); ++i)
  {
    
    
    pcl::PointXYZRGB p;
    int label = points[i].w;
    for(int line=0; line < 6; ++line)
    {
    
    
      if (label == line)
      {
    
    
        p.x = points[i].x;
        p.y = points[i].y;
        p.z = points[i].z;
        int rgb = (static_cast<int>(color[label].x) << 16 | static_cast<int>(color[label].y) << 8 | static_cast<int>(color[label].z));
        p.rgb = *reinterpret_cast<float*>(&rgb);
        cloud->points.push_back(p);
        each_lane[label].push_back(points[i]);
      }
      
    }
  }
  
}

void points2Marker(std::vector<float4> &points, float3 &color, std::vector<double> &coef, 
                    visualization_msgs::Marker &line)
{
    
    
  float xMax = 0.0;
  float xMin = 0.0;
  double step = 0.5;
  for (auto p : points)
  {
    
    
    if (xMax < p.x) xMax = p.x; 
  }
  int step_num = std::ceil((xMax - xMin) / step);

  line.lifetime = ros::Duration(0.3);
  line.type = visualization_msgs::Marker::LINE_STRIP;
  line.action = visualization_msgs::Marker::ADD;
  line.ns = "lane"; 
  line.scale.x = 0.1;
  line.color.r = color.x / 255;
  line.color.g = color.y / 255;
  line.color.b = color.z / 255;
  line.color.a = 1.;
  line.pose.orientation.w = 1.;
  for (size_t j = 0; j < step_num + 1; ++j)
  {
    
    
      float x_value = xMin + j * step;
      if (x_value > xMax)
      {
    
    
          x_value = xMax;
      }
      geometry_msgs::Point p;
      p.x = x_value;
      // p.y = coef[0]*x_value*x_value + coef[1]*x_value + coef[2]; // for y = ax^2 + bx + c
      p.y = -(coef[0] / coef[1])*x_value - (coef[2] / coef[1]); // for Ax+By+C=0
      p.z = -1.4;
      line.points.push_back(p);   
  }
}

int ransac_line_fitting(std::vector<float4> &in_cloud, std::vector<double> &best_model, std::vector<float4> &inliers,
  int maxiter=200, int samples=2, int consensus_thres=10, double dis_thres=0.1)
{
    
    
	int point_num = in_cloud.size();
	std::default_random_engine rng;
	rng.seed(10);
	std::uniform_int_distribution<unsigned> uniform(0, point_num-1);

	std::set<int> selectIndexs, consensusIndexs;
  std::vector<float4> selectPoints;
	int bestconsensus_num = 0;
  double best_error = 0;
  double tmp_error = 0;
  int consensus_error_num = 0;
	int iter = 0;
	double tmp_A=0, tmp_B=0, tmp_C=0;  // Ax + By + C = 0
	while (iter < maxiter)
	{
    
    
		selectIndexs.clear();
		selectPoints.clear();
		while(1)
		{
    
    
			int index = uniform(rng);
			selectIndexs.insert(index);
			if(selectIndexs.size() == samples) // 2 samples
			{
    
    
				break;
			}
		}
		std::set<int>::iterator selectiter = selectIndexs.begin();
		while(selectiter != selectIndexs.end())
		{
    
    
			int index = *selectiter;
			selectPoints.push_back(in_cloud[index]);
			selectiter++;
		}
		// 这里是直线拟合，// Step2: 进行模型参数估计 (y2 - y1)*x - (x2 - x1)*y + (y2 - y1)x2 - (x2 - x1)y2= 0
		double deltaY = selectPoints[1].y - selectPoints[0].y;
		double deltaX = selectPoints[1].x - selectPoints[0].x;
		tmp_A = deltaY;
		tmp_B = -deltaX;
		tmp_C = -deltaY * selectPoints[1].x + deltaX * selectPoints[1].y;
		int dataiter = 0;
		double meanError = 0;
		std::set<int> tmpConsensusIndexs;
		while(dataiter < point_num)
		{
    
    
			double dist = (tmp_A*in_cloud[dataiter].x + tmp_B*in_cloud[dataiter].y + tmp_C) / sqrt(tmp_A*tmp_A + tmp_B*tmp_B);
			dist = dist > 0 ? dist : -dist;
			if(dist <= dis_thres)
			{
    
    
				tmpConsensusIndexs.insert(dataiter);
        inliers.push_back(in_cloud[dataiter]);
			}
			meanError += dist;
			dataiter++;
		}
    double model_mean_error = 0;
    // 判断一致性: 满足一致性集合的最小元素个数条件 + 至少比上一次的好
		if (tmpConsensusIndexs.size() >= consensus_thres && tmpConsensusIndexs.size() >= bestconsensus_num)
		{
    
    
			bestconsensus_num = consensusIndexs.size();
      model_mean_error = meanError / point_num;
      consensusIndexs.clear();
			consensusIndexs = tmpConsensusIndexs;
      best_model[0] = tmp_A;
      best_model[1] = tmp_B;
      best_model[2] = tmp_C;
		}
    if (model_mean_error < best_error)
    {
    
    
      best_error = model_mean_error;
    }
    if (tmp_error != best_error)
    {
    
    
      tmp_error = best_error;
    }
    else
    {
    
    
      consensus_error_num += 1;
      if (consensus_error_num > 10)
      {
    
    
        break;
      }
    }
		iter++;
	}
	// return isNonFind;
}



void callback(const sensor_msgs::PointCloud2::Ptr &in_cloud_msg)
{
    
    
  // Getinfo();
  cudaEvent_t start, stop;
  float elapsedTime = 0.0f;
  cudaStream_t stream = NULL;

  checkCudaErrors(cudaEventCreate(&start));
  checkCudaErrors(cudaEventCreate(&stop));
  checkCudaErrors(cudaStreamCreate(&stream));

  LaneDet laneDet(Model_File, stream);
  pcl::PointCloud<PointXYZIRT>::Ptr in_cloud(new pcl::PointCloud<PointXYZIRT>);
  // pcl::PointCloud<pcl::PointXYZI>::Ptr in_cloud(new pcl::PointCloud<pcl::PointXYZI>);
  pcl::fromROSMsg(*in_cloud_msg, *in_cloud);
  
  int num_points = in_cloud->size();
  float *points_array = new float[num_points * 255];
  for (size_t i = 0; i < num_points; ++i) {
    
    
        PointXYZIRT point = in_cloud->at(i);
        // pcl::PointXYZI point = in_cloud->at(i);
        points_array[i * 4 + 0] = point.x;
        points_array[i * 4 + 1] = point.y;
        points_array[i * 4 + 2] = point.z;
        points_array[i * 4 + 3] = point.intensity;
    }
  unsigned int points_data_size = num_points * 4 * sizeof(float);
  std::cout << "<char> points num: "<< num_points << std::endl;
  std::cout << "find points num: "<< points_data_size << std::endl;

  std::vector<float4> result;
  float *points_data = nullptr;
  checkCudaErrors(cudaMallocManaged((void **)&points_data, points_data_size));
  checkCudaErrors(cudaMemcpy(points_data, points_array, points_data_size, cudaMemcpyDefault));
  checkCudaErrors(cudaDeviceSynchronize());

  cudaEventRecord(start, stream);

  laneDet.doinfer(points_data, num_points, result);
  cudaEventRecord(stop, stream);
  cudaEventSynchronize(stop);
  cudaEventElapsedTime(&elapsedTime, start, stop);
  std::cout<<"TIME: laneDet: "<< elapsedTime <<" ms." <<std::endl; 
  std::vector<float3> color = {
    
    {
    
    255,0,0}, {
    
    0,255,0}, {
    
    0,0,255}, {
    
    255,255,0}, {
    
    0,255,255}, {
    
    255,0,255}};
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb (new pcl::PointCloud<pcl::PointXYZRGB>);
  std::vector<std::vector<float4> > each_lane(6);
  points2Msg(result, color, each_lane, cloud_rgb);
  std::vector<double> coef_line(3); // a, b, c
  std::vector<float4> inliers;
  visualization_msgs::MarkerArray line_markers;
  for (int i = 0; i < each_lane.size(); ++i)
  {
    
    
    visualization_msgs::Marker line_marker;
    line_marker.id = i;
    line_marker.header = mHeader;
    std::cerr << each_lane[i].size() << std::endl;
    int points_num = each_lane[i].size();
    if (points_num > 3)
    {
    
    
      // ransac_curve_fitting(each_lane[i], coef_line, inliers, 200, 3, 10, 0.3);
      ransac_line_fitting(each_lane[i], coef_line, inliers, 200, 2, 10, 0.2);
      points2Marker(inliers, color[i], coef_line, line_marker);
      line_markers.markers.push_back(line_marker);
    }
  }
  
  sensor_msgs::PointCloud2 cloud_msg, cloud_src_msg;
  pcl::toROSMsg(*cloud_rgb, cloud_msg);
  pcl::toROSMsg(*in_cloud, cloud_src_msg);
  cloud_msg.header = cloud_src_msg.header = mHeader;

  pub_cloud.publish(cloud_src_msg);
  pub_lane_points.publish(cloud_msg);
  pub_lane_markers.publish(line_markers);
  ROS_INFO("publish points successfull...");
  std::cerr << cloud_msg.header << cloud_rgb->size() << std::endl;

  checkCudaErrors(cudaFree(points_data));

  std::cout << ">>>>>>>>>>>" <<std::endl;

  checkCudaErrors(cudaEventDestroy(start));
  checkCudaErrors(cudaEventDestroy(stop));
  checkCudaErrors(cudaStreamDestroy(stream));
}

class LaneDetector
{
    
    
  public:
    LaneDetector(ros::NodeHandle nh);
    ~LaneDetector(){
    
    };
    bool doProcess();
  private:
    void callback(const sensor_msgs::PointCloud2::Ptr &msg);
};

int main(int argc, char **argv)
{
    
    
  ros::init(argc, argv, "lane_yht");
  ros::NodeHandle nh;
  pub_cloud = nh.advertise<sensor_msgs::PointCloud2>("/lidar_cloud", 10);
  pub_lane_points = nh.advertise<sensor_msgs::PointCloud2>("/lane_points", 10);
  pub_lane_markers = nh.advertise<visualization_msgs::MarkerArray>("/lane_markers", 10);
  mHeader.frame_id = "base_link";
  mHeader.stamp = ros::Time::now();

  ros::Subscriber sub_cloud = nh.subscribe("/rslidar_points", 10, &callback);
  
  ros::spin();

  return 0;
}


# SPDX-FileCopyrightText: Copyright (c) 2021 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# SPDX-License-Identifier: Apache-2.0
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

cmake_minimum_required(VERSION 2.8.7)
project(lane_demo)

# msgs
# add_subdirectory(custom_msgs)

find_package(CUDA REQUIRED)
find_package(catkin REQUIRED COMPONENTS
  roscpp
  pcl_ros
  sensor_msgs
  # custom_msgs
)

find_package(PCL REQUIRED)



if(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL  "aarch64")
  set(CMAKE_C_COMPILER /usr/bin/aarch64-linux-gnu-gcc)
  set(CMAKE_CXX_COMPILER /usr/bin/aarch64-linux-gnu-g++)
  set(CUDA_INSTALL_TARGET_DIR targets/aarch64-linux)
elseif(${CMAKE_HOST_SYSTEM_PROCESSOR} STREQUAL "x86_64")
  set(CMAKE_C_COMPILER /usr/bin/gcc)
  set(CMAKE_CXX_COMPILER /usr/bin/g++)
  set(CUDA_INSTALL_TARGET_DIR targets/x86_64-linux)
endif()

set(CUDA_TOOLKIT_ROOT_DIR /usr/local/cuda)
set(CUDA_INCLUDE_DIRS ${CUDA_TOOLKIT_ROOT_DIR}/${CUDA_INSTALL_TARGET_DIR}/include)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS_RELEASE "-Wno-deprecated-declarations -O2")
add_compile_options(-W)
add_compile_options(-std=c++11)

set( SMS 30 32 35 37 50 52 53 60 61 62 70 72 75 87)
foreach(sm ${SMS})
	set(GENCODE ${GENCODE} -gencode arch=compute_${sm},code=sm_${sm})
endforeach()
set(HIGHEST_SM 87)
set(GENCODE ${GENCODE} -gencode arch=compute_${HIGHEST_SM},code=compute_${HIGHEST_SM})

set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS}
    -ccbin ${CMAKE_CXX_COMPILER}
    -Xcompiler -DWIN_INTERFACE_CUSTOM
    -Xcompiler -I/usr/aarch64-linux-gnu/include/
    -Xlinker -lsocket
    -Xlinker -rpath=/usr/lib/aarch64-linux-gnu/
    -Xlinker -rpath=/usr/aarch64-linux-gnu/lib/
    -Xlinker -L/usr/lib/aarch64-linux-gnu/
    -Xlinker -L/usr/aarch64-linux-gnu/lib/
)

if(${CMAKE_BUILD_TYPE} STREQUAL "Debug")
  message("Using Debug Mode")
  set(CUDA_NVCC_FLAGS ${CUDA_NVCC_FLAGS} -g -G --ptxas-options=-v)
endif()

set(TENSORRT_INCLUDE_DIRS /opt/TensorRT-8.4.3.1/include)
set(TENSORRT_LIBRARY_DIRS /opt/TensorRT-8.4.3.1/lib)

set(ROS_INCLUDE_DIRS /opt/ros/melodic/include)
set(ROS_LIBRARY_DIRS /opt/ros/melodic/lib)

include_directories(
    ${CUDA_INCLUDE_DIRS}
    ${TENSORRT_INCLUDE_DIRS}
    include
    ${ROS_INCLUDE_DIRS}
    ${PCL_INCLUDE_DIRS}
    ${catkin_INCLUDE_DIRS}
)
# message("ddd   " ${catkin_LIBRARIES})
link_directories(
	${TENSORRT_LIBRARY_DIRS}
	/usr/lib/aarch64-linux-gnu
  /usr/aarch64-linux-gnu/lib/
  ${ROS_LIBRARY_DIRS}
)

file(GLOB_RECURSE SOURCE_FILES
    src/*.cu
    src/*.cpp
)

cuda_add_executable(${PROJECT_NAME} main.cpp ${SOURCE_FILES})

target_link_libraries(${PROJECT_NAME}
    libnvinfer.so
    libnvonnxparser.so
    ${Boost_LIBRARIES}
    ${catkin_LIBRARIES}
)

猜你喜欢

转载自blog.csdn.net/qq_39506862/article/details/128472077
今日推荐
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
面壁智能发布 Eurux-8x22B 开源大模型 —— 堪称「理科状元」
开源日报 | 谷歌扶持鸿蒙上位；开源Rabbit R1；Docker加持的安卓手机；微软的焦虑和野心；海尔电器把开放平台关了
中国码农的“35岁魔咒”
蘭雅 CorelDRAW 插件 2024.5.1 国际劳动节版，免费下载
Arc Browser for Windows 1.0 正式 GA
90后程序员开发视频搬运软件、不到一年获利超 700 万，结局很刑！
周排行
Java自定义时间格式
同步整形电路
在开发中最最最常用的字符串的属性大集合
Linux 查看端口占用并杀掉
Java基础四：ArrayList
多线程之死锁就是这么简单
mysql 基础命令集
awk 命令详解
Centos6.3编译安装nginx+php步骤
OCR （Optical Character Recognition，光学字符识别）
每日归档
更多
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)
2024-05-02(0)
2024-05-01(4)
2024-04-30(1)
2024-04-29(40)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022