最近项目比较忙没空更新博客,发现问保存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
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
//计时
clock_t clock_start, clock_end;
std::vector all_pose;
std::vector all_pose_stamp;
std::vector> all_points;
std::vector 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_cloud_temp;
pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
std::vector cloud_vec;
for(int i=0;i::Ptr pointCloud( new pcl::PointCloud );
pcl::PointCloud::Ptr pointCloud_filter( new pcl::PointCloud );
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_numpoints.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 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'
但这样数据会在保存前堆在内存里,如果不降采样,内存就很吃紧,跑不了大数据,也很难加入实时优化。