博客

最近项目比较忙没空更新博客，发现问保存LOAM生成点云数据的小伙伴挺多，现写个博客说一下。

方法一。可以改loam程序，一段时间不再输入数据则保存pcd。

方法二。可以使用ros实时录包。

两个方法本质一样，都是提取每帧位姿信息和点云信息，之后pcl填充点云，如果连续几秒没新数据就生成pcd。介绍一下我写的方法二。

首先下载loam，我使用的

https://github.com/cuitaixiang/LOAM_NOTED

也就是源LOAM程序，编译并添加源到.bashrc文件。

依次开四个cmd分别输入运行。ros核，录包，运行loam，输入数据（记得改路径）

数据：

链接: https://pan.baidu.com/s/1GaZ2eGZdfc-cluSc-bkQng 提取码: 9zsp

roscore

rosbag record /integrated_to_init /velodyne_cloud_3

roslaunch loam_velodyne loam_velodyne.launch

rosbag play '/home/eminbogen/data/nsh_indoor_outdoor.bag'

这样一个有每帧位姿和点云的rosbag就生成了，之后用catkin_make编译一个拼接程序，记得添加ros源到.bashrc里。

cmakelist：

cmake_minimum_required(VERSION 2.8.3)
project(rosbag_make)

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")

#find_package(Eigen3 REQUIRED)
find_package(PCL REQUIRED)
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  rospy
  std_msgs
  turtlesim
  message_generation
)

generate_messages(
  DEPENDENCIES
  std_msgs
)

catkin_package(
   CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs turtlesim message_runtime
   INCLUDE_DIRS include
)

include_directories(include ${catkin_INCLUDE_DIRS} "/usr/local/include/eigen3" "/home/eminbogen/pkg/lastootsLAStools/src" ${PCL_INCLUDE_DIRS})

add_executable(pcd_get src/pcd_get.cpp)
target_link_libraries(pcd_get ${catkin_LIBRARIES} ${PCL_LIBRARIES})

pcd_get.cpp：

pcd保存路径要改一下，如果你跑的不是loam录制topic应该要更改。

#include <Eigen/Geometry> 
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <boost/thread.hpp>
#include <time.h>
//计时
clock_t  clock_start, clock_end;
std::vector<Eigen::Isometry3d> all_pose;
std::vector<double> all_pose_stamp;
std::vector<std::vector<pcl::PointXYZ>> all_points;
std::vector<double> all_points_stamp;

void odomeCallback(const nav_msgs::Odometry laserOdometry)
{
    Eigen::Quaterniond q( laserOdometry.pose.pose.orientation.w, 
			  laserOdometry.pose.pose.orientation.x, 
			  laserOdometry.pose.pose.orientation.y, 
			  laserOdometry.pose.pose.orientation.z );
    Eigen::Isometry3d T(q);
    T.pretranslate( Eigen::Vector3d( laserOdometry.pose.pose.position.x, 
				     laserOdometry.pose.pose.position.y, 
				     laserOdometry.pose.pose.position.z ));
    all_pose.push_back( T );
    all_pose_stamp.push_back(double(laserOdometry.header.stamp.sec));
}

void cloudCallback(const sensor_msgs::PointCloud2 ros_cloud)
{
    //ROS_INFO("cloud is going");
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud_temp;
    pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
    std::vector<pcl::PointXYZ> cloud_vec;

    for(int i=0;i<int(pcl_cloud_temp.points.size());i++)
    {
	pcl::PointXYZ temp_one_point;
	temp_one_point.x=pcl_cloud_temp.points[i].x;
	temp_one_point.y=pcl_cloud_temp.points[i].y;
	temp_one_point.z=pcl_cloud_temp.points[i].z;
	cloud_vec.push_back(temp_one_point);
    }
    all_points.push_back(cloud_vec);
    all_points_stamp.push_back(double(ros_cloud.header.stamp.sec));
    cloud_vec.clear();
}

void pcd_maker()
{
    int pcd_num=0;
    int state_pcd=2;
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZ> ); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_filter( new pcl::PointCloud<pcl::PointXYZ> ); 
    ROS_INFO("pcd_all_begin");
    while(state_pcd)
    {
	ROS_INFO("pcd_num=%d",pcd_num);
	if((int(all_points_stamp.size())-10>pcd_num)&&(int(all_pose_stamp.size())-10>pcd_num*2))
	{   
	    if(state_pcd==2){state_pcd=1;}
	    Eigen::Isometry3d T = all_pose[pcd_num*2];
	    for ( int cloud_temp_num=0; cloud_temp_num<int(all_points[pcd_num].size()); cloud_temp_num++ )
	    {
		Eigen::Vector3d point;  
		point[0] = all_points[pcd_num][cloud_temp_num].x;
		point[1] = all_points[pcd_num][cloud_temp_num].y; 
		point[2] = all_points[pcd_num][cloud_temp_num].z;
		Eigen::Vector3d pointWorld = T*point;

		pcl::PointXYZ p ;
		p.x = pointWorld[0];
		p.y = pointWorld[1];
		p.z = pointWorld[2];
		pointCloud->points.push_back( p );
	    }
	    pcd_num++;
	    clock_start=clock();
	}
	clock_end=clock();
	if(state_pcd==1&&double(clock_end-clock_start)/ CLOCKS_PER_SEC>5){state_pcd=0;}
    }
    pointCloud->is_dense = false;
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (pointCloud);
    sor.setLeafSize (0.2, 0.2, 0.2);
    sor.filter (*pointCloud_filter);
    pcl::io::savePCDFileBinary("/home/eminbogen/my_ros/map.pcd", *pointCloud_filter );
}

int main(int argc, char **argv)
{ 
    ros::init(argc, argv, "pcd_get");
    ros::NodeHandle n;
    ROS_INFO("begin");
    ros::Subscriber odome_sub = n.subscribe("/integrated_to_init", 50, odomeCallback);
    ros::Subscriber cloud_sub = n.subscribe("/velodyne_cloud_3", 50, cloudCallback);
    boost::thread server(pcd_maker);
    ros::spin();
    return 0;
}

依次打开三个cmd运行。改路径！

roscore

rosrun rosbag_make pcd_get

rosbag play '/home/eminbogen/my_ros/2020-07-10-18-27-27.bag'

但这样数据会在保存前堆在内存里，如果不降采样，内存就很吃紧，跑不了大数据，也很难加入实时优化。