R3Live系列学习（一）Loam-Livox源码阅读_r3live livox

对SLAM的学习从未停止，再来开个新坑~

近日火星实验室提出了又一个大作“R3Live”，它生成了纹理贴合不错的彩色点云，甚至还重建成了mesh带入到游戏中，使人身临其境。罗马不是一天建成的，这一大作是沿袭了该实验室多年来的积累，结合了vins-mono、fast-lio等里程计的特性，加上livox雷达的稠密点云属性（他们超喜欢livox雷达），目前为止是我看视频看到的最好建图效果，可惜还没开源，因此我准备沿着他们的发展道路，一步步来学习，乃至复现。

loam-livox是livox雷达建图的第一个开源算法，作为第一批在19年就使用livox雷达的螃蟹人，我很难忘记当时livox画面累积的效果给仅接触过16线雷达的我（穷）的震撼，但使用loam-livox建图的过程中，则发现有内存耗尽等问题，并且由于FOV角太小，在面对缺乏结构变化的场景会失效，这些都是单一传感器的缺陷。在此我抛开问题，单纯看它的优点。

首先明确的一点是一帧点云的数据排序，它像万花筒一样延伸，因此数据是按万花筒的动作画圆排序，在投影平面上是连续的！这对理解帧数据的处理至关重要。

整个工程思路与loam相似，分两个节点，分别用于特征提取和建图优化。

一、特征提取

定位到laser_feature_extractor.hpp的laserCloudHandler函数，这是ros收取雷达数据的回调函数，并在下面的语句中提取了线特征和面特征。

laserCloudScans = m_livox.extract_laser_features( laserCloudIn, laserCloudMsg->header.stamp.toSec() );

在extract_laser_features函数中，核心是如下的两个函数。

//按点云延伸的拐点分为若干个花瓣
int clutter_size = projection_scan_3d_2d( laserCloudIn, scan_id_index );
 
//计算角点、平面点
compute_features();

livox雷达的视野更像一个相机，将其投影到深度图，更容易处理。

