【Chrono Engine学习总结】5-sensor-5.2-导出lidar数据的方法与原理探究

2024-02-17 13:52
文章标签 数据 学习 总结 方法 原理 导出 engine sensor 探究 5.2 lidar chrono

本文主要是介绍【Chrono Engine学习总结】5-sensor-5.2-导出lidar数据的方法与原理探究，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

由于Chrono的官方教程在一些细节方面解释的并不清楚，自己做了一些尝试，做学习总结。

1、Sensor数据生成流程回顾

Chrono里面，sensor的数据生成、可视化、以及保存，都需要单独进行设置才能实现。sensor数据的采集流程如下https://api.projectchrono.org/sensor_overview.html：
在这里插入图片描述
可以看出，sensor数据更新是另一个线程中进行，当场景/物体发生变化后，获得sensor的原始数据，之后应用各种滤波器，最后将滤波后的数据输出，可以在主线程中进行调用/保存。这就是sensor数据采集的整体流程。

2、数据输出的两种方式

2.1 通过滤波器实现

在官方的demo_SEN_gator和demo_SEN_HMMWV中，给出了通过滤波器方式实现数据输出的例子。关键核心步骤如下：

// 1. 创建一个sensor的manager
auto manager = chrono_types::make_shared<ChSensorManager>(gator.GetSystem());
// 2. 创建一个sensor
auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), gator.GetChassisBody(), lidar_extrinsics);
// 3. 添加“保存点云”滤波器。只有加上这一行，才能够在后续仿真中自动输出
vlp16->PushFilter(chrono_types::make_shared<ChFilterSavePtCloud>("C:/Users/larrydong/Desktop/lidar_output/"));  // 包含这个filter，在每次Apply对应的filter时(update中)，会执行保存到文件，并输出Beam count
// 4. 添加sensor到管理器
manager->AddSensor(vlp16);// 循环部分
manager->Update();  
gator.Synchronize(time, driver_inputs, terrain);
gator.Advance(step_size);

程序运行之后，将会在滤波器给出的输出路径，输出第N帧数据的csv文件，文件内为点云数据。

2.2 通过直接读取Buffer实现

官方的tutorial中给出了一个读取数据的方式https://api.projectchrono.org/lidar_sensor.html：
在这里插入图片描述
但并没有详细解释。进行了探究，发现，只需要在主循环中，判断读到了Buffer之后，即可输出数据，从这里写入文件即可。但这里存在一个问题，Buffer的大小是多少，即有多少个lidar点？这个问题在下一小节进行探讨。

3、详细探究lidar数据更新过程

3.1 关于Buffer读取的探究

在2.2中，给出了通过直接读取lidar的Buffer的方式读取数据。我们查看GetMostRecentBuffer函数，调用了ChSensor中的GetMostRecentBufferHelper，进一步调用了ChFilterAccess中的GetBuffer()。从这个最终的函数中，可以看出获取了buffer的宽、高、时间戳、和更新次数(LaunchedCount)，因此在2.2中获取Buffer之后，可以通过数据的宽和高（对于lidar来说是Nx1的），获取lidar数据。

例如，20Hz的lidar，我们在main循环中，每步都输出Buffer的基本信息。代码：
在这里插入图片描述输出如下：
在这里插入图片描述即当前点云有24272个点，对应时间是3.8s和3.85s，对应第76和77帧。但我们发现，Buffer虽然没有更新，但依然是存在的，因此不能单纯通过Buffer是否存在，判断有无更新，而是需要通过launchCount这个数进行判断。但这时，我们仍不清楚lidar的数据在什么时刻“输出”的。我们马上会说到这个问题。

3.2 采用Filter方式输出

例子当中，我们把lidar这个sensor绑定到一个车辆vehicle当中。这部分其实很容易实现，只需要在创建一个sensor时指定attach的实体，给上我们的vehicle即可。这里的vehicle用的是Gator。

auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), gator.GetChassisBody(), 					// 绑定到Gator的底盘上，Gator的底盘本质上还是一个ChBodylidar_extrinsics);							// 相对于底盘的位置参数，即lidar外参

在main当中，我们有三个关键函数：sensor manager的update()，gator的Synchronize，以及gator的Advance()。经测试标明，在lidar输出了数据之后，Advance()执行时，会输出lidar数据到指定路径。具体的实现是调用了ChFilterSavePtCloud的Apply函数实现的，如下：

在这里插入图片描述

3.3 函数详解

需要提醒的是，调用GetMostRecentBuffer和Filter两种方式输出并不冲突。前者是我们自己读取Buffer获取，后者是仿真里面“例行公事”完成的，例行公事的函数就是这个Advance。只不过，我并不是很清楚，什么时候lidar的Buffer中更新了数据、什么时候执行的Advance。

提醒的第2点，sensor绑定vehicle后，更新的部分之和vehicle的函数有关，哪怕是sensor的可视化，也与irrlicht等可视化模块无关。只有当sensor的filter添加了可视化的filter之后，才能显示sensor的数据，并且窗口中数据的更新是跟vehicle的，并不是跟vis模块的。

在“认可”上面两条之后，我们探讨这部分的几个函数。

manager->Update()

在这里插入图片描述我们看sensor manager的Update，可以看到，包括：对所有光学传感器更新、非光学传感器更新两部分。前者依赖OptiX库，底层实现在 ChOptixEngine.cpp 中。对于光学的UpdateSensors，具体如下：
在这里插入图片描述在这个Update完成之后，可以认为lidar已经“采集了”一个新的数据，但这个数据并没有“传输”出来。此时，在执行Adance()函数时，会更新可视化窗口。即下图左侧的这个窗口，是由Sensor管理的，在sensor更新时才会更新，更新的频率和右侧的仿真窗口不同，右侧的是可视化例如irrlicht控制的。
在这里插入图片描述

gator.Advance()
这个Advance函数很复杂， 包括了vehicle和各种子系统的更新。由于Gator是一个wheelvehicle，也是一个vehicle，所以Gator的advance在完成了Gator一些独特的操作之后，再次调用ChWheelVehicle的advance，完成wheel特有的操作之后，再调用了ChVehicle的advance。同时，vehicle包括一个物理系统（之前博客讲过，如果有vehicle之后，就不需要再定义仿真物理系统了），最终执行的是物理系统的DoStepDynamic
在这里插入图片描述到这一步往下就没有深究。但正如上面所说，具体advance时，执行了更新图像、保存数据等操作。

何时更新图形、何时保存数据？
我们来看最后一个问题，到底是什么时候保存的数据、什么时候更新的lidar显示？由于上面已经说明了，lidar的显示和输出，并不是同步的。

为了充分模拟传感器，chrono在场景变化后，lidar的采集数据是立刻变化的，可视化的窗口会进行更新，但此时的数据并没有输出出来。这个输出延时，就是对传感器设置时有一个lag参数，表示“从采集到数据，到数据输出”的时间延迟。

可以这么理解，仿真过程是这个样子：

  1. 当达到物理仿真频率时（很高，例如1ms），物理环境就发生变换：
  2. 当达到sensor的采集频率时（不高，例如20Hz的lidar是50ms）， lidar传感器获得了一次完整的frame，这一步由manager->Update实现；但在这个过程中，采集过程是另一个线程实现的，因此数据是连续的，并不是单纯的在“这一个时刻”获得的；
  3. 在sensor有效的update之后，Advance时对可视化窗口进行了更新（但“传输”的数据还没有更新）
  4. 经过当sensor的lag之后，如果定义了获取数据的Filter(ChFilterXYZIAccess)，此时输出数据获得了更新，即GetMostRecentBuffer 获取到的数据进行了更新；如果又定义了保存点云的Filter(ChFilterSavePtCloud)，此时Advance时会输出csv文件到路径。

