LidarView源码分析(九)vtkVelodyneBasePacketInterpreter类

纯虚函数

vtkVelodyneBasePacketInterpreter继承自vtkLidarPacketInterpreter。

vtkLidarPacketInterpreter继承自vtkInterpreter。

vtkInterpreter是一个虚基类,其中的纯虚函数有:

   /**
   * @brief ProcessPacket process the data packet to get the current informations
   * @param data raw data packet
   * @param dataLength size of the data packet
   * @param PacketReceptionTime
   */
  virtual void ProcessPacket(unsigned char const * data, unsigned int dataLength) = 0;


  /**
   * @brief IsValidPacket check if the packet is valid
   * @param data raw data packet
   * @param dataLength size of the data packet
   */
  virtual bool IsValidPacket(unsigned char const * data, unsigned int dataLength) = 0;


  /**
   * @brief IsNewData check if new data is available
   */
  virtual bool IsNewData() = 0;


  /**
   * @brief ResetCurrentData reset the current data
   */
  virtual void ResetCurrentData() = 0;

vtkLidarPacketInterpreter中的纯虚函数有如下:

 /**
   * @brief LoadCalibration read a provided calibration file to initialize the sensor's
   * calibration parameters (angles corrections, distances corrections, ...) which will be
   * used later on while processing the packet.
   * @param filename should be garanty to exist, as no check will be perform.
   */
  virtual void LoadCalibration(const std::string& filename) = 0;


  /**
   * @brief PreProcessPacket is use to construct the frame index and get some corretion
   * or live calibration comming from the data. A warning should be raise in case the calibration
   * information does not match the data (ex: factory field, number of laser, ...)
   * @param data raw data packet
   * @param dataLength size of the data packet
   * @param packetInfo[out] Miscellaneous information about the packet
   */
  virtual bool PreProcessPacket(unsigned char const * data, unsigned int dataLength,
                                fpos_t filePosition = fpos_t(), double packetNetworkTime = 0,
                                std::vector<FrameInformation>* frameCatalog = nullptr) = 0;


  /**
   * @brief IsLidarPacket check if the given packet is really a lidar packet
   * @param data raw data packet
   * @param dataLength size of the data packet
   */
  virtual bool IsLidarPacket(unsigned char const * data, unsigned int dataLength) = 0;


 /**
   * @brief GetSensorInformation return information to display to the user
   * @param shortVersion True to have a succinct version of the sensor information
   * @return
   */
  virtual std::string GetSensorInformation(bool shortVersion = false) = 0;


  /**
   * @brief CreateNewEmptyFrame construct a empty polyData with the right DataArray and allocate some
   * space. No CellArray should be created as it can be create once the frame is ready.
   * @param numberOfPoints indicate the space to allocate @todo change the meaning
   */
  virtual vtkSmartPointer<vtkPolyData> CreateNewEmptyFrame(vtkIdType numberOfPoints, vtkIdType prereservedNumberOfPoints = 0) = 0;

vtkLidarPacketInterpreter中实现了vtkInterpreter类中的纯虚函数函数如下:

bool vtkLidarPacketInterpreter::IsNewData()
{
  return this->IsNewFrameReady();
}

bool vtkLidarPacketInterpreter::IsValidPacket(unsigned char const * data, unsigned int dataLength)
{
  return this->IsLidarPacket(data, dataLength);
}

void vtkLidarPacketInterpreter::ResetCurrentData()
{
  this->ResetCurrentFrame();
}

vtkLidarPacketInterpreter中定义了一个纯虚函数:

  /**
   * @brief GetSensorName return the name of the lidar sensor used
   */
  virtual std::string GetSensorName() = 0;

该函数用于在子类中实现,用于获取特定子类的传感器名称。

