二、位姿匹配，建图
1、laser_mapping.cpp
1、主函数构造，构建laserMapping节点，调用Laser_mapping类中的laser_mapping函数，和process函数
2、laser_mapping.hpp
包含：
1.struct Data_pair
struct Data_pair
{
sensor_msgs::PointCloud2ConstPtr m_pc_corner;
sensor_msgs::PointCloud2ConstPtr m_pc_full;
sensor_msgs::PointCloud2ConstPtr m_pc_plane;
bool m_has_pc_corner = 0;
bool m_has_pc_full = 0;
bool m_has_pc_plane = 0;
//-------------------------------后面回调函数会调用------------------------------------
void add_pc_corner(sensor_msgs::PointCloud2ConstPtr ros_pc)
{
m_pc_corner = ros_pc;
m_has_pc_corner = true;
}
void add_pc_plane(sensor_msgs::PointCloud2ConstPtr ros_pc)
{
m_pc_plane = ros_pc;
m_has_pc_plane = true;
}
void add_pc_full(sensor_msgs::PointCloud2ConstPtr ros_pc)
{
m_pc_full = ros_pc;
m_has_pc_full = true;
}
bool is_completed()
{
return (m_has_pc_corner & m_has_pc_full & m_has_pc_plane);
}
};
2、class Point_cloud_registration
3、class Laser_mapping
1、进入laser_mapping函数
1）定义了一些点云指针变量并对
// 初始化点云类型 m_laser_cloud_corner_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_surf_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_surround = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_corner_from_map = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_surf_from_map = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_corner_from_map_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_surf_from_map_last = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); m_laser_cloud_full_res = pcl::PointCloud<PointType>::Ptr(new pcl::PointCloud<PointType>()); //初始化ros参数 init_parameters(m_ros_node_handle);
2）接收来自上一节点的角点、面点和所有点，调用回调函数
// livox_corners 每帧的corners, plannes, corners + plannes
m_sub_laser_cloud_corner_last = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_corners", 10000, &Laser_mapping::laserCloudCornerLastHandler, this);
m_sub_laser_cloud_surf_last = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_surface", 10000, &Laser_mapping::laserCloudSurfLastHandler, this);
m_sub_laser_cloud_full_res = m_ros_node_handle.subscribe<sensor_msgs::PointCloud2>("/pc2_full", 10000, &Laser_mapping::laserCloudFullResHandler, this);
回调函数laserCloudCornerLastHandler，开启多线程
// 把三个回调函数中的laser_data合并起来，压入到 m_queue_avail_data
void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudCornerLast2)
{
std::unique_lock<std::mutex> lock(m_mutex_buf);
Data_pair *data_pair = get_data_pair(laserCloudCornerLast2->header.stamp.toSec()); // NOTE pair the corner data with other feature in same timestamp注:将角数据与相同时间戳的其他特征配对
data_pair->add_pc_corner(laserCloudCornerLast2); // TODO if two corner cloud came within 1 sec,and both not processed yet, the first is overwrited??
if (data_pair->is_completed())
{
m_queue_avail_data.push(data_pair);//把数据对压入队列中
}
}
主要部分是三个回调函数：get_data_pair比较时间戳，找到匹配的时间戳后输出数据对data_pair -->用到最一开始的struct Data_pair -->将数据对压入队列m_queue_avail_data中（-->process进程中处理数据）
队列m_queue_avail_data同时保存了
m_pc_corner
m_pc_plane
m_pc_full
3）发布消息
// 每隔100帧发布当前帧周围1000m范围内降采样后的points
m_pub_laser_cloud_surround = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surround", 10000);
// 每一帧corners在map下的points，pose graph 更新前的地图
m_pub_last_corner_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/features_corners", 10000);
// 每一帧plannes在map下的points，pose graph 更新前的地图
m_pub_last_surface_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/features_surface", 10000);
m_pub_match_corner_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/match_pc_corners", 10000);
// local map corners
m_pub_match_surface_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/match_pc_surface", 10000);
// local map plannes
m_pub_pc_aft_loop = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/pc_aft_loop_closure", 10000);
// 每有一次闭环用pose graph更新后的poses，对地图的各个小部分进行更新，也就是重新计算在map下的points
m_pub_debug_pts = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/pc_debug", 10000);
// topic上的数据为空
// 没用到
m_pub_laser_cloud_map = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/laser_cloud_map", 10000);
m_pub_laser_cloud_full_res = m_ros_node_handle.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_registered", 10000);
// 每一帧corners + plannes 在map下的points, pose graph 更新前的地图
m_pub_odom_aft_mapped = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init", 10000);
// 与local map 匹配得到的每帧的位姿
// 没用到
m_pub_odom_aft_mapped_hight_frec = m_ros_node_handle.advertise<nav_msgs::Odometry>("/aft_mapped_to_init_high_frec", 10000);
// 每隔10帧发布全部与local map 匹配得到的每帧的位姿
m_pub_laser_aft_mapped_path = m_ros_node_handle.advertise<nav_msgs::Path>("/aft_mapped_path", 10000);
m_pub_laser_aft_loopclosure_path = m_ros_node_handle.advertise<nav_msgs::Path>("/aft_loopclosure_path", 10000);
// 每有一次闭环就会计算一次pose graph， 然后把计算后的位姿发布到该topic
2、进入process函数
1)m_service_pub_surround_pts,发布周围点
m_service_pub_surround_pts = new std::future<void>(std::async(std::launch::async, &Laser_mapping::service_pub_surround_pts, this));
a) 这里调用了service_pub_surround_pts函数，每隔100帧发布当前帧周围1000m范围内降采样后的points
//创建滤波对象，格子大小为0.5 pcl::VoxelGrid<PointType> down_sample_filter_surface; down_sample_filter_surface.setLeafSize(m_surround_pointcloud_resolution, m_surround_pointcloud_resolution, m_surround_pointcloud_resolution); //定义变量 pcl::PointCloud<PointType> pc_temp; sensor_msgs::PointCloud2 ros_laser_cloud_surround;//转换成ROS形式发布周围点 // std::this_thread::sleep_for(std::chrono::nanoseconds(10));//10纳秒，0.000,01 ms pcl::PointCloud<PointType>::Ptr laser_cloud_surround(new pcl::PointCloud<PointType>()); // 当前帧周围的map cells(corners + plannes) laser_cloud_surround->reserve(1e8);//预先设定容量到指定值 std::this_thread::sleep_for(std::chrono::milliseconds(1000)); ////睡眠1000毫秒（1秒） int last_update_index = 0;
注：引用了std::this_thread::sleep_for函数是C11的休眠函数，表示当前线程休眠一段时间，休眠期间不与其他线程竞争CPU，根据线程需求，等待若干时间。
b) 进入while循环
while (1)
{
while (m_current_frame_index - last_update_index < 100) // 每100帧发布一次
{
std::this_thread::sleep_for(std::chrono::milliseconds(1500));//休眠1.5s
}
last_update_index = m_current_frame_index;//100帧之后开始发布，当前帧索引为最新更新
pcl::PointCloud<PointType> pc_temp;
laser_cloud_surround->clear();
if (m_pt_cell_map_full.get_cells_size() == 0)//判断是否有单元格
continue;
//寻找半径范围内的单元格
std::vector<Points_cloud_map<float>::Mapping_cell_ptr> cell_vec = m_pt_cell_map_full.find_cells_in_radius(m_t_w_curr, 1000.0);
// 获得当前帧周围的map cells(corners + plannes)
for (size_t i = 0; i < cell_vec.size(); i++)
{ //如果需要体素滤波
if (m_down_sample_replace)
{//对第i个单元格的点进行体素滤波并存放到pc_temp当中
down_sample_filter_surface.setInputCloud(cell_vec[i]->get_pointcloud().makeShared());
down_sample_filter_surface.filter(pc_temp);
*laser_cloud_surround += pc_temp;
}
else
{
*laser_cloud_surround += cell_vec[i]->get_pointcloud();
}
}
if (laser_cloud_surround->points.size())//发布周围点
{
down_sample_filter_surface.setInputCloud(laser_cloud_surround);
down_sample_filter_surface.filter(*laser_cloud_surround);
pcl::toROSMsg(*laser_cloud_surround, ros_laser_cloud_surround);
ros_laser_cloud_surround.header.stamp = ros::Time::now();
ros_laser_cloud_surround.header.frame_id = "camera_init";
m_pub_laser_cloud_surround.publish(ros_laser_cloud_surround);
}
}
}
1.m_current_frame_index为当前帧索引
2.m_pt_cell_map_full为全部帧在map的下标
2) 这里没有开启回环检测 m_loop_closure_if_enable=0
if (m_loop_closure_if_enable)
{
m_service_loop_detection = new std::future<void>(std::async(std::launch::async, &Laser_mapping::service_loop_detection, this));
}
3）进入while循环
a) 如果队列为空，休眠0.001s
while (m_queue_avail_data.empty())
{
sleep(0.0001);
}
b) 判断队列中的数据是否超过最大限制，如果超过。依次删除
while (m_queue_avail_data.size() >= (unsigned int)m_max_buffer_size) // 如果队列中数据超过限制，依次删除队列中的第一个元素直到数量达到要求
{
ROS_WARN("Drop lidar frame in mapping for real time performance !!!");
(*m_logger_common.get_ostream()) << "Drop lidar frame in mapping for real time performance !!!" << endl;
m_queue_avail_data.pop();
}
c) 从队列的第一项开始处理
// 从队列的最开始处理，将队列中的第一项赋值给current_data_pair，后续将对current_data_pair进行处理，同时删除队列中的这个数据以便于下次循环的进行 Data_pair *current_data_pair = m_queue_avail_data.front(); m_queue_avail_data.pop(); m_mutex_buf.unlock(); //时间戳 m_time_pc_corner_past = current_data_pair->m_pc_corner->header.stamp.toSec();
d) 给队列的第一项赋予时间戳 first_time_stamp
if (first_time_stamp < 0)
{
first_time_stamp = m_time_pc_corner_past;
}
(*m_logger_common.get_ostream()) << "Messgage time stamp = " << m_time_pc_corner_past - first_time_stamp << endl;
e) 将current_data_pair中的ROS消息转换为点云信息
m_mutex_querypointcloud.lock(); 不懂
m_mutex_querypointcloud.unlock();
//与多线程进程读取Laser_mapping::process_new_scan中的点相关 m_mutex_querypointcloud.lock(); m_laser_cloud_corner_last->clear(); //将current_data_pair中的ROS消息转换为点云信息 pcl::fromROSMsg(*(current_data_pair->m_pc_corner), *m_laser_cloud_corner_last); m_laser_cloud_surf_last->clear(); pcl::fromROSMsg(*(current_data_pair->m_pc_plane), *m_laser_cloud_surf_last); m_laser_cloud_full_res->clear(); pcl::fromROSMsg(*(current_data_pair->m_pc_full), *m_laser_cloud_full_res); m_mutex_querypointcloud.unlock(); delete current_data_pair;//转换后删除当前数据
f) 维护最大线程池[线程的部分还不太懂]
std::liststd::future<int > m_thread_pool; m_maximum_parallel_thread = 2；
Common_tools::maintain_maximum_thread_pool<std::future<int> *>(m_thread_pool, m_maximum_parallel_thread);
调用了common_tool中的maintain_maximum_thread_pool函数
g) 开启process_new_scan线程
std::future<int> *thd = new std::future<int>(std::async(std::launch::async, &Laser_mapping::process_new_scan, this));
3、进入process_new_scan函数
1、记录本帧数据中的最大最小时间戳，这里的intensity存放的是时间
float min_t, max_t; // 本帧数据中最小，最大时间戳 find_min_max_intensity(current_laser_cloud_full.makeShared(), min_t, max_t); // 点的intensity存放的是时间戳，找到当前帧的最大最小时间戳，是为了后面调用refine_blur消除畸变
2、时间戳的转换，帧间转换
m_time_odom = m_last_time_stamp; //m_last_time_stamp的初始为0 //这里的m_last_time_stamp是在上一帧处理时的最大时间戳，也是当前帧的最小时间戳 m_minimum_pt_time_stamp = m_last_time_stamp; m_maximum_pt_time_stamp = max_t; //这里将max_t赋值给m_last_time_stamp是为了进行下一帧处理时充当下一帧的最小时间戳 m_last_time_stamp = max_t;
3、调用Point_cloud_registration类，调用init_pointcloud_registration函数【不太懂】
local map与当前帧匹配前，设置注册参数
用定义好的类创建一个对象 ,类的使用 pc_reg就是我们使用的类
Point_cloud_registration pc_reg; ////用定义好的类创建一个对象 pc_reg init_pointcloud_registration(pc_reg); // local map与当前帧匹配前，set registration params m_current_frame_index++;
4、对数据进行滤波降采样
//对数据进行滤波降采样并存储在下方
pcl::PointCloud<PointType>::Ptr laserCloudCornerStack(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfStack(new pcl::PointCloud<PointType>());
//对数据进行滤波降采样
if (m_if_input_downsample_mode)//=1
{
down_sample_filter_corner.setInputCloud(current_laser_cloud_corner_last.makeShared());
down_sample_filter_corner.filter(*laserCloudCornerStack);
down_sample_filter_surface.setInputCloud(current_laser_cloud_surf_last.makeShared());
down_sample_filter_surface.filter(*laserCloudSurfStack);
}
else
{
*laserCloudCornerStack = current_laser_cloud_corner_last; // NOTE 调取上一轮的边角点
*laserCloudSurfStack = current_laser_cloud_surf_last;
}
int laser_corner_pt_num = laserCloudCornerStack->points.size();
int laser_surface_pt_num = laserCloudSurfStack->points.size();
5、保存数据：当前帧所有点云和时间戳
//保存数据
if (m_if_save_to_pcd_files && PCD_SAVE_RAW)
{
m_pcl_tools_raw.save_to_pcd_files("raw", current_laser_cloud_full, m_current_frame_index);
}
6、位姿，在3中对世界坐标系和map坐标系的位姿进行定义，包括旋转和平移
m_q_w_last = m_q_w_curr; // NOTE 保存当前位姿 m_t_w_last = m_t_w_curr;
7、建立面点，角点的地图指针和kdtree
pcl::PointCloud<PointType>::Ptr laser_cloud_corner_from_map(new pcl::PointCloud<PointType>()); pcl::PointCloud<PointType>::Ptr laser_cloud_surf_from_map(new pcl::PointCloud<PointType>()); int reg_res = 0; //历史地图数据=============================这里互斥锁的设置？？？ m_mutex_buff_for_matching_corner.lock(); *laser_cloud_corner_from_map = *m_laser_cloud_corner_from_map_last; // local map corners kdtree_corner_from_map = m_kdtree_corner_from_map_last; // local_map_kdtree_corners m_mutex_buff_for_matching_surface.unlock(); m_mutex_buff_for_matching_surface.lock(); *laser_cloud_surf_from_map = *m_laser_cloud_surf_from_map_last; // local map plannes kdtree_surf_from_map = m_kdtree_surf_from_map_last; // local_map_kdtree_plannes m_mutex_buff_for_matching_corner.unlock();
8、调用了上述reg_res类中的find_out_incremental_transfrom函数，输入的是角地图，面地图，及其kdtree,当前帧的角点和面点，这里是匹配点查询和位姿优化
reg_res = pc_reg.find_out_incremental_transfrom(laser_cloud_corner_from_map, laser_cloud_surf_from_map, // local map kdtree_corner_from_map, kdtree_surf_from_map, // local map kdtree laserCloudCornerStack, laserCloudSurfStack); // 当前帧corners, plannes
1）位姿（旋转和平移）增量
Eigen::Map<Eigen::Quaterniond> q_w_incre = Eigen::Map<Eigen::Quaterniond>(m_para_buffer_incremental); Eigen::Map<Eigen::Vector3d> t_w_incre = Eigen::Map<Eigen::Vector3d>(m_para_buffer_incremental + 4); // <1>: 用上一帧end到当前帧end TF 作为初值 // <2>: incre_q = I, incre_t = T， T：curr_finished_keyframe_center - history_finished_keyframe_center // <2>: curr_q = I, curr_t = T 在scene_alignment.hpp中设置的初值 // <2>: last_q = I, last_t = 0
2)变量定义赋值
m_kdtree_corner_from_map = kdtree_corner_from_map; m_kdtree_surf_from_map = kdtree_surf_from_map; pcl::PointCloud<PointType> laser_cloud_corner_from_map = *in_laser_cloud_corner_from_map; pcl::PointCloud<PointType> laser_cloud_surf_from_map = *in_laser_cloud_surf_from_map; int laserCloudCornerFromMapNum = laser_cloud_corner_from_map.points.size(); int laserCloudSurfFromMapNum = laser_cloud_surf_from_map.points.size(); int laser_corner_pt_num = laserCloudCornerStack->points.size(); int laser_surface_pt_num = laserCloudSurfStack->points.size(); int surf_avail_num = 0; int corner_avail_num = 0; float minimize_cost = summary.final_cost; PointType pointOri, pointSel; int corner_rejection_num = 0; int surface_rejecetion_num = 0; int if_undistore_in_matching = 1;
3）位姿转换，消除畸变
最开始的前50帧数据，poses默认m_q_w_curr = I, m_t_w_curr = 0
之后每帧的位姿都是通过与local map 匹配得到
if (laserCloudCornerFromMapNum > CORNER_MIN_MAP_NUM && laserCloudSurfFromMapNum > SURFACE_MIN_MAP_NUM && m_current_frame_index > m_mapping_init_accumulate_frames)
a)求解位姿，最大迭代次数20
m_timer->tic("Build kdtree");
*(m_logger_timer->get_ostream()) << m_timer->toc_string("Build kdtree") << std::endl;
Eigen::Quaterniond q_last_optimize(1.f, 0.f, 0.f, 0.f);
Eigen::Vector3d t_last_optimize(0.f, 0.f, 0.f);
int iterCount = 0;
std::vector<int> m_point_search_Idx;
std::vector<float> m_point_search_sq_dis;
//最大迭代次数20
for (iterCount = 0; iterCount < m_para_icp_max_iterations; iterCount++) // 计算位姿时, yaml: 15; loop closure, yaml: 5
{
b）定义相关变量，和求解的ceres模块
m_point_search_Idx.clear(); m_point_search_sq_dis.clear(); corner_avail_num = 0; surf_avail_num = 0; corner_rejection_num = 0; surface_rejecetion_num = 0; ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);//核函数 ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization(); ceres::Problem::Options problem_options; ceres::ResidualBlockId block_id; ceres::Problem problem(problem_options); std::vector<ceres::ResidualBlockId> residual_block_ids; problem.AddParameterBlock(m_para_buffer_incremental, 4, q_parameterization);//四维 // 为问题添加一个大小适当的参数块。具有相同参数的重复调用将被忽略。使用相同的双指针但大小不同的重复调用将导致未定义行为。 problem.AddParameterBlock(m_para_buffer_incremental + 4, 3);
c1) 角点 转换到地图坐标系下 并做运动补偿 pointOri转换前的点，pointSel转换后的点
for (int i = 0; i < laser_corner_pt_num; i++) // curr帧corners
{
pointOri = laserCloudCornerStack->points[i];
if ((!std::isfinite(pointOri.x)) ||
(!std::isfinite(pointOri.y)) ||
(!std::isfinite(pointOri.z)))
continue;
pointAssociateToMap(&pointOri, &pointSel, refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), if_undistore_in_matching);
// 用上一帧时刻的位姿作为初值，把当前帧下的点转化到map下
这里调用了pointAssociateToMap函数；将点转换到map下
m_if_motion_deblur =0 为分段处理；=1为线性插值 这里为0 论文C In-frame motion compensation
void pointAssociateToMap(PointType const *const pi, PointType *const po,
double interpolate_s = 1.0, int if_undistore = 0)
{
Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
Eigen::Vector3d point_w;
if (m_if_motion_deblur == 0 || if_undistore == 0 || interpolate_s == 1.0)
{
point_w = m_q_w_curr * point_curr + m_t_w_curr;
}
//进入else
else
{
if (interpolate_s > 1.0 || interpolate_s < 0.0)
{
screen_printf("Input interpolate_s = %.5f\r\n", interpolate_s);
}
if (1) // Using rodrigues for fast compute.
{
//https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula
Eigen::Vector3d interpolate_T = m_t_w_incre * (interpolate_s * BLUR_SCALE);
double interpolate_R_theta = m_interpolatation_theta * interpolate_s;
Eigen::Matrix<double, 3, 3> interpolate_R_mat;
interpolate_R_mat = Eigen::Matrix3d::Identity() + sin(interpolate_R_theta) * m_interpolatation_omega_hat + (1 - cos(interpolate_R_theta)) * m_interpolatation_omega_hat_sq2;
point_w = m_q_w_last * (interpolate_R_mat * point_curr + interpolate_T) + m_t_w_last;
}
else
{
Eigen::Quaterniond interpolate_q = m_q_I.slerp(interpolate_s * BLUR_SCALE, m_q_w_incre);
Eigen::Vector3d interpolate_T = m_t_w_incre * (interpolate_s * BLUR_SCALE);
// 先转换到上一帧end下
point_w = m_q_w_last * (interpolate_q * point_curr + interpolate_T) + m_t_w_last;
// 再转换到map下
}
}
po->x = point_w.x();
po->y = point_w.y();
po->z = point_w.z();
po->intensity = pi->intensity;
}
c2) kdtree 确定线点 找出里当前角点周围最近的5个点，最远的点距离当前点不能超过50m，协方差矩阵∑的特征值的最大值是第二大特征值的三倍，则认为该点在线上；这要还要根据论文做进一步分析
pointAssociateToMap(&pointOri, &pointSel, refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), if_undistore_in_matching);
m_kdtree_surf_from_map.nearestKSearch(pointSel, plane_search_num, m_point_search_Idx, m_point_search_sq_dis);
if (m_point_search_sq_dis[plane_search_num - 1] < m_maximum_dis_plane_for_match) // 50m
{
std::vector<Eigen::Vector3d> nearCorners;
Eigen::Vector3d center(0, 0, 0);
if (IF_PLANE_FEATURE_CHECK)
{
for (int j = 0; j < plane_search_num; j++)
{
Eigen::Vector3 dtmp(laser_cloud_corner_from_map.points[m_point_search_Idx[j]].x,
laser_cloud_corner_from_map.points[m_point_search_Idx[j]].y,
laser_cloud_corner_from_map.points[m_point_search_Idx[j]].z);
center = center + tmp;
nearCorners.push_back(tmp);
}
center = center / (float)(plane_search_num); // 计算这五个点的均值
Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
//计算各点的残差，构造协方差矩阵
for (int j = 0; j < plane_search_num; j++)
{
Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
}
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);
if ((saes.eigenvalues()[2] > 3 * saes.eigenvalues()[0]) &&
(saes.eigenvalues()[2] < 10 * saes.eigenvalues()[1]))
{
planeValid = true;
}
else
{
planeValid = false;
}
}
c3) // 对这5个points组成的集合计算协方差，特征值分解，最大特征值是否是次大特征的3倍以上，
// 若不是，忽略掉该点，拒绝points num++；
// 若是，计算残差
Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
if (line_is_avail) // 1
{
if (ICP_LINE) // 1, //loop clousre(scene_alignment.hpp)时：0, 不用corners计算loop closure
{
ceres::CostFunction *cost_function;
auto pt_1 = pcl_pt_to_eigend(laser_cloud_corner_from_map.points[m_point_search_Idx[0]]); // j point
auto pt_2 = pcl_pt_to_eigend(laser_cloud_corner_from_map.points[m_point_search_Idx[1]]); // l point
if ((pt_1 - pt_2).norm() < 0.0001)
continue;
if (m_if_motion_deblur) // 线性插值
{
cost_function = ceres_icp_point2line_mb<double>::Create(curr_point,
pt_1, pt_2,refine_blur(pointOri.intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp) * 1.0,Eigen::Matrix<double, 4, 1>(m_q_w_last.w(), m_q_w_last.x(), m_q_w_last.y(), m_q_w_last.z()),m_t_w_last); //pointOri.intensity );
}
else
{ // 分段
cost_function = ceres_icp_point2line<double>::Create(curr_point,pt_1,pt_2,Eigen::Matrix<double, 4, 1>(m_q_w_last.w(), m_q_w_last.x(), m_q_w_last.y(), m_q_w_last.z()),m_t_w_last);
}
// 直线到点的距离向量，由垂足指向点
block_id = problem.AddResidualBlock(cost_function, loss_function, m_para_buffer_incremental, m_para_buffer_incremental + 4);
residual_block_ids.push_back(block_id);
corner_avail_num++;
}
}
else
{ // 忽略掉该point
corner_rejection_num++;
}
}
} // end curr帧corners
c4）同样的，对面点进行约束 ，最终求得的是位姿的旋转和平移的增量
d) 根据残差项的大小去除20%的离群点 这一部分在学习到ceres_slove时再细看
for (size_t ii = 0; ii < 1; ii++)
{
options.linear_solver_type = slover_type;
options.max_num_iterations = m_para_cere_max_iterations;
options.max_num_iterations = m_para_cere_prerun_times; // 先让ceres迭代2次，根据结果的残差项大小，大约删除掉20%的outliers
options.minimizer_progress_to_stdout = false;
options.check_gradients = false; // TODO true时, 总是 Gradient error detected. Terminating solver
options.gradient_check_relative_precision = 1e-10;
set_ceres_solver_bound(problem, m_para_buffer_incremental); // 设置参数 m_t_w_incre最小值，最大值[-2，2]
ceres::Solve(options, &problem, &summary);
residual_block_ids_temp.clear();
ceres::Problem::EvaluateOptions eval_options;
eval_options.residual_blocks = residual_block_ids;
double total_cost = 0.0;
std::vector<double> residuals; // 残差是：点到直线的距离向量，点到面的距离向量
problem.Evaluate(eval_options, &total_cost, &residuals, nullptr, nullptr);
double m_inliner_ratio_threshold = compute_inlier_residual_threshold(residuals, m_inlier_ratio); // 内点所占比例大致为80%
m_inlier_threshold = std::max(m_inliner_dis, m_inliner_ratio_threshold); // 计算位姿时, max(0.02, ); 闭环匹配时: max(0.2, )
for (unsigned int i = 0; i < residual_block_ids.size(); i++)
{
if ((fabs(residuals[3 * i + 0]) + fabs(residuals[3 * i + 1]) + fabs(residuals[3 * i + 2])) > m_inlier_threshold) // std::min( 1.0, 10 * avr_cost )
{
problem.RemoveResidualBlock(residual_block_ids[i]);
}
else
{
residual_block_ids_temp.push_back(residual_block_ids[i]);
}
}
residual_block_ids = residual_block_ids_temp;
}
e) 得到当前帧的位姿
options.linear_solver_type = slover_type;
options.max_num_iterations = m_para_cere_max_iterations;
options.minimizer_progress_to_stdout = false;
options.check_gradients = false; // TODO true时, 总是 Gradient error detected. Terminating solver
options.gradient_check_relative_precision = 1e-10;
set_ceres_solver_bound(problem, m_para_buffer_incremental);
ceres::Solve(options, &problem, &summary);
if (m_if_motion_deblur) // 线性插值； loop closure时为0
{
compute_interpolatation_rodrigue(q_w_incre, m_interpolatation_omega, m_interpolatation_theta, m_interpolatation_omega_hat);
m_interpolatation_omega_hat_sq2 = m_interpolatation_omega_hat * m_interpolatation_omega_hat;
}
m_t_w_curr = m_q_w_last * t_w_incre + m_t_w_last; // loop closure时，last=(I,0)
m_q_w_curr = m_q_w_last * q_w_incre; // loop closure时, curr = incre
m_angular_diff = (float)m_q_w_curr.angularDistance(m_q_w_last) * 57.3; // delta角度
m_t_diff = (m_t_w_curr - m_t_w_last).norm(); // delata平移
minimize_cost = summary.final_cost;
if (q_last_optimize.angularDistance(q_w_incre) < 57.3 * m_minimum_icp_R_diff &&
(t_last_optimize - t_w_incre).norm() < m_minimum_icp_T_diff)
{
screen_out << "----- Terminate, iteration times = " << iterCount << "-----" << endl;
break;
}
else
{
q_last_optimize = q_w_incre;
t_last_optimize = t_w_incre;
}
} // end icp 计算位姿
screen_printf("===== corner factor num %d , surf factor num %d=====\n", corner_avail_num, surf_avail_num);
if (laser_corner_pt_num != 0 && laser_surface_pt_num != 0)
{
m_logger_common->printf("Corner total num %d | use %d | rate = %d \% \r\n", laser_corner_pt_num, corner_avail_num, (corner_avail_num)*100 / laser_corner_pt_num);
m_logger_common->printf("Surface total num %d | use %d | rate = %d \% \r\n", laser_surface_pt_num, surf_avail_num, (surf_avail_num)*100 / laser_surface_pt_num);
}
*(m_logger_timer->get_ostream()) << m_timer->toc_string("Pose optimization") << std::endl;
if (g_export_full_count < 5)
{
*(m_logger_common->get_ostream()) << summary.FullReport() << endl;
g_export_full_count++;
}
else
{
*(m_logger_common->get_ostream()) << summary.BriefReport() << endl;
}
*(m_logger_common->get_ostream()) << "Last R:" << m_q_w_last.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * 57.3 << " ,T = " << m_t_w_last.transpose() << endl;
*(m_logger_common->get_ostream()) << "Curr R:" << m_q_w_curr.toRotationMatrix().eulerAngles(0, 1, 2).transpose() * 57.3 << " ,T = " << m_t_w_curr.transpose() << endl;
*(m_logger_common->get_ostream()) << "Iteration time: " << iterCount << endl;
m_logger_common->printf("Motion blur = %d | ", m_if_motion_deblur);
m_logger_common->printf("Cost = %.5f| inlier_thr = %.2f |blk_size = %d | corner_num = %d | surf_num = %d | angle dis = %.2f | T dis = %.2f \r\n",
minimize_cost, m_inlier_threshold, summary.num_residual_blocks, corner_avail_num, surf_avail_num, m_angular_diff, m_t_diff);
m_inlier_threshold = m_inlier_threshold * summary.final_cost / summary.initial_cost; //
// 计算位姿结束; 计算最终的匹配, used in loop closure(scene_alignment.hpp)
// 计算位姿时: delta_angle > 4度 或 最终的cost > 2.0, 认为本次计算失败，当前帧的位姿置为上一帧的位姿
// loop closure时: 没有通过函数的返回值是0 or 1 来判断是否对齐, 而是通过m_inlier_threshold来判断
if (m_angular_diff > m_para_max_angular_rate || minimize_cost > m_max_final_cost)
{
*(m_logger_common->get_ostream()) << "**** Reject update **** " << endl;
*(m_logger_common->get_ostream()) << summary.FullReport() << endl;
for (int i = 0; i < 7; i++)
{
m_para_buffer_RT[i] = m_para_buffer_RT_last[i]; // 其他地方没有使用这两个变量，没有用到
}
m_last_time_stamp = m_minimum_pt_time_stamp;
m_q_w_curr = m_q_w_last;
m_t_w_curr = m_t_w_last;
return 0;
}
m_final_opt_summary = summary;
}
else
{
screen_out << "time Map corner and surf num are not enough" << std::endl;
}
return 1;
}
4）保存当前帧经消除畸变后的角点和面点
PointType pointOri, pointSel;
//下边两个存储的是经过消除畸变的corners和plannes
pcl::PointCloud<PointType>::Ptr pc_new_feature_corners(new pcl::PointCloud<PointType>()); // 当前帧经消除畸变后的corners在map下坐标
pcl::PointCloud<PointType>::Ptr pc_new_feature_surface(new pcl::PointCloud<PointType>()); // 当前帧经消除畸变后的plannes在map下坐标
for (int i = 0; i < laser_corner_pt_num; i++)
{
pc_reg.pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel, refine_blur(laserCloudCornerStack->points[i].intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), g_if_undistore);
pc_new_feature_corners->push_back(pointSel); // 上一行refine_blur 是利用时间戳消除畸变,返回一个时间/位置的比例.pointAssociateToMap这个函数把消除畸变并且转换到地图坐标系下的点记录到&pointSel里.
}
for (int i = 0; i < laser_surface_pt_num; i++)
{
pc_reg.pointAssociateToMap(&laserCloudSurfStack->points[i], &pointSel, refine_blur(laserCloudSurfStack->points[i].intensity, m_minimum_pt_time_stamp, m_maximum_pt_time_stamp), g_if_undistore);
pc_new_feature_surface->push_back(pointSel); // 平面点除畸变,写入地图坐标系
}
5) 对角点和面点进行滤波
//进行滤波 down_sample_filter_corner.setInputCloud(pc_new_feature_corners); down_sample_filter_corner.filter(*pc_new_feature_corners); down_sample_filter_surface.setInputCloud(pc_new_feature_surface); down_sample_filter_surface.filter(*pc_new_feature_surface); //r_diff 旋转角度距离 t_diff平移距离 double r_diff = m_q_w_curr.angularDistance(m_last_his_add_q) * 57.3; // 换成度数 double t_diff = (m_t_w_curr - m_last_his_add_t).norm(); // 世界坐标减去上次增量后归一化 pc_reg.pointcloudAssociateToMap(current_laser_cloud_full, current_laser_cloud_full, g_if_undistore); // 全部点云转换 // 转换到map坐标系下
上面保存的数据在pointAssociateToMap下利用时间戳消除畸变并转换到地图坐标系下,存储到pc_new_feature_corners\pc_new_feature_surface中 current_laser_cloud_full通过pointcloudAssociateToMap将全部点云消除畸变并转换坐标系后重新储存在current_laser_cloud_full中
6）保留400帧作为历史地图，并对超过和不足的进行处理
//历史关键帧不足m_maximum_history_size则添加，超过m_maximum_history_size则删除
m_mutex_mapping.lock();
if (m_laser_cloud_corner_history.size() < (size_t)m_maximum_history_size || // 400帧
(t_diff > history_add_t_step) || // 0
(r_diff > history_add_angle_step * 57.3)) // 0
{
m_last_his_add_q = m_q_w_curr; // 存档当前迭代位姿
m_last_his_add_t = m_t_w_curr;
m_laser_cloud_corner_history.push_back(*pc_new_feature_corners); // 存档历史点云
m_laser_cloud_surface_history.push_back(*pc_new_feature_surface);
m_mutex_dump_full_history.lock();
m_laser_cloud_full_history.push_back(current_laser_cloud_full);
m_his_reg_error.push_back(pc_reg.m_inlier_threshold); // 每一帧点云与local map匹配计算出位姿后的cost，the smaller the more certain about calculated pose.
m_mutex_dump_full_history.unlock();
}
else
{
screen_printf("==== Reject add history, T_norm = %.2f, R_norm = %.2f ====\r\n", t_diff, r_diff);
}
screen_out << "m_pt_cell_map_corners.size() = " << m_pt_cell_map_corners.get_cells_size() << endl;
screen_out << "m_pt_cell_map_planes.size() = " << m_pt_cell_map_planes.get_cells_size() << endl;
if (m_laser_cloud_corner_history.size() > (size_t)m_maximum_history_size) // 400帧初始化是100
{
(m_laser_cloud_corner_history.front()).clear();
m_laser_cloud_corner_history.pop_front();//直接pop_front不行吗
}
if (m_laser_cloud_surface_history.size() > (size_t)m_maximum_history_size) // 400帧
{
(m_laser_cloud_surface_history.front()).clear();
m_laser_cloud_surface_history.pop_front();
}
if (m_laser_cloud_full_history.size() > (size_t)m_maximum_history_size) // 400帧
{
m_mutex_dump_full_history.lock();
m_laser_cloud_full_history.front().clear();
m_laser_cloud_full_history.pop_front();
m_his_reg_error.pop_front();
m_mutex_dump_full_history.unlock();
}
// m_laser_cloud_corner_history, m_laser_cloud_surface_history, m_laser_cloud_full_history
// 算上当前帧在内，保留400帧 history frame
7）将处理好的点云数据转为eigen形式并添加到m_pt_cell_map_corners/planes中
m_if_mapping_updated_corner = true;
m_if_mapping_updated_surface = true;
//将处理好的点云数据转为eigen形式并添加到m_pt_cell_map_corners/planes中
m_pt_cell_map_corners.append_cloud(PCL_TOOLS::pcl_pts_to_eigen_pts<float, PointType>(pc_new_feature_corners));
m_pt_cell_map_planes.append_cloud(PCL_TOOLS::pcl_pts_to_eigen_pts<float, PointType>(pc_new_feature_surface));
// 把每一帧corners在map下点加入到m_pt_cell_map_corners
// 把每一帧plannes在map下点加入到m_pt_cell_map_planes
*(m_logger_common.get_ostream()) << "New added regtime " << point_cloud_current_timestamp << endl;
if ((m_lastest_pc_reg_time < point_cloud_current_timestamp) || (point_cloud_current_timestamp < 10.0))
{
m_q_w_curr = pc_reg.m_q_w_curr; // 如线程得到最新位姿则更新位姿
m_t_w_curr = pc_reg.m_t_w_curr;
m_lastest_pc_reg_time = point_cloud_current_timestamp;
}
else
{
*(m_logger_common.get_ostream()) << "***** older update, reject update pose *****" << endl;
}
8) 回环检测，这部分先不看
9）角点，面点的调试 0
10）发布局部地图数据 0
11) 发布当前位姿 和 当前轨迹
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "camera_init";
odomAftMapped.child_frame_id = "aft_mapped";
odomAftMapped.header.stamp = ros::Time().fromSec(time_odom);
odomAftMapped.pose.pose.orientation.x = m_q_w_curr.x();
odomAftMapped.pose.pose.orientation.y = m_q_w_curr.y();
odomAftMapped.pose.pose.orientation.z = m_q_w_curr.z();
odomAftMapped.pose.pose.orientation.w = m_q_w_curr.w();
odomAftMapped.pose.pose.position.x = m_t_w_curr.x();
odomAftMapped.pose.pose.position.y = m_t_w_curr.y();
odomAftMapped.pose.pose.position.z = m_t_w_curr.z();
m_pub_odom_aft_mapped.publish(odomAftMapped); // name: Odometry aft_mapped_to_init 高频里程计
geometry_msgs::PoseStamped pose_aft_mapped;
pose_aft_mapped.header = odomAftMapped.header;
pose_aft_mapped.pose = odomAftMapped.pose.pose;
m_laser_after_mapped_path.header.stamp = odomAftMapped.header.stamp;
m_laser_after_mapped_path.header.frame_id = "camera_init";
if (m_current_frame_index % 10 == 0) // 每隔10帧发布一次位姿
{
m_laser_after_mapped_path.poses.push_back(pose_aft_mapped);
m_pub_laser_aft_mapped_path.publish(m_laser_after_mapped_path);
}
12)发布tf 当前帧到地图坐标的转换关系
static tf::TransformBroadcaster br; tf::Transform transform; tf::Quaternion q; transform.setOrigin(tf::Vector3(m_t_w_curr(0), m_t_w_curr(1), m_t_w_curr(2))); q.setW(m_q_w_curr.w()); q.setX(m_q_w_curr.x()); q.setY(m_q_w_curr.y()); q.setZ(m_q_w_curr.z()); transform.setRotation(q); br.sendTransform(tf::StampedTransform(transform, odomAftMapped.header.stamp, "camera_init", "aft_mapped"));
4、返回process函数，不断循环
m_logger_timer.get_ostream 这里没懂
三 没懂得地方
1、线程std::future
2、上锁 m_mutex_buf.lock();
3、数据得存储 (*m_logger_common.get_ostream())
4、分段处理
正在学习的可以一起沟通交流，qq1078087141
扫一扫
1402
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
