该类负责读取pcap文件,并调用vtkLidarPacketInterpreter类进行数据解析。然后将数据传入vtk的渲染管线中。
可以通过各种索引获取数据:帧数,网络时间,数据时间。
在渲染管线中调用的函数为RequestData,获取数据的函数为GetFrame。
class LVIOLIDAR_EXPORT vtkLidarReader : public vtkPolyDataAlgorithm
{
public:
static vtkLidarReader* New();
vtkTypeMacro(vtkLidarReader, vtkPolyDataAlgorithm)
int GetNumberOfFrames() { return this->FrameCatalog.size(); }
/**
* @copydoc FileName
*/
vtkGetMacro(FileName, std::string);
virtual void SetFileName(const std::string& filename);
vtkGetMacro(DetectFrameDropping, bool);
vtkSetMacro(DetectFrameDropping, bool);
vtkMTimeType GetMTime() override;
/**
* @brief GetSensorInformation return some sensor information used for display purposes
* @param shortVersion True to have a succinct version of the sensor information
*/
virtual std::string GetSensorInformation(bool shortVersion = false);
/**
* @copydoc vtkLidarPacketInterpreter::CalibrationFileName
*/
virtual void SetCalibrationFileName(const std::string& filename);
/**
* @brief SetDummyProperty a trick to workaround failure to wrap LaserSelection, this actually
* only calls Modified, however for some obscure reason, doing the same from python does not have
* the same effect
* @todo set how to remove this methode as it is a workaround
*/
void SetDummyProperty(int);
/**
* @copydoc vtkLidarPacketInterpreter
*/
vtkGetObjectMacro(Interpreter, vtkLidarPacketInterpreter);
vtkSetObjectMacro(Interpreter, vtkLidarPacketInterpreter);
/**
* @copydoc NetworkTimeToDataTime
*/
vtkGetMacro(NetworkTimeToDataTime, double);
/**
* @brief GetFrame returns the requested frame
* @param frameNumber beteween 0 and vtkLidarReader::GetNumberOfFrames()
*/
virtual vtkSmartPointer GetFrame(int frameNumber);
/**
* @brief GetFrameForPacketTime returns the requested frame
* @param packetTime udp packet time requested
*/
virtual vtkSmartPointer GetFrameForPacketTime(double packetTime);
/**
* @brief GetFrameForDataTime returns the requested frame
* @param dataTime data time requested
*/
virtual vtkSmartPointer GetFrameForDataTime(double dataTime);
/**
* @brief GetFrameIndexForPacketTime returns the frame index
* corresponding to the packet time asked
* @param packetTime udp packet time requested
*/
virtual int GetFrameIndexForPacketTime(double packetTime);
/**
* @brief GetFrameIndexForDataTime returns the frame index
* corresponding to the packet time asked
* @param packetTime udp packet time requested
*/
virtual int GetFrameIndexForDataTime(double dataTime);
/**
* @brief Open open the pcap file
* @todo a decition should be made if the opening/closing of the pcap should be handle by
* the class itself of the class user. Currently this is not clear
* @param reassemble Controls vtkPacketFileReader IP Reassembly
*/
virtual void Open(bool reassemble = true);
/**
* @brief Close close the pcap file
* @todo a decition should be made if the opening/closing of the pcap should be handle by
* the class itself of the class user. Currently this is not clear
*/
virtual void Close();
/**
* @brief SaveFrame save the packet corresponding to the desired frames in a pcap file.
* Because we are saving network packet, part of previous and/or next frames could be included in
* generated the pcap
* @param startFrame first frame to record
* @param endFrame last frame to record, this frame is included
* @param filename where to save the generate pcap file
*/
virtual void SaveFrame(int startFrame, int endFrame, const std::string& filename);
vtkGetMacro(ShowFirstAndLastFrame, bool);
vtkSetMacro(ShowFirstAndLastFrame, bool);
vtkGetMacro(UseRelativeStartTime, bool);
vtkSetMacro(UseRelativeStartTime, bool);
vtkGetMacro(UsePacketTimeForDisplayTime, bool);
vtkSetMacro(UsePacketTimeForDisplayTime, bool);
int GetLidarPort() { return this->LidarPort; }
void SetLidarPort(int _arg);
protected:
vtkLidarReader();
~vtkLidarReader() = default;
int RequestData(vtkInformation* request,
vtkInformationVector** inputVector,
vtkInformationVector* outputVector) override;
int RequestInformation(vtkInformation*, vtkInformationVector**, vtkInformationVector*) override;
int FillOutputPortInformation(int port, vtkInformation* info) override;
//! Indicate if we should detect that some frames are dropped
bool DetectFrameDropping = false;
//! Last Frame processed, this is important if we want to detect frame dropping
int LastFrameProcessed = 0;
//! The calibrationFileName to used and set to the Interpreter once one has been set
std::string CalibrationFileName = "";
//! Name of the pcap file to read
std::string FileName = "";
//! Interpret the packet to create a frame, all the magic happen here
vtkLidarPacketInterpreter* Interpreter = nullptr;
//! Miscellaneous information about a frame that enable:
//! - Quick jump to a frame index
//! - Computation of an adjustedtimestamp
//! - ...
std::vector FrameCatalog;
//! Show/Hide the first and last frame that most of the time are partial frames
bool ShowFirstAndLastFrame = false;
//! If activated, set first frame timestamp to 0. It can be used to synchronize multiple pcap
bool UseRelativeStartTime = false;
//! libpcap wrapped reader which enable to get the raw pcap packet from the pcap file
vtkPacketFileReader* Reader = nullptr;
//! Filter the packet to only read the packet received on a specify port
//! To read all packet use -1
int LidarPort = -1;
//! True to display the packet time in the UI pipeline
//! False to display the network time in the UI pipeline
bool UsePacketTimeForDisplayTime = false;
private:
/**
* @brief ReadFrameInformation read the whole pcap and create a frame index.
* In case the calibration is contained in the pcap file, this will also read it
*/
int ReadFrameInformation();
/**
* @brief SetTimestepInformation Set the timestep available
* @param info
*/
void SetTimestepInformation(vtkInformation* info);
vtkLidarReader(const vtkLidarReader&) = delete;
void operator=(const vtkLidarReader&) = delete;
/**
* @brief The timeshift between these two times.
* When added to "network time" it gives "data time".
* Example of "network time": the pipeline time (in general)
* Example of "data time": the time of the lidar points.
*/
double NetworkTimeToDataTime = 0.0;
};
RequestData函数是vtk渲染管线中调用的函数,用户传递输入输出数据。需要用户重载该函数。vtkLidarReader作为源(source)类,只是用了输出数据,未使用输入数据。
该函数的主要流程是获取输出数据指针,判断雷达数据解析器是否存在,使用GetFrame获取数据,将该数据拷贝到输出数据指针中。
// vtk pipeline渲染管线调用函数,作为source,该类只有输出数据,无输入数据。
int vtkLidarReader::RequestData(vtkInformation* vtkNotUsed(request),
vtkInformationVector** vtkNotUsed(inputVector),
vtkInformationVector* outputVector)
{
// 获取输出数据指针,用于后续往其中写入数据
vtkPolyData* output = vtkPolyData::GetData(outputVector);
vtkTable* calibration = vtkTable::GetData(outputVector, 1);
auto p = vtkPolyData::GetData(outputVector->GetInformationObject(0));
vtkInformation* info = outputVector->GetInformationObject(0);
// 在void vtkLidarReader::SetFileName(const std::string& filename)函数中赋值
if (this->FileName.empty())
{
vtkErrorMacro("FileName has not been set.");
return 0;
}
// vtkLidarPacketInterpreter* Interpreter = nullptr;
if (!this->Interpreter)
{
vtkErrorMacro("Interpreter has not been set.");
return 0;
}
// 判断该Interpreter是否已经加载了配置文件
// 该函数返回的成员变量在vtkLidarPacketInterpreter中定义bool IsCalibrated = false;
// 初始化为false,在vtkVelodyneBasePacketInterpreter::LoadCalibration函数中进行修改
// 如果加载配置文件成功,则改为true
if (!this->Interpreter->GetIsCalibrated())
{
vtkErrorMacro("The calibration could not be determined from the pcap file!");
return 0;
}
// 浅拷贝,不拷贝数据内存,只拷贝数据的指针,两个对象实例指向同一个内存空间
calibration->ShallowCopy(this->Interpreter->GetCalibrationTable());
// This mean that the reader did not manage to parser the pcap file
if (this->FrameCatalog.size() <= 1)
{
return 1;
}
double timestep = 0.0;
if (info->Has(vtkStreamingDemandDrivenPipeline::UPDATE_TIME_STEP()))
{
timestep = info->Get(vtkStreamingDemandDrivenPipeline::UPDATE_TIME_STEP());
}
int frameRequested = 0;
if (this->UsePacketTimeForDisplayTime)
{ // 通过数据包的网络时间查找对应的雷达点云帧索引。
frameRequested = GetFrameIndexForDataTime(timestep);
}
else
{ // 通过数据的获取时间查找对应的雷达点云帧索引。
frameRequested = GetFrameIndexForPacketTime(timestep);
}
// detect frame dropping
if (this->DetectFrameDropping)
{
int step = frameRequested - this->LastFrameProcessed;
if (step > 1)
{
std::stringstream text;
text << "WARNING : At frame " << std::right << std::setw(6) << frameRequested << " Drop "
<< std::right << std::setw(2) << step - 1 << " frame(s)\n";
vtkWarningMacro(<< text.str());
}
}
this->LastFrameProcessed = frameRequested;
//! @todo we should no open the pcap file everytime a frame is requested !!!
this->Open();
// 调用GetFrame函数,获取数据,其返回类型为tkSmartPointer
output->ShallowCopy(this->GetFrame(frameRequested));
this->Close();
return 1;
}
该函数返回的是vtkSmartPointer
vtkSmartPointer vtkLidarReader::GetFrame(int frameNumber)
{
// 清空当前帧的内存,当前帧类型为vtkSmartPointer CurrentFrame;
// 当前帧数据在vtkLidarPacketInterpreter类中进行保存。该类负责对雷达数据包进行解析,并生成点云帧
this->Interpreter->ResetCurrentFrame();
// 清空加载的所有帧数据
// 保存加载的所有帧的类型为std::vector > Frames;
this->Interpreter->ClearAllFramesAvailable();
// 读取pcap的Reader不可用
if (!this->Reader)
{
vtkErrorMacro("GetFrame() called but packet file reader is not open.");
return 0;
}
// 该解析器是否加载了配置文件
if (!this->Interpreter->GetIsCalibrated())
{
vtkErrorMacro("Calibration data has not been loaded.");
return 0;
}
const unsigned char* data = 0;
unsigned int dataLength = 0;
double timeSinceStart = 0;
// Update the interpreter meta data according to the requested frame
FrameInformation currInfo = this->FrameCatalog[frameNumber];
// 加载需要帧的元数据
// this->FrameCatalog类型为std::vector
// FrameInformation中保存了所需要帧的第一包数据的起始位置,时间等信息。
this->Interpreter->SetParserMetaData(this->FrameCatalog[frameNumber]);
// 将文件流指定到该帧数据的起始位置。
this->Reader->SetFilePosition(&currInfo.FilePosition);
// 获取该帧的内容,一帧数据由多个数据包组成
while (this->Reader->NextPacket(data, dataLength, timeSinceStart))
{
// If the current packet is not a lidar packet,
// skip it and update the file position
if (!this->Interpreter->IsLidarPacket(data, dataLength))
{
continue;
}
// Process the lidar packet and check
// if the required frame is ready
// 该函数调用了vtkLidarPacketInterpreter::ProcessPacket函数
this->Interpreter->ProcessPacketWrapped(data, dataLength, timeSinceStart);
// IsNewFrameReady的判断很简单,只要this->Frames中有数据,则为true
if (this->Interpreter->IsNewFrameReady())
{
// 返回this->Frames中的最后一个位置的数据
return this->Interpreter->GetLastFrameAvailable();
}
}
// 如果数据没有达到一帧的标准,但是运行到这也没有其他数据,则强行将数据划分为 一帧
this->Interpreter->SplitFrame(true);
return this->Interpreter->GetLastFrameAvailable();
}
vtkLidarReader::vtkLidarReader()
{
// source ,无输入数据
this->SetNumberOfInputPorts(0);
// 有两个输出数据
this->SetNumberOfOutputPorts(2);
}
该函数用于告诉渲染管线输出数据的类型。
int vtkLidarReader::FillOutputPortInformation(int port, vtkInformation* info)
{
if (port == 0) // port0 输出的是点云数据,数据结构为vtkPolyData
{
info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkPolyData");
return 1;
}
if (port == 1) // port1 输出的是数据的信息(元数据或者一些配置相关的数据),输出的数据结构为vtkTable
{
info->Set(vtkDataObject::DATA_TYPE_NAME(), "vtkTable");
return 1;
}
return 0;
}
该函数用于获取雷达的信息。该信息是雷达数据解析类传出来的。
std::string vtkLidarReader::GetSensorInformation(bool shortVersion)
{ // 返回雷达的信息,调用的vtkLidarPacketInterpreter类
// vtkLidarPacketInterpreter是虚基类,需要用户实现自己的数据解析类
return this->Interpreter->GetSensorInformation(shortVersion);
}
获取时间。用于在渲染管线中判断数据是否有改变。
vtkMTimeType vtkLidarReader::GetMTime()
{
// 返回最新修改时间,该时间在vtk pipeline中使用,用于判断是否有新数据。
if (this->Interpreter)
{
return std::max(this->Superclass::GetMTime(), this->Interpreter->GetMTime());
}
return this->Superclass::GetMTime();
}
读取pcap信息。在每次打开pcap文件时调用。用于获取每帧数据在pcap文件中的位置。其中调用了雷达解析器类的PreProcessPacketWrapped函数,该函数是对纯虚函数PreProcessPacket的包装。需要在PreProcessPacket中判定是否到了一帧数据的结束位置。
该函数返回pcap数据包含的雷士数据帧数。
int vtkLidarReader::ReadFrameInformation()
{
this->Open();
const unsigned char* data = 0;
unsigned int dataLength = 0;
bool firstIteration = true;
// reset the frame catalog to build a new one
this->FrameCatalog.clear();
// reset the interpreter parser meta data
this->Interpreter->ResetParserMetaData();
// vtkPacketFileReader* Reader
// vtkPacketFileReader是对libpcap的封装,用于读取pcap数据
if (!this->Reader)
{
vtkErrorMacro("Could not open the pcap file");
return 0;
}
// keep track of the file position
// and the network timestamp of the
// current udp packet to process
fpos_t lastFilePosition;
double lastPacketNetworkTime = 0;
this->Reader->GetFilePosition(&lastFilePosition);
// 读取文件中的所有数据,进行数据解析,保存每一帧数据的其实位置
while (this->Reader->NextPacket(data, dataLength, lastPacketNetworkTime))
{
// This command sends a signal that can be observed from outside
// and that is used to diplay a Qt progress dialog from Python
// This progress dialog is not displaying a progress percentage,
// thus it is ok to pass 0.0
// Qt进度条
this->UpdateProgress(0.0);
// If the current packet is not a lidar packet,
// skip it and update the file position
// 判断是否是雷达数据包,该方法由用户实现
if (!this->Interpreter->IsLidarPacket(data, dataLength))
{ // 如果该数据不符合雷达数据包的要求,记录当前位置,并进行下一次循环
this->Reader->GetFilePosition(&lastFilePosition);
continue;
}
// add an index for the first Lidar packet
// 如果使用雷达数据解析器进行一帧数据的划分,并且为第一帧,进入该分支进行第一帧的处理
if (firstIteration && this->GetInterpreter()->GetFramingMethod() == INTERPRETER_FRAMING)
{
// it is possible that the first packet contains 2 frames
// (end and start of one), and as we rely on the packet header time
// this 2 frames will have the same timestep. So to avoid that we
// artificatially move the first timeStep back by one.
this->FrameCatalog.push_back(this->Interpreter->GetParserMetaData());
this->FrameCatalog.back().FilePosition =
lastFilePosition; // Ensure that the first index point on a real fileposition
firstIteration = false;
}
// Get information about the current packet
// 当前雷达包数据预处理。在PreProcessPacketWrapped中调用了this->Interpreter->PreProcessPacket
// PreProcessPacket函数为纯虚函数,由用户实现,用于保存每帧数据在pcap文件中的位置。
this->Interpreter->PreProcessPacketWrapped(
data, dataLength, lastFilePosition, lastPacketNetworkTime, &this->FrameCatalog);
this->Reader->GetFilePosition(&lastFilePosition);
}
// 如果this->FrameCatalog中只有大小为1,则说明该pcap中没有雷达数据
// 在上面的while循环中必定会在this->FrameCatalog中存入一个数据
if (this->FrameCatalog.size() == 1)
{
vtkErrorMacro("The reader could not parse the pcap file");
}
// 计算数据获取时间和网络传输时间之间的时间差
this->NetworkTimeToDataTime = 0.0; // default value if no frames seen
if (this->FrameCatalog.size() > 0)
{
std::vector diffs(this->FrameCatalog.size());
for (size_t i = 0; i < this->FrameCatalog.size(); i++)
{
diffs[i] =
this->FrameCatalog[i].FirstPacketDataTime - this->FrameCatalog[i].FirstPacketNetworkTime;
}
this->NetworkTimeToDataTime = ComputeMedian(diffs);
}
if (this->FrameCatalog.size() == 1)
{
vtkErrorMacro("The reader could not parse the pcap file");
}
// 返回当前的帧数
return this->GetNumberOfFrames();
}
// 在RequestInformation函数中被调用
// 用于获取数据信息:每帧的时间以及真个数据的开始时间和结束时间
void vtkLidarReader::SetTimestepInformation(vtkInformation* info)
{ // 当无数据时,直接返回
if (this->FrameCatalog.size() == 1)
{
return;
}
size_t numberOfTimesteps = this->FrameCatalog.size();
std::vector timesteps(numberOfTimesteps);
double timeOffset = this->GetInterpreter()->GetTimeOffset();
// Add a default offset when using UseRelativeStartTime to let the first frame to 0
if (this->UseRelativeStartTime && timeOffset == 0.)
{
timeOffset = 1.;
}
// 记录每一帧的时间(数据时间或者网络时间)
for (size_t i = 0; i < numberOfTimesteps; ++i)
{
if (this->UsePacketTimeForDisplayTime)
{
timesteps[i] = this->FrameCatalog[i].FirstPacketDataTime;
}
else
{
timesteps[i] = this->FrameCatalog[i].FirstPacketNetworkTime;
}
if (this->UseRelativeStartTime && i == 1)
{
timeOffset = timeOffset - timesteps[1];
}
// Skip first frame for firstAndLastFrame option
if (i >= 1)
{
timesteps[i] += timeOffset;
}
}
if (this->FrameCatalog.size())
{
double* firstTimestepPointer = ×teps.front();
double* lastTimestepPointer = ×teps.back();
// Remove the first and last frame timestep
// in case you have less that 3 timstep we will still show the partial
// frame to avoid showing nothing
if (!this->ShowFirstAndLastFrame && numberOfTimesteps >= 3)
{
firstTimestepPointer++;
lastTimestepPointer--;
numberOfTimesteps -= 2;
}
double timeRange[2] = { *firstTimestepPointer, *lastTimestepPointer };
// In order to avoid to display
info->Set(
vtkStreamingDemandDrivenPipeline::TIME_STEPS(), firstTimestepPointer, numberOfTimesteps);
info->Set(vtkStreamingDemandDrivenPipeline::TIME_RANGE(), timeRange, 2);
}
else
{ // 如果没有数据,则将vtkInformation中的相关键值对删掉。
info->Remove(vtkStreamingDemandDrivenPipeline::TIME_STEPS());
info->Remove(vtkStreamingDemandDrivenPipeline::TIME_RANGE());
}
}
// 打开新数据时,保存数据名称,并清空每帧的信息
void vtkLidarReader::SetFileName(const std::string& filename)
{
if (filename == this->FileName)
{
return;
}
this->FileName = filename;
this->FrameCatalog.clear();
this->Modified(); // 告诉vtk渲染管线,数据有变化,需要进行重新数据加载
}
通过数据包的网络时间获取雷达数据,该函数中调用了GetFrame函数。数据帧信息的索引通过GetFrameForPacketTime函数获取。
// 通过网络时间获取对应的点云帧。
// 首先通过GetFrameIndexForPacketTime函数得到packetTime对应的帧的index
// 然后调用GetFrame获取对应的数据
vtkSmartPointer vtkLidarReader::GetFrameForPacketTime(double packetTime)
{
return this->GetFrame(this->GetFrameIndexForPacketTime(packetTime));
}
通过数据包的数据时间获取雷达数据。
// 通过数据时间获取对应的点云帧
// 首先通过GetFrameForDataTime函数得到dataTime对应的帧的index
// 然后调用GetFrame获取对应的数据
vtkSmartPointer vtkLidarReader::GetFrameForDataTime(double dataTime)
{
return this->GetFrame(this->GetFrameIndexForDataTime(dataTime));
}
// 通过网络时间得到对应点云帧在this->FrameCatalog中的索引
int vtkLidarReader::GetFrameIndexForPacketTime(double packetTime)
{
double timeOffset = this->GetInterpreter()->GetTimeOffset();
if (this->UseRelativeStartTime && this->FrameCatalog.size() >= 1)
{
if (timeOffset == 0.)
{
timeOffset = 1.;
}
timeOffset = timeOffset - this->FrameCatalog[1].FirstPacketNetworkTime;
}
// iterating over all timesteps until finding the first one with a greater time value
auto idx = std::lower_bound(this->FrameCatalog.begin(),
this->FrameCatalog.end(),
packetTime - timeOffset,
[](FrameInformation& fp, double d) { return fp.FirstPacketNetworkTime < d; });
if (idx == this->FrameCatalog.end())
{
// vtkErrorMacro("Cannot meet timestep request: " << packetTime);
return 0;
}
auto frameRequested = std::distance(this->FrameCatalog.begin(), idx);
return static_cast(frameRequested);
}
// 通过数据时间得到对应点云帧在this->FrameCatalog中的索引
int vtkLidarReader::GetFrameIndexForDataTime(double dataTime)
{
double timeOffset = this->GetInterpreter()->GetTimeOffset();
if (this->UseRelativeStartTime && this->FrameCatalog.size() >= 1)
{
if (timeOffset == 0.)
{
timeOffset = 1.;
}
timeOffset = timeOffset - this->FrameCatalog[1].FirstPacketDataTime;
}
// iterating over all timesteps until finding the first one with a greater time value
auto idx = std::lower_bound(this->FrameCatalog.begin(),
this->FrameCatalog.end(),
dataTime - timeOffset,
[](FrameInformation& fp, double d) { return fp.FirstPacketDataTime < d; });
if (idx == this->FrameCatalog.end())
{
// vtkErrorMacro("Cannot meet timestep request: " << dataTime);
return 0;
}
auto frameRequested = std::distance(this->FrameCatalog.begin(), idx);
return static_cast(frameRequested);
}
//-----------------------------------------------------------------------------
int vtkLidarReader::RequestInformation(vtkInformation* vtkNotUsed(request),
vtkInformationVector** vtkNotUsed(inputVector),
vtkInformationVector* outputVector)
{
if (!this->Interpreter)
{
vtkErrorMacro("No packet interpreter selected.");
}
// load the calibration file only now to allow to set it before the interpreter.
// 判断雷达数据解析器的配置文件是否一致。
if (this->Interpreter->GetCalibrationFileName() != this->CalibrationFileName)
{
this->Interpreter->SetCalibrationFileName(this->CalibrationFileName);
this->Interpreter->LoadCalibration(this->CalibrationFileName);
}
if (this->Interpreter && !this->FileName.empty())
{ // 读取文件数据信息。
this->ReadFrameInformation();
}
vtkInformation* info = outputVector->GetInformationObject(0);
this->SetTimestepInformation(info);
return 1;
}