主要是LaserScanMatcher的构造函数：

ros::init(argc, argv, "LaserScanMatcher");
ros::NodeHandle nh;
ros::NodeHandle nh_private("~");
scan_tools::LaserScanMatcher laser_scan_matcher(nh, nh_private);
ros::spin();

构造函数里初始化一系列参数，重点就是回调函数scanCallback

// 第一帧scan, cache the tf from base to the scanner
if (!initialized_)
{
  createCache(scan_msg);    // caches the sin and cos of all angles
  // cache the static tf from base to laser
  if (!getBaseToLaserTf(scan_msg->header.frame_id))
  {
    ROS_WARN("Skipping scan");
    return;
  }
  // 保存scan数据到  LDP  prev_ldp_scan_;
  laserScanToLDP(scan_msg, prev_ldp_scan_);
  last_icp_time_ = scan_msg->header.stamp;
  initialized_ = true;
}
// 第二帧及以后的scan
index++;		// static unsigned int index;
ROS_INFO("scan index: %d", index);
LDP  curr_ldp_scan;
laserScanToLDP(scan_msg, curr_ldp_scan);
processScan(curr_ldp_scan, scan_msg->header.stamp);

公式计算

newKeyframeNeeded

if (fabs(tf::getYaw(d.getRotation())) > kf_dist_angular_)
   return   true;
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x*x + y*y > kf_dist_linear_sq_)
   return  true;

return false;

优先判断相对旋转

调用是这样：

// 交换新旧frame, key frame 的意思应该是当前配准时的参考帧
// 这个在机器人一直走直线或原地旋转时很难发生
if (newKeyframeNeeded(corr_ch))
{
	// generate  a  keyframe
	ld_free(prev_ldp_scan_);
	prev_ldp_scan_ = curr_ldp_scan;
	// 更新下一个时间段的 f2b_kf
	f2b_kf_ = f2b_;
}

prev_ldp_scan_之前是第一帧scan的数据，只有需要 key frame时，才会更新它，但在processScan开头又会把估计位姿和真值初始化为0。 如果没有达到运动阈值，prev_ldp_scan_还是第一帧scan，时间也是第一帧的时间，仍以第一帧为基准进行ICP配准