因此,作为继承自vtkLidarPacketInterpreter的子类,需要实现的函数有:

 /**
   * @brief ProcessPacket process the data packet to get the current informations
   * @param data raw data packet
   * @param dataLength size of the data packet
   * @param PacketReceptionTime
   */
  virtual void ProcessPacket(unsigned char const * data, unsigned int dataLength) = 0;


  /**
   * @brief LoadCalibration read a provided calibration file to initialize the sensor's
   * calibration parameters (angles corrections, distances corrections, ...) which will be
   * used later on while processing the packet.
   * @param filename should be garanty to exist, as no check will be perform.
   */
  virtual void LoadCalibration(const std::string& filename) = 0;


  /**
   * @brief PreProcessPacket is use to construct the frame index and get some corretion
   * or live calibration comming from the data. A warning should be raise in case the calibration
   * information does not match the data (ex: factory field, number of laser, ...)
   * @param data raw data packet
   * @param dataLength size of the data packet
   * @param packetInfo[out] Miscellaneous information about the packet
   */
  virtual bool PreProcessPacket(unsigned char const * data, unsigned int dataLength,
                                fpos_t filePosition = fpos_t(), double packetNetworkTime = 0,
                                std::vector<FrameInformation>* frameCatalog = nullptr) = 0;


  /**
   * @brief IsLidarPacket check if the given packet is really a lidar packet
   * @param data raw data packet
   * @param dataLength size of the data packet
   */
  virtual bool IsLidarPacket(unsigned char const * data, unsigned int dataLength) = 0;


  /**
   * @brief GetSensorInformation return information to display to the user
   * @param shortVersion True to have a succinct version of the sensor information
   * @return
   */
  virtual std::string GetSensorInformation(bool shortVersion = false) = 0;


 /**
   * @brief CreateNewEmptyFrame construct a empty polyData with the right DataArray and allocate some
   * space. No CellArray should be created as it can be create once the frame is ready.
   * @param numberOfPoints indicate the space to allocate @todo change the meaning
   */
  virtual vtkSmartPointer<vtkPolyData> CreateNewEmptyFrame(vtkIdType numberOfPoints, vtkIdType prereservedNumberOfPoints = 0) = 0;


  /**
   * @brief GetSensorName return the name of the lidar sensor used
   */
  virtual std::string GetSensorName() = 0;

vtkVelodyneBasePacketInterpreter

vtkVelodyneBasePacketInterpreter作为velodyne雷达数据包的解析器的基类,并没有实现ProcessPacket、PreProcessPacket、IsLidarPacket、GetSensorInformation、CreateNewEmptyFrame函数,这些函数需要到vtkVelodyneBasePacketInterpreter的子类中实现。因此vtkVelodyneBasePacketInterpreter仍然是一个纯虚基类。

vtkVelodyneBasePacketInterpreter的主要工作是加载配置文件,计算角度查找表等一些公用功能。

ComputeCorrectedValues

在VelodyneInterpreterCommon.h中定义了RawValues和CorrectedValues结构体。所有解析后的数据都会存到RawValues中,然后调用vtkVelodyneBasePacketInterpreter中的函数ComputeCorrectedValues将RawValues转换成CorrectedValues,其过程包含将雷达的测量数据转换成以米或者角度为单位的物理值,然后将极坐标转换到三维直角坐标系。

// Raw input values required by ComputeCorrectedValues.
struct RawValues
{
  //! @brief The azimuthal angle, in centidegrees.
  unsigned short azimuth;
  //! @brief The altitudinal angle, in centidegrees.
  unsigned short elevation;
  //! @brief The distance, in raw units.
  uint32_t distance;
  //! @brief The intensity, in raw units.
  uint32_t intensity;

  RawValues(
    decltype(azimuth) azm,
    decltype(elevation) elev,
    decltype(distance) dist,
    decltype(intensity) inten = 0
  )
    : azimuth { azm }
    , elevation { elev }
    , distance { dist }
    , intensity { inten }
  {
  }
};

//-----------------------------------------------------------------------------
// ComputeCorrectedValues needs to calculate multiple corrected values which
// may or may not be used by different interpreters. Collect these in a common
// struct so that the interpreter can easily ignore unused values without
// creating multiple dummy variables just to invoke the function.
struct CorrectedValues
{
  double position[3];
  double distance;
  short intensity;
  double elevation;
};

