思岚Aurora基础使用之数据可视化篇-程序员充电站

思岚Aurora基础使用之数据可视化篇

在我们能够从雷达得到数据之后，我们就可以用openCV对其进行可视化，方便我们观察数据的趋势。所以本篇文章介绍如何进行可视化这一操作。

数据的可视化，其实就是在一块画布上显示出一些数据信息，所以我们应该首先要知道我们要可视化哪些信息，把这些信息打包成一个整体，为后续的可视化提供便利，可视化的流程也很简单：

打包数据 → 数据滤波 → 调用CV接口可视化

一、数据合并

通过前文我们可以知道，我们需要将位姿数据pose，以及imu的数据可视化出来，所以我们需要把他们先合成为一种数据。在C语言中，我们常常使用结构体来进行这种操作，但到了C++中，我们就可以把结构体提升为一个类，用初始化列表的方式来填充数据，这样就方便的多。我们定义这种合成数据如下，这其中就包括了：

位姿数据
加速度数据
陀螺仪数据

之后我们可视化的时候就传这种数据类型就可以了

structTrajectoryPoint{uint64_ttimestamp_ns;// 时间戳（纳秒）slamtec_aurora_sdk_pose_t pose;// 位姿信息（x,y,z,roll,pitch,yaw）// IMU数据doubleaccel_x,accel_y,accel_z;// 加速度计数据 (m/s²)doublegyro_x,gyro_y,gyro_z;// 陀螺仪数据 (rad/s)// 构造函数TrajectoryPoint()=default;/** * @brief 构造函数 * @param ts 时间戳（纳秒） * @param p 位姿信息 * @param imu IMU数据 */TrajectoryPoint(uint64_tts,constslamtec_aurora_sdk_pose_t&p,constslamtec_aurora_sdk_imu_data_t&imu);};TrajectoryPoint::TrajectoryPoint(uint64_tts,constslamtec_aurora_sdk_pose_t&p,constslamtec_aurora_sdk_imu_data_t&imu):timestamp_ns(ts),pose(p),accel_x(imu.acc[0]),accel_y(imu.acc[1]),accel_z(imu.acc[2]),gyro_x(imu.gyro[0]),gyro_y(imu.gyro[1]),gyro_z(imu.gyro[2]){}

二、数据滤波

如果只对传来的数据进行打包，然后送到CV进行可视化，显然是不行的，会遇到诸多问题，比如当数据突然抖动时，出现数据延迟、错位传送时，都会让我们的可视化界面出现突然地抖动。因此在数据可视化前，我们应该对其进行滤波，保证数据的稳定性。这里我们不讨论卡尔曼滤波，只对基础滤波做讲解，我们的数据滤波模块定义如下：

#ifndefTRAJECTORY_FILTER_HPP#defineTRAJECTORY_FILTER_HPP#include<vector>#include<memory>#include"TrajectoryPoint.hpp"#include"KalmanFilter.hpp"/** * @brief 轨迹滤波器 - 对轨迹数据进行卡尔曼滤波处理 * * 提供简单的接口，自动处理时间间隔计算和滤波器状态管理 */classTrajectoryFilter{public:/** * @brief 构造函数 * @param enable_kalman 是否启用卡尔曼滤波（默认启用） * @param process_noise 过程噪声系数 * @param measurement_noise 测量噪声系数 */TrajectoryFilter(boolenable_kalman=true,doubleprocess_noise=0.01,doublemeasurement_noise=0.1);/** * @brief 处理单个轨迹点 * @param point 原始轨迹点 * @return 滤波后的轨迹点 */TrajectoryPointfilterPoint(constTrajectoryPoint&point);/** * @brief 批量处理轨迹数据 * @param trajectory 原始轨迹 * @return 滤波后的轨迹 */std::vector<TrajectoryPoint>filterTrajectory(conststd::vector<TrajectoryPoint>&trajectory);/** * @brief 启用/禁用卡尔曼滤波 * @param enable true启用，false禁用 */voidsetKalmanEnabled(boolenable);/** * @brief 检查卡尔曼滤波是否启用 */boolisKalmanEnabled()const{returnenable_kalman_;}/** * @brief 重置滤波器状态 */voidreset();/** * @brief 获取当前估计速度 * @param vx X方向速度 * @param vy Y方向速度 * @param vz Z方向速度 * @return 是否成功获取 */boolgetEstimatedVelocity(double&vx,double&vy,double&vz)const;private:std::unique_ptr<KalmanFilter>kalman_filter_;boolenable_kalman_;uint64_tlast_timestamp_ns_;boolfirst_point_;};#endif// TRAJECTORY_FILTER_HPP

在这部分，我们主要完成的是时间戳去重的滤波，避免同样的数据反复出现，以及防止“时间倒流”问题，即先传的数据后到达的问题，进而保证轨迹数据的时间单调性。

TrajectoryPointTrajectoryFilter::filterPoint(constTrajectoryPoint&point){// 如果禁用卡尔曼滤波，直接返回原始数据if(!enable_kalman_){returnpoint;}// 第一个点，初始化时间戳if(first_point_){last_timestamp_ns_=point.timestamp_ns;first_point_=false;kalman_filter_->initialize(point);returnpoint;}// 计算时间间隔doubledt=(point.timestamp_ns-last_timestamp_ns_)/1e9;// 转换为秒last_timestamp_ns_=point.timestamp_ns;// 防止时间间隔异常if(dt<=0||dt>1.0){std::cout<<"[TrajectoryFilter] 警告: 异常时间间隔 "<<dt<<"s, 跳过滤波"<<std::endl;returnpoint;}// 卡尔曼滤波处理returnkalman_filter_->process(point,dt);}

还有一部分滤波，我们是在可视化的部分完成的，这部分的滤波是为了防止画面出现太大的抖动，所以我对位置变化间隔以及时间变化间隔设置了一个阈值，超过阈值才会更新对应的数据，同时对数据做裁剪，如果数据点超过了地图的显示范围，我们就用灰色来显示它。

std::vector<TrajectoryPoint>TrajectoryVisualizer::filterTrajectory(conststd::vector<TrajectoryPoint>&trajectory){if(trajectory.size()<2)returntrajectory;std::vector<TrajectoryPoint>filtered;filtered.reserve(trajectory.size());filtered.push_back(trajectory[0]);for(size_t i=1;i<trajectory.size();++i){constauto&current=trajectory[i];constauto&last=filtered.back();doubledx=current.pose.translation.x-last.pose.translation.x;doubledy=current.pose.translation.y-last.pose.translation.y;doubledistance=sqrt(dx*dx+dy*dy);uint64_ttime_diff=current.timestamp_ns-last.timestamp_ns;if(distance>position_threshold_||time_diff>min_time_interval_){filtered.push_back(current);}}returnfiltered;}

完成这些滤波后，我们的数据质量就已得到提高，就可以继续进行可视化操作了。

三、数据可视化

当我们拥有了一段稳定的数据后，就可以用openCV对其进行可视化，我们可视化其实就是完成：初始化画布，画出坐标系标出刻度，把点加入画布并且连成线，在特定区域显示出IMU信息。利用CV的接口我们就可以很轻松的完成这些任务，我们先看看这个模块的整体框架：

#ifndefTRAJECTORY_VISUALIZER_HPP#defineTRAJECTORY_VISUALIZER_HPP#include<vector>#include<string>#include<mutex>#include<opencv2/opencv.hpp>#include"TrajectoryPoint.hpp"/** * @brief 轨迹可视化类 * * 负责实时显示轨迹、保存图像和导出数据 */classTrajectoryVisualizer{public:/** * @brief 构造函数 * @param width 画布宽度（像素） * @param height 画布高度（像素） * @param fixed_scale 固定缩放比例（像素/米） * @param map_range 地图显示范围（±米） */TrajectoryVisualizer(intwidth=800,intheight=600,doublefixed_scale=50.0,doublemap_range=10.0);/** * @brief 设置固定缩放比例 * @param scale 缩放比例（像素/米） */voidsetFixedScale(doublescale);/** * @brief 设置地图显示范围 * @param range 显示范围（±米） */voidsetMapRange(doublerange);/** * @brief 更新轨迹显示 * @param trajectory 轨迹点向量 */voidupdateTrajectory(conststd::vector<TrajectoryPoint>&trajectory);/** * @brief 显示可视化窗口 */voidshow();/** * @brief 保存轨迹图像 * @param filename 文件名 */voidsaveImage(conststd::string&filename);/** * @brief 保存轨迹数据为CSV格式（用于神经网络训练） * @param trajectory 轨迹点向量 * @param filename 文件名 */staticvoidsaveTrajectoryData(conststd::vector<TrajectoryPoint>&trajectory,conststd::string&filename);/** * @brief 保存轨迹数据为JSON格式（可选） * @param trajectory 轨迹点向量 * @param filename 文件名 */staticvoidsaveTrajectoryDataJSON(conststd::vector<TrajectoryPoint>&trajectory,conststd::string&filename);private:/** * @brief 轨迹滤波函数 - 减少噪声和漂移 * @param trajectory 原始轨迹 * @return 滤波后的轨迹 */std::vector<TrajectoryPoint>filterTrajectory(conststd::vector<TrajectoryPoint>&trajectory);/** * @brief 绘制坐标系 */voiddrawCoordinateSystem();/** * @brief 绘制信息面板 * @param trajectory 轨迹点向量 * @param original_count 原始数据点数量 */voiddrawInfo(conststd::vector<TrajectoryPoint>&trajectory,size_t original_count=0);// 成员变量cv::Mat canvas_;// 画布intcanvas_width_,canvas_height_;// 画布尺寸intcenter_x_,center_y_;// 画布中心点doublefixed_scale_;// 固定缩放比例（像素/米）doublemap_range_;// 地图显示范围（米）std::mutex canvas_mutex_;// 画布互斥锁// 滤波参数doubleposition_threshold_;// 位置变化阈值（米）uint64_tmin_time_interval_;// 最小时间间隔（纳秒）};#endif// TRAJECTORY_VISUALIZER_HPP

可以发现，这些接口无非就是对参数的修改，把数据保存成不同类型的格式，对数据进行滤波（前文已经提到），对画布进行更新，在这里我们最应该关心的是在画布上如何进行操作。

由于要用到openCV，这里对代码中用到的常用接口进行归纳：

类别	函数/类	功能说明	使用场景	代码示例
画布创建	`cv::Mat::zeros()`	创建指定大小的全黑画布	初始化画布、清空画布	`cv::Mat::zeros(height, width, CV_8UC3)`
基础绘制	`cv::line()`	绘制直线	绘制坐标轴、网格线、轨迹连线	`cv::line(canvas, pt1, pt2, color, thickness)`
`cv::circle()`	绘制圆形	标记起点、当前位置、原点	`cv::circle(canvas, center, radius, color, -1)`
`cv::rectangle()`	绘制矩形	绘制地图边界框	`cv::rectangle(canvas, rect, color, 2)`
`cv::arrowedLine()`	绘制箭头线	显示朝向、视场方向	`cv::arrowedLine(canvas, start, end, color, 2, cv::LINE_AA)`
文本绘制	`cv::putText()`	在图像上绘制文本	显示状态信息、坐标标签、数据信息	`cv::putText(canvas, text, pos, font, scale, color, thickness)`
数据结构	`cv::Point`	2D点坐标	存储像素坐标位置	`cv::Point(x, y)`
`cv::Scalar`	颜色值（BGR）	定义绘制颜色	`cv::Scalar(B, G, R)`如`(255, 0, 0)`= 蓝色
`cv::Rect`	矩形区域	定义边界框区域	`cv::Rect(x, y, width, height)`
`cv::Mat`	图像矩阵	存储画布数据	`cv::Mat canvas_`
显示与保存	`cv::imshow()`	显示图像窗口	实时显示轨迹	`cv::imshow("窗口名", canvas)`
`cv::waitKey()`	等待键盘输入	刷新窗口、检测按键	`cv::waitKey(1)`
`cv::imwrite()`	保存图像到文件	导出轨迹图像	`cv::imwrite(filename, canvas)`
绘制常量	`cv::FONT_HERSHEY_SIMPLEX`	字体类型	文本绘制时的字体选择	在`putText`中使用
`cv::LINE_AA`	抗锯齿线型	绘制平滑线条	在`arrowedLine`中使用
`CV_8UC3`	8位3通道图像	创建彩色图像	在`Mat::zeros`中使用

了解了基本接口后，我们就来看看如何做到对数据的实时更新：

首先，为了防止一张画布同时被多次写入，我们就需要一把锁来确保原子性。

接下来，在每次更新的时候我们都清空画布，重新绘制坐标系，接着从保存数据的vector中持续拿出数据送给滤波器，然后拿到滤波后的数据。

拿到数据后，根据数据的x，y坐标，把他们放在画布的对应点上（这里要注意雷达的坐标轴定义与openCV的坐标轴定义的区别，要进行一下转换）。

然后，我们把第i个点和第i-1个点用线连起来，对所有点都进行这样的操作，我们就可以得到一条轨迹线，并且根据这两点定义出一个向量，画出箭头来表示此时雷达的所朝方向，同时增加两个视场方向共同反应出雷达此时的朝向。

最后，我们把IMU信息和位姿信息的实际数据显示出来即可。

voidTrajectoryVisualizer::updateTrajectory(conststd::vector<TrajectoryPoint>&trajectory){std::lock_guard<std::mutex>lock(canvas_mutex_);// 清空画布canvas_=cv::Mat::zeros(canvas_height_,canvas_width_,CV_8UC3);// 绘制坐标系drawCoordinateSystem();// 绘制轨迹if(trajectory.size()<2){cv::putText(canvas_,"Waiting for trajectory data...",cv::Point(10,30),cv::FONT_HERSHEY_SIMPLEX,0.7,cv::Scalar(255,255,255),2);return;}// 滤波轨迹数据std::vector<TrajectoryPoint>filtered_trajectory=filterTrajectory(trajectory);if(filtered_trajectory.size()<2){cv::putText(canvas_,"Processing trajectory data...",cv::Point(10,30),cv::FONT_HERSHEY_SIMPLEX,0.7,cv::Scalar(255,255,0),2);return;}// 绘制轨迹线// 坐标系映射：雷达X(前后) → 图像Y(上下), 雷达Y(左右) → 图像X(左右)std::vector<cv::Point>trajectory_points;std::vector<bool>point_in_bounds;for(constauto&point:filtered_trajectory){// 正确的坐标映射：// - 雷达左右移动(Y) → 图像水平方向(X)// - 雷达前后移动(X) → 图像垂直方向(Y, 向前为向上)intx=center_x_+static_cast<int>(point.pose.translation.y*fixed_scale_);inty=center_y_-static_cast<int>(point.pose.translation.x*fixed_scale_);boolin_bounds=(std::abs(point.pose.translation.x)<=map_range_&&std::abs(point.pose.translation.y)<=map_range_);intclipped_x=std::max(0,std::min(canvas_width_-1,x));intclipped_y=std::max(0,std::min(canvas_height_-1,y));trajectory_points.push_back(cv::Point(clipped_x,clipped_y));point_in_bounds.push_back(in_bounds);}// 绘制轨迹线（渐变颜色）for(size_t i=1;i<trajectory_points.size();++i){floatratio=static_cast<float>(i)/trajectory_points.size();cv::Scalar color;if(point_in_bounds[i]&&point_in_bounds[i-1]){color=cv::Scalar(0,255*(1-ratio),255*ratio);}else{color=cv::Scalar(128,128,128);}intline_thickness=(point_in_bounds[i]&&point_in_bounds[i-1])?2:1;cv::line(canvas_,trajectory_points[i-1],trajectory_points[i],color,line_thickness);}// 标记起始点和当前点if(!trajectory_points.empty()){// 起始点if(point_in_bounds[0]){cv::circle(canvas_,trajectory_points[0],5,cv::Scalar(0,255,0),-1);}else{cv::circle(canvas_,trajectory_points[0],3,cv::Scalar(0,128,0),-1);}// 当前点size_t last_idx=trajectory_points.size()-1;cv::Point current_pt=trajectory_points[last_idx];if(point_in_bounds[last_idx]){cv::circle(canvas_,current_pt,5,cv::Scalar(0,0,255),-1);// 绘制朝向箭头constauto&pose=filtered_trajectory[last_idx].pose;doubleyaw=pose.rpy.yaw;doublearrow_len=25.0;// 调整角度：因为坐标系旋转了90度，需要补偿// yaw=0表示雷达朝向X轴正方向(前)，在图像中应该是向上(-Y方向)doubledisplay_angle=yaw-M_PI/2.0;cv::Pointarrow_end(current_pt.x+static_cast<int>(arrow_len*cos(display_angle)),current_pt.y-static_cast<int>(arrow_len*sin(display_angle)));cv::arrowedLine(canvas_,current_pt,arrow_end,cv::Scalar(0,0,255),2,cv::LINE_AA);// 绘制视场方向辅助箭头doubleoffset_angle=M_PI/6;doublearrow_len2=20.0;for(doubleoffset:{-offset_angle,offset_angle}){doubletheta=display_angle+offset;cv::Pointside_end(current_pt.x+static_cast<int>(arrow_len2*cos(theta)),current_pt.y-static_cast<int>(arrow_len2*sin(theta)));cv::arrowedLine(canvas_,current_pt,side_end,cv::Scalar(0,128,255),1,cv::LINE_AA);}}else{cv::circle(canvas_,current_pt,3,cv::Scalar(0,0,128),-1);}}// 显示信息drawInfo(trajectory);}

voidTrajectoryVisualizer::drawInfo(conststd::vector<TrajectoryPoint>&trajectory,size_t original_count){if(trajectory.empty())return;constauto&latest=trajectory.back();std::string pos_info="Position: ("+std::to_string(latest.pose.translation.x).substr(0,6)+", "+std::to_string(latest.pose.translation.y).substr(0,6)+", "+std::to_string(latest.pose.translation.z).substr(0,6)+")";std::string yaw_info="Yaw: "+std::to_string(latest.pose.rpy.yaw*180.0/M_PI).substr(0,6)+"°";std::string count_info="Points: "+std::to_string(trajectory.size());std::string map_info="Map: ±"+std::to_string(map_range_).substr(0,4)+"m, Scale: "+std::to_string(fixed_scale_).substr(0,4)+"px/m";boolin_range=(std::abs(latest.pose.translation.x)<=map_range_&&std::abs(latest.pose.translation.y)<=map_range_);std::string range_status=in_range?"IN RANGE":"OUT OF RANGE";cv::Scalar range_color=in_range?cv::Scalar(0,255,0):cv::Scalar(0,0,255);std::string legend="Green=Start, Red=Current, Gray=OutOfRange";cv::putText(canvas_,pos_info,cv::Point(10,canvas_height_-90),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(255,255,255),1);cv::putText(canvas_,yaw_info,cv::Point(10,canvas_height_-70),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(255,255,255),1);cv::putText(canvas_,count_info,cv::Point(10,canvas_height_-50),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(255,255,255),1);cv::putText(canvas_,map_info,cv::Point(10,canvas_height_-30),cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(255,255,255),1);cv::putText(canvas_,range_status,cv::Point(10,canvas_height_-10),cv::FONT_HERSHEY_SIMPLEX,0.5,range_color,1);cv::putText(canvas_,legend,cv::Point(canvas_width_-250,canvas_height_-10),cv::FONT_HERSHEY_SIMPLEX,0.4,cv::Scalar(200,200,200),1);}