通过对一个完整的过程打断点与输出，验证了上面的理解：
在这里插入图片描述

  • “Width xxx” 这一行输出，是仿真每次循环（每1ms）时输出一次；
  • “Beam count” 这一行输出是Advance中更新图像窗口时的输出，精准的每50次执行一次；
  • 在0.1s和0.15s之间，可以看出点云的数据Buffer发生了变化，也就是这个时刻，lidar已经update的数据“传输”了、GetMostRecentBuffer发生了更新，因此此时Advance时能够保存点云到路径；
  • 在每次Update之后，经过了20次loop，更新的buff和输出点云，这个20次就是sensor的lag参数控制的。我设置成了0.02s，因此经过1ms的循环20次，“传输”了。

4、完整代码

这个代码太长了，主要是用来测试上面的内容。

#include "chrono/core/ChRealtimeStep.h"
#include "chrono/utils/ChUtilsInputOutput.h"
#include "chrono/physics/ChBodyEasy.h"
#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/driver/ChInteractiveDriverIRR.h"
#include "chrono_vehicle/wheeled_vehicle/ChWheeledVehicleVisualSystemIrrlicht.h"
#include "chrono_models/vehicle/gator/Gator.h"
#include "chrono_thirdparty/filesystem/path.h"
#include "chrono_sensor/sensors/Sensor.h"
#include "chrono_sensor/sensors/ChCameraSensor.h"
#include "chrono_sensor/sensors/ChLidarSensor.h"
#include "chrono_sensor/ChSensorManager.h"
#include "chrono_sensor/filters/ChFilterAccess.h"
#include "chrono_sensor/filters/ChFilterPCfromDepth.h"
#include "chrono_sensor/filters/ChFilterVisualize.h"
#include "chrono_sensor/filters/ChFilterSave.h"
#include "chrono_sensor/filters/ChFilterSavePtCloud.h"
#include "chrono_sensor/filters/ChFilterVisualizePointCloud.h"
#include "chrono_sensor/filters/ChFilterAccess.h"
#include "chrono_sensor/filters/ChFilterVisualize.h"
#include "chrono_sensor/sensors//ChSensorBuffer.h"
#include <iostream>using namespace chrono;
using namespace chrono::irrlicht;
using namespace chrono::vehicle;
using namespace chrono::vehicle::gator;
using namespace chrono::sensor;// =============================================================================// Initial vehicle location and orientation
ChVector<> initLoc(0, 0, 0.5);
ChQuaternion<> initRot(1, 0, 0, 0);// Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
VisualizationType chassis_vis_type = VisualizationType::PRIMITIVES;
VisualizationType suspension_vis_type = VisualizationType::PRIMITIVES;
VisualizationType steering_vis_type = VisualizationType::PRIMITIVES;
VisualizationType wheel_vis_type = VisualizationType::PRIMITIVES;
VisualizationType tire_vis_type = VisualizationType::PRIMITIVES;// Collision type for chassis (PRIMITIVES, HULLS, or NONE)
CollisionType chassis_collision_type = CollisionType::NONE;// Type of tire model (RIGID, TMEASY)
TireModelType tire_model = TireModelType::TMEASY;// Rigid terrain
RigidTerrain::PatchType terrain_model = RigidTerrain::PatchType::BOX;// Contact method
ChContactMethod contact_method = ChContactMethod::NSC;// Simulation step sizes
double step_size = 1e-3;
double tire_step_size = step_size;// Time interval between two render frames
double render_step_size = 1.0 / 50;  // FPS = 50// SENSOR PARAMETERS
// Save sensor data
bool sensor_save = false;// Visualize sensor data
bool sensor_vis = true;// =============================================================================int main(int argc, char* argv[]) {GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";chrono::SetChronoDataPath("E:/codeGit/chrono/chrono/build/data/");              // change the default data loading path.chrono::vehicle::SetDataPath("E:/codeGit/chrono/chrono/build/data/vehicle/");              // change the vehicle data path// --------------// Create vehicle// --------------Gator gator;gator.SetContactMethod(contact_method);gator.SetChassisCollisionType(chassis_collision_type);gator.SetChassisFixed(false);gator.SetInitPosition(ChCoordsys<>(initLoc, initRot));gator.SetTireType(tire_model);gator.SetTireStepSize(tire_step_size);gator.SetAerodynamicDrag(0.5, 5.0, 1.2);gator.Initialize();gator.SetChassisVisualizationType(chassis_vis_type);gator.SetSuspensionVisualizationType(suspension_vis_type);gator.SetSteeringVisualizationType(steering_vis_type);gator.SetWheelVisualizationType(wheel_vis_type);gator.SetTireVisualizationType(tire_vis_type);// Associate a collision systemgator.GetSystem()->SetCollisionSystemType(ChCollisionSystem::Type::BULLET);// ------------------// Create the terrain// ------------------RigidTerrain terrain(gator.GetSystem());ChContactMaterialData minfo;minfo.mu = 0.9f;minfo.cr = 0.01f;minfo.Y = 2e7f;auto patch_mat = minfo.CreateMaterial(contact_method);std::shared_ptr<RigidTerrain::Patch> patch;patch = terrain.AddPatch(patch_mat, CSYSNORM, 100.0, 100.0);patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);patch->SetColor(ChColor(0.8f, 0.8f, 0.5f));terrain.Initialize();auto ground_body = patch->GetGroundBody();      // ground_body: ChBody 是地形的ChBodyauto vis_mat1 = chrono_types::make_shared<ChVisualMaterial>();vis_mat1->SetKdTexture(vehicle::GetDataFile("terrain/textures/grass.jpg"));         // Kd, Ks, Ke：一些材质的反射、发光等设定。vis_mat1->SetWeightTexture(GetChronoDataFile("sensor/textures/weight1.png"));       // 两种地形材料的混合权重。weight1和2是互补的。vis_mat1->SetSpecularColor({ .0f, .0f, .0f });vis_mat1->SetRoughness(1.f);vis_mat1->SetUseSpecularWorkflow(false);auto vis_mat2 = chrono_types::make_shared<ChVisualMaterial>();vis_mat2->SetKdTexture(vehicle::GetDataFile("terrain/textures/dirt.jpg"));vis_mat2->SetWeightTexture(GetChronoDataFile("sensor/textures/weight2.png"));vis_mat2->SetSpecularColor({ .0f, .0f, .0f });vis_mat2->SetRoughness(1.f);vis_mat2->SetUseSpecularWorkflow(false);auto visual_shape = ground_body->GetVisualModel()->GetShape(0);visual_shape->SetMaterial(0, vis_mat1);         // 将第i个material设置为某个特定的materialvisual_shape->AddMaterial(vis_mat2);// -------------------------------------// Create the vehicle Irrlicht interface// -------------------------------------auto vis = chrono_types::make_shared<ChWheeledVehicleVisualSystemIrrlicht>();vis->SetWindowTitle("Gator Demo");vis->SetChaseCamera(ChVector<>(0.0, 0.0, 2.0), 5.0, 0);vis->Initialize();vis->AddTypicalLights();vis->AddSkyBox();vis->AddLogo();vis->AttachVehicle(&gator.GetVehicle());// ------------------------// Create the driver system// ------------------------// Create the interactive driver systemChInteractiveDriverIRR driver(*vis);// Set the time response for steering and throttle keyboard inputs.double steering_time = 1.0;  // time to go from 0 to +1 (or from 0 to -1)double throttle_time = 1.0;  // time to go from 0 to +1double braking_time = 0.3;   // time to go from 0 to +1driver.SetSteeringDelta(render_step_size / steering_time);driver.SetThrottleDelta(render_step_size / throttle_time);driver.SetBrakingDelta(render_step_size / braking_time);driver.Initialize();// 传感器部分auto manager = chrono_types::make_shared<ChSensorManager>(gator.GetSystem());manager->scene->AddPointLight({ 100, 100, 100 }, { 2, 2, 2 }, 5000);auto lidar_extrinsics = chrono::ChFrame<double>({ 0, 0, 1 }, Q_from_AngAxis(0, { 1, 0, 0 }));// Lidar from JSON file - Velodyne VLP-16auto vlp16 = chrono::sensor::Sensor::CreateFromJSON(GetChronoDataFile("sensor/json/Velodyne/HDL-32E.json"), gator.GetChassisBody(), lidar_extrinsics);vlp16->PushFilter(chrono_types::make_shared<ChFilterSavePtCloud>("C:/Users/larrydong/Desktop/lidar_output/"));  // 包含这个filter，在每次Apply对应的filter时(update中)，会执行保存到文件，并输出Beam countmanager->AddSensor(vlp16);// ---------------// Simulation loop// ---------------// output vehicle massstd::cout << "VEHICLE MASS: " << gator.GetVehicle().GetMass() << std::endl;// Number of simulation steps between miscellaneous eventsint render_steps = (int)std::ceil(render_step_size / step_size);// Initialize simulation frame countersint step_number = 0;int render_frame = 0;float orbit_radius = 10.f;float orbit_rate = 1;ChRealtimeStepTimer realtime_timer;         // Chwhile (vis->Run()) {double time = gator.GetSystem()->GetChTime();chrono::sensor::UserXYZIBufferPtr data_ptr;data_ptr = vlp16->GetMostRecentBuffer< chrono::sensor::UserXYZIBufferPtr>();if (data_ptr->Buffer) {using namespace std;cout << "==>    Width: " << data_ptr->Width << ", height: " << data_ptr->Height << ",  ts: " << data_ptr->TimeStamp << ", launched count: " << data_ptr->LaunchedCount << endl;}manager->Update();// Render scene and output POV-Ray dataif (step_number % render_steps == 0) {vis->BeginScene();vis->Render();              // 这一步更新的lidar？vis->EndScene();render_frame++;}// Get driver inputsDriverInputs driver_inputs = driver.GetInputs();// Update modules (process inputs from other modules)driver.Synchronize(time);terrain.Synchronize(time);gator.Synchronize(time, driver_inputs, terrain);vis->Synchronize(time, driver_inputs);// Advance simulation for one timestep for all modulesdriver.Advance(step_size);terrain.Advance(step_size);gator.Advance(step_size);vis->Advance(step_size);// Increment frame numberstep_number++;// Spin in place for real time to catch uprealtime_timer.Spin(step_size);}return 0;
}