template < typename T >
int projection_scan_3d_2d( pcl::PointCloud< T > &laserCloudIn, std::vector< float > &scan_id_index )
{
    unsigned int pts_size = laserCloudIn.size();
    m_pts_info_vec.clear();
    m_pts_info_vec.resize( pts_size );
    m_raw_pts_vec.resize( pts_size );
    std::vector< int > edge_idx;
    std::vector< int > split_idx;
    scan_id_index.resize( pts_size );
    m_map_pt_idx.clear();
    m_map_pt_idx.reserve( pts_size );
    std::vector< int > zero_idx;
 
    m_input_points_size = 0;
 
    //遍历整个点云
    for ( unsigned int idx = 0; idx < pts_size; idx++ )
    {
        m_raw_pts_vec[ idx ] = laserCloudIn.points[ idx ];
        Pt_infos *pt_info = &m_pts_info_vec[ idx ];
        m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) );
        pt_info->raw_intensity = laserCloudIn.points[ idx ].intensity;
        pt_info->idx = idx;
 
        //按下标分配微小的时间戳差距！由于时间戳是扫描到第一个点的时刻，因此所有点的时间戳会有细微不同
        pt_info->time_stamp = m_current_time + ( ( float ) idx ) * m_time_internal_pts;
        
        m_last_maximum_time_stamp = pt_info->time_stamp;
        m_input_points_size++;
 
        //nan点
        if ( !std::isfinite( laserCloudIn.points[ idx ].x ) ||
                !std::isfinite( laserCloudIn.points[ idx ].y ) ||
                !std::isfinite( laserCloudIn.points[ idx ].z ) )
        {
            add_mask_of_point( pt_info, e_pt_nan );
            continue;
        }
 
        //距离过近的点
        if ( laserCloudIn.points[ idx ].x == 0 )
        {
            if ( idx == 0 )
            {
                // TODO: handle this case.
                screen_out << "First point should be normal!!!" << std::endl;
 
                pt_info->pt_2d_img << 0.01, 0.01;
                pt_info->polar_dis_sq2 = 0.0001;
                add_mask_of_point( pt_info, e_pt_000 );
                //return 0;
            }
            else
            {
                pt_info->pt_2d_img = m_pts_info_vec[ idx - 1 ].pt_2d_img;
                pt_info->polar_dis_sq2 = m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
                add_mask_of_point( pt_info, e_pt_000 );
                continue;
            }
        }
 
        m_map_pt_idx.insert( std::make_pair( laserCloudIn.points[ idx ], pt_info ) );
 
        //求解每个点的深度
        pt_info->depth_sq2 = depth2_xyz( laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].y, laserCloudIn.points[ idx ].z );
 
        //投影至像素点的坐标
        pt_info->pt_2d_img << laserCloudIn.points[ idx ].y / laserCloudIn.points[ idx ].x, laserCloudIn.points[ idx ].z / laserCloudIn.points[ idx ].x;
        
        //投影的像素点距离投影中心点的距离
        pt_info->polar_dis_sq2 = dis2_xy( pt_info->pt_2d_img( 0 ), pt_info->pt_2d_img( 1 ) );
 
        //估计强度，在投影平面上，距中心点越远的点强度小的可能性越大
        eval_point( pt_info );
 
        //求解投影平面的边缘点
        if ( pt_info->polar_dis_sq2 > m_max_edge_polar_pos )
        {
            add_mask_of_point( pt_info, e_pt_circle_edge, 2 );
        }
 
        // Split scans
        if ( idx >= 1 )
        {
            //在投影平面上，本个点与上个点的延伸方向
            float dis_incre = pt_info->polar_dis_sq2 - m_pts_info_vec[ idx - 1 ].polar_dis_sq2;
 
            if ( dis_incre > 0 ) // far away from zero
            {
                pt_info->polar_direction = 1;
            }
 
            if ( dis_incre < 0 ) // move toward zero
            {
                pt_info->polar_direction = -1;
            }
 
            //本个点向中心点延伸，上个点向外延伸
            if ( pt_info->polar_direction == -1 && m_pts_info_vec[ idx - 1 ].polar_direction == 1 )
            {
                if ( edge_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
                {
                    split_idx.push_back( idx );//这就是花瓣拐角处，每两个拐角确定一个花瓣
                    edge_idx.push_back( idx );
                    continue;
                }
            }
 
            if ( pt_info->polar_direction == 1 && m_pts_info_vec[ idx - 1 ].polar_direction == -1 )
            {
                if ( zero_idx.size() == 0 || ( idx - split_idx[ split_idx.size() - 1 ] ) > 50 )
                {
                    split_idx.push_back( idx );
 
                    zero_idx.push_back( idx );
                    continue;
                }
            }
        }
    }
    split_idx.push_back( pts_size - 1 );
 
    int   val_index = 0;
    int   pt_angle_index = 0;
    float scan_angle = 0;
    int   internal_size = 0;
 
    //cout<<"split_idx.size():"<<split_idx.size()<<endl;
 
    //每个花瓣两个拐点，至少要三个花瓣
    if( split_idx.size() < 6) // minimum 3 petal of scan.
        return 0;
    
    //再次遍历点云的所有点
    for ( int idx = 0; idx < ( int ) pts_size; idx++ )
    {
        if ( val_index < split_idx.size() - 2 )
        {
            //在当前花瓣上处理
            if ( idx == 0 || idx > split_idx[ val_index + 1 ] )
            {
                //处理完毕后跳到下一个花瓣
                if ( idx > split_idx[ val_index + 1 ] )
                {
                    val_index++;
                }
 
                //两个拐点之间的点数目
                internal_size = split_idx[ val_index + 1 ] - split_idx[ val_index ];
 
                //将拐点之间的点分为内外两类，分别求得点的极坐标
                if ( m_pts_info_vec[ split_idx[ val_index + 1 ] ].polar_dis_sq2 > 10000 )
                {
                    pt_angle_index = split_idx[ val_index + 1 ] - ( int ) ( internal_size * 0.20 );
                    scan_angle = atan2( m_pts_info_vec[ pt_angle_index ].pt_2d_img( 1 ), m_pts_info_vec[ pt_angle_index ].pt_2d_img( 0 ) ) * 57.3;
                    scan_angle = scan_angle + 180.0;
                }
                else
                {
                    pt_angle_index = split_idx[ val_index + 1 ] - ( int ) ( internal_size * 0.80 );
                    scan_angle = atan2( m_pts_info_vec[ pt_angle_index ].pt_2d_img( 1 ), m_pts_info_vec[ pt_angle_index ].pt_2d_img( 0 ) ) * 57.3;
                    scan_angle = scan_angle + 180.0;
                }
            }
        }
        m_pts_info_vec[ idx ].polar_angle = scan_angle;
        scan_id_index[ idx ] = scan_angle;
    }
 
    return split_idx.size() - 1;
}

 提取线面特征的方式遵循loam的规则，提取相邻的点并计算曲率。