GetXMLColorTable

该函数将从配置文件中加载的颜色信息,进行输出。文件中的颜色信息是红绿蓝三个元素,在此加上了第四个元素。

void vtkVelodyneBasePacketInterpreter::GetXMLColorTable(double XMLColorTable[4 * HDL_MAX_NUM_LASERS])
{
  for (int i = 0; i < HDL_MAX_NUM_LASERS; ++i)
  {
    XMLColorTable[i * 4] = static_cast<double>(i) / 63.0 * 255.0;
    for (int j = 0; j < 3; ++j)
    {
      XMLColorTable[i * 4 + j + 1] = this->XMLColorTable[i][j];
    }
  }
}

HDL64LoadCorrectionsFromStreamData

该函数从数据流中加载HDL64型号雷达的配置数据,保存到对应的配置变量中。该过程和雷达传输数据的具体内容相关。

bool vtkVelodyneBasePacketInterpreter::HDL64LoadCorrectionsFromStreamData(vtkRollingDataAccumulator * rollingCalibrationData)
{
  std::vector<unsigned char> data;
  if (!rollingCalibrationData->getAlignedRollingData(data))
  {
    return false;
  }
  // the rollingCalibrationData considers the marker to be "#" in reserved4
  const int idxDSRDataFromMarker =
    static_cast<int>(-reinterpret_cast<unsigned long>(&((HDLLaserCorrectionByte*)0)->reserved4));
  const int HDL64_RollingData_NumLaser = 64;
  for (int dsr = 0; dsr < HDL64_RollingData_NumLaser; ++dsr)
  {
    const HDLLaserCorrectionByte* correctionStream = reinterpret_cast<const HDLLaserCorrectionByte*>
      // The 64 here is the length of the 4 16-byte cycle
      //    containing one dsr information
      (&data[idxDSRDataFromMarker + 64 * dsr]);
    if (correctionStream->channel != dsr)
    {
      return false;
    }
    HDLLaserCorrection& vvCorrection = laser_corrections_[correctionStream->channel];
    vvCorrection.verticalCorrection = correctionStream->verticalCorrection / 100.0;
    vvCorrection.rotationalCorrection = correctionStream->rotationalCorrection / 100.0;
    vvCorrection.distanceCorrection = correctionStream->farDistanceCorrection / 1000.0;

    vvCorrection.distanceCorrectionX = correctionStream->distanceCorrectionX / 1000.0;
    vvCorrection.distanceCorrectionY = correctionStream->distanceCorrectionV / 1000.0;
    vvCorrection.verticalOffsetCorrection = correctionStream->verticalOffset / 1000.0;
    // The following manipulation is needed because of the two byte for this
    //  parameter are not side-by-side
    vvCorrection.horizontalOffsetCorrection =
      rollingCalibrationData->fromTwoLittleEndianBytes<signed short>(
        correctionStream->horizontalOffsetByte1, correctionStream->horizontalOffsetByte2) /
      1000.0;
    vvCorrection.focalDistance = correctionStream->focalDistance / 1000.0;
    vvCorrection.focalSlope = correctionStream->focalSlope / 1000.0;
    vvCorrection.closeSlope = correctionStream->focalSlope / 1000.0;
    vvCorrection.minIntensity = correctionStream->minIntensity;
    vvCorrection.maxIntensity = correctionStream->maxIntensity;
  }

  // Get the last cycle of live correction file
  const last4cyclesByte* lastCycle = reinterpret_cast<const last4cyclesByte*>(
    &data[idxDSRDataFromMarker + 64 * HDL64_RollingData_NumLaser]);
  this->SensorPowerMode = lastCycle->powerLevelStatus;
  this->ReportedSensorReturnMode = ((lastCycle->multipleReturnStatus == 0)
      ? STRONGEST_RETURN
      : ((lastCycle->multipleReturnStatus == 1) ? LAST_RETURN : DUAL_RETURN));

  this->CalibrationReportedNumLasers = HDL64_RollingData_NumLaser;
  this->PrecomputeCorrectionCosSin();
  this->IsCalibrated = true;
  return true;
}

