APM中t265数据解析

libraries/AP_VisualOdom/AP_VisualOdom.h

参数有相机的类别, 相机相对飞控的位移, 相机相对飞控摆放的方向(前后左右向下), 相机数据的缩放系数, 延迟, 速度\位置\yaw的噪声

1
2
3
4
5
6
7


float posErr = 0;
float angErr = 0;
if (!isnan(m.pose_covariance[0])) {
    posErr = cbrtf(sq(m.pose_covariance[0])+sq(m.pose_covariance[6])+sq(m.pose_covariance[11]));
    angErr = cbrtf(sq(m.pose_covariance[15])+sq(m.pose_covariance[18])+sq(m.pose_covariance[20]));
}
// 噪声直接是将协方差矩阵中的相加

主要函数有

init()

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17


    // create backend
    switch (VisualOdom_Type(_type.get())) {
    case VisualOdom_Type::None:
        // do nothing
        break;
#if AP_VISUALODOM_MAV_ENABLED
    case VisualOdom_Type::MAV:
        _driver = new AP_VisualOdom_MAV(*this);
        break;
#endif
#if AP_VISUALODOM_INTELT265_ENABLED
    case VisualOdom_Type::IntelT265:
    case VisualOdom_Type::VOXL:
        _driver = new AP_VisualOdom_IntelT265(*this);
        break;
#endif
    }

如果是t265的话默认成VOXL, 这两个应该用的是类似的数据

handle_vision_position_delta_msg()

调用_driver->handle_vision_position_delta_msg()获取角度和位置数据
handle_vision_position_estimate()

获取四元数或者三轴角度然后调用_driver->handle_vision_position_estimate()进行位置估计

libraries/AP_VisualOdom/AP_VisualOdom_IntelT265.cpp

handle_vision_position_estimate()

1
2
3
4
5
6


handle_voxl_camera_reset_jump(pos, att, reset_counter);
// 先调用判断传来的数据有没有跳变, 如果有跳变的话就重设参数, 该参数是从mavlink接受来的, 并不是飞控自己判断的
// 然后直接将获取到的位姿发送给导航或传感器等
rotate_and_correct_position(pos);
rotate_attitude(att);
// 矫正位姿

handle_vision_speed_estimate()

1
2
3


// rotate velocity to align with vehicle
Vector3f vel_corrected = vel;
rotate_velocity(vel_corrected)

关于上位机传来的数据并没有怎么处理, 仅仅将位移和速度矫正了一下