void compute_features()
{
    unsigned int pts_size = m_raw_pts_vec.size();
    size_t       curvature_ssd_size = 2;
    int          critical_rm_point = e_pt_000 | e_pt_nan;
    float        neighbor_accumulate_xyz[ 3 ] = { 0.0, 0.0, 0.0 };
 
    //再次遍历所有点云
    for ( size_t idx = curvature_ssd_size; idx < pts_size - curvature_ssd_size; idx++ )
    {
        //cout<<"curvature_ssd_size:"<<curvature_ssd_size<<endl;
 
        //忽略过近与nan点
        if ( m_pts_info_vec[ idx ].pt_type & critical_rm_point )
        {
            continue;
        }
 
        /*********** Compute curvate ************/
        neighbor_accumulate_xyz[ 0 ] = 0.0;
        neighbor_accumulate_xyz[ 1 ] = 0.0;
        neighbor_accumulate_xyz[ 2 ] = 0.0;
 
        //与loam相似，查找曲线上前后2个点，共5个点
        for ( size_t i = 1; i <= curvature_ssd_size; i++ )
        {
            if ( ( m_pts_info_vec[ idx + i ].pt_type & e_pt_000 ) || ( m_pts_info_vec[ idx - i ].pt_type & e_pt_000 ) )
            {
                if ( i == 1 )
                {
                    m_pts_info_vec[ idx ].pt_label |= e_label_near_zero;
                }
                else
                {
                    m_pts_info_vec[ idx ].pt_label = e_label_invalid;
                }
                break;
            }
            else if ( ( m_pts_info_vec[ idx + i ].pt_type & e_pt_nan ) || ( m_pts_info_vec[ idx - i ].pt_type & e_pt_nan ) )
            {
                if ( i == 1 )
                {
                    m_pts_info_vec[ idx ].pt_label |= e_label_near_nan;
                }
                else
                {
                    m_pts_info_vec[ idx ].pt_label = e_label_invalid;
                }
                break;
            }
            else
            {
                neighbor_accumulate_xyz[ 0 ] += m_raw_pts_vec[ idx + i ].x + m_raw_pts_vec[ idx - i ].x;
                neighbor_accumulate_xyz[ 1 ] += m_raw_pts_vec[ idx + i ].y + m_raw_pts_vec[ idx - i ].y;
                neighbor_accumulate_xyz[ 2 ] += m_raw_pts_vec[ idx + i ].z + m_raw_pts_vec[ idx - i ].z;
            }
        }
 
        if(m_pts_info_vec[ idx ].pt_label == e_label_invalid)
        {
            continue;
        }
 
        //求其他四个点与本个点的坐标差值，即看它们是否在空间中一条直线上，差值大则说明有角点
        neighbor_accumulate_xyz[ 0 ] -= curvature_ssd_size * 2 * m_raw_pts_vec[ idx ].x;
        neighbor_accumulate_xyz[ 1 ] -= curvature_ssd_size * 2 * m_raw_pts_vec[ idx ].y;
        neighbor_accumulate_xyz[ 2 ] -= curvature_ssd_size * 2 * m_raw_pts_vec[ idx ].z;
        m_pts_info_vec[ idx ].curvature = neighbor_accumulate_xyz[ 0 ] * neighbor_accumulate_xyz[ 0 ] + neighbor_accumulate_xyz[ 1 ] * neighbor_accumulate_xyz[ 1 ] +
                                            neighbor_accumulate_xyz[ 2 ] * neighbor_accumulate_xyz[ 2 ];
 
        /*********** Compute plane angle ************/
        //计算本个平面与视角的夹角，忽略掉与视野方向角度较小的平面
        Eigen::Matrix< float, 3, 1 > vec_a( m_raw_pts_vec[ idx ].x, m_raw_pts_vec[ idx ].y, m_raw_pts_vec[ idx ].z );
        Eigen::Matrix< float, 3, 1 > vec_b( m_raw_pts_vec[ idx + curvature_ssd_size ].x - m_raw_pts_vec[ idx - curvature_ssd_size ].x,
                                            m_raw_pts_vec[ idx + curvature_ssd_size ].y - m_raw_pts_vec[ idx - curvature_ssd_size ].y,
                                            m_raw_pts_vec[ idx + curvature_ssd_size ].z - m_raw_pts_vec[ idx - curvature_ssd_size ].z );
        m_pts_info_vec[ idx ].view_angle = Eigen_math::vector_angle( vec_a  , vec_b, 1 ) * 57.3;
 
        //printf( "Idx = %d, angle = %.2f\r\n", idx,  m_pts_info_vec[ idx ].view_angle );
        
        //满足曲率与平面度的点将设为平面点
        if ( m_pts_info_vec[ idx ].view_angle > minimum_view_angle )
        {
            if( m_pts_info_vec[ idx ].curvature < thr_surface_curvature )
            {
                m_pts_info_vec[ idx ].pt_label |= e_label_surface;
            }
 
            float sq2_diff = 0.1;
 
            if ( m_pts_info_vec[ idx ].curvature > thr_corner_curvature )
            {
                //仅收纳凸出的边缘，可能因为凹进去的边缘误差较大
                if ( m_pts_info_vec[ idx ].depth_sq2 <= m_pts_info_vec[ idx - curvature_ssd_size ].depth_sq2 &&
                        m_pts_info_vec[ idx ].depth_sq2 <= m_pts_info_vec[ idx + curvature_ssd_size ].depth_sq2 )
                {
                    if ( abs( m_pts_info_vec[ idx ].depth_sq2 - m_pts_info_vec[ idx - curvature_ssd_size ].depth_sq2 ) < sq2_diff * m_pts_info_vec[ idx ].depth_sq2 ||
                            abs( m_pts_info_vec[ idx ].depth_sq2 - m_pts_info_vec[ idx + curvature_ssd_size ].depth_sq2 ) < sq2_diff * m_pts_info_vec[ idx ].depth_sq2 )
                        m_pts_info_vec[ idx ].pt_label |= e_label_corner;
                }
            }
        }
    }
}

 处理完毕后，再卷巴卷巴就准备将线面特征分别用rosmsg发送给下一个节点了。