GetLaserCorrections

将配置数据进行输出。

void vtkVelodyneBasePacketInterpreter::GetLaserCorrections(double verticalCorrection[HDL_MAX_NUM_LASERS],
  double rotationalCorrection[HDL_MAX_NUM_LASERS], double distanceCorrection[HDL_MAX_NUM_LASERS],
  double distanceCorrectionX[HDL_MAX_NUM_LASERS], double distanceCorrectionY[HDL_MAX_NUM_LASERS],
  double verticalOffsetCorrection[HDL_MAX_NUM_LASERS],
  double horizontalOffsetCorrection[HDL_MAX_NUM_LASERS], double focalDistance[HDL_MAX_NUM_LASERS],
  double focalSlope[HDL_MAX_NUM_LASERS], double minIntensity[HDL_MAX_NUM_LASERS],
  double maxIntensity[HDL_MAX_NUM_LASERS])
{
  for (int i = 0; i < HDL_MAX_NUM_LASERS; ++i)
  {
    verticalCorrection[i] = laser_corrections_[i].verticalCorrection;
    rotationalCorrection[i] = laser_corrections_[i].rotationalCorrection;
    distanceCorrection[i] = laser_corrections_[i].distanceCorrection;
    distanceCorrectionX[i] = laser_corrections_[i].distanceCorrectionX;
    distanceCorrectionY[i] = laser_corrections_[i].distanceCorrectionY;
    verticalOffsetCorrection[i] = laser_corrections_[i].verticalOffsetCorrection;
    horizontalOffsetCorrection[i] =
      laser_corrections_[i].horizontalOffsetCorrection;
    focalDistance[i] = laser_corrections_[i].focalDistance;
    focalSlope[i] = laser_corrections_[i].focalSlope;
    minIntensity[i] = laser_corrections_[i].minIntensity;
    maxIntensity[i] = laser_corrections_[i].maxIntensity;
  }
}

GetDefaultRecordFileName

生成保存数据的名字。文件名中包含保存数据的时间,Velodyne,传感器名称以及后缀“-Data”。

std::string vtkVelodyneBasePacketInterpreter::GetDefaultRecordFileName()
{
  // Add time string YYYY-mm-dd-HH-MM-SS
  std::string defaultFileName = vtkLidarPacketInterpreter::GetDefaultRecordFileName();

  // Add Lidar sensor name
  defaultFileName += "_Velodyne-" + this->GetSensorName() + "-Data";

  return defaultFileName;
}

PrecomputeCorrectionCosSin

预先计算cos和sin的值。

void
vtkVelodyneBasePacketInterpreter::PrecomputeCorrectionCosSin()
{
  for (int i = 0; i < HDL_MAX_NUM_LASERS; i++)
  {
    HDLLaserCorrection & correction = laser_corrections_[i];
    correction.cosVertCorrection =
      std::cos(degreesToRadians(correction.verticalCorrection));
    correction.sinVertCorrection =
      std::sin(degreesToRadians(correction.verticalCorrection));
    correction.cosRotationalCorrection =
      std::cos(degreesToRadians(correction.rotationalCorrection));
    correction.sinRotationalCorrection =
      std::sin(degreesToRadians(correction.rotationalCorrection));
    correction.sinVertOffsetCorrection =
      correction.verticalOffsetCorrection * correction.sinVertCorrection;
    correction.cosVertOffsetCorrection =
      correction.verticalOffsetCorrection * correction.cosVertCorrection;
  }
}

InitTrigonometricTables

预先计算cos和sin的值。

