激光SLAM之NDT算法（2）-建图
最近的项目里用到了室外激光SLAM进行定位，所以我总结一下项目经验，顺便根据项目最终效果说一下自己对于算法源码中这段代码的理解。
首先，算法框架中用到了不止一种传感器，包括IMU，odometry和GPS等。所有的传感器数据都是为了能构造一个合理且较好的激光里程计。
这段代码很好理解，我大致讲一下流程吧，首先当第一帧点云输入时，我们构造一个位姿向量，x=0,y=0,z=0，旋转角初始化为0，存入map)中,并将其设置为ndt的target参数。当第二针点云进来时，进行点云匹配得到这堆点云的位姿（以lidar为坐标系），由于设置有shift值来判断这帧点云是否距上一帧点云位移了1m，所以我们会根据得到的位姿进行计算，如果小于1m,丢弃，用下一帧点云重复这个过程，如果大于1m，则存入map,并重置它为ndt的target参数，以此类推 ，对下一帧点云重复这个动作。代码如下：
void ndt_mapping::points_callback(const sensor_msgs::PointCloud2::ConstPtr& input)
{
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());
//static tf::TransformBroadcaster br;
tf::Transform transform;
pcl::fromROSMsg(*input, tmp);
double r;
Eigen::Vector3d point_pos;
pcl::PointXYZI p;
for (pcl::PointCloud<pcl::PointXYZI>::const_iterator item = tmp.begin(); item != tmp.end(); item++)
{
if(use_imu_){
// deskew(TODO:inplement of predicting pose by imu)
point_pos.x() = (double)item->x;
point_pos.y() = (double)item->y;
point_pos.z() = (double)item->z;
double s = scan_rate_ * (double(item->intensity) - int(item->intensity));
point_pos.x() -= s * current_pose_msg_.pose.position.x;//current_pose_imu_
point_pos.y() -= s * current_pose_msg_.pose.position.y;
point_pos.z() -= s * current_pose_msg_.pose.position.z;
Eigen::Quaterniond start_quat, end_quat, mid_quat;
mid_quat.setIdentity();
end_quat = Eigen::Quaterniond(
current_pose_msg_.pose.orientation.w,
current_pose_msg_.pose.orientation.x,
current_pose_msg_.pose.orientation.y,
current_pose_msg_.pose.orientation.z);
start_quat = mid_quat.slerp(s, end_quat);
point_pos = start_quat.conjugate() * start_quat * point_pos;
point_pos.x() += current_pose_msg_.pose.position.x;
point_pos.y() += current_pose_msg_.pose.position.y;
point_pos.z() += current_pose_msg_.pose.position.z;
p.x = point_pos.x();
p.y = point_pos.y();
p.z = point_pos.z();
}
else{
p.x = (double)item->x;
p.y = (double)item->y;
p.z = (double)item->z;
}
p.intensity = (double)item->intensity;
// minmax
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;
}
voxel_grid_filter_.setInputCloud(scan_ptr);
voxel_grid_filter_.filter(*filtered_scan_ptr);
ndt.setInputSource(filtered_scan_ptr);//设置配准的点云
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map_));
if (is_first_map_ == true){
ndt.setInputTarget(map_ptr);//设置点云配准目标
is_first_map_ = false;
}
Eigen::Translation3f init_translation(current_pose_.x, current_pose_.y, current_pose_.z);
Eigen::AngleAxisf init_rotation_x(current_pose_.roll, Eigen::Vector3f::UnitX());
Eigen::AngleAxisf init_rotation_y(current_pose_.pitch, Eigen::Vector3f::UnitY());
Eigen::AngleAxisf init_rotation_z(current_pose_.yaw, Eigen::Vector3f::UnitZ());
Eigen::Matrix4f init_guess =
(init_translation * init_rotation_z * init_rotation_y * init_rotation_x).matrix() * tf_btol_;
pcl::PointCloud<pcl::PointXYZI>::Ptr output_cloud(new pcl::PointCloud<pcl::PointXYZI>);
ndt.align(*output_cloud, init_guess);
t_localizer = ndt.getFinalTransformation();
t_base_link = t_localizer * tf_ltob_;
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);
tf::Matrix3x3 mat_b;
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 current_pose_.
current_pose_.x = t_base_link(0, 3);current_pose_.y = t_base_link(1, 3);current_pose_.z = t_base_link(2, 3);
mat_b.getRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw, 1);//mat2rpy
transform.setOrigin(tf::Vector3(current_pose_.x, current_pose_.y, current_pose_.z));
q.setRPY(current_pose_.roll, current_pose_.pitch, current_pose_.yaw); //q from rpy
transform.setRotation(q);//trans from q
br_.sendTransform(tf::StampedTransform(transform, input->header.stamp, "map", "base_link"));
double shift = sqrt(pow(current_pose_.x - previous_pose_.x, 2.0) + pow(current_pose_.y - previous_pose_.y, 2.0));
if (shift >= min_add_scan_shift_)
{
map_ += *transformed_scan_ptr;
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;
ndt.setInputTarget(map_ptr);
sensor_msgs::PointCloud2::Ptr map_msg_ptr(new sensor_msgs::PointCloud2);
pcl::toROSMsg(*map_ptr, *map_msg_ptr);
ndt_map_pub_.publish(*map_msg_ptr);
}
current_pose_msg_.header.frame_id = "map";
current_pose_msg_.header.stamp = input->header.stamp;
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_);
那么，效果如何呢？
这么来看地图构建的还不错，而且场地也很大，但是呢，这个图我并为构建完成，因为在后面的时候，出现了漂移，这个也是可以预期的，毕竟我只用了激光雷达，而且系统中并未有后端设计和回环检测，所以，地图飘是可以预见的
这段代码可以看出，这中构建激光里程计 方法其实更像是点云拼接。
如果不考虑回环检测和后端优化，我们大可通过加入其他传感器来减小累计误差，注意，我说的是减小，它本身的误差已经很大了，只是相对于只用激光雷达来说，能让有个大致的样子。
另外不得不说的是速度问题，由于我用的是pcl_ndt的，所以速度极慢。建图的准确度对与匹配定位来说，是有一定影响的，所以关于建图这一块，我个人的话，还是需要研究的，如果有新的进度会跟大家分享的，暂时这篇先到这里。