还是建议参考官方的两个demo：

在这里插入图片描述

5、小结

吐槽一下Chrono的官方文档，有些内容讲的真的不透彻；

个人建议：采用Filter的方式输出lidar数据，避免用Buffer的方式判断有无更新、而且效率也不一定高。Filter的输出在“sensor更新->延迟->Advance”后存储，文件名是“编号.csv”，可以通过采集频率确定每个编号对应的时间戳。

这篇关于【Chrono Engine学习总结】5-sensor-5.2-导出lidar数据的方法与原理探究的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/717966。

相关文章

一文教你Python如何快速精准抓取网页数据

一文教你Python如何快速精准抓取网页数据

《一文教你Python如何快速精准抓取网页数据》这篇文章主要为大家详细介绍了如何利用Python实现快速精准抓取网页数据,文中的示例代码简洁易懂,具有一定的借鉴价值,有需要的小伙伴可以了解下... 目录1. 准备工作2. 基础爬虫实现3. 高级功能扩展3.1 抓取文章详情3.2 保存数据到文件4. 完整示例
阅读更多...
使用Java将各种数据写入Excel表格的操作示例

使用Java将各种数据写入Excel表格的操作示例

《使用Java将各种数据写入Excel表格的操作示例》在数据处理与管理领域,Excel凭借其强大的功能和广泛的应用,成为了数据存储与展示的重要工具,在Java开发过程中,常常需要将不同类型的数据,本文... 目录前言安装免费Java库1. 写入文本、或数值到 Excel单元格2. 写入数组到 Excel表格
阅读更多...
redis中使用lua脚本的原理与基本使用详解

