智能数字图像处理:图卷积SGN代码(pytorch)之seq_transformation.py解读

本文详细解读了智能数字图像处理中用于图卷积网络SGN的seq_transformation.py代码,包括加载文件、seq_translation方法、align_frames方法、split_dataset方法以及相关辅助方法的实现。seq_translation方法主要负责骨骼关节数据的预处理,如帧缺失值处理和坐标归一化;align_frames方法确保所有序列具有相同的帧长度;split_dataset方法则将数据划分为训练、验证和测试集,以便于后续的模型训练和评估。

1.加载前面处理的各种文件路径
root_path = './'
stat_path = osp.join(root_path, 'statistics')
setup_file = osp.join(stat_path, 'setup.txt')
camera_file = osp.join(stat_path, 'camera.txt')
performer_file = osp.join(stat_path, 'performer.txt')
replication_file = osp.join(stat_path, 'replication.txt')
label_file = osp.join(stat_path, 'label.txt')
skes_name_file = osp.join(stat_path, 'skes_available_name.txt')

denoised_path = osp.join(root_path, 'denoised_data')
raw_skes_joints_pkl = osp.join(denoised_path, 'raw_denoised_joints.pkl')
frames_file = osp.join(denoised_path, 'frames_cnt.txt')

2.    camera = np.loadtxt(camera_file, dtype=np.int) -》加载camera.txt文件给camera

3.performer = np.loadtxt(performer_file, dtype=np.int)  -》加载performer.txt赋值给performer

4.label = np.loadtxt(label_file, dtype=np.int) - 1  -》加载label.txt标签文件

5.frames_cnt = np.loadtxt(frames_file, dtype=np.int)-》加载frames_cnt.txt文件

6.skes_name = np.loadtxt(skes_name_file, dtype=np.string_)-》加载骨骼名字文件skes_available_name.txt

7.with open(raw_skes_joints_pkl, 'rb') as fr:
        skes_joints = pickle.load(fr)  -》遍历raw_denoised_joints.pkl文件获取骨骼关节

8.skes_joints = seq_translation(skes_joints)-》使用seq_translation方法将序列到序列的网络和注意机制进行翻译

    skes_joints = align_frames(skes_joints, frames_cnt)  -》对齐到相同的帧长度

 9.evaluations = ['CS', 'CV']-》评估模式

    for evaluation in evaluations:
        split_dataset(skes_joints, label, performer, camera, evaluation, save_path)-》传入split_dataset方法将数据保存到.h5文件中

解读seq_translation方法

1.for idx, ske_joints in enumerate(skes_joints):-》从 skes_joints遍历得到id和关节点

2.num_frames = ske_joints.shape[0]-》获取总帧数
        num_bodies = 1 if ske_joints.shape[1] == 75 else 2-》如果关节矩阵形状=75则是1,否则为2

3.if num_bodies == 2:
            missing_frames_1 = np.where(ske_joints[:, :75].sum(axis=1) == 0)[0]-》获取帧总量如果等于0赋值给missing_frames_1
            missing_frames_2 = np.where(ske_joints[:, 75:].sum(axis=1) == 0)[0]
            cnt1 = len(missing_frames_1)-》获取missing_frames_1长度
            cnt2 = len(missing_frames_2)

4.i = 0 -》获得actor1的“真实”第一帧
        while i < num_frames:-》当i小于总帧数
            if np.any(ske_joints[i, :75] != 0):
                break-》跳出
            i += 1

5.origin = np.copy(ske_joints[i, 3:6]) -》起源:关节2

6.for f in range(num_frames):
            if num_bodies == 1:
                ske_joints[f] -= np.tile(origin, 25)-》前25关节点给行为者1
            else:  # for 2 actors
                ske_joints[f] -= np.tile(origin, 50)-》后25个给行为者2

7.if (num_bodies == 2) and (cnt1 > 0):-》如果行为者等于2且行为者1长度大于0
            ske_joints[missing_frames_1, :75] = np.zeros((cnt1, 75), dtype=np.float32)-》ske_joints用0填充

8.if (num_bodies == 2) and (cnt2 > 0):-》如果行为者等于2且行为者2长度大于0
            ske_joints[missing_frames_2, 75:] = np.zeros((cnt2, 75), dtype=np.float32)-》ske_joints用0填充

9.skes_joints[idx] = ske_joints -》更新节点 

解读align_frames方法