二、建图优化

在收取上一个节点发来的角点、面点数据后，本节点进行位姿估计，核心函数是find_out_incremental_transfrom函数。令人欣慰的是，这个版本加入了线程池并发处理，并且对超出的帧数据进行丢弃，基本上不会有内存耗尽的问题了，但实时性还是在危险的边缘试探。

激光雷达采样频率较低，因此会伴随运动导致畸变。loam-livox是直接采取时间戳插值去畸变，不妨看看。在本节点收到的点云强度，是上一个节点存入的时间戳！

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
      {
          //按时间戳与上一帧的运动结果矫正本帧的运动畸变，一般情况下有imu则优先使用imu的积分结果
          if ( interpolate_s > 1.0 || interpolate_s < 0.0 )
          {
              screen_printf( "Input interpolate_s = %.5f\r\n", interpolate_s );
          }
 
          //做线性插值时一般是先将旋转矩阵分解为轴角，按系数相乘后再用罗德里格斯公式转换为旋转矩阵。而这里的位姿变换的先验，是使用上一帧的运动估计进而猜想的直线运动（类似于ORB_SLAM），因此，切勿大幅度运动呀！
          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;
          }
          
      }
 
      po->x = point_w.x();
      po->y = point_w.y();
      po->z = point_w.z();
      po->intensity = pi->intensity;
      //po->intensity = 1.0;
  }

