LidarView源码分析（十）vtkVelodyneLegacyPacketInterpreter类-CSDN博客

本文链接：https://blog.csdn.net/Luiiiiiiiiiis/article/details/131836432

纯虚函数

vtkVelodyneLegacyPacketInterpreter的基类是vtkVelodyneBasePacketInterpreter。

需要实现的纯虚函数如下：

 /**
   * @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 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;

vtkVelodyneLegacyPacketInterpreter

vtkVelodyneLegacyPacketInterpreter是用于解析雷达数据包的具体实现类。该类中实现了解析Velodyne雷达数据包的函数，并将数据累积成完整的一帧数据。解析雷达数据包的函数为ProcessPacket，保存完整一帧数据的函数为SplitFrame。具体的解析过程见下图：

ProcessPacket

声明如下：

void ProcessPacket(unsigned char const * data, unsigned int dataLength) override;

该函数接受一个无符号字符数组以及该数组的长度。在该函数内对数组中的内容进行解析，如果数据未满一帧，则保存在this->CurrentFrame中，如果数据已经积累够一帧，则使用SplitFrame将完整的一帧数据存放在this->Frames中，然后将this->CurrentFrame重置，用于下一帧数据的接受。

该函数中还根据数据包中存储的雷达信息进行了不同的处理，保存了不同的信息，包括雷达型号，回波类型，线数等。具体见一下代码：

void vtkVelodyneLegacyPacketInterpreter::ProcessPacket(unsigned char const * data, unsigned int dataLength)
{
  if (!this->IsLidarPacket(data, dataLength))
  {
    return;
  }

  // Cast Generic Packet DataStructure: 12 HDLFiringData Blocks/TimeStamp/Factory Bytes
  // 该方式需要确认data的长度和HDLDataPacket类的长度一致
  // 以及保证数据属性的顺序和HDLDataPacket中变量的声明顺序一直
  // 以及data中的数据和DHLDataPacket在内存中的存储方式一直（同为大端或者同为小端）
  const HDLDataPacket* dataPacket = reinterpret_cast<const HDLDataPacket*>(data);

  // HDL64 Specific
  this->IsHDL64Data |= dataPacket->isHDL64();
  // Accumulate HDL64 Status byte data
  if (IsHDL64Data && this->IsCorrectionFromLiveStream &&
    !this->IsCalibrated)
  {
    this->rollingCalibrationData->appendData(dataPacket->TohTimestamp, dataPacket->factoryField1, dataPacket->factoryField2);
    // 从数据流中获取配置信息
    this->HDL64LoadCorrectionsFromStreamData(this->rollingCalibrationData);
    return;
  }

  // Check once for calibration file consistency
  if (this->ShouldCheckSensor)
  {
    // 数据包中包含了雷达型号信息，判断和配置文件中的是否一直
    // 如果是从数据流中获取的配置信息，则直接返回true
    this->CheckReportedSensorAndCalibrationFileConsistent(dataPacket);
    ShouldCheckSensor = false;
  }

  // Process Time
  this->CurrTimestamp = dataPacket->TohTimestamp;

  // Check if the time has rolled during this packet
  if (1e-6 * static_cast<double>(this->CurrTimestamp) < this->ParserMetaData.FirstPacketDataTime)
  {
    reinterpret_cast<VelodyneSpecificFrameInformation*>
        (this->ParserMetaData.SpecificInformation.get())->NbrOfRollingTime++;
  }
  this->ParserMetaData.FirstPacketDataTime = 1e-6 * static_cast<double>(this->CurrTimestamp);

  const unsigned long long timestamp = this->ComputeTimestamp(this->CurrTimestamp, this->ParserMetaData);

  // Update the rpm computation (by packets)
  // 在SplitFrame中通过RpmCalculator计算了RPM，并保存到this->Rpm中，然后重置了this->RpmCalculator_
  this->RpmCalculator_->AddData(dataPacket->firingData[0].getRotationalPosition(), this->CurrTimestamp);

  // Update the transforms here and then call internal transform
  if (SensorTransform) this->SensorTransform->Update();

  // Get Interpreter State Information
  VelodyneSpecificFrameInformation* velodyneFrameInfo =
      reinterpret_cast<VelodyneSpecificFrameInformation*>(this->ParserMetaData.SpecificInformation.get());

  // Firing Block Skipping Feature (Mid Frame Capture handling)
  int firingDataBlockIdx = velodyneFrameInfo->FiringToSkip;
  velodyneFrameInfo->FiringToSkip = 0;

  // Compute the list of total azimuth advanced during one full firing block
  bool isVLS128 = dataPacket->isVLS128();
  std::vector<int> diffs(HDL_FIRING_PER_PKT - 1);
  for (int i = 0; i < HDL_FIRING_PER_PKT - 1; ++i)
  {
    int localDiff = 0;

    if (!isVLS128)
    {
        localDiff = (36000 + 18000 + dataPacket->firingData[i + 1].getRotationalPosition() -
                      dataPacket->firingData[i].getRotationalPosition()) %
                      36000 - 18000;
    }
    diffs[i] = localDiff;
  }

  if (!IsHDL64Data)
  { // with HDL64, it should be filled by LoadCorrectionsFromStreamData
    this->ReportedSensor = dataPacket->getSensorType();
    this->ReportedSensorReturnMode = dataPacket->getDualReturnSensorMode();
  }

  std::sort(diffs.begin(), diffs.end());
  // Assume the median of the packet's rotationalPosition differences
  int azimuthDiff = diffs[HDL_FIRING_PER_PKT / 2];
  if (this->IsHDL64Data)
  {
    azimuthDiff = diffs[HDL_FIRING_PER_PKT - 2];
  }

  // assert(azimuthDiff > 0);

  // Add DualReturn-specific arrays if newly detected dual return packet
  if (dataPacket->isDualModeReturn() && !this->HasDualReturn)
  {
    this->HasDualReturn = true;
    AddArrayIfNotNull(this->CurrentFrame->GetPointData(), this->DistanceFlag);
    AddArrayIfNotNull(this->CurrentFrame->GetPointData(), this->IntensityFlag);
    AddArrayIfNotNull(this->CurrentFrame->GetPointData(), this->DualReturnMatching);
  }

  for (; firingDataBlockIdx < HDL_FIRING_PER_PKT; ++firingDataBlockIdx)
  {
    const HDLFiringData* firingData = &(dataPacket->firingData[firingDataBlockIdx]);
    const HDLFiringData* extData = NULL;
    // clang-format off
    int multiBlockLaserIdOffset =
        (firingData->blockIdentifier == BLOCK_0_TO_31)  ?  0 :(
        (firingData->blockIdentifier == BLOCK_32_TO_63) ? 32 :(
        (firingData->blockIdentifier == BLOCK_64_TO_95) ? 64 :(
        (firingData->blockIdentifier == BLOCK_96_TO_127)? 96 :(
                                                           0))));
    // clang-format on

    // Skip dummy blocks of VLS-128 dual mode last 4 blocks
    if (isVLS128 && (firingData->blockIdentifier == 0 || firingData->blockIdentifier == 0xFFFF || this->SkipExtDataBlock))
    {
      this->SkipExtDataBlock = false;
      continue;
    }

    if (isVLS128 && (dataPacket->getExtDataPacketType() > EXT_MODE_NONE))
    {
        // For extended data look ahead to every third block
        if (!dataPacket->isDualReturnFiringBlock(firingDataBlockIdx))
        {
            extData = &(dataPacket->firingData[firingDataBlockIdx+2]);
        }
        else
        {
            extData = &(dataPacket->firingData[firingDataBlockIdx+1]);

            // Skip next block as it contains extended data, not firing data
            this->SkipExtDataBlock = true;
        }
    }

    if (this->CurrentFrameState->hasChangedWithValue(*firingData))
    {
      // 如果累积够一帧数据。则进行数据帧拆分。
      // 注意，此时firingData中保存的是下一帧的数据。
      // 只有通过下一帧的数据和当前存储的数据进行比较，才能得出已经够一帧了。
      // 在此处调用SplitFrame，意味这拆分帧在由ProcessPacket函数进行控制
      // 即FramingMethod_t framingMethodAskingForSplitFrame = FramingMethod_t::INTERPRETER_FRAMING
      // 拆分完当前帧后，会重置this->CurrentFrame，用于后续继续向其添加数据。
      this->SplitFrame();
    }

    if (isVLS128)
    {
      azimuthDiff = dataPacket->getRotationalDiffForVLS128(firingDataBlockIdx, LastAzimuth);
      if (dataPacket->isDualModeReturn())
      {
        // Save last azimuth from final block in packet
        if (firingData->blockIdentifier == BLOCK_96_TO_127 && dataPacket->isDualReturnFiringBlock(firingDataBlockIdx))
        {
            this->LastAzimuth = dataPacket->firingData[firingDataBlockIdx].getRotationalPosition();
        }
      }
    }

    // FOV Reduction Feature: Jump Detection > 1 degree
    // WIP to be tested with non-VLS128 DATA, lastazimuth must be saved for both
    if (azimuthDiff * 0.01 > 1)
    {
      azimuthDiff = this->LastAzimuthDiff;
    }else{
      this->LastAzimuthDiff = azimuthDiff;
    }

    // Skip this firing every PointSkip
    if (this->FiringsSkip == 0 || firingDataBlockIdx % (this->FiringsSkip + 1) == 0)
    {
      // 处理数据包中的数据
      this->ProcessFiring(firingData, multiBlockLaserIdOffset, firingDataBlockIdx, azimuthDiff, timestamp,
        this->CurrTimestamp, dataPacket->isDualReturnFiringBlock(firingDataBlockIdx), dataPacket->isDualModeReturn(), extData, dataPacket->getExtDataPacketType());
    }
  }
}

ProcessFiring

ProcessFiring在ProcessPacket中被调用，用来处理数据包中存储的雷达测量数据。一个数据包中包含一些雷达的信息，以及测量数据。

void vtkVelodyneLegacyPacketInterpreter::ProcessFiring(const HDLFiringData *firingData, int multiBlockFiringLaserIdOffset, int firingDataBlockIdx, int azimuthDiff, unsigned long long timestamp, unsigned int rawtime, bool isThisFiringDualReturnData, bool isDualReturnPacket, const HDLFiringData *extData, int extDataPacketType)
{
  // First return block of a dual return packet: init last point of laser
  if (!isThisFiringDualReturnData &&
    (!this->IsHDL64Data || (this->IsHDL64Data && ((firingDataBlockIdx % 4) == 0))))
  {
    this->FirstPointIdOfDualReturnPair = this->Points->GetNumberOfPoints();
  }

  unsigned short firingElevation100th = firingData->getElevation100th();

  for (int dsrBase32 = 0; dsrBase32 < HDL_LASER_PER_FIRING; dsrBase32++)
  {
    const unsigned char channelNumberOr_dsrBase32_forVLP16 = static_cast<unsigned char>(dsrBase32 + multiBlockFiringLaserIdOffset);
    unsigned char channelNumber = channelNumberOr_dsrBase32_forVLP16;
    const unsigned short azimuth = firingData->getRotationalPosition();

    // Detect VLP-16 data and adjust laser id if necessary
    int firingWithinBlock = 0;

    if (this->CalibrationReportedNumLasers == 16)
    {
      if (multiBlockFiringLaserIdOffset != 0)
      {
        if (!this->alreadyWarnedForIgnoredHDL64FiringPacket)
        {
          vtkGenericWarningMacro("Error: Received a HDL-64 UPPERBLOCK firing packet "
                                 "with a VLP-16 calibration file. Ignoring the firing.");
          this->alreadyWarnedForIgnoredHDL64FiringPacket = true;
        }
        return;
      }
      if (channelNumber >= 16)
      {
        channelNumber -= 16;
        firingWithinBlock = 1;
      }
    }

    // 时间插值
    // Interpolate azimuths and timestamps per laser within firing blocks
    double timestampadjustment = 0;
    int azimuthadjustment = 0;
    if (this->UseIntraFiringAdjustment)
    {
      double blockdsr0 = 0, nextblockdsr0 = 1;
      switch (this->CalibrationReportedNumLasers)
      {
        case 128:
        {
          timestampadjustment =
            VLS128AdjustTimeStamp(firingDataBlockIdx, dsrBase32, isDualReturnPacket,
              extDataPacketType, this->VLS128_UseIntraFiringTimingFromOldFWVersions);

          if (isDualReturnPacket)
          {
              // With VLS-128 dual return packets only one dsr0 per packet, so this method will be used to
              // ensure azimuthadjustment is correctly derived below
              if (extDataPacketType > EXT_MODE_NONE)
              {
                nextblockdsr0 = VLS128AdjustTimeStamp(11, 32, isDualReturnPacket, extDataPacketType,
                  this->VLS128_UseIntraFiringTimingFromOldFWVersions);
                blockdsr0 = VLS128AdjustTimeStamp(0, 0, isDualReturnPacket, extDataPacketType,
                  this->VLS128_UseIntraFiringTimingFromOldFWVersions);
              }
              else
              {
                nextblockdsr0 = VLS128AdjustTimeStamp(7, 32, isDualReturnPacket, extDataPacketType,
                  this->VLS128_UseIntraFiringTimingFromOldFWVersions);
                blockdsr0 = VLS128AdjustTimeStamp(0, 0, isDualReturnPacket, extDataPacketType,
                  this->VLS128_UseIntraFiringTimingFromOldFWVersions);
              }
          }
          else
          {
            // dsr0 occurs every fourth block with VLS-128 single return packets
            nextblockdsr0 =
              VLS128AdjustTimeStamp((firingDataBlockIdx / 4) * 4 + 4, 0, isDualReturnPacket,
                extDataPacketType, this->VLS128_UseIntraFiringTimingFromOldFWVersions);
            blockdsr0 = VLS128AdjustTimeStamp((firingDataBlockIdx / 4) * 4, 0, isDualReturnPacket,
              extDataPacketType, this->VLS128_UseIntraFiringTimingFromOldFWVersions);
          }
          break;
        }
        case 64:
        {
          timestampadjustment = -HDL64EAdjustTimeStamp(firingDataBlockIdx, dsrBase32, isDualReturnPacket);
          nextblockdsr0 = -HDL64EAdjustTimeStamp(
            firingDataBlockIdx + (isDualReturnPacket ? 4 : 2), 0, isDualReturnPacket);
          blockdsr0 = -HDL64EAdjustTimeStamp(firingDataBlockIdx, 0, isDualReturnPacket);
          break;
        }
        case 32:
        {
          if (this->ReportedSensor == VLP32AB || this->ReportedSensor == VLP32C)
          {
            timestampadjustment = VLP32AdjustTimeStamp(firingDataBlockIdx, dsrBase32, isDualReturnPacket);
            nextblockdsr0 = VLP32AdjustTimeStamp(
              firingDataBlockIdx + (isDualReturnPacket ? 2 : 1), 0, isDualReturnPacket);
            blockdsr0 = VLP32AdjustTimeStamp(firingDataBlockIdx, 0, isDualReturnPacket);
          }
          else
          {
            timestampadjustment = HDL32AdjustTimeStamp(firingDataBlockIdx, dsrBase32, isDualReturnPacket);
            nextblockdsr0 = HDL32AdjustTimeStamp(
              firingDataBlockIdx + (isDualReturnPacket ? 2 : 1), 0, isDualReturnPacket);
            blockdsr0 = HDL32AdjustTimeStamp(firingDataBlockIdx, 0, isDualReturnPacket);
          }
          break;
        }
        case 16:
        {
          timestampadjustment =
            VLP16AdjustTimeStamp(firingDataBlockIdx, channelNumber, firingWithinBlock, isDualReturnPacket);
          nextblockdsr0 = VLP16AdjustTimeStamp(
            firingDataBlockIdx + (isDualReturnPacket ? 2 : 1), 0, 0, isDualReturnPacket);
          blockdsr0 = VLP16AdjustTimeStamp(firingDataBlockIdx, 0, 0, isDualReturnPacket);
          break;
        }
        default:
        {
          timestampadjustment = 0.0;
          blockdsr0 = 0.0;
          nextblockdsr0 = 1.0;
        }
      }
      azimuthadjustment = vtkMath::Round(
        azimuthDiff * ((timestampadjustment - blockdsr0) / (nextblockdsr0 - blockdsr0)));
      timestampadjustment = vtkMath::Round(timestampadjustment);

    }

    if ((!this->IgnoreZeroDistances || firingData->laserReturns[dsrBase32].distance != 0.0) &&
       static_cast<bool>(this->LaserSelection->GetTuple1(channelNumber)))
    {
      const unsigned short adjustedAzimuth =
        (36000 + (static_cast<int>(azimuth) + azimuthadjustment)) % 36000;
      // 处理一个测量数据。该函数在for循环中，即一个包中包含多个测量数据。
      this->PushFiringData(channelNumber, channelNumberOr_dsrBase32_forVLP16, adjustedAzimuth, firingElevation100th,
        timestamp + static_cast<unsigned long long>(timestampadjustment),
        rawtime + static_cast<unsigned int>(timestampadjustment),
        &(firingData->laserReturns[dsrBase32]),
        isThisFiringDualReturnData, extDataPacketType, &(extData->laserReturns[dsrBase32]));
    }
  }
}

PushFiringData

将一个测量数据转换成点以及其对应的属性，添加到this->CurrentFrame中。类中由一些列指针指向this->CurrentFrame中的数据，将数据加入到对应的属性指针中，即保存到了this->CurrentFrame中。

在将测量值转换成对应的以mi或者角度为单位的物理量后，主要调用了InsertNextValueIfNotNull函数将值添加到对应的数组中。

void vtkVelodyneLegacyPacketInterpreter::PushFiringData(unsigned char channelNumber,
              const unsigned char channelNumberOr_dsrBase32_forVLP16, unsigned short azimuth,
              const unsigned short firingElevation100th, const unsigned long long timestamp,
              const unsigned int rawtime, const HDLLaserReturn* laserReturn,
              const bool isFiringDualReturnData,
              const int extDataPacketType, const HDLLaserReturn* extData)
{
  azimuth %= 36000;
  const vtkIdType thisPointId = this->Points->GetNumberOfPoints();
  // double firingElevation = static_cast<double>(firingElevation100th) / 100.0;

  // Compute raw position
  // 获取测量数据
  bool applyIntensityCorrection =
    this->WantIntensityCorrection && this->IsHDL64Data && !(this->SensorPowerMode == CorrectionOn);

  RawValues rawValues(azimuth, firingElevation100th, laserReturn->distance, laserReturn->intensity);
  CorrectedValues correctedValues; // 保存三维直角坐标系形式的点

  // 将测量数据转换成米或者度的物理量。
  // 该函数在vtkVelodyneBasePacketInterpreter中定义
  this->ComputeCorrectedValues(
      rawValues,
      channelNumber,
      correctedValues,
      applyIntensityCorrection
    );

  double & distanceM = correctedValues.distance; // 距离
  short intensity = correctedValues.intensity;  // 强度
  double (& pos)[3] = correctedValues.position;  // 位置x, y, z

  uint32_t temp = 0;

  // Apply sensor transform
//  cout << "this->SensorTransform" << this->SensorTransform->GetPosition()[0]
// << " " << this->SensorTransform->GetPosition()[0] << " " << this->SensorTransform->GetPosition()[0] << endl;
  if (SensorTransform) this->SensorTransform->InternalTransformPoint(pos, pos);

  // 根据不同的坐标系进行范围滤波，滤掉超出范围的点
  if (this->shouldBeCroppedOut(pos))
    return;

  //ASSUME CONFIDENCE //WIP Triple return mode not implemented
  if (extDataPacketType > EXT_MODE_NONE)
  {
      // First 2 bytes are swapped 0|1|2 -> 1|0|2
      const unsigned char * bytes = reinterpret_cast<const unsigned char*>(extData);
      temp = (static_cast<unsigned int>(bytes[1]) << 16)
           + (static_cast<unsigned int>(bytes[0]) << 8)
           + (static_cast<unsigned int>(bytes[2]) << 0);
      if (isFiringDualReturnData)
      {
        if (this->HideDropPoints && (temp & 0x800000))
        {
          return;
        }
      }
      else
      {
        if (this->HideDropPoints && (temp & 0x800) )
        {
          return;
        }
      }
  }

  // Do not add any data before here as this might short-circuit
  if (isFiringDualReturnData)
  {
    const vtkIdType dualPointId = this->LastPointId[channelNumberOr_dsrBase32_forVLP16]; //Can't be channelNumber because of VLP16 dual mode data layout
    if (dualPointId < this->FirstPointIdOfDualReturnPair)
    {
      // No matching point from first set (skipped?)
      InsertNextValueIfNotNull(this->Flags, DUAL_DOUBLED);
      InsertNextValueIfNotNull(this->DistanceFlag, 0);
      InsertNextValueIfNotNull(this->DualReturnMatching, -1); // std::numeric_limits<vtkIdType>::quiet_NaN()
      InsertNextValueIfNotNull(this->IntensityFlag, 0);
    }
    else
    {
      const short dualIntensity = this->Intensity->GetValue(dualPointId);
      const double dualDistance = this->Distance->GetValue(dualPointId);
      unsigned int firstFlags = this->Flags->GetValue(dualPointId);
      unsigned int secondFlags = 0;

      if (dualDistance == distanceM && intensity == dualIntensity)
      {
        if(this->IgnoreZeroDistances)
        {
          // ignore duplicate point and leave first with original flags
          return;
        }
        // Otherwise we add the duplicate point
      }

      if (dualIntensity < intensity)
      {
        firstFlags &= ~DUAL_INTENSITY_HIGH;
        secondFlags |= DUAL_INTENSITY_HIGH;
      }
      else
      {
        firstFlags &= ~DUAL_INTENSITY_LOW;
        secondFlags |= DUAL_INTENSITY_LOW;
      }

      if (dualDistance < distanceM)
      {
        firstFlags &= ~DUAL_DISTANCE_FAR;
        secondFlags |= DUAL_DISTANCE_FAR;
      }
      else
      {
        firstFlags &= ~DUAL_DISTANCE_NEAR;
        secondFlags |= DUAL_DISTANCE_NEAR;
      }

      // We will output only one point so return out of this
      if (this->DualReturnFilter)
      {
        if (!(secondFlags & this->DualReturnFilter))
        {
          // second return does not match filter; skip
          SetValueIfNotNull(this->Flags, dualPointId, firstFlags);
          SetValueIfNotNull(this->DistanceFlag, dualPointId, MapDistanceFlag(firstFlags));
          SetValueIfNotNull(this->IntensityFlag, dualPointId, MapIntensityFlag(firstFlags));
          return;
        }
        if (!(firstFlags & this->DualReturnFilter))
        {
          // first return does not match filter; replace with second return
          this->Points->SetPoint(dualPointId, pos);
          SetValueIfNotNull(this->Distance, dualPointId, distanceM);
          SetValueIfNotNull(this->DistanceRaw, dualPointId, laserReturn->distance);
          SetValueIfNotNull(this->Intensity, dualPointId, intensity);
          SetValueIfNotNull(this->Timestamp, dualPointId, timestamp);
          SetValueIfNotNull(this->RawTime, dualPointId, rawtime);
          SetValueIfNotNull(this->Flags, dualPointId, secondFlags);
          SetValueIfNotNull(this->DistanceFlag, dualPointId, MapDistanceFlag(secondFlags));
          SetValueIfNotNull(this->IntensityFlag, dualPointId, MapIntensityFlag(secondFlags));
          return;
        }
      }

      SetValueIfNotNull(this->Flags, dualPointId, firstFlags);
      SetValueIfNotNull(this->DistanceFlag, dualPointId, MapDistanceFlag(firstFlags));
      SetValueIfNotNull(this->IntensityFlag, dualPointId, MapIntensityFlag(firstFlags));
      InsertNextValueIfNotNull(this->Flags, secondFlags);
      InsertNextValueIfNotNull(this->DistanceFlag, MapDistanceFlag(secondFlags));
      InsertNextValueIfNotNull(this->IntensityFlag, MapIntensityFlag(secondFlags));
      // The first return indicates the dual return
      // and the dual return indicates the first return
      InsertNextValueIfNotNull(this->DualReturnMatching, dualPointId);
      SetValueIfNotNull(this->DualReturnMatching, dualPointId, thisPointId);
    }
  }
  else
  {
    InsertNextValueIfNotNull(this->Flags, DUAL_DOUBLED);
    InsertNextValueIfNotNull(this->DistanceFlag, 0);
    InsertNextValueIfNotNull(this->IntensityFlag, 0);
    InsertNextValueIfNotNull(this->DualReturnMatching, -1); // std::numeric_limits<vtkIdType>::quiet_NaN()
  }

  // 添加数据
  this->Points->InsertNextPoint(pos);
  InsertNextValueIfNotNull(this->PointsX, pos[0]);
  InsertNextValueIfNotNull(this->PointsY, pos[1]);
  InsertNextValueIfNotNull(this->PointsZ, pos[2]);
  InsertNextValueIfNotNull(this->Azimuth, azimuth);
  InsertNextValueIfNotNull(this->Intensity, intensity);
  InsertNextValueIfNotNull(this->LaserId, channelNumber);
  InsertNextValueIfNotNull(this->Timestamp, timestamp);
  InsertNextValueIfNotNull(this->RawTime, rawtime);
  InsertNextValueIfNotNull(this->Distance, distanceM);
  InsertNextValueIfNotNull(this->DistanceRaw, laserReturn->distance);
  this->LastPointId[channelNumberOr_dsrBase32_forVLP16] = thisPointId;
  InsertNextValueIfNotNull(this->VerticalAngle, correctedValues.elevation);

  //ASSUME CONFIDENCE //WIP Triple return mode not implemented
  if (extDataPacketType > EXT_MODE_NONE)
  {
    if (isFiringDualReturnData)
    { // Second Return
      InsertNextValueIfNotNull(this->BinaryFlags, u32_to_str((temp & 0xFFF000) >> 12));
      InsertNextValueIfNotNull(this->Drop, (temp & 0x800000) >> 23);
      // Reserved >> 22
      InsertNextValueIfNotNull(this->RS, (temp & 0x200000) >> 21);
      InsertNextValueIfNotNull(this->RL, (temp & 0x100000) >> 20);
      InsertNextValueIfNotNull(this->RG, (temp & 0x080000) >> 19);
      InsertNextValueIfNotNull(this->Interference, (temp & 0x060000) >> 17);
      InsertNextValueIfNotNull(this->SunLevel, (temp & 0x018000) >> 15);
      InsertNextValueIfNotNull(this->Confidence, (temp & 0x007000) >> 12);
    }
    else
    { // First Return
      InsertNextValueIfNotNull(this->BinaryFlags, u32_to_str(temp & 0xFFF));
      InsertNextValueIfNotNull(this->Drop, (temp & 0x800) >> 11);
      // Reserved >> 10
      InsertNextValueIfNotNull(this->RS, (temp & 0x200) >> 9);
      InsertNextValueIfNotNull(this->RL, (temp & 0x100) >> 8);
      InsertNextValueIfNotNull(this->RG, (temp & 0x080) >> 7);
      InsertNextValueIfNotNull(this->Interference, (temp & 0x060) >> 5);
      InsertNextValueIfNotNull(this->SunLevel    , (temp & 0x018) >> 3);
      InsertNextValueIfNotNull(this->Confidence  , (temp & 0x007) >> 0);
    }
  }
}

SplitFrame

该函数用于拆分帧，将满足一帧的数据进行保存。

bool vtkVelodyneLegacyPacketInterpreter::SplitFrame(bool force,
                                                    FramingMethod_t framingMethodAskingForSplitFrame)
{
  // 如果对应的数组不为空，则添加到this->CurrentFrame中
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->BinaryFlags);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->Drop);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->Confidence);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->Interference);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->SunLevel);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->RS);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->RL);
  AddArrayIfNotEmpty(this->CurrentFrame->GetPointData(), this->RG);

  // 调用基类中的SplitFrame
  if (this->vtkLidarPacketInterpreter::SplitFrame(force, framingMethodAskingForSplitFrame))
  {
    // 更新针对于该解析器的一些变量，用于下一帧的统计
    for (size_t n = 0; n < HDL_MAX_NUM_LASERS; ++n)
    {
      this->LastPointId[n] = -1;
    }
    // Stats RPM
    this->Rpm = this->RpmCalculator_->GetRPM();
    this->RpmCalculator_->ResetRPM();
    // Stats HZ
    this->Frequency = RpmCalculator_->ComputeFrequency(this->CurrTimestamp);

    return true;
  }

  return false;
}

在vtkVelodyneLegacyPacketInterpreter中的SplitFrame，只是对特定于vtkVelodyneLegacyPacketInterpreter的一些变量进行了设置，然后调用了vtkLidarPacketInterpreter中的SplitFrame，在其中显示添加对this->CurrentFrame中添加了vtkCellArray，不全了vtkPolyData的拓扑数据，然后将当前帧添加到this->Frames，最后使用CreateNewEmptyFrame为this->CurrentFrame创建一个新的空白帧，用于保存下一帧的数据。

bool vtkLidarPacketInterpreter::SplitFrame(bool force, FramingMethod_t framingMethodAskingForSplitFrame)
{
  if ((force || this->FramingMethod == framingMethodAskingForSplitFrame) && this->CurrentFrame)
  {
    const vtkIdType nPtsOfCurrentDataset= this->CurrentFrame->GetNumberOfPoints();
    if (this->IgnoreEmptyFrames && (nPtsOfCurrentDataset == 0) && !force)
    {
      return false;
    }

    // add vertex to the polydata
    this->CurrentFrame->SetVerts(NewVertexCells(this->CurrentFrame->GetNumberOfPoints()));
    // split the frame
    this->Frames.push_back(this->CurrentFrame);
    // create a new frame
    this->CurrentFrame = this->CreateNewEmptyFrame(0, nPtsOfCurrentDataset);

    return true;
  }
  return false;
}

CreateNewEmptyFrame

该函数比较简单，构造了一个空白的vtkPolyData，然后构造一些成员变量指针，将这些指针添加到vtkPolyData中，最后返回vtkPolyData。因为使用指针进行数据传递，可以很方便的使用不同的成员变量访问同一个属性内存。

vtkSmartPointer<vtkPolyData> vtkVelodyneLegacyPacketInterpreter::CreateNewEmptyFrame(vtkIdType numberOfPoints, vtkIdType prereservedNumberOfPoints)
{
  const int defaultPrereservedNumberOfPointsPerFrame = 60000;
  // prereserve for 50% points more than actually received in previous frame
  prereservedNumberOfPoints = std::max(static_cast<int>(prereservedNumberOfPoints * 1.5), defaultPrereservedNumberOfPointsPerFrame);

  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();

  // points
  vtkNew<vtkPoints> points;
  points->SetDataTypeToFloat();
  points->Allocate(prereservedNumberOfPoints);
  if (numberOfPoints > 0 )
  {
    points->SetNumberOfPoints(numberOfPoints);
  }
  points->GetData()->SetName("Points_m_XYZ");
  polyData->SetPoints(points.GetPointer());
//  polyData->SetVerts(NewVertexCells(numberOfPoints));

  // intensity
  this->Points = points.GetPointer();
  InitArrayForPolyData(true,  PointsX, "X", numberOfPoints,
    prereservedNumberOfPoints, polyData, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  PointsY, "Y", numberOfPoints,
    prereservedNumberOfPoints, polyData, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  PointsZ, "Z", numberOfPoints,
    prereservedNumberOfPoints, polyData, this->EnableAdvancedArrays);

  InitArrayForPolyData(false, Intensity, "intensity", numberOfPoints,
    prereservedNumberOfPoints, polyData);
    
  InitArrayForPolyData(true,  BinaryFlags, "binary_flags_string", numberOfPoints,
    prereservedNumberOfPoints, nullptr);
  InitArrayForPolyData(true,  Drop, "drop", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  Confidence, "confidence", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  Interference, "interference", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  SunLevel, "sun_level", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  RS, "retro_shadow", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  RL, "range_limited", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  RG, "retro_ghost", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(false, LaserId, "laser_id", numberOfPoints,
    prereservedNumberOfPoints, polyData);
  InitArrayForPolyData(false, Azimuth, "azimuth", numberOfPoints,
    prereservedNumberOfPoints, polyData);
  InitArrayForPolyData(false, Distance, "distance_m", numberOfPoints,
    prereservedNumberOfPoints, polyData);
  InitArrayForPolyData(true,  DistanceRaw, "distance_raw", numberOfPoints,
    prereservedNumberOfPoints, polyData, this->EnableAdvancedArrays);
  InitArrayForPolyData(false, Timestamp, "adjustedtime", numberOfPoints,
    prereservedNumberOfPoints, polyData);
  InitArrayForPolyData(false, RawTime, "timestamp", numberOfPoints,
    prereservedNumberOfPoints, polyData);
  InitArrayForPolyData(true,  DistanceFlag, "dual_distance", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(true,  IntensityFlag, "dual_intensity", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(false, Flags, "dual_flags", numberOfPoints,
    prereservedNumberOfPoints, nullptr);
  InitArrayForPolyData(true,  DualReturnMatching, "dual_return_matching", numberOfPoints,
    prereservedNumberOfPoints, nullptr, this->EnableAdvancedArrays);
  InitArrayForPolyData(false, VerticalAngle, "vertical_angle", numberOfPoints,
    prereservedNumberOfPoints, polyData);

  if (this->HasDualReturn)
  {
    AddArrayIfNotNull(polyData->GetPointData(), this->DistanceFlag);
    AddArrayIfNotNull(polyData->GetPointData(), this->IntensityFlag);
    AddArrayIfNotNull(polyData->GetPointData(), this->DualReturnMatching);
  }

  // Select array Intensity by default
  polyData->GetPointData()->SetActiveScalars("intensity");

  return polyData;
}

IsLidarPacket

该函数通过数据长度来判断是否为雷达数据包。除此之外，还可以对data中的数据进行判断，比如校验和。

bool vtkVelodyneLegacyPacketInterpreter::IsLidarPacket(unsigned char const* vtkNotUsed(data), unsigned int dataLength)
{
  if (dataLength == HDLDataPacket::getDataByteLength())
  {
    // Data-Packet Specifications says that laser-packets are 1206 byte long.
    //  That is : (2+2+(2+1)*32)*12 + 4 + 1 + 1
    //                #lasers^   ^#firingPerPkt
    return true;
  }
  return false;
}

GetSensorName

获取雷达名称。使用了vtkDataPacket.h中的宏定义和一个获取名字的函数。

std::string vtkVelodyneLegacyPacketInterpreter::GetSensorName()
{
  return DataPacketFixedLength::SensorTypeToString(this->ReportedSensor);
}

// vtkDataPacket.h
enum SensorType
{
  HDL32E = 0x21,     // decimal: 33
  VLP16 = 0x22,      // decimal: 34
  VLP32AB = 0x23,    // decimal: 35
  VLP16HiRes = 0x24, // decimal: 36
  VLP32C = 0x28,     // decimal: 40

  // Work around : this is not defined by any specification
  // But it is usefull to define
  HDL64 = 0xa0,  // decimal: 160
  VLS128 = 0xa1, // decimal: 161
};

static inline std::string SensorTypeToString(SensorType type)
{
  switch (type)
  {
    case SensorType::HDL32E:
      return "HDL-32E";
    case SensorType::VLP16:
      return "VLP-16";
    case SensorType::VLP32AB:
      return "VLP-32AB";
    case SensorType::VLP16HiRes:
      return "VLP-16 Hi-Res";
    case SensorType::VLP32C:
      return "VLP-32C";
    case SensorType::HDL64:
      return "HDL-64";
    case SensorType::VLS128:
      return "VLS-128";
    default:
      return "Unknown";
  }