数据预处理（18）_将单线激光雷达的LaserScan数据从极坐标下转到笛卡尔坐标系并存储

方法1：

void AutoGetLine::TransScanToPoints(const sensor_msgs::LaserScan& scan_in,std::vector<Eigen::Vector3d>& Points)
{
    size_t pts_num=scan_in.ranges.size();
    Eigen::ArrayXXd ranges_d(pts_num,2);//pt_num行，2列
    Eigen::ArrayXXd output_xy(pts_num,2);
    Eigen::ArrayXXd angle(pts_num,2);
    for(size_t i=0;i<pts_num;i++)
    {
        ranges_d(i,0)=(double)scan_in.ranges[i];//距离
        ranges_d(i,1)=(double)scan_in.ranges[i];//距离
        angle(i,0)=cos(scan_in.angle_min+(double)i*scan_in.angle_increment);
        angle(i,1)=sin(scan_in.angle_min+(double)i*scan_in.angle_increment);
    }
    output_xy=ranges_d*angle;
    for(size_t i=0;i<pts_num;i++)
    {
        double range_cutoff=10;//假设有效距离10m
        const float range=scan_in.ranges[i];
        if(range<range_cutoff&& range>scan_in.range_min)
            Points.push_back(Eigen::Vector3d(output_xy(i,0),output_xy(i,1),0));
        else
            Points.push_back(Eigen::Vector3d(1000.0,1000.0,0));
        }
    }
}

方法2：这里转换的方式非常巧妙，先是把第一帧激光线按照角度划分，然后给定相应角度的索引值，激光测量的距离值直接按照索引值对应的角度、正余弦即可计算出笛卡尔坐标系下的X、Y。

//数据结构
struct CachedData
{
  std::vector<unsigned int> indices;
  std::vector<double> bearings;
  std::vector<double> cos_bearings;
  std::vector<double> sin_bearings;
};

struct RangeData
{
  std::vector<double> ranges;
  std::vector<double> xs;
  std::vector<double> ys;
};
//数据设置
void LineExtraction::setCachedData(const std::vector<double>& bearings,
                                   const std::vector<double>& cos_bearings,
                                   const std::vector<double>& sin_bearings,
                                   const std::vector<unsigned int>& indices)
{
  c_data_.bearings = bearings;
  c_data_.cos_bearings = cos_bearings;
  c_data_.sin_bearings = sin_bearings;
  c_data_.indices = indices;
}

void LineExtraction::setRangeData(const std::vector<double>& ranges)
{
  r_data_.ranges = ranges;
  r_data_.xs.clear();
  r_data_.ys.clear();
  for (std::vector<unsigned int>::const_iterator cit = c_data_.indices.begin(); cit != c_data_.indices.end(); ++cit)
  {
    r_data_.xs.push_back(c_data_.cos_bearings[*cit] * ranges[*cit]);
    r_data_.ys.push_back(c_data_.sin_bearings[*cit] * ranges[*cit]);
  }
}
//缓存第一帧数据
void LineExtractionROS::cacheData(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
  std::vector<double> bearings, cos_bearings, sin_bearings;
  std::vector<unsigned int> indices;
  const std::size_t num_measurements = std::ceil((scan_msg->angle_max - scan_msg->angle_min) / scan_msg->angle_increment);
  for (std::size_t i = 0; i < num_measurements; ++i)
  {
    const double b = scan_msg->angle_min + i * scan_msg->angle_increment;
    bearings.push_back(b);
    cos_bearings.push_back(cos(b));
    sin_bearings.push_back(sin(b));
    indices.push_back(i);
  }

  line_extraction_.setCachedData(bearings, cos_bearings, sin_bearings, indices);
  ROS_DEBUG("Data has been cached.");
}

//laserscan的回调函数
void LineExtractionROS::laserScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
  if (!data_cached_)//因为每一帧数据角度的变化是固定的，只运行一次作缓存即可
  {
    cacheData(scan_msg); 
    data_cached_ = true;
  }
  std::vector<double> scan_ranges_doubles(scan_msg->ranges.begin(), scan_msg->ranges.end());
  line_extraction_.setRangeData(scan_ranges_doubles);
}