redis中使用lua脚本的原理与基本使用详解

《redis中使用lua脚本的原理与基本使用详解》在Redis中使用Lua脚本可以实现原子性操作、减少网络开销以及提高执行效率,下面小编就来和大家详细介绍一下在redis中使用lua脚本的原理... 目录Redis 执行 Lua 脚本的原理基本使用方法使用EVAL命令执行 Lua 脚本使用EVALSHA命令
阅读更多...
Java 中的 @SneakyThrows 注解使用方法(简化异常处理的利与弊)

Java 中的 @SneakyThrows 注解使用方法(简化异常处理的利与弊)

《Java中的@SneakyThrows注解使用方法(简化异常处理的利与弊)》为了简化异常处理,Lombok提供了一个强大的注解@SneakyThrows,本文将详细介绍@SneakyThro... 目录1. @SneakyThrows 简介 1.1 什么是 Lombok？2. @SneakyThrows
阅读更多...
判断PyTorch是GPU版还是CPU版的方法小结

判断PyTorch是GPU版还是CPU版的方法小结

《判断PyTorch是GPU版还是CPU版的方法小结》PyTorch作为当前最流行的深度学习框架之一,支持在CPU和GPU(NVIDIACUDA)上运行,所以对于深度学习开发者来说,正确识别PyTor... 目录前言为什么需要区分GPU和CPU版本？性能差异硬件要求如何检查PyTorch版本？方法1：使用命
阅读更多...
python处理带有时区的日期和时间数据

