一. 模块介绍
ndt_mapping模块主要负责基于传感器输入的点云、imu、里程计等信息，基于ndt匹配优化算法，建立点云地图。
二. 源码解析
关键输入：
ros::Subscriber points_sub = nh.subscribe("points_raw", 100000, points_callback);
ros::Subscriber imu_sub = nh.subscribe(_imu_topic, 100000, imu_callback);
关键输出：
ndt_map_pub = nh.advertise<sensor_msgs::PointCloud2>("/ndt_map", 1000);
current_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/current_pose", 1000);
ndt_mapping主要回调函数points_callback
static 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());
static tf::TransformBroadcaster br;
tf::Transform transform;
current_scan_time = input->header.stamp;
pcl::fromROSMsg(*input, tmp);
// 点云过滤min_scan_range最小范围max_scan_range最大范围
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
// 第一帧scan输入，会将其当成直接插入到map中，作为组成map的第一帧子图，将雷达坐标系转到base_link坐标系下
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);
//note-tianyu 根据对ndt优化的相关参数进行配置
pcl::PointCloud<pcl::PointXYZI>::Ptr map_ptr(new pcl::PointCloud<pcl::PointXYZI>(map));
if (_method_type == MethodType::PCL_GENERIC) //调用pcl库
{
ndt.setTransformationEpsilon(trans_eps);
ndt.setStepSize(step_size);
ndt.setResolution(ndt_res);
ndt.setMaximumIterations(max_iter);
ndt.setInputSource(filtered_scan_ptr);
}
else if (_method_type == MethodType::PCL_ANH) //cpu ndt
{
anh_ndt.setTransformationEpsilon(trans_eps);
anh_ndt.setStepSize(step_size);
anh_ndt.setResolution(ndt_res);
anh_ndt.setMaximumIterations(max_iter);
anh_ndt.setInputSource(filtered_scan_ptr);
}
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU) //gpu ndt
{
anh_gpu_ndt.setTransformationEpsilon(trans_eps);
anh_gpu_ndt.setStepSize(step_size);
anh_gpu_ndt.setResolution(ndt_res);
anh_gpu_ndt.setMaximumIterations(max_iter);
anh_gpu_ndt.setInputSource(filtered_scan_ptr);
}
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP) //pcl_OPENMP
{
omp_ndt.setTransformationEpsilon(trans_eps);//迭代次数
omp_ndt.setStepSize(step_size);//计算优化的步长
omp_ndt.setResolution(ndt_res);//残差取多少
omp_ndt.setMaximumIterations(max_iter);//最大迭代次数
omp_ndt.setInputSource(filtered_scan_ptr);//找输入点云在地图中的匹配
}
#endif
static bool is_first_map = true;
//是否为第一张地图，是则新建setInputTarget
if (is_first_map == true)
{
if (_method_type == MethodType::PCL_GENERIC)
ndt.setInputTarget(map_ptr);
else if (_method_type == MethodType::PCL_ANH)
anh_ndt.setInputTarget(map_ptr);
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt.setInputTarget(map_ptr);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setInputTarget(map_ptr);
#endif
is_first_map = 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;
if (_use_imu == true && _use_odom == true)
imu_odom_calc(current_scan_time);
if (_use_imu == true && _use_odom == false)
imu_calc(current_scan_time);
if (_use_imu == false && _use_odom == true)
odom_calc(current_scan_time);
// 根据imu和odo等传感器的配置情况来定义guess_pose_for_ndt
pose guess_pose_for_ndt;
if (_use_imu == true && _use_odom == true)
guess_pose_for_ndt = guess_pose_imu_odom;
else if (_use_imu == true && _use_odom == false)
guess_pose_for_ndt = guess_pose_imu;
else if (_use_imu == false && _use_odom == true)
guess_pose_for_ndt = guess_pose_odom;
else
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);
// ndt优化的初始值预测
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>);
if (_method_type == MethodType::PCL_GENERIC)
{
ndt.align(*output_cloud, init_guess);// 由此进入正式的ndt算法运算
fitness_score = ndt.getFitnessScore();//匹配程度的打分
t_localizer = ndt.getFinalTransformation();//雷达坐标系在map坐标系的坐标变换
has_converged = ndt.hasConverged();//是否收敛
final_num_iteration = ndt.getFinalNumIteration();//迭代次数
transformation_probability = ndt.getTransformationProbability();//置信度
}
else if (_method_type == MethodType::PCL_ANH)
{
anh_ndt.align(init_guess);
fitness_score = anh_ndt.getFitnessScore();
t_localizer = anh_ndt.getFinalTransformation();
has_converged = anh_ndt.hasConverged();
final_num_iteration = anh_ndt.getFinalNumIteration();
}
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
{
anh_gpu_ndt.align(init_guess);
fitness_score = anh_gpu_ndt.getFitnessScore();
t_localizer = anh_gpu_ndt.getFinalTransformation();
has_converged = anh_gpu_ndt.hasConverged();
final_num_iteration = anh_gpu_ndt.getFinalNumIteration();
}
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
{
omp_ndt.align(*output_cloud, init_guess);
fitness_score = omp_ndt.getFitnessScore();
t_localizer = omp_ndt.getFinalTransformation();
has_converged = omp_ndt.hasConverged();
final_num_iteration = omp_ndt.getFinalNumIteration();
}
#endif
t_base_link = t_localizer * tf_ltob;// 坐标转换
// 根据ndt匹配优化结果将scan的坐标由lidar坐标系换算到map坐标系下
pcl::transformPointCloud(*scan_ptr, *transformed_scan_ptr, t_localizer);
tf::Matrix3x3 mat_l, mat_b;
// 提取出两个R
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为地图坐标系到车身坐标系的位姿
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)
// 计算前后两帧pose的diff量
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 = calcDiffForRadian(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;
current_pose_imu.x = current_pose.x;
current_pose_imu.y = current_pose.y;
current_pose_imu.z = current_pose.z;
current_pose_imu.roll = current_pose.roll;
current_pose_imu.pitch = current_pose.pitch;
current_pose_imu.yaw = current_pose.yaw;
current_pose_odom.x = current_pose.x;
current_pose_odom.y = current_pose.y;
current_pose_odom.z = current_pose.z;
current_pose_odom.roll = current_pose.roll;
current_pose_odom.pitch = current_pose.pitch;
current_pose_odom.yaw = current_pose.yaw;
current_pose_imu_odom.x = current_pose.x;
current_pose_imu_odom.y = current_pose.y;
current_pose_imu_odom.z = current_pose.z;
current_pose_imu_odom.roll = current_pose.roll;
current_pose_imu_odom.pitch = current_pose.pitch;
current_pose_imu_odom.yaw = current_pose.yaw;
current_velocity_imu_x = current_velocity_x;
current_velocity_imu_y = current_velocity_y;
current_velocity_imu_z = current_velocity_z;
// 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;
offset_imu_x = 0.0;
offset_imu_y = 0.0;
offset_imu_z = 0.0;
offset_imu_roll = 0.0;
offset_imu_pitch = 0.0;
offset_imu_yaw = 0.0;
offset_odom_x = 0.0;
offset_odom_y = 0.0;
offset_odom_z = 0.0;
offset_odom_roll = 0.0;
offset_odom_pitch = 0.0;
offset_odom_yaw = 0.0;
offset_imu_odom_x = 0.0;
offset_imu_odom_y = 0.0;
offset_imu_odom_z = 0.0;
offset_imu_odom_roll = 0.0;
offset_imu_odom_pitch = 0.0;
offset_imu_odom_yaw = 0.0;
// Calculate the shift between added_pos and current_pos
// 计算两frame之间的距离间隔，当大于一定阈值时，才像向地图中插入新的点云（子地图）
double shift = sqrt(pow(current_pose.x - added_pose.x, 2.0) + pow(current_pose.y - added_pose.y, 2.0));
//点云插入阈值min_add_scan_shift
if (shift >= min_add_scan_shift)
{
map += *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;
if (_method_type == MethodType::PCL_GENERIC)
ndt.setInputTarget(map_ptr); //note-tianyu
else if (_method_type == MethodType::PCL_ANH)
{
if (_incremental_voxel_update == true)
anh_ndt.updateVoxelGrid(transformed_scan_ptr);
else
anh_ndt.setInputTarget(map_ptr);
}
#ifdef CUDA_FOUND
else if (_method_type == MethodType::PCL_ANH_GPU)
anh_gpu_ndt.setInputTarget(map_ptr);
#endif
#ifdef USE_PCL_OPENMP
else if (_method_type == MethodType::PCL_OPENMP)
omp_ndt.setInputTarget(map_ptr);
#endif
}
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);
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);
// 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 << ","
<< final_num_iteration << ","
<< fitness_score << ","
<< ndt_res << ","
<< step_size << ","
<< trans_eps << ","
<< max_iter << ","
<< voxel_leaf_size << ","
<< min_scan_range << ","
<< max_scan_range << ","
<< min_add_scan_shift << 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: " << has_converged << std::endl;
std::cout << "Fitness score: " << fitness_score << std::endl;
std::cout << "Number of iteration: " << final_num_iteration << 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 << "-----------------------------------------------------------------" << std::endl;
}
119行imu_calc使用了imu建图 源码解析
static void imu_calc(ros::Time current_time)
{
static ros::Time previous_time = current_time;
double diff_time = (current_time - previous_time).toSec();
//diff_imu_roll 欧拉角的变化=角速度 * △t
double diff_imu_roll = imu.angular_velocity.x * diff_time;
double diff_imu_pitch = imu.angular_velocity.y * diff_time;
double diff_imu_yaw = imu.angular_velocity.z * diff_time;
current_pose_imu.roll += diff_imu_roll;
current_pose_imu.pitch += diff_imu_pitch;
current_pose_imu.yaw += diff_imu_yaw;
// 计算imu在的线加速度在地图坐标系下xyz方向的线加速度分量 l_acc_map = RzRyRx * l_acc_imu
//计算公式参考roll、pitch和yaw的旋转矩阵
double accX1 = imu.linear_acceleration.x;
double accY1 = std::cos(current_pose_imu.roll) * imu.linear_acceleration.y -
std::sin(current_pose_imu.roll) * imu.linear_acceleration.z;
double accZ1 = std::sin(current_pose_imu.roll) * imu.linear_acceleration.y +
std::cos(current_pose_imu.roll) * imu.linear_acceleration.z;
double accX2 = std::sin(current_pose_imu.pitch) * accZ1 + std::cos(current_pose_imu.pitch) * accX1;
double accY2 = accY1;
double accZ2 = std::cos(current_pose_imu.pitch) * accZ1 - std::sin(current_pose_imu.pitch) * accX1;
double accX = std::cos(current_pose_imu.yaw) * accX2 - std::sin(current_pose_imu.yaw) * accY2;
double accY = std::sin(current_pose_imu.yaw) * accX2 + std::cos(current_pose_imu.yaw) * accY2;
double accZ = accZ2;
// 高中物理公式s = v0*t + (a*t^2)/2
offset_imu_x += current_velocity_imu_x * diff_time + accX * diff_time * diff_time / 2.0;
offset_imu_y += current_velocity_imu_y * diff_time + accY * diff_time * diff_time / 2.0;
offset_imu_z += current_velocity_imu_z * diff_time + accZ * diff_time * diff_time / 2.0;
current_velocity_imu_x += accX * diff_time;
current_velocity_imu_y += accY * diff_time;
current_velocity_imu_z += accZ * diff_time;
offset_imu_roll += diff_imu_roll;
offset_imu_pitch += diff_imu_pitch;
offset_imu_yaw += diff_imu_yaw;
guess_pose_imu.x = previous_pose.x + offset_imu_x;
guess_pose_imu.y = previous_pose.y + offset_imu_y;
guess_pose_imu.z = previous_pose.z + offset_imu_z;
guess_pose_imu.roll = previous_pose.roll + offset_imu_roll;
guess_pose_imu.pitch = previous_pose.pitch + offset_imu_pitch;
guess_pose_imu.yaw = previous_pose.yaw + offset_imu_yaw;
previous_time = current_time;
}
imu_callback回调函数
static void imu_callback(const sensor_msgs::Imu::Ptr& input)
{
// std::cout << __func__ << std::endl;
// imu是否需要调整方向，有时候可能涉及到imu正装倒装的问题
if (_imu_upside_down)
imuUpsideDown(input);
const ros::Time current_time = input->header.stamp;
static ros::Time previous_time = current_time;
const double diff_time = (current_time - previous_time).toSec();
// 将四元数转成欧拉角
double imu_roll, imu_pitch, imu_yaw;
tf::Quaternion imu_orientation;
tf::quaternionMsgToTF(input->orientation, imu_orientation);
tf::Matrix3x3(imu_orientation).getRPY(imu_roll, imu_pitch, imu_yaw);
// 限制欧拉角大小不超过M_PI
imu_roll = wrapToPmPi(imu_roll);
imu_pitch = wrapToPmPi(imu_pitch);
imu_yaw = wrapToPmPi(imu_yaw);
static double previous_imu_roll = imu_roll, previous_imu_pitch = imu_pitch, previous_imu_yaw = imu_yaw;
const double diff_imu_roll = calcDiffForRadian(imu_roll, previous_imu_roll);
const double diff_imu_pitch = calcDiffForRadian(imu_pitch, previous_imu_pitch);
const double diff_imu_yaw = calcDiffForRadian(imu_yaw, previous_imu_yaw);
imu.header = input->header;
imu.linear_acceleration.x = input->linear_acceleration.x;
// imu.linear_acceleration.y = input->linear_acceleration.y;
// imu.linear_acceleration.z = input->linear_acceleration.z;
imu.linear_acceleration.y = 0;
imu.linear_acceleration.z = 0;
// 根据欧拉角变换来更新角速度, 不相信imu输入的角速度值
if (diff_time != 0)
{
imu.angular_velocity.x = diff_imu_roll / diff_time;
imu.angular_velocity.y = diff_imu_pitch / diff_time;
imu.angular_velocity.z = diff_imu_yaw / diff_time;
}
else
{
imu.angular_velocity.x = 0;
imu.angular_velocity.y = 0;
imu.angular_velocity.z = 0;
}
imu_calc(input->header.stamp);
previous_time = current_time;
previous_imu_roll = imu_roll;
previous_imu_pitch = imu_pitch;
previous_imu_yaw = imu_yaw;
}
1828
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
