1. 源由

AP_VisualOdom主要是配合視覺定位應用，通常視覺定位應用于室內或者無GPS等輔助定位系統的環境。

2. 類定義

AP_VisualOdom 類用于視覺里程計系統在無人機上的實現。這個類具有多種方法和屬性，用于初始化和操作視覺里程計傳感器。

這些私有成員變量用于存儲類的內部狀態，包括傳感器類型、位置偏移、方向、縮放比例、延遲時間、噪聲等信息，以及一個后端驅動指針 _driver。

總體來說，這個類設計用于處理和管理無人機上的視覺里程計傳感器，提供了豐富的接口和功能來初始化、操作和查詢傳感器狀態。

2.1 類與構造函數

class AP_VisualOdom
{
public:AP_VisualOdom();static AP_VisualOdom *get_singleton() {return _singleton;}

• AP_VisualOdom 類是一個包含各種方法和屬性的類。
• AP_VisualOdom 構造函數用于初始化對象。
• get_singleton 靜態方法返回一個單例模式的實例。

2.2 枚舉類型

enum class VisualOdom_Type {None = 0,
#if AP_VISUALODOM_MAV_ENABLEDMAV = 1,
#endif
#if AP_VISUALODOM_INTELT265_ENABLEDIntelT265 = 2,VOXL = 3,
#endif
};

• VisualOdom_Type 枚舉定義了幾種視覺里程計類型，如 None, MAV, IntelT265 和 VOXL。

2.3 公共方法

void init();
bool enabled() const;
bool healthy() const;
enum Rotation get_orientation() const;
float get_pos_scale() const;
const Vector3f &get_pos_offset(void) const;
uint16_t get_delay_ms() const;
float get_vel_noise() const;
float get_pos_noise() const;
float get_yaw_noise() const;
int8_t get_quality_min() const;
int8_t quality() const;

這些方法提供了對視覺里程計傳感器的各種操作和查詢功能：

• init：檢測并初始化傳感器。
• enabled：返回傳感器是否啟用。
• healthy：返回傳感器是否處于健康狀態（是否接收到數據）。
• get_orientation：獲取用戶定義的傳感器方向。
• get_pos_scale：獲取應用于位置估計的縮放比例。
• get_pos_offset：獲取相機相對于機體坐標系原點的位置偏移。
• get_delay_ms：獲取傳感器的延遲時間（毫秒）。
• get_vel_noise：獲取速度測量噪聲。
• get_pos_noise：獲取位置測量噪聲。
• get_yaw_noise：獲取航向測量噪聲。
• get_quality_min：獲取質量閾值。
• quality：返回質量測量值。

2.4 消息處理

#if HAL_GCS_ENABLED
void handle_vision_position_delta_msg(const mavlink_message_t &msg);
#endif
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
void handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality);
void handle_vision_speed_estimate(uint64_t remote_time_us, uint32_t time_ms, const Vector3f &vel, uint8_t reset_counter, int8_t quality);

這些方法用于處理視覺里程計傳感器的消息：

• handle_vision_position_delta_msg：處理視覺位置增量消息（僅在 HAL_GCS_ENABLED 時可用）。
• handle_pose_estimate：處理姿態估計數據。
• handle_vision_speed_estimate：處理速度估計數據。

2.5 其他功能

void request_align_yaw_to_ahrs();
void align_position_to_ahrs(bool align_xy, bool align_z);
bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
VisualOdom_Type get_type(void) const {return _type;
}

• request_align_yaw_to_ahrs：請求將傳感器的航向與車輛的姿態參考系統（AHRS/EKF）對齊。
• align_position_to_ahrs：更新位置偏移以對齊AHRS位置。
• pre_arm_check：在啟動檢查時返回 false 表示檢查失敗，并提供失敗消息。
• get_type：獲取傳感器類型。

2.6 私有成員

private:static AP_VisualOdom *_singleton;AP_Enum<VisualOdom_Type> _type;AP_Vector3f _pos_offset;AP_Int8 _orientation;AP_Float _pos_scale;AP_Int16 _delay_ms;AP_Float _vel_noise;AP_Float _pos_noise;AP_Float _yaw_noise;AP_Int8 _quality_min;AP_VisualOdom_Backend *_driver;
};

3. 框架設計

3.1 啟動代碼 AP_VisualOdom::init

傳統Ardupilot啟動方式。

AP_Vehicle::setup└──> visual_odom.init

AP_VisualOdom::init()
└── switch (VisualOdom_Type(_type.get()))├── case VisualOdom_Type::None:│   └── // do nothing├── #if AP_VISUALODOM_MAV_ENABLED│   ├── case VisualOdom_Type::MAV:│   │   └── _driver = new AP_VisualOdom_MAV(*this);│   └── #endif├── #if AP_VISUALODOM_INTELT265_ENABLED│   ├── case VisualOdom_Type::IntelT265:│   ├── case VisualOdom_Type::VOXL:│   │   └── _driver = new AP_VisualOdom_IntelT265(*this);│   └── #endif└── default:└── // handle default case if necessary