作用:以相同的帧长度对齐所有序列。

#include "calibration_component.h" namespace mrp { namespace calib { CalibrationComponent::CalibrationComponent( mrp::common::LoggerPtr i_lg, mrp_interfaces::calibration_component::InitExternalParam i_init_param, mrp_interfaces::calibration_component::RobotParam i_robot_param) : logger_(i_lg) { config_ = std::make_shared<CalibCfg>( i_init_param.laser.x, i_init_param.laser.y, i_init_param.laser.z, i_init_param.laser.roll, i_init_param.laser.pitch, i_init_param.laser.yaw, i_init_param.imu.x, i_init_param.imu.y, i_init_param.imu.z, i_init_param.imu.roll, i_init_param.imu.pitch, i_init_param.imu.yaw, i_robot_param.model, i_robot_param.l, i_robot_param.steer_offset, i_robot_param.d); sensor_dispatch_ = std::make_shared<sensor::SensorDispatch>(i_lg, config_); LOG_COUT_INFO(logger_, "CalibrationComponent start"); } CalibrationComponent::~CalibrationComponent() {} void CalibrationComponent::AddSensorData( const std_msgs::msg::Header& header, const std::vector<mrp_interfaces::msg::MotorStatusData>& motor_datas) { sensor::MotorData motor_data; motor_data.time = common::FromRos(header.stamp); switch (config_->robot_.model) { case 0: break; case 1: break; case 2: for (auto& msg : motor_datas) { if (msg.motor_name == "travel_driver") { motor_data.motor_data.insert( {msg.motor_name, msg.actual_data.twist.linear.x}); } else if (msg.motor_name == "turning_driver") { motor_data.motor_data.insert( {msg.motor_name, msg.actual_data.steer_angle}); } } break; default: break; } if (config_->mode_ == 0 || config_->mode_ == 1) { if (sensor_dispatch_->GetLaserCalibStatus() == 1) { sensor_dispatch_->HandleOdometry(motor_data); } } CalcuteOdom(motor_data); } void CalibrationComponent::AddSensorData( const sensor_msgs::msg::LaserScan::SharedPtr msg) { if (config_->mode_ == 0 || config_->mode_ == 1) { if (sensor_dispatch_->GetLaserCalibStatus() == 1) { sensor_dispatch_->HandleLaserScan(msg); } } } void CalibrationComponent::AddSensorData( const sensor_msgs::msg::Imu::SharedPtr msg) { if (config_->mode_ == 0 || config_->mode_ == 2) { if (config_->mode_ == 0 && sensor_dispatch_->GetLaserCalibStatus() == 1) { return; } if (sensor_dispatch_->GetImuCalibStatus() == 1) { sensor_dispatch_->HandleImu(msg); } } } void CalibrationComponent::CalcuteOdom(const sensor::MotorData& msg) { odom_buffer_.push_back(msg); if (odom_buffer_.size() < 2) return; double v = 0, w = 0; switch (config_->robot_.model) { case 0: break; case 1: break; case 2: { double vd = (odom_buffer_.front().motor_data.at("travel_driver") + odom_buffer_.back().motor_data.at("travel_driver")) / 2.0; double delta = (odom_buffer_.front().motor_data.at("turning_driver") + odom_buffer_.back().motor_data.at("turning_driver")) / 2.0; v = vd * (cos(delta) + config_->robot_.d * sin(delta) / config_->robot_.l); w = vd * sin(delta) / config_->robot_.l; } break; default: break; } auto delta_time = common::ToSeconds(odom_buffer_.back().time - odom_buffer_.front().time); current_odom_.yaw += w * delta_time; current_odom_.x += v * cos(current_odom_.yaw) * delta_time; current_odom_.y += v * sin(current_odom_.yaw) * delta_time; odom_buffer_.pop_front(); } int CalibrationComponent::StartCalibration( mrp_interfaces::calibration_component::CalibMode i_calib_mode) { int error_code = 1; std::string error_info = ""; do { if (i_calib_mode.mode < 0 || i_calib_mode.mode > 2) { error_code = -1; error_info = "calib mode error"; break; } if (config_->robot_.model < 0 || config_->robot_.model > 2) { error_code = -2; error_info = "robot model error"; break; } config_->SetMode(i_calib_mode.mode, i_calib_mode.move_dist); int laser_status = 0, imu_status = 0; if (config_->mode_ == 0 || config_->mode_ == 1) { laser_status = 1; } if (config_->mode_ == 0 || config_->mode_ == 2) { imu_status = 1; } sensor_dispatch_->InitCalibSolve(laser_status, imu_status); move_initialized_ = false; control_times_.clear(); error_info = "start calibrate success"; } while (0); LOG_COUT_INFO(logger_, error_info); return error_code; } void CalibrationComponent::StopCalibration() { sensor_dispatch_->StopCalibSolve(); } void CalibrationComponent::GetCalibrationMoveCommand( std::vector<mrp_interfaces::msg::MotorControlData>& cmd_vel) { constexpr double v_limit = 0.2, delta_limit = 0.7, a = 0.5; constexpr double /* move_distance = 5.0, */ move_yaw = 0.3; if (config_->mode_ == 0 || config_->mode_ == 1) { control_times_.push_back(common::Now()); if (!move_initialized_) { origin_x = current_odom_.x; origin_y = current_odom_.y; origin_yaw = current_odom_.yaw; is_forward_phase_ = true; is_left_phase_ = true; move_initialized_ = true; LOG_COUT_INFO(logger_, "calibration auto move start"); } // 计算相对原点距离 const double distance = std::hypot(current_odom_.x - origin_x, current_odom_.y - origin_y); if (is_forward_phase_ && distance > config_->move_dist_) { is_forward_phase_ = false; } else if (!is_forward_phase_ && distance < 0.5) { is_forward_phase_ = true; } double current_yaw = std::fmod(current_odom_.yaw + M_PI, 2 * M_PI) - M_PI; if (current_yaw - origin_yaw > move_yaw) { if (is_forward_phase_) is_left_phase_ = false; else is_left_phase_ = true; } else if (current_yaw - origin_yaw < -move_yaw) { if (is_forward_phase_) is_left_phase_ = true; else is_left_phase_ = false; } double control_timer = 0.0; if (control_times_.size() >= 2) { control_timer = common::ToSeconds(control_times_.back() - control_times_.front()); control_times_.pop_front(); } switch (config_->robot_.model) { case 0: break; case 1: break; case 2: { double target_v = is_forward_phase_ ? v_limit : -v_limit; double target_delta = is_left_phase_ ? delta_limit : -delta_limit; if (sensor_dispatch_->GetLaserCalibStatus() != 1 || sensor_dispatch_->GetLaserDataNum() == 1) { target_v = 0.0; target_delta = 0.0; } double v = 0; if (!odom_buffer_.empty()) v = odom_buffer_.back().motor_data.at("travel_driver") + a * std::copysign(1.0, target_v) * control_timer; double delta = target_delta; if (std::fabs(target_v) <= 1e-4) { v = 0; } if (std::fabs(v) > v_limit) v = target_v; if (std::fabs(delta) > delta_limit) delta = target_delta; mrp_interfaces::msg::MotorControlData travel_motor_control_data; travel_motor_control_data.motor_name = "travel_driver"; travel_motor_control_data.control.twist.linear.x = v; cmd_vel.push_back(travel_motor_control_data); mrp_interfaces::msg::MotorControlData turning_motor_control_data; turning_motor_control_data.motor_name = "turning_driver"; turning_motor_control_data.control.steer_angle = delta; cmd_vel.push_back(turning_motor_control_data); } default: break; } } else if (config_->mode_ == 2) { switch (config_->robot_.model) { case 0: break; case 1: break; case 2: { mrp_interfaces::msg::MotorControlData travel_motor_control_data; travel_motor_control_data.motor_name = "travel_driver"; travel_motor_control_data.control.twist.linear.x = 0; cmd_vel.push_back(travel_motor_control_data); mrp_interfaces::msg::MotorControlData turning_motor_control_data; turning_motor_control_data.motor_name = "turning_driver"; turning_motor_control_data.control.steer_angle = 0; cmd_vel.push_back(turning_motor_control_data); } break; default: break; } } } int CalibrationComponent::GetCalibrationStatusAndResult( std::map<std::string, mrp_interfaces::calibration_component::ExternalParam>& calib_result) { std::vector<int> calib_status; calib_status.clear(); calib_result.clear(); int laser_calib_status = sensor_dispatch_->GetLaserCalibStatus(); int odom_calib_status = sensor_dispatch_->GetLaserCalibStatus(); int imu_calib_status = sensor_dispatch_->GetImuCalibStatus(); switch (laser_calib_status) { case 0: calib_status.push_back(static_cast<int>(ActionStatus::NONE)); calib_status.push_back(static_cast<int>(ActionStatus::NONE)); break; case 1: calib_status.push_back(static_cast<int>(ActionStatus::RUNNING)); calib_status.push_back(static_cast<int>(ActionStatus::RUNNING)); break; case 2: calib_status.push_back(static_cast<int>(ActionStatus::RUNNING)); calib_status.push_back(static_cast<int>(ActionStatus::RUNNING)); break; case 3: calib_status.push_back(static_cast<int>(ActionStatus::FINISHED)); calib_status.push_back(static_cast<int>(ActionStatus::FINISHED)); break; case 4: calib_status.push_back(static_cast<int>(ActionStatus::FAILED)); calib_status.push_back(static_cast<int>(ActionStatus::FAILED)); default: break; } switch (imu_calib_status) { case 0: calib_status.push_back(static_cast<int>(ActionStatus::NONE)); break; case 1: calib_status.push_back(static_cast<int>(ActionStatus::RUNNING)); break; case 3: calib_status.push_back(static_cast<int>(ActionStatus::FINISHED)); break; default: break; } Eigen::Vector<double, 6> laser_calib_result, imu_calib_result; double steer_offset; sensor_dispatch_->GetCalibResult(laser_calib_result, imu_calib_result, steer_offset); calib_result.insert(std::make_pair( "laser", mrp_interfaces::calibration_component::ExternalParam{ -laser_calib_result[0], -laser_calib_result[1], -laser_calib_result[2], -laser_calib_result[3], -laser_calib_result[4], -laser_calib_result[5]})); calib_result.insert( {"odom", mrp_interfaces::calibration_component::ExternalParam{ 0.0, 0.0, 0.0, 0.0, 0.0, steer_offset}}); calib_result.insert( {"imu", mrp_interfaces::calibration_component::ExternalParam{ imu_calib_result[0], imu_calib_result[1], imu_calib_result[2], imu_calib_result[3], imu_calib_result[4], imu_calib_result[5]}}); double calib_status_com = 0; if (config_->mode_ == 0) { if (calib_status[0] == static_cast<int>(ActionStatus::FINISHED) && calib_status[1] == static_cast<int>(ActionStatus::FINISHED) && calib_status[2] == static_cast<int>(ActionStatus::FINISHED)) { calib_status_com = static_cast<int>(ActionStatus::FINISHED); } else if (std::find(calib_status.begin(), calib_status.end(), static_cast<int>(ActionStatus::FAILED)) != calib_status.end()) { calib_status_com = static_cast<int>(ActionStatus::FAILED); } else if (std::find(calib_status.begin(), calib_status.end(), static_cast<int>(ActionStatus::RUNNING)) != calib_status.end()) { calib_status_com = static_cast<int>(ActionStatus::RUNNING); } } else if (config_->mode_ == 1) { if (calib_status[0] == static_cast<int>(ActionStatus::FINISHED) && calib_status[1] == static_cast<int>(ActionStatus::FINISHED)) { calib_status_com = static_cast<int>(ActionStatus::FINISHED); } else if (std::find(calib_status.begin(), calib_status.end(), static_cast<int>(ActionStatus::FAILED)) != calib_status.end()) { calib_status_com = static_cast<int>(ActionStatus::FAILED); } else if (std::find(calib_status.begin(), calib_status.end(), static_cast<int>(ActionStatus::RUNNING)) != calib_status.end()) { calib_status_com = static_cast<int>(ActionStatus::RUNNING); } } else if (config_->mode_ == 2) { if (calib_status[2] == static_cast<int>(ActionStatus::FINISHED)) { calib_status_com = static_cast<int>(ActionStatus::FINISHED); } else if (calib_status[2] == static_cast<int>(ActionStatus::FAILED)) { calib_status_com = static_cast<int>(ActionStatus::FAILED); } else if (calib_status[2] == static_cast<int>(ActionStatus::RUNNING)) { calib_status_com = static_cast<int>(ActionStatus::RUNNING); } } return calib_status_com; } void CalibrationComponent::DebugPrint() { sensor_dispatch_->DebugInfo(); } } // namespace calib } // namespace mrp请问分别实现了什么逻辑功能?请按工作流解释
最新发布
06-18
<think>我们正在分析一个传感器外参标定库中的CalibrationComponent类。该类用于标定传感器(如激光雷达、IMU)相对于机器人本体的外参(位置和姿态)。我们将重点解释关键方法的功能和工作流程。###1.类概述`CalibrationComponent`类负责协调传感器外参标定过程。它接收传感器数据(里程计、激光雷达、IMU),根据标定模式(激光+里程计+IMU、激光+里程计、IMU)执行标定,并输出标定结果。###2.关键方法解析####2.1`AddSensorData`方法此方法有三个重载版本,分别用于添加不同类型的传感器数据:-**里程计数据**:接收电机状态数据(`motor_datas`),用于计算机器人的运动(位置和朝向)。这些数据在标定激光雷达和里程计时必须实时添加。-**激光雷达数据**:接收激光扫描数据(`LaserScan`),用于标定激光雷达的外参。-**IMU数据**:接收IMU测量数据(`Imu`),用于标定IMU的外参。**工作流程**:1.数据被添加到内部缓冲区(例如`odom_buffer_`)或直接传递给标定算法。2.在标定过程中,这些数据将被用于构建运动约束或优化外参。####2.2`StartCalibration`方法**功能**:启动标定过程,初始化标定状态。-**输入参数**:标定模式(`i_calib_mode`),包括:-模式:指定要标定的传感器组合(0:激光+里程计+IMU;1:激光+里程计;2:IMU)。-移动距离:标定过程中机器人需要移动的距离(单位:米)。-**返回值**:错误代码(-1:模式错误;-2:机器人模型错误)。**逻辑流程**:1.验证标定模式和机器人模型是否支持。2.重置内部状态(如清除缓冲区、重置里程计位置)。3.将标定状态设置为`INITIALIZING`(初始化中)。4.准备标定所需的初始条件(例如记录初始位姿`origin_x`,`origin_y`,`origin_yaw`)。####2.3`GetCalibrationMoveCommand`方法**功能**:生成标定过程中机器人的运动指令。-**输出**:电机控制指令(`motor_datas`),用于控制机器人按标定要求运动。**运动策略**(以单舵轮模型为例):1.标定通常需要机器人执行特定轨迹(例如:前进一段距离,再后退;或左右转弯)。2.方法根据标定进度(`is_forward_phase_`,`is_left_phase_`)生成当前阶段的运动指令。3.例如:-前进阶段:发送正向速度指令。-转弯阶段:发送转向角指令。####2.4`GetCalibrationStatusAndResult`方法**功能**:获取当前标定状态和标定结果。-**输出参数**:`calib_result`(标定结果字典),键为传感器类型(如"lidar"、"odom"、"imu"),值为对应的外参(`ExternalParam`结构,包含x、y、z、roll、pitch、yaw偏移)。-**返回值**:标定状态(枚举值:`NONE`、`WAITING`、`INITIALIZING`、`RUNNING`、`PAUSED`、`FINISHED`、`FAILED`)。**工作流程**:1.检查标定算法是否完成。2.若完成,从标定算法中获取外参结果并填充到`calib_result`。3.返回当前状态(例如:进行中`RUNNING`,完成`FINISHED`)。###3.标定工作流整个标定过程是一个状态机,典型流程如下:```mermaidgraphTDA[StartCalibration]-->B[初始化状态和变量]B-->C[进入INITIALIZING状态]C-->D[生成运动指令]D-->E[添加传感器数据]E-->F{是否达到移动距离?}F--是-->G[优化计算外参]G-->H[状态设为FINISHED]F--否-->D```###4.辅助方法-`CalcuteOdom`:根据电机数据计算机器人里程计(位置和朝向)。-`DebugPrint`:调试用,打印内部状态。###5.内部状态管理-`current_odom_`:记录当前里程计位姿。-`odom_buffer_`:存储历史里程计数据(用于时间同步)。-`move_initialized_`:标记运动初始化是否完成。-`origin_x`,`origin_y`,`origin_yaw`:标定起始位姿。###总结`CalibrationComponent`类通过协调传感器数据采集、机器人运动控制和标定算法,实现传感器外参的在线标定。其核心是状态机管理和数据流处理,确保在机器人执行特定轨迹时收集足够数据以优化外参。§§相关问题§§1.如何确保不同传感器数据的时间同步?2.标定算法具体如何优化外参?3.机器人模型(差速、阿克曼、单舵轮)如何影响标定过程?4.标定过程中如果传感器数据丢失会怎样处理?5.标定结果的可信度如何评估?</think>### CalibrationComponent 类关键方法功能解析 #### 1. AddSensorData 方法 **功能**:接收并预处理传感器原始数据 **工作流程**: 1. 三个重载版本分别处理不同类型传感器数据: - **里程计数据**:接收电机状态数据,存入 `odom_buffer_` 队列 - **激光雷达数据**:直接传递给 `sensor_dispatch_` 处理 - **IMU数据**:直接传递给 `sensor_dispatch_` 处理 2. 内部维护时间序列缓冲区(如 `odom_buffer_`),确保数据按时间顺序处理 3. 触发里程计算法:`CalcuteOdom()` 根据电机数据更新 `current_odom_`(当前位姿) **关键逻辑**: ```cpp void AddSensorData(const vector<MotorStatusData>& motor_datas) { odom_buffer_.push_back(ProcessMotorData(motor_datas)); // 数据入队 if (calib_status == ActionStatus::RUNNING) { CalcuteOdom(odom_buffer_.back()); // 实时更新位姿 } } ``` #### 2. StartCalibration 方法 **功能**:初始化标定过程 **工作流程**: 1. 验证标定模式有效性(模式代码范围检查) 2. 检查机器人模型兼容性(差速/阿克曼/单舵轮) 3. 重置内部状态: - 清空 `odom_buffer_` - 重置 `current_odom_` 为零位 - 设置 `origin_x/y/yaw` 为起始位姿 4. 将状态设为 `INITIALIZING` 5. 返回错误码(成功返回0) **参数说明**: ```mermaid graph LR A[CalibMode] --> B[mode] A --> C[move_dist] B --> D[0-激光+里程计+IMU] B --> E[1-激光+里程计] B --> F[2-IMU] ``` #### 3. GetCalibrationMoveCommand 方法 **功能**:生成标定运动指令序列 **工作流程**: 1. 根据标定阶段(`is_forward_phase_`, `is_left_phase_`)生成不同运动模式 2. 典型运动序列: - 阶段1:直线前进指定距离(`move_dist`) - 阶段2:原地左转90度 - 阶段3:直线前进相同距离 - 阶段4:原地右转90度(回位) 3. 输出电机控制指令到 `motor_datas` **运动控制方程**: 对于单舵轮模型,转向角 $\theta$ 和速度 $v$ 满足: $$ \begin{cases} v = v_{\text{max}} \cdot \text{sgn}(d_{\text{target}} - d_{\text{current}}) \\ \theta = k_p \cdot (\phi_{\text{target}} - \phi_{\text{current}}) \end{cases} $$ 其中 $d$ 为移动距离,$\phi$ 为朝向角[^1]。 #### 4. GetCalibrationStatusAndResult 方法 **功能**:获取标定状态和结果 **工作流程**: 1. 检查标定状态机: ```mermaid stateDiagram-v2 [*] --> NONE NONE --> INITIALIZING: StartCalibration INITIALIZING --> RUNNING: 数据就绪 RUNNING --> FINISHED: 移动完成 RUNNING --> FAILED: 数据异常 ``` 2. 成功时填充 `calib_result`: - **激光雷达**:外参(x,y,yaw) - **IMU**:姿态偏移量 - **里程计**:舵轮零偏(存入yaw字段) 3. 返回枚举状态值(0-6) ### 标定系统工作流 ```mermaid sequenceDiagram participant User participant CalibComponent participant Robot User->>CalibComponent: StartCalibration(mode) CalibComponent->>CalibComponent: 状态=INITIALIZING loop 标定循环 CalibComponent->>Robot: GetCalibrationMoveCommand() Robot-->>CalibComponent: AddSensorData(里程计) Robot-->>CalibComponent: AddSensorData(激光/IMU) CalibComponent->>CalibComponent: 更新current_odom_ alt 移动距离达标 CalibComponent->>CalibComponent: 状态=FINISHED else 数据异常 CalibComponent->>CalibComponent: 状态=FAILED end end User->>CalibComponent: GetCalibrationStatusAndResult() ``` ### 关键技术点 1. **时间同步**:通过 `control_times_` 队列管理传感器时间戳 2. **位姿计算**:`CalcuteOdom()` 实现运动学模型解算 3. **状态管理**:有限状态机(ActionStatus)控制标定流程 4. **外参表示**:使用 `ExternalParam` 结构体统一表示(x,y,z,roll,pitch,yaw)
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值