python处理带有时区的日期和时间数据

《python处理带有时区的日期和时间数据》这篇文章主要为大家详细介绍了如何在Python中使用pytz库处理时区信息,包括获取当前UTC时间,转换为特定时区等,有需要的小伙伴可以参考一下... 目录时区基本信息python datetime使用timezonepandas处理时区数据知识延展时区基本信息
阅读更多...
Qt实现网络数据解析的方法总结

Qt实现网络数据解析的方法总结

《Qt实现网络数据解析的方法总结》在Qt中解析网络数据通常涉及接收原始字节流,并将其转换为有意义的应用层数据,这篇文章为大家介绍了详细步骤和示例,感兴趣的小伙伴可以了解下... 目录1. 网络数据接收2. 缓冲区管理（处理粘包/拆包）3. 常见数据格式解析3.1 jsON解析3.2 XML解析3.3 自定义
阅读更多...
SpringMVC 通过ajax 前后端数据交互的实现方法

SpringMVC 通过ajax 前后端数据交互的实现方法

《SpringMVC通过ajax前后端数据交互的实现方法》：本文主要介绍SpringMVC通过ajax前后端数据交互的实现方法,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价... 在前端的开发过程中，经常在html页面通过AJAX进行前后端数据的交互，SpringMVC的controll
阅读更多...
Java中的工具类命名方法

Java中的工具类命名方法

《Java中的工具类命名方法》：本文主要介绍Java中的工具类究竟如何命名,本文给大家介绍的非常详细,对大家的学习或工作具有一定的参考借鉴价值,需要的朋友参考下吧... 目录Java中的工具类究竟如何命名？先来几个例子几种命名方式的比较到底如何命名 ？总结Java中的工具类究竟如何命名？先来几个例子JD
阅读更多...
Spring Security自定义身份认证的实现方法

Spring Security自定义身份认证的实现方法

《SpringSecurity自定义身份认证的实现方法》：本文主要介绍SpringSecurity自定义身份认证的实现方法,下面对SpringSecurity的这三种自定义身份认证进行详细讲解,... 目录1.内存身份认证（1）创建配置类（2）验证内存身份认证2.JDBC身份认证（1）数据准备 （2）配置依
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2