//------------------------------------------------------------------------------
// Code from the legacy packet format interpreter.
//------------------------------------------------------------------------------
void
vtkVelodyneBasePacketInterpreter::InitTrigonometricTables()
{
  if (cos_lookup_table_.size() == 0 || sin_lookup_table_.size() == 0)
  {
    cos_lookup_table_.resize(HDL_NUM_ROT_ANGLES);
    sin_lookup_table_.resize(HDL_NUM_ROT_ANGLES);
    for (unsigned int i = 0; i < HDL_NUM_ROT_ANGLES; i++)
    {
      double rad           = degreesToRadians(i / 100.0);
      cos_lookup_table_[i] = std::cos(rad);
      sin_lookup_table_[i] = std::sin(rad);
    }
  }
}

LoadCalibration

该函数是在vtkLidarPacketInterpreter中定义的纯虚函数,在此进行了重载。

该函数用于加载雷达的配置文件。使用了boost::property_tree::ptree。

ptree的定义如下:

typedef basic_ptree<std::string, std::string> ptree;

即其键和值都是std::string类型的。

该函数中读取了一系列用于雷达数据解析的参数,并且将保存在this->CalibrationData中。该变量是在vtkLidarPacketInterpreter中定义的。

 ///! Calibration data store in a table
  vtkNew<vtkTable> CalibrationData;

 LoadCalibration的代码如下:

void vtkVelodyneBasePacketInterpreter::LoadCalibration(const std::string& filename)
{
  // the HDL64 allow autocalibration, so no calibration can be provided
  if (filename.empty())
  {
    this->IsCalibrated = false;
    this->IsCorrectionFromLiveStream = true;
    return;
  }
  else
  {
    this->IsCorrectionFromLiveStream = false;
  }

  boost::property_tree::ptree pt;
  try
  {
    read_xml(filename, pt, boost::property_tree::xml_parser::trim_whitespace);
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro(
      "LoadCalibration: error reading calibration file: " << filename);
    return;
  }
  // Read framing logic mode if available.
  try
  {
    BOOST_FOREACH (boost::property_tree::ptree::value_type& v, pt.get_child("boost_serialization.DB"))
    {
      if (v.first == "framingLogic")
      {
        // TODO
        // Do this automatically with boost macros.
        auto framingLogic = v.second.data();
        std::transform(framingLogic.begin(), framingLogic.end(), framingLogic.begin(), ::toupper);

        if (framingLogic == toString(FramingLogic::FL_AZIMUTH_CROSSING))
        {
          this->FrameLogic = FramingLogic::FL_AZIMUTH_CROSSING;
        }
        else if (framingLogic == toString(FramingLogic::FL_VDIR_CHANGE))
        {
          this->FrameLogic = FramingLogic::FL_VDIR_CHANGE;
        }
        else //if (framingLogic == toString(FramingLogic::FL_DEFAULT))
        {
          this->FrameLogic = FramingLogic::FL_DEFAULT;
        }
        break;
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing DB in the calibration file: " << filename);
    return;
  }

  // Read distLSB if provided
  BOOST_FOREACH (boost::property_tree::ptree::value_type& v, pt.get_child("boost_serialization.DB"))
  {
    if (v.first == "distLSB_")
    { // Stored in cm in xml
      DistanceResolutionM = atof(v.second.data().c_str()) / 100.0;
    }
  }

  int i, j;
  i = 0;
  try {
    BOOST_FOREACH (
      boost::property_tree::ptree::value_type& p, pt.get_child("boost_serialization.DB.colors_"))
    {
      if (p.first == "item")
      {
        j = 0;
        try
        {
          BOOST_FOREACH (boost::property_tree::ptree::value_type& v, p.second.get_child("rgb"))
          if (v.first == "item")
          {
            std::stringstream ss;
            double val;
            ss << v.second.data();
            ss >> val;

            XMLColorTable[i][j] = val;
            j++;
          }
        }
        catch (boost::exception const&)
        {
          vtkGenericWarningMacro("Missing rgb value in the calibration file " << filename);
          return;
        }
        i++;
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing Colors in the calibration file: " << filename);
    return;
  }

  int enabledCount = 0;
  try
  {
    BOOST_FOREACH (
      boost::property_tree::ptree::value_type& v, pt.get_child("boost_serialization.DB.enabled_"))
    {
      std::stringstream ss;
      if (v.first == "item")
      {
        ss << v.second.data();
        int test = 0;
        ss >> test;
        if (!ss.fail() && test == 1)
        {
          enabledCount++;
        }
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing enabled property in the calibration file: " << filename);
    return;
  }

  this->CalibrationReportedNumLasers = enabledCount;

  // Getting min & max intensities from XML
  int laserId = 0;
  int minIntensity[HDL_MAX_NUM_LASERS], maxIntensity[HDL_MAX_NUM_LASERS];
  try
  {
    BOOST_FOREACH (boost::property_tree::ptree::value_type& v,
      pt.get_child("boost_serialization.DB.minIntensity_"))
    {
      std::stringstream ss;
      if (v.first == "item")
      {
        ss << v.second.data();
        ss >> minIntensity[laserId];
        laserId++;
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing min Intensity in the calibration file: " << filename);
    return;
  }

  laserId = 0;
  try
  {
    BOOST_FOREACH (boost::property_tree::ptree::value_type& v,
      pt.get_child("boost_serialization.DB.maxIntensity_"))
    {
      std::stringstream ss;
      if (v.first == "item")
      {
        ss << v.second.data();
        ss >> maxIntensity[laserId];
        laserId++;
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing max Intensity in the calibration file: " << filename);
    return;
  }

  try
  {
    BOOST_FOREACH (
      boost::property_tree::ptree::value_type& v, pt.get_child("boost_serialization.DB.points_"))
    {
      if (v.first == "item")
      {
        boost::property_tree::ptree points = v.second;
        BOOST_FOREACH (boost::property_tree::ptree::value_type& px, points)
        {
          if (px.first == "px")
          {
            boost::property_tree::ptree calibrationData = px.second;
            int index = -1;
            HDLLaserCorrection xmlData;

            BOOST_FOREACH (boost::property_tree::ptree::value_type& item, calibrationData)
            {
              if (item.first == "id_")
                index = atoi(item.second.data().c_str());
              if (item.first == "rotCorrection_")
                xmlData.rotationalCorrection = atof(item.second.data().c_str());
              if (item.first == "vertCorrection_")
                xmlData.verticalCorrection = atof(item.second.data().c_str());
              if (item.first == "distCorrection_")
                xmlData.distanceCorrection = atof(item.second.data().c_str());
              if (item.first == "distCorrectionX_")
                xmlData.distanceCorrectionX = atof(item.second.data().c_str());
              if (item.first == "distCorrectionY_")
                xmlData.distanceCorrectionY = atof(item.second.data().c_str());
              if (item.first == "vertOffsetCorrection_")
                xmlData.verticalOffsetCorrection = atof(item.second.data().c_str());
              if (item.first == "horizOffsetCorrection_")
                xmlData.horizontalOffsetCorrection = atof(item.second.data().c_str());
              if (item.first == "focalDistance_")
                xmlData.focalDistance = atof(item.second.data().c_str());
              if (item.first == "focalSlope_")
                xmlData.focalSlope = atof(item.second.data().c_str());
              if (item.first == "closeSlope_")
                xmlData.closeSlope = atof(item.second.data().c_str());
            }
            if (index != -1 && index < HDL_MAX_NUM_LASERS)
            {
              laser_corrections_[index] = xmlData;
              // Angles are already stored in degrees in xml
              // Distances are stored in centimeters in xml, and we store meters.
              laser_corrections_[index].distanceCorrection /= 100.0;
              laser_corrections_[index].distanceCorrectionX /= 100.0;
              laser_corrections_[index].distanceCorrectionY /= 100.0;
              laser_corrections_[index].verticalOffsetCorrection /= 100.0;
              laser_corrections_[index].horizontalOffsetCorrection /= 100.0;
              laser_corrections_[index].focalDistance /= 100.0;
              laser_corrections_[index].focalSlope /= 100.0;
              laser_corrections_[index].closeSlope /= 100.0;
              if (laser_corrections_[index].closeSlope == 0.0)
                laser_corrections_[index].closeSlope = laser_corrections_[index].focalSlope;
              laser_corrections_[index].minIntensity = minIntensity[index];
              laser_corrections_[index].maxIntensity = maxIntensity[index];
            }
          }
        }
      }
    }
  }
  catch (boost::exception const&)
  {
    vtkGenericWarningMacro("Missing Points in the calibration file: " << filename);
    return;
  }

  int idx = 0;
  BOOST_FOREACH (boost::property_tree::ptree::value_type& v,
    pt.get_child("boost_serialization.DB.minIntensity_"))
  {
    std::stringstream ss;
    if (v.first == "item")
    {
      ss << v.second.data();
      int intensity = 0;
      ss >> intensity;
      if (!ss.fail() && idx < HDL_MAX_NUM_LASERS)
      {
        laser_corrections_[idx].minIntensity = intensity;
      }
      idx++;
    }
  }

  idx = 0;
  BOOST_FOREACH (boost::property_tree::ptree::value_type& v,
    pt.get_child("boost_serialization.DB.maxIntensity_"))
  {
    std::stringstream ss;
    if (v.first == "item")
    {
      ss << v.second.data();
      int intensity = 0;
      ss >> intensity;
      if (!ss.fail() && idx < HDL_MAX_NUM_LASERS)
      {
        laser_corrections_[idx].maxIntensity = intensity;
      }
      idx++;
    }
  }

  PrecomputeCorrectionCosSin();
  this->IsCalibrated = true;
  this->CalibrationData->Initialize();
//  // Copy the calibration into a vtkTable
  #define AddToCalibrationDataRowNamed(name, field)                                     \
  auto array##field = vtkSmartPointer<vtkDoubleArray>::New();                           \
  array##field->SetName(name);                                                          \
  for (int i = 0; i < this->CalibrationReportedNumLasers; i++)                          \
  {                                                                                     \
    array##field->InsertNextTuple1(this->laser_corrections_[i].field);                  \
  }                                                                                     \
  this->CalibrationData->AddColumn(array##field);

  AddToCalibrationDataRowNamed("rotationalCorrection",      rotationalCorrection)
  AddToCalibrationDataRowNamed("verticalCorrection",        verticalCorrection)
  AddToCalibrationDataRowNamed("distanceCorrection",        distanceCorrection)
  AddToCalibrationDataRowNamed("distanceCorrectionX",       distanceCorrectionX)
  AddToCalibrationDataRowNamed("distanceCorrectionY",       distanceCorrectionY)
  AddToCalibrationDataRowNamed("verticalOffsetCorrection",  verticalOffsetCorrection)
  AddToCalibrationDataRowNamed("horizontalOffsetCorrection",horizontalOffsetCorrection)
  AddToCalibrationDataRowNamed("focalDistance",             focalDistance)
  AddToCalibrationDataRowNamed("focalSlope",                focalSlope)
  AddToCalibrationDataRowNamed("closeSlope",                closeSlope)
  AddToCalibrationDataRowNamed("minIntensity",              minIntensity)
  AddToCalibrationDataRowNamed("maxIntensity",              maxIntensity)
  AddToCalibrationDataRowNamed("sinRotationalCorrection",   sinRotationalCorrection)
  AddToCalibrationDataRowNamed("cosRotationalCorrection",   cosRotationalCorrection)
  AddToCalibrationDataRowNamed("sinVertCorrection",         sinVertCorrection)
  AddToCalibrationDataRowNamed("cosVertCorrection",         cosVertCorrection)
  AddToCalibrationDataRowNamed("sinVertOffsetCorrection",   sinVertOffsetCorrection)
  AddToCalibrationDataRowNamed("cosVertOffsetCorrection",   cosVertOffsetCorrection)
}

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值