0. 前言
最近空闲时间在看点云地图的动态加载，这部分在自动驾驶领域是非常有必要的。由于点云地图的稠密性，导致我们在大场景中没办法一次性加载所有的地图，这就需要我们将地图切分成多个子地图。
1. NDT降维
建立好的点云文件中，有很多点是重合的，需要通过采用合适的downsample_resolution以减小点云文件体积，便于传输和加载，通常降采样后体积可以降到原来的一半以下。同时由于NDT的特性，降采样后并不会影响最终的匹配定位效果。
所以作为大地图我们需要先通过NDT对地图进行预处理，来减少频繁的IO操作并降低内存的占用。
/**初始化pose**/
curr_pose.reset(new geometry_msgs::PoseStamped());
curr_pose->pose.position.x = private_nh.param<float>("init_x", 0.0f);
curr_pose->pose.position.y = private_nh.param<float>("init_y", 0.0f);
curr_pose->pose.position.z = 0.0f;
/**加载全局地图并发布一次**/ // maybe add voxelgrid down sample
full_map.reset(new pcl::PointCloud<pcl::PointXYZI>());
pcl::io::loadPCDFile(globalmap_pcd, *full_map); //full_map指向全局地图
/** 地图下采样 **/
boost::shared_ptr <pcl::VoxelGrid<pcl::PointXYZI>> voxelgrid(new pcl::VoxelGrid<pcl::PointXYZI>());
voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
voxelgrid->setInputCloud(full_map);
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZI>());
voxelgrid->filter(*filtered);
full_map = filtered;
int points_num = 0;
int grid_num =full_map.size();
for (int i = 0; i < grid_num; i++)
{
if (full_map[i].cloud.points.size() > 0)
{
pcl::io::savePCDFileBinary(full_map[i].filename, full_map[i].cloud);
std::cout << "Wrote " << full_map[i].cloud.points.size() << " points to "
<< full_map[i].filename << "." << std::endl;
points_num += full_map[i].cloud.points.size();
}
}
write_csv(grids);
std::cout << "Total points num: " << points_num << " points." << std::endl;
2. PCD地图分割
这里的相关代码作者LitoNeo已经开源。这部分主要思路是去掉z轴，仅对平面x、y方向上的点云按自定义的grid size划分方形网格。在定位的时候根据当前的GPS位置，可实时加载所在区域的网格点云，大量減小內存占用。
点云地图网格划分是离线处理的过程，处理完成后，每一个网格PCD都按一定的格式命名，并且在csv文件中记录，方便后续按文件名加载。
部分csv文件命名:
50_-100_-150.pcd,-100,-150,0,-50,-100,0
50_-50_-150.pcd,-50,-150,0,0,-100,0
50_0_-150.pcd,0,-150,0,50,-100,0
50_50_-150.pcd,50,-150,0,100,-100,0
50_-100_-100.pcd,-100,-100,0,-50,-50,0
50_-50_-100.pcd,-50,-100,0,0,-50,0
50_0_-100.pcd,0,-100,0,50,-50,0
50_50_-100.pcd,50,-100,0,100,-50,0
50_100_-100.pcd,100,-100,0,150,-50,0
50_-100_-50.pcd,-100,-50,0,-50,0,0
50_-50_-50.pcd,-50,-50,0,0,0,0
50_0_-50.pcd,0,-50,0,50,0,0
50_50_-50.pcd,50,-50,0,100,0,0
50_100_-50.pcd,100,-50,0,150,0,0
50_150_-50.pcd,150,-50,0,200,0,0
50_-150_0.pcd,-150,0,0,-100,50,0
50_-100_0.pcd,-100,0,0,-50,50,0
自定义结构体
struct pcd_xyz_grid
{
std::string filename; //网格PCD文件名
std::string name;
int grid_id; //网格编号
int grid_id_x; //行号
int grid_id_y; //列号
int lower_bound_x; //x_min
int lower_bound_y; //y_min
int upper_bound_x; //x_max
int upper_bound_y; //y_max
pcl::PointCloud<pcl::PointXYZ> cloud;
};
具体分割代码实现
/** 加载原始点云地图 **/
PointCloud map;
PointCloud tmp;
for (int i = 0; i < files.size(); i++)
{
if (pcl::io::loadPCDFile<Point>(files[i], tmp) == -1)
{
std::cout << "Failed to load " << files[i] << "." << std::endl;
}
map += tmp;
std::cout << "Finished to load " << files[i] << "." << std::endl;
}
std::cout << "Finished to load all PCDs: " << map.size() << " points."
/** 考虑对整个点云地图文件的点在xy方向的最大最小值，并设定网格划分规则 **/
double min_x = 10000000000.0;
double max_x = -10000000000.0;
double min_y = 10000000000.0;
double max_y = -10000000000.0;
for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
{
if (p->x < min_x)
{
min_x = p->x;
}
if (p->x > max_x)
{
max_x = p->x;
}
if (p->y < min_y)
{
min_y = p->y;
}
if (p->y > max_y)
{
max_y = p->y;
}
}
/** 设定的grid_size对网格进行划分 **/
int div_x = (max_x_b - min_x_b) / grid_size;
int div_y = (max_y_b - min_y_b) / grid_size;
int grid_num = div_x * div_y;
/** 边界网格的xy方向上的坐标 **/
int min_x_b = grid_size * static_cast<int>(floor(min_x / grid_size));
int max_x_b = grid_size * static_cast<int>(floor(max_x / grid_size) + 1);
int min_y_b = grid_size * static_cast<int>(floor(min_y / grid_size));
int max_y_b = grid_size * static_cast<int>(floor(max_y / grid_size) + 1);
/**重新按网格序号组织点云 **/
for (PointCloud::const_iterator p = map.begin(); p != map.end(); p++)
{
int idx = static_cast<int>(floor((p->x - static_cast<float>(min_x_b)) / grid_size));
int idy = static_cast<int>(floor((p->y - static_cast<float>(min_y_b)) / grid_size));
int id = idy * div_x + idx;
const Point &tmp = *p;
grids[id].cloud.points.push_back(tmp);
}
/**一定格式定义文件名并保存 **/
std::vector<pcd_xyz_grid> grids(grid_num);
for (int y = 0; y < div_y; y++)
{
for (int x = 0; x < div_x; x++)
{
int id = div_x * y + x;
grids[id].grid_id = id; //序号
grids[id].grid_id_x = x; //行号
grids[id].grid_id_y = y; //列号
grids[id].lower_bound_x = min_x_b + grid_size * x; //方格的四个顶点
grids[id].lower_bound_y = min_y_b + grid_size * y;
grids[id].upper_bound_x = min_x_b + grid_size * (x + 1);
grids[id].upper_bound_y = min_y_b + grid_size * (y + 1);
grids[id].filename = OUT_DIR + std::to_string(grid_size) + "_" +
std::to_string(grids[id].lower_bound_x) + "_" +
std::to_string(grids[id].lower_bound_y) + ".pcd";
grids[id].name = std::to_string(grid_size) + "_" +
std::to_string(grids[id].lower_bound_x) + "_" +
std::to_string(grids[id].lower_bound_y) + ".pcd";
}
}
int points_num = 0;
for (int i = 0; i < grid_num; i++)
{
if (grids[i].cloud.points.size() > 0)
{
pcl::io::savePCDFileBinary(grids[i].filename, grids[i].cloud);
std::cout << "Wrote " << grids[i].cloud.points.size() << " points to "
<< grids[i].filename << "." << std::endl;
points_num += grids[i].cloud.points.size();
}
}
write_csv(grids);
std::cout << "Total points num: " << points_num << " points." << std::endl;
void write_csv(std::vector<pcd_xyz_grid> &grids)
{
std::string whole_file_name = OUT_DIR + FILE_NAME;
std::ofstream ofs(whole_file_name.c_str());
int grid_num = grids.size();
for (int i = 0; i < grid_num; i++)
{
if (grids[i].cloud.points.size() > 0)
{
ofs << grids[i].name
<< "," << grids[i].lower_bound_x
<< "," << grids[i].lower_bound_y
<< "," << 0.0
<< "," << grids[i].upper_bound_x
<< "," << grids[i].upper_bound_y
<< "," << 0.0 << std::endl;
}
}
}
3.动态地图加载
/**读网格点云地图，并记录文件名**/
constexpr double MARGIN_UNIT = 100; // meter
gps_tools_.lla_origin_ << origin_latitude, origin_longitude, origin_altitude;//地图起点，后续lla转xyz
if (grid_size == "noupdate") //是否动态更新
margin = -1;
else if (grid_size == "1x1")
margin = 0;
else if (grid_size == "3x3")
margin = MARGIN_UNIT * 1;
else if (grid_size == "5x5")
margin = MARGIN_UNIT * 2;
else if (grid_size == "7x7")
margin = MARGIN_UNIT * 3;
else if (grid_size == "9x9")
margin = MARGIN_UNIT * 4;
else {
std::cout << "grid_size 有误..." << std::endl;
return EXIT_FAILURE;
}
std::string front_path;
getAllFiles(area_directory, pcd_paths); //获取pcd文件
if (pcd_paths.size() == 0) {
return EXIT_FAILURE;
}
int pos = pcd_paths[0].find_last_of('/');//获取路径前缀
std::string path_name(pcd_paths[0].substr(0, pos + 1));
front_path = path_name;
if (margin < 0) {
can_download = false; //不在线下载
} else {
can_download = false; //分块更新
for (const std::string &path : pcd_paths) {
int pos = path.find_last_of('/');
std::string file_name(path.substr(pos + 1));
pcd_names.push_back(file_name);
}
}
/**校验csv文件中读取到的pcd文件是否存在**/
string arealist_path = "*****";//csv 路径
if (margin < 0) {
int err = 0;
publish_pcd(create_pcd(pcd_paths, &err), &err); //不分块
} else {
std::cout << "can_download... " << std::endl;
n.param<int>("update_rate", update_rate, DEFAULT_UPDATE_RATE);
fallback_rate = update_rate * 2;
gnss_sub = n.subscribe("pos", 5, publish_gnss_pcd); //有更新
if (can_download) {
AreaList areas = read_arealist(arealist_path); //读取csv记录的pcd文件
for (const Area &area : areas) {
for (const std::string &path : pcd_names) {
if (path == area.path) {
// 将csv记录的并且文件夹中有的pcd文件，放进downloaded_areas中
cache_arealist(area, downloaded_areas);
}
}
}
gnss_time = current_time = ros::Time::now();//当前时间，以gnss为准
}
/**读取csv文件，并查找**/
struct Area {
std::string path;
double x_min;
double y_min;
double z_min;
double x_max;
double y_max;
double z_max;
};
typedef std::vector<Area> AreaList;
typedef std::vector<std::vector<std::string>> Tbl;
AreaList read_arealist(const std::string &path) {
Tbl tbl = read_csv(path); //逐行读取
AreaList ret; //用定义的area重新封装
for (const std::vector<std::string> &cols : tbl) {
Area area;
area.path = cols[0];
area.x_min = std::stod(cols[1]);
area.y_min = std::stod(cols[2]);
area.z_min = std::stod(cols[3]);
area.x_max = std::stod(cols[4]);
area.y_max = std::stod(cols[5]);
area.z_max = std::stod(cols[6]);
ret.push_back(area);
}
return ret;
}
Tbl read_csv(const std::string &path) {//逐行读取csv文件
std::ifstream ifs(path.c_str());
std::string line;
Tbl ret;
while (std::getline(ifs, line)) {
std::istringstream iss(line);
std::string col;
std::vector<std::string> cols;
while (std::getline(iss, col, ','))
cols.push_back(col);
ret.push_back(cols);
}
return ret;
}
void cache_arealist(const Area &area, AreaList &areas) {
for (const Area &a : areas) {//没有的话加入
if (a.path == area.path)
return;
}
areas.push_back(area);
}
gps位置更新，进行坐标转换
void publish_gnss_pcd(const sensor_msgs::NavSatFixPtr &gps_msg) {
if (std::isnan(gps_msg->latitude + gps_msg->longitude + gps_msg->altitude)) {
ROS_INFO("GPS LLA NAN...");
return;
}
if (gps_msg->status.status == 4 || gps_msg->status.status == 5 || gps_msg->status.status == 1 ||
gps_msg->status.status == 2) {
ros::Time now = ros::Time::now();//注意更新频率是否符合预定要求
if (((now - current_time).toSec() * 1000) < fallback_rate)
return;
if (((now - gnss_time).toSec() * 1000) < update_rate)
return;
gnss_time = now;
Eigen::Vector3d lla = gps_tools_.GpsMsg2Eigen(*gps_msg);
Eigen::Vector3d ecef = gps_tools_.LLA2ECEF(lla);
Eigen::Vector3d enu = gps_tools_.ECEF2ENU(ecef);
gps_tools_.gps_pos_ = enu;
gps_pos_ = enu;
geometry_msgs::Point pose;
pose.x = gps_pos_(0);
pose.y = gps_pos_(1);
pose.z = gps_pos_(2);
std::cout << "area lla : " << gps_pos_(0) << ", " << gps_pos_(1) << ", " << gps_pos_(2)<< std::endl;
publish_pcd(create_pcd(pose)); //pub当前的网格
}
根据位置去查询相应的网格地图
sensor_msgs::PointCloud2 create_pcd(const geometry_msgs::Point &p) {
sensor_msgs::PointCloud2 pcd, part;
std::unique_lock<std::mutex> lock(downloaded_areas_mtx);
for (const Area &area : downloaded_areas) {//遍历一下
if (is_in_area(p.x, p.y, area, margin)) { //判断当前位置在哪些网格里面
std::string pcd_name = front_path + area.path;//实际的PCD文件路径
if (pcd.width == 0)
pcl::io::loadPCDFile(pcd_name.c_str(), pcd);
else {
std::cout << "success load: " << area.path << std::endl;
pcl::io::loadPCDFile(pcd_name.c_str(), pcd);
pcd.width += part.width; //所有符合条件的网格全pub出来
pcd.row_step += part.row_step;
pcd.data.insert(pcd.data.end(), part.data.begin(), part.data.end());
}
}
}
return pcd;
}
bool is_in_area(double x, double y, const Area &area, double m) {
return ((area.x_min - m) <= x && x <= (area.x_max + m) && (area.y_min - m) <= y && y <= (area.y_max + m));
}
4. 获取所需匹配范围
到这一步，我们已经拿到了大概区域的地图，下一步则是需要通过NDT匹配来实现车辆的定位。
void gnss_callback(const sensor_msgs::NavSatFixPtr &gps_msg) {
if (std::isnan(gps_msg->latitude + gps_msg->longitude + gps_msg->altitude)) {
ROS_INFO("GPS LLA NAN...");
return;
}
gpsTools gpsTools;
gpsTools.lla_origin_ << origin_latitude, origin_longitude, origin_altitude;
if (gps_msg->status.status == 4 || gps_msg->status.status == 5 || gps_msg->status.status == 1 ||
gps_msg->status.status == 2) {
// 经纬转xy
Eigen::Vector3d lla = gpsTools.GpsMsg2Eigen(*gps_msg);
Eigen::Vector3d ecef = gpsTools.LLA2ECEF(lla);
Eigen::Vector3d enu = gpsTools.ECEF2ENU(ecef);
gpsTools.gps_pos_ = enu;
// 更新当前pose
gps_pos_ = enu;
has_pos_ = true;
curr_pose->pose.position.x = gps_pos_(0);
curr_pose->pose.position.y = gps_pos_(1);
curr_pose->pose.position.z = gps_pos_(2);
sensor_msgs::PointCloud2 full_map = create_pcd(curr_pose);// 加载当前位置的局部地图
}
}
kdtree.setInputCloud(full_map);// 用kdtree索引
clock_t start = clock();
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
pcl::PointXYZI searchPoint;
searchPoint.x = curr_pose->pose.position.x;// 近邻搜索
searchPoint.y = curr_pose->pose.position.y;
searchPoint.z = curr_pose->pose.position.z;
pcl::PointCloud<pcl::PointXYZI>::Ptr trimmed_cloud(new pcl::PointCloud <pcl::PointXYZI>);
float z_min_threshold = lidar_height + trim_low; // 取指定高度区间的点云
float z_max_threshold = lidar_height + trim_high;
// 搜索当前位置x， 100m半斤范围内的点云，id存在pointIdxRadiusSearch中
if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) {
trimmed_cloud->points.reserve(60000);
for (int i : pointIdxRadiusSearch) {
if (full_map->points[i].z > z_min_threshold && full_map->points[i].z < z_max_threshold) {
pcl::PointXYZI cpt;
cpt.x = full_map->points[i].x;
cpt.y = full_map->points[i].y;
cpt.z = full_map->points[i].z;
cpt.intensity = full_map->points[i].intensity;
trimmed_cloud->points.push_back(cpt);
}
}
trimmed_cloud->width = trimmed_cloud->points.size(); // 点云数量
trimmed_cloud->height = 1;
cout << "full_map.size()\t:" << full_map->size() << endl << "trimmed_cloud->width:\t"
<< trimmed_cloud->width << endl;
}
trimmed_cloud->header.frame_id = "map";
pcl_conversions::toPCL(curr_pose->header.stamp, trimmed_cloud->header.stamp);
localmap_pub.publish(trimmed_cloud); // pub当前的map
NODELET_INFO(" local map updated on x:%f, y:%f, z:%f", searchPoint.x, searchPoint.y, searchPoint.z);
clock_t end = clock();
NODELET_INFO("trim time = %f seconds", (double) (end - start) / CLOCKS_PER_SEC);
下面将扫到的激光点与提取出来的点云地图进行NDT/ICP匹配即可得到两者的R,t。
void points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
double r;
pcl::PointXYZI p;
pcl::PointCloud<pcl::PointXYZI> tmp, scan;
pcl::PointCloud<pcl::PointXYZI>::Ptr filtered_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
pcl::PointCloud<pcl::PointXYZI>::Ptr transformed_scan_ptr(new pcl::PointCloud<pcl::PointXYZI>());
tf::Quaternion q;
Eigen::Matrix4f t_localizer(Eigen::Matrix4f::Identity());
Eigen::Matrix4f t_base_link(Eigen::Matrix4f::Identity());
tf::TransformBroadcaster br;
tf::Transform transform;
current_scan_time = input->header.stamp;
pcl::fromROSMsg(*input, tmp);
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = tmp.begin(); item != tmp.end(); item++)
{
p.x = (double)item->x;
p.y = (double)item->y;
p.z = (double)item->z;
p.intensity = (double)item->intensity;
r = sqrt(pow(p.x, 2.0) + pow(p.y, 2.0));
if (min_scan_range < r && r < max_scan_range)
{
scan.push_back(p);
}
}
pcl::PointCloud<pcl::PointXYZI>::Ptr scan_ptr(new pcl::PointCloud<pcl::PointXYZI>(scan));
// Add initial point cloud to velodyne_map
if (initial_scan_loaded == 0)
{
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, tf_btol_);
map += *transformed_scan_ptr;
initial_scan_loaded = 1;
}
// Apply voxelgrid filter
pcl::VoxelGrid<pcl::PointXYZI> voxel_grid_filter_;
voxel_grid_filter_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
voxel_grid_filter_.setInputCloud(scan_ptr);
voxel_grid_filter_.filter(*filtered_scan_ptr);
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map));
ndt.setTransformationEpsilon(trans_eps_);
ndt.setStepSize(step_size_);
ndt.setResolution(ndt_res_);
ndt.setMaximumIterations(max_iter_);
ndt.setInputSource(filtered_scan_ptr);
if (isMapUpdate == true)
{
ndt.setInputTarget(map_ptr);
isMapUpdate = false;
}
guess_pose.x = previous_pose_.x + diff_x;
guess_pose.y = previous_pose_.y + diff_y;
guess_pose.z = previous_pose_.z + diff_z;
guess_pose.roll = previous_pose_.roll;
guess_pose.pitch = previous_pose_.pitch;
guess_pose.yaw = previous_pose_.yaw + diff_yaw;
pose guess_pose_for_ndt;
guess_pose_for_ndt = guess_pose;
Eigen::AngleAxisf init_rotation_x(guess_pose_for_ndt.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(guess_pose_for_ndt.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(guess_pose_for_ndt.yaw, Eigen::Vector3f::UnitZ());
Eigen::Translation3f init_translation(guess_pose_for_ndt.x, guess_pose_for_ndt.y, guess_pose_for_ndt.z);
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_;
t3_end = ros::Time::now();
d3 = t3_end - t3_start;
t4_start = ros::Time::now();
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess);
fitness_score = ndt.getFitnessScore();
t_localizer = ndt.getFinalTransformation();
t_base_link = t_localizer * tf_ltob_;
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);
tf::Matrix3x3 mat_l, mat_b;
mat_l.setValue(static_cast<double>(t_localizer(0, 0)), static_cast<double>(t_localizer(0, 1)),
static_cast<double>(t_localizer(0, 2)), static_cast<double>(t_localizer(1, 0)),
static_cast<double>(t_localizer(1, 1)), static_cast<double>(t_localizer(1, 2)),
static_cast<double>(t_localizer(2, 0)), static_cast<double>(t_localizer(2, 1)),
static_cast<double>(t_localizer(2, 2)));
mat_b.setValue(static_cast<double>(t_base_link(0, 0)), static_cast<double>(t_base_link(0, 1)),
static_cast<double>(t_base_link(0, 2)), static_cast<double>(t_base_link(1, 0)),
static_cast<double>(t_base_link(1, 1)), static_cast<double>(t_base_link(1, 2)),
static_cast<double>(t_base_link(2, 0)), static_cast<double>(t_base_link(2, 1)),
static_cast<double>(t_base_link(2, 2)));
// Update localizer_pose.
localizer_pose.x = t_localizer(0, 3);
localizer_pose.y = t_localizer(1, 3);
localizer_pose.z = t_localizer(2, 3);
mat_l.getRPY(localizer_pose.roll, localizer_pose.pitch, localizer_pose.yaw, 1);
// Update ndt_pose.
ndt_pose.x = t_base_link(0, 3);
ndt_pose.y = t_base_link(1, 3);
ndt_pose.z = t_base_link(2, 3);
mat_b.getRPY(ndt_pose.roll, ndt_pose.pitch, ndt_pose.yaw, 1);
current_pose_.x = ndt_pose.x;
current_pose_.y = ndt_pose.y;
current_pose_.z = ndt_pose.z;
current_pose_.roll = ndt_pose.roll;
current_pose_.pitch = ndt_pose.pitch;
current_pose_.yaw = ndt_pose.yaw;
transform.setOrigin(tf::Vector3(current_pose_.x, current_pose_.y, current_pose_.z));
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw);
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, current_scan_time, "map", "base_link"));
scan_duration = current_scan_time - previous_scan_time;
double secs = scan_duration.toSec();
// Calculate the offset (curren_pos - previous_pos)
diff_x = current_pose_.x - previous_pose_.x;
diff_y = current_pose_.y - previous_pose_.y;
diff_z = current_pose_.z - previous_pose_.z;
diff_yaw = current_pose_.yaw - previous_pose_.yaw;
diff = sqrt(diff_x * diff_x + diff_y * diff_y + diff_z * diff_z);
current_velocity_x = diff_x / secs;
current_velocity_y = diff_y / secs;
current_velocity_z = diff_z / secs;
// Update position and posture. current_pos -> previous_pos
previous_pose_.x = current_pose_.x;
previous_pose_.y = current_pose_.y;
previous_pose_.z = current_pose_.z;
previous_pose_.roll = current_pose_.roll;
previous_pose_.pitch = current_pose_.pitch;
previous_pose_.yaw = current_pose_.yaw;
previous_scan_time.sec = current_scan_time.sec;
previous_scan_time.nsec = current_scan_time.nsec;
// Calculate the shift between added_pos and current_pos
double shift = sqrt(pow(current_pose_.x - added_pose.x, 2.0) + pow(current_pose_.y - added_pose.y, 2.0));
if (shift >= min_add_scan_shift_)
{
submap_size += shift;
map += *transformed_scan_ptr;
submap += *transformed_scan_ptr;
added_pose.x = current_pose_.x;
added_pose.y = current_pose_.y;
added_pose.z = current_pose_.z;
added_pose.roll = current_pose_.roll;
added_pose.pitch = current_pose_.pitch;
added_pose.yaw = current_pose_.yaw;
isMapUpdate = true;
}
sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(submap, *map_msg_ptr);
map_msg_ptr->header.frame_id = "map";
ndt_map_pub_.publish(*map_msg_ptr);
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw);
current_pose__msg_.header.frame_id = "map";
current_pose__msg_.header.stamp = current_scan_time;
current_pose__msg_.pose.position.x = current_pose_.x;
current_pose__msg_.pose.position.y = current_pose_.y;
current_pose__msg_.pose.position.z = current_pose_.z;
current_pose__msg_.pose.orientation.x = q.x();
current_pose__msg_.pose.orientation.y = q.y();
current_pose__msg_.pose.orientation.z = q.z();
current_pose__msg_.pose.orientation.w = q.w();
current_pose__pub_.publish(current_pose__msg_);
if (submap_size >= max_submap_size)
{
std::string s1 = "submap_";
std::string s2 = std::to_string(submap_num);
std::string s3 = ".pcd";
std::string pcd_filename = s1 + s2 + s3;
if (submap.size() != 0)
{
if (pcl::io::savePCDFileBinary(pcd_filename, submap) == -1)
{
std::cout << "Failed saving " << pcd_filename << "." << std::endl;
}
std::cout << "Saved " << pcd_filename << " (" << submap.size() << " points)" << std::endl;
map = submap;
submap.clear();
submap_size = 0.0;
}
submap_num++;
}
// Write log
if (!ofs)
{
std::cerr << "Could not open " << filename << "." << std::endl;
exit(1);
}
ofs << input->header.seq << ","
<< input->header.stamp << ","
<< input->header.frame_id << ","
<< scan_ptr->size() << ","
<< filtered_scan_ptr->size() << ","
<< std::fixed << std::setprecision(5) << current_pose_.x << ","
<< std::fixed << std::setprecision(5) << current_pose_.y << ","
<< std::fixed << std::setprecision(5) << current_pose_.z << ","
<< current_pose_.roll << ","
<< current_pose_.pitch << ","
<< current_pose_.yaw << ","
<< ndt_res_ << ","
<< step_size_ << ","
<< trans_eps_ << ","
<< max_iter_ << ","
<< voxel_leaf_size_ << ","
<< min_scan_range << ","
<< max_scan_range << ","
<< min_add_scan_shift_ << ","
<< max_submap_size << std::endl;
std::cout << "-----------------------------------------------------------------" << std::endl;
std::cout << "Sequence number: " << input->header.seq << std::endl;
std::cout << "Number of scan points: " << scan_ptr->size() << " points." << std::endl;
std::cout << "Number of filtered scan points: " << filtered_scan_ptr->size() << " points." << std::endl;
std::cout << "transformed_scan_ptr: " << transformed_scan_ptr->points.size() << " points." << std::endl;
std::cout << "map: " << map.points.size() << " points." << std::endl;
std::cout << "NDT has converged: " << ndt.hasConverged() << std::endl;
std::cout << "Fitness score: " << fitness_score << std::endl;
std::cout << "Number of iteration: " << ndt.getFinalNumIteration() << std::endl;
std::cout << "(x,y,z,roll,pitch,yaw):" << std::endl;
std::cout << "(" << current_pose_.x << ", " << current_pose_.y << ", " << current_pose_.z << ", " << current_pose_.roll
<< ", " << current_pose_.pitch << ", " << current_pose_.yaw << ")" << std::endl;
std::cout << "Transformation Matrix:" << std::endl;
std::cout << t_localizer << std::endl;
std::cout << "shift: " << shift << std::endl;
std::cout << "current submap size: " << submap_size << std::endl;
std::cout << "-----------------------------------------------------------------" << std::endl;
}
5. 参考链接
https://zhuanlan.zhihu.com/p/77745476?from_voters_page=true
http://xchu.net/2019/10/30/32pcd-divider/
评论（1）
您还未登录，请登录后发表或查看评论