根據不同的傳感器類型選擇性地實例化不同的后端驅動對象，從而初始化并準備使用視覺里程計系統。

• VisualOdom_Type::None：如果 _type 是 None，則什么也不做。
• VisualOdom_Type::MAV：如果 _type 是 MAV，則創建一個 AP_VisualOdom_MAV 的實例并將其賦給 _driver。
• VisualOdom_Type::IntelT265 或 VisualOdom_Type::VOXL：如果 _type 是 IntelT265 或 VOXL，則創建一個 AP_VisualOdom_IntelT265 的實例并將其賦給 _driver。
• 其他情況：可能需要處理默認情況，具體實現取決于代碼中是否有定義。

3.2 消息處理

3.2.1 AP_VisualOdom::handle_vision_position_delta_msg

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_POSITION_DELTA└──> GCS_MAVLINK::handle_vision_position_delta└──> AP_VisualOdom::handle_vision_position_delta_msg

handle_vision_position_delta_msg(msg)
├── if HAL_GCS_ENABLED
│   ├── if !enabled()
│   │   └── return
│   └── if _driver != nullptr
│       └── _driver->handle_vision_position_delta_msg(msg)
└── (end of method)

• 如果 HAL_GCS_ENABLED 沒有定義，整個方法將被忽略，不會編譯和執行。
• 如果 enabled() 方法返回 false，則立即退出方法，不執行后續的邏輯。
• 如果 _driver 不為 nullptr，則調用 _driver 對象的 handle_vision_position_delta_msg 方法，將 msg 作為參數傳遞給它。

這樣的設計確保了在滿足條件且傳感器已啟用時，有效地將接收到的視覺位置增量消息傳遞給相應的后端處理。

3.2.2 AP_VisualOdom::handle_pose_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message //MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE/MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE└──> GCS_MAVLINK::handle_vision_position_estimate/handle_global_vision_position_estimate/handle_vicon_position_estimate└──> GCS_MAVLINK::handle_common_vision_position_estimate_data└──> AP_VisualOdom::handle_pose_estimate

handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
│
├── if (!enabled())
│   └── return;
│
└── if (_driver != nullptr)│├── Quaternion attitude│   └── attitude.from_euler(roll, pitch, yaw)│└── _driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality)

這段代碼是用于處理姿態估計數據并將其發送給擴展卡爾曼濾波器（EKF）的方法。
目的是將來自視覺里程計傳感器的姿態估計數據傳送給后端處理，以便進一步的狀態估計和導航。

1. 參數：

   • remote_time_us：遠程時間戳（微秒）。
   • time_ms：本地時間戳（毫秒）。
   • x, y, z：位置坐標（米）。
   • roll, pitch, yaw：姿態角（弧度）。
   • posErr：位置誤差。
   • angErr：角度誤差。
   • reset_counter：重置計數器。
   • quality：質量評估（-1 表示失敗，0 表示未知，1 最差，100 最佳）。

2. 功能：

   • enabled() 方法檢查傳感器是否啟用，如果未啟用則立即返回。
   • 如果 _driver 驅動器不為空，創建一個 Quaternion 對象 attitude，通過 from_euler(roll, pitch, yaw) 方法將歐拉角轉換為四元數。
   • 調用 _driver->handle_pose_estimate() 方法，將處理過的姿態估計數據和質量評估傳遞給后端處理。

3. API：

   • uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t qualit
   • uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality

// general purpose method to consume position estimate data and send to EKF
// distances in meters, roll, pitch and yaw are in radians
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, float roll, float pitch, float yaw, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
{// exit immediately if not enabledif (!enabled()) {return;}// call backendif (_driver != nullptr) {// convert attitude to quaternion and call backendQuaternion attitude;attitude.from_euler(roll, pitch, yaw);_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);}
}// general purpose method to consume position estimate data and send to EKF
// quality of -1 means failed, 0 means unknown, 1 is worst, 100 is best
void AP_VisualOdom::handle_pose_estimate(uint64_t remote_time_us, uint32_t time_ms, float x, float y, float z, const Quaternion &attitude, float posErr, float angErr, uint8_t reset_counter, int8_t quality)
{// exit immediately if not enabledif (!enabled()) {return;}// call backendif (_driver != nullptr) {_driver->handle_pose_estimate(remote_time_us, time_ms, x, y, z, attitude, posErr, angErr, reset_counter, quality);}
}

3.2.3 AP_VisualOdom::handle_vision_speed_estimate

SCHED_TASK_CLASS(GCS,                  (GCS*)&copter._gcs,          update_receive, 400, 180, 102),└──> GCS::update_receive└──> GCS_MAVLINK::update_receive└──> GCS_MAVLINK::packetReceived└──> GCS_MAVLINK::handle_message  //MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE/MAVLINK_MSG_ID_ODOMETRY└──> GCS_MAVLINK::handle_vision_speed_estimate/handle_odometry└──> AP_VisualOdom::handle_vision_speed_estimate

handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)
└── if not enabled()└── return
└── if _driver != nullptr└── _driver->handle_vision_speed_estimate(remote_time_us, time_ms, vel, reset_counter, quality)

用于處理視覺里程計傳感器的速度估計數據，并將其發送到擴展卡爾曼濾波器（EKF）的通用方法。

1. 函數簽名和說明：

   • 函數名：handle_vision_speed_estimate
   • 參數：
     • remote_time_us：遠程時間戳（微秒）
     • time_ms：本地時間戳（毫秒）
     • vel：速度估計的三維向量（NED坐標系，單位為米每秒）
     • reset_counter：重置計數器
     • quality：質量評估，-1 表示失敗，0 表示未知，1 表示最差，100 表示最佳。

2. 功能說明：

   • 首先，檢查視覺里程計是否啟用（調用了類的 enabled() 方法）。如果未啟用，則立即返回，不執行后續操作。
   • 然后，檢查 _driver 指針是否為 nullptr。如果 _driver 不為空，則調用 _driver 對象的 handle_vision_speed_estimate 方法，將速度估計數據傳遞給后端處理。

3. 執行流程：

   • 如果視覺里程計未啟用，函數直接返回，不執行后續任何操作。
   • 如果視覺里程計已啟用且 _driver 指針有效，則通過 _driver 對象處理速度估計數據。

3.3 輔助函數

3.3.1 AP_VisualOdom::enabled

通過判斷類型設置，確認視覺傳感是否被使能。

// return true if sensor is enabled
bool AP_VisualOdom::enabled() const
{return ((_type != VisualOdom_Type::None));
}

3.3.2 AP_VisualOdom::healthy

首先檢查傳感器是否已啟用，然后再檢查是否有有效的驅動程序實例來處理傳感器數據。
只有在這兩個條件都滿足時，才會進一步檢查傳感器的健康狀態并返回結果。

healthy()
├── if (!enabled())
│   └── return false
├── if (_driver == nullptr)
│   └── return false
└── return _driver->healthy()

3.3.3 AP_VisualOdom::quality

返回質量測量值（百分比）。

• 0：Unknown
• 1：Worst
• 100：Best

// return quality as a measure from 0 ~ 100
// -1 means failed, 0 means unknown, 1 is worst, 100 is best
int8_t AP_VisualOdom::quality() const
{if (_driver == nullptr) {return 0;}return _driver->quality();
}

3.4 參數接口

3.4.1 AP_VisualOdom::get_orientation

獲取用戶定義的傳感器方向。

    // get user defined orientationenum Rotation get_orientation() const { return (enum Rotation)_orientation.get(); }

3.4.2 AP_VisualOdom::get_pos_scale

獲取應用于位置估計的縮放比例。

    // get user defined scaling applied to position estimatesfloat get_pos_scale() const { return _pos_scale; }

3.4.3 AP_VisualOdom::get_pos_offset

獲取相機相對于機體坐標系原點的位置偏移。

    // return a 3D vector defining the position offset of the camera in meters relative to the body frame originconst Vector3f &get_pos_offset(void) const { return _pos_offset; }

3.4.4 AP_VisualOdom::get_vel_noise

獲取速度測量噪聲。

    // return velocity measurement noise in m/sfloat get_vel_noise() const { return _vel_noise; }

3.4.5 AP_VisualOdom::get_pos_noise

獲取位置測量噪聲。

    // return position measurement noise in mfloat get_pos_noise() const { return _pos_noise; }

3.4.6 AP_VisualOdom::get_yaw_noise

獲取航向測量噪聲。

    // return yaw measurement noise in radfloat get_yaw_noise() const { return _yaw_noise; }

3.4.7 AP_VisualOdom::get_delay_ms

獲取傳感器的延遲時間（毫秒）。

    // return the sensor delay in milliseconds (see _DELAY_MS parameter)uint16_t get_delay_ms() const { return MAX(0, _delay_ms); }

3.4.8 AP_VisualOdom::get_quality_min

后端驅動獲取配置最小質量閾值。

    // return quality thresholdint8_t get_quality_min() const { return _quality_min; }

4. 總結

AP_VisualOdom視覺定位傳感器應用遵循《ArduPilot之開源代碼Sensor Drivers設計》。

• ront-end / back-end分層
• 消息機制驅動
• 評估視覺位置、姿態、速度

后續我們繼續研討三個后端程序：

• AP_VisualOdom_Backend
• AP_VisualOdom_IntelT265
• AP_VisualOdom_MAV

5. 參考資料

【1】ArduPilot開源飛控系統之簡單介紹
【2】ArduPilot之開源代碼Task介紹
【3】ArduPilot飛控啟動&運行過程簡介
【4】ArduPilot之開源代碼Library&Sketches設計
【5】ArduPilot之開源代碼Sensor Drivers設計