在去畸变的同时，也将本帧点云转换到了世界坐标系下了，下一步准备求解位姿：

int find_out_incremental_transfrom( pcl::PointCloud< PointType >::Ptr in_laser_cloud_corner_from_map,
                                      pcl::PointCloud< PointType >::Ptr in_laser_cloud_surf_from_map,
                                      pcl::KdTreeFLANN< PointType > &   kdtree_corner_from_map,
                                      pcl::KdTreeFLANN< PointType > &   kdtree_surf_from_map,
                                      pcl::PointCloud< PointType >::Ptr laserCloudCornerStack,
                                      pcl::PointCloud< PointType >::Ptr laserCloudSurfStack )
  {
    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 );
 
    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();
    //screen_printf( "map corner num %d  surf num %d \n", laserCloudCornerFromMapNum, laserCloudSurfFromMapNum );
 
    //printf_line;
 
    *(m_logger_timer->get_ostream())<< m_timer->toc_string( "Query points for match" ) << std::endl;
    m_timer->tic("Pose optimization");
 
    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;
    //printf_line;
 
    //特征数量要足够
    if ( laserCloudCornerFromMapNum > CORNER_MIN_MAP_NUM && laserCloudSurfFromMapNum > SURFACE_MIN_MAP_NUM && m_current_frame_index > m_mapping_init_accumulate_frames )
    {
        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;
 
        //多次迭代求解，按理说一次基本上就能收敛，一次收敛不了后面也很难收敛
        for ( iterCount = 0; iterCount < m_para_icp_max_iterations; iterCount++ )
        {
            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;
 
            //所优化的参数为p、q
            problem.AddParameterBlock( m_para_buffer_incremental, 4, q_parameterization );
            problem.AddParameterBlock( m_para_buffer_incremental + 4, 3 );
 
            for ( int i = 0; i < laser_corner_pt_num; i++ )
            {
                if ( laser_corner_pt_num > 2 * m_maximum_allow_residual_block )
                {
                    if(m_rand_float.rand_uniform() * laser_corner_pt_num >  2 * m_maximum_allow_residual_block)
                    {
                        continue;
                    }
                }
 
                pointOri = laserCloudCornerStack->points[ i ];
                //printf_line;
                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 );
 
                //查找本帧的角点与上一帧的最近的角点（直线）
                if(m_kdtree_corner_from_map.nearestKSearch( pointSel, line_search_num, m_point_search_Idx, m_point_search_sq_dis ) != line_search_num)
                {
                  continue;
                }
 
                //防止误查找
                if ( m_point_search_sq_dis[ line_search_num - 1 ] < m_maximum_dis_line_for_match )
                {
                    bool                         line_is_avail = true;
                    std::vector<Eigen::Vector3d> nearCorners;
                    Eigen::Vector3d              center( 0, 0, 0 );
                    if ( IF_LINE_FEATURE_CHECK )
                    {
                        for ( int j = 0; j < line_search_num; j++ )
                        {
                            Eigen::Vector3d tmp( 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 ) line_search_num );
 
                        Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
 
                        //计算直线度的方差
                        for ( int j = 0; j < line_search_num; j++ )
                        {
                            Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[ j ] - center;
                            covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
                        }
 
                        //检查直线度是否足够（特征值分解可得到一个出众的特征值）
                        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes( covMat );
 
                        // note Eigen library sort eigenvalues in increasing order
 
                        if ( saes.eigenvalues()[ 2 ] > 3 * saes.eigenvalues()[ 1 ] )
                        {
                            line_is_avail = true;
                        }
                        else
                        {
                            line_is_avail = false;
                        }
                    }
 
                    Eigen::Vector3d curr_point( pointOri.x, pointOri.y, pointOri.z );
                    if ( line_is_avail )
                    {
                        if ( ICP_LINE )
                        {
                            ceres::CostFunction *cost_function;
 
                            //将pcl点转换为eigen向量
                            auto                 pt_1 = pcl_pt_to_eigend( laser_cloud_corner_from_map.points[ m_point_search_Idx[ 0 ] ] );
                            auto                 pt_2 = pcl_pt_to_eigend( laser_cloud_corner_from_map.points[ m_point_search_Idx[ 1 ] ] );
                            if((pt_1 -pt_2).norm() < 0.0001)
                              continue;
                            if ( m_if_motion_deblur )
                            {
                                //printf_line;
                                //求解线特征之间的优化，优化的变量是直线向量与另一个向量的投影之差，其实就是夹角大小
                                //相对地，面特征之间的优化，优化的变量是一个平面的法向量与另一个平面上两点之间的向量的乘积
                                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 );
                            }
                            //printf_line;
                            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
                    {
                        corner_rejection_num++;
                    }
                }
            }
 
    .......
}

三、回环检测

除了位姿求解部分，该版本还添加了发布地图线程与回环检测的线程，回环后位姿优化部分，它也是利用ceres建立“图优化”模型，处理方式与vins类似，而其回环触发条件是新颖之处。重点看它的回环触发条件，主要实现在cell_map_keyframe.hpp中。

在单帧处理函数process中，计算位姿完毕后，将这帧点云（世界坐标系）作网格划分，加入到地图网格中。

//对每个体素网格中的点云进行方差矩阵分析，方差过大则认为是无特征（球形特征）
//对方差矩阵进行特征值分解，若只有一个出众特征值则是线特征，否则是面特征
m_keyframe_need_precession_list.front()->update_features_of_each_cells();
            
//根据关键帧中所有的面特征，选择最大的面特征（基于特征值分解）的特征向量，与其它线、面特征的向量相乘
//得到线特征、面特征分别的“特征矩阵”，
m_keyframe_need_precession_list.front()->analyze();

回环检测以及位姿矫正的处理在service_loop_detection线程中。从代码中可以看出，在老版本回环检测的实现中，是将特征栅格投影至图中，依靠opencv的直方图进行对比：

FUNC_T static float similiarity_of_two_image_opencv( const Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > &img_a, const Eigen::Matrix< float, Eigen::Dynamic, Eigen::Dynamic > &img_b, int method = CV_COMP_CORREL )
{
    cv::Mat hist_a, hist_b;
    cv::eigen2cv( img_a, hist_a );
    cv::eigen2cv( img_b, hist_b );
    return cv::compareHist( hist_a, hist_b, method ); // https://docs.opencv.org/2.4/modules/imgproc/doc/histograms.html?highlight=comparehist
}

而在新版本的实现中，关键帧之间的比较更复杂了一点，虽然在我看来，在大部分场景下两个版本表现差距不大，但老版本的处理方式缺少一帧中线面特征各自的相关性，更着重于数量，这一项新版本实现较好。

//向线特征或面特征的特征矩阵中，增加边界，类似于卷积神经网络的padding操作
res_img = add_padding_to_feature_image( img_b, PHI_RESOLUTION*0.5, THETA_RESOLUTION*0.5 );
            
//使用模板匹配中的标准相关性匹配，通过两幅图像相乘，若结果越大则相关性越好
max_res = max_similiarity_of_two_image_opencv(img_a, res_img);

总体来看，不得不说，loam-livox整体的代码风格和阅读体验还是有一定提升空间，可能也是学生执笔的缘故，这点情有可原。另外，工程中存在大量的遍历和内存申请释放，以及写文件行为，拖慢了运行速度，这些都可以酌情优化。但是，作为早在19年发布的livox雷达所配套的单一传感器的SLAM算法，是整个系列的开山之作，使我受益匪浅，也值得大家阅读。