slam_system.mbt

// SLAM 同时定位与地图构建（v31.0.0）
// 特征点跟踪、位姿估计、地图构建、回环检测、优化

///|
/// SLAM 算法类型
pub(all) enum SLAMAlgorithm {
  MonoSLAM
  ORBSLAM
  LSD_SLAM
  SVO
  DSO
  PTAM
  KinectFusion
  ElasticFusion
  Unknown
} derive(Eq)

///|
/// 相机位姿
pub(all) struct CameraPose {
  position : Array[Double]
  rotation : Array[Double]
  timestamp : Double
  confidence : Double
}

///|
/// 3D 地图点
pub(all) struct MapPoint {
  position : Array[Double]
  color : Array[Int]
  normal : Array[Double]
  descriptor : Array[Double]
  num_observations : Int
  is_outlier : Bool
}

///|
/// 关键帧
pub(all) struct KeyFrame {
  pose : CameraPose
  image : @types.Image
  features : Array[Array[Double]]
  descriptors : Array[Array[Double]]
  map_points : Array[Int]
  timestamp : Double
  is_loop_closure : Bool
}

///|
/// SLAM 状态
pub(all) struct SLAMState {
  current_pose : CameraPose
  trajectory : Array[CameraPose]
  map_points : Array[MapPoint]
  keyframes : Array[KeyFrame]
  num_frames_processed : Int
  num_lost_frames : Int
  is_tracking : Bool
  is_mapping : Bool
  processing_time_ms : Double
}

///|
/// SLAM 配置
pub(all) struct SLAMConfig {
  algorithm : SLAMAlgorithm
  feature_threshold : Double
  max_features : Int
  keyframe_interval : Int
  min_parallax : Double
  use_loop_closure : Bool
  use_bundle_adjustment : Bool
  camera_fx : Double
  camera_fy : Double
  camera_cx : Double
  camera_cy : Double
}

///|
/// 特征跟踪结果
pub(all) struct FeatureTrackingResult {
  tracked_features : Array[Array[Double]]
  num_tracked : Int
  num_lost : Int
  parallax : Double
  is_keyframe : Bool
}

///|
/// 回环检测结果
pub(all) struct LoopClosureResult {
  is_loop_detected : Bool
  loop_frame_index : Int
  similarity_score : Double
  transformation : Array[Double]
  num_inliers : Int
}

///|
/// 创建默认 SLAM 配置
pub fn create_default_slam_config() -> SLAMConfig {
  SLAMConfig::{
    algorithm: SLAMAlgorithm::ORBSLAM,
    feature_threshold: 20.0,
    max_features: 1000,
    keyframe_interval: 5,
    min_parallax: 10.0,
    use_loop_closure: true,
    use_bundle_adjustment: true,
    camera_fx: 500.0,
    camera_fy: 500.0,
    camera_cx: 320.0,
    camera_cy: 240.0,
  }
}

///|
/// 检测特征点（简化版：Harris 角点）
pub fn detect_features_slam(
  img : @types.Image,
  max_features : Int,
  threshold : Double,
) -> Array[Array[Double]] {
  let w = img.width
  let h = img.height
  let features : Array[Array[Double]] = []

  for y in 1..<(h - 1) {
    for x in 1..<(w - 1) {
      let idx = y * w + x
      let gx = img.data[idx + 1].to_double() - img.data[idx - 1].to_double()
      let gy = img.data[idx + w].to_double() - img.data[idx - w].to_double()
      let response = gx * gx + gy * gy

      if response > threshold && features.length() < max_features {
        features.push([x.to_double(), y.to_double(), response])
      }
    }
  }

  features
}

///|
/// 特征点跟踪（简化版：基于光流的块匹配）
pub fn track_features_slam(
  prev_img : @types.Image,
  curr_img : @types.Image,
  prev_features : Array[Array[Double]],
  search_radius : Int,
) -> FeatureTrackingResult {
  let w = prev_img.width
  let h = prev_img.height
  let tracked : Array[Array[Double]] = []
  let mut num_lost = 0
  let mut total_parallax = 0.0

  for feat in prev_features {
    let px = feat[0].to_int()
    let py = feat[1].to_int()
    let mut best_x = px
    let mut best_y = py
    let mut best_score = 1000000.0

    for dy in -search_radius..<(search_radius + 1) {
      for dx in -search_radius..<(search_radius + 1) {
        let nx = px + dx
        let ny = py + dy
        if nx >= 2 && nx < w - 2 && ny >= 2 && ny < h - 2 {
          // 计算块匹配误差（简化版：中心像素差）
          let prev_pixel = prev_img.data[py * w + px].to_double()
          let curr_pixel = curr_img.data[ny * w + nx].to_double()
          let diff = (prev_pixel - curr_pixel).abs()
          if diff < best_score {
            best_score = diff
            best_x = nx
            best_y = ny
          }
        }
      }
    }

    if best_score < 50.0 {
      let dx = (best_x - px).to_double()
      let dy = (best_y - py).to_double()
      let parallax = (dx * dx + dy * dy).sqrt()
      total_parallax += parallax
      tracked.push([best_x.to_double(), best_y.to_double(), parallax])
    } else {
      num_lost += 1
    }
  }

  let avg_parallax = if tracked.length() > 0 {
    total_parallax / tracked.length().to_double()
  } else {
    0.0
  }

  FeatureTrackingResult::{
    tracked_features: tracked,
    num_tracked: tracked.length(),
    num_lost,
    parallax: avg_parallax,
    is_keyframe: avg_parallax > 10.0 && tracked.length() > 50,
  }
}

///|
/// 估计相机位姿（简化版：基于特征点视差）
pub fn estimate_camera_pose(
  tracking_result : FeatureTrackingResult,
  config : SLAMConfig,
  prev_pose : CameraPose,
) -> CameraPose {
  ignore(config)
  // 简化的位姿估计：基于平均视差
  let avg_parallax = tracking_result.parallax
  let forward_motion = avg_parallax * 0.01
  let dz = forward_motion

  CameraPose::{
    position: [
      prev_pose.position[0],
      prev_pose.position[1],
      prev_pose.position[2] + dz,
    ],
    rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
    timestamp: prev_pose.timestamp + 0.033,
    confidence: tracking_result.num_tracked.to_double() /
    (tracking_result.num_tracked + tracking_result.num_lost).to_double(),
  }
}

///|
/// 三角化 3D 点（简化版）
pub fn triangulate_points(
  features : Array[Array[Double]],
  pose1 : CameraPose,
  pose2 : CameraPose,
  config : SLAMConfig,
) -> Array[MapPoint] {
  let points : Array[MapPoint] = []

  for feat in features {
    let x = feat[0]
    let y = feat[1]

    // 简化的三角化：基于视差估计深度
    let disparity = (pose2.position[0] - pose1.position[0]).abs() + 0.001
    let depth = config.camera_fx * 0.1 / disparity

    // 反投影到 3D
    let x3d = (x - config.camera_cx) * depth / config.camera_fx
    let y3d = (y - config.camera_cy) * depth / config.camera_fy
    let z3d = depth

    points.push(MapPoint::{
      position: [x3d, y3d, z3d],
      color: [128, 128, 128],
      normal: [0.0, 0.0, 1.0],
      descriptor: [],
      num_observations: 2,
      is_outlier: false,
    })
  }

  points
}

///|
/// 回环检测（简化版：基于特征点数量相似度）
pub fn detect_loop_closure(
  current_features : Array[Array[Double]],
  keyframes : Array[KeyFrame],
  threshold : Double,
) -> LoopClosureResult {
  let mut best_score = 0.0
  let mut best_index = -1

  for i in 0..<keyframes.length() {
    let kf = keyframes[i]
    // 简化的相似度：特征点数量比较
    let score = if kf.features.length() > 0 {
      current_features.length().to_double() / kf.features.length().to_double()
    } else {
      0.0
    }

    if score > best_score && score > threshold {
      best_score = score
      best_index = i
    }
  }

  if best_index >= 0 {
    LoopClosureResult::{
      is_loop_detected: true,
      loop_frame_index: best_index,
      similarity_score: best_score,
      transformation: [
        1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
        1.0,
      ],
      num_inliers: 50,
    }
  } else {
    LoopClosureResult::{
      is_loop_detected: false,
      loop_frame_index: -1,
      similarity_score: 0.0,
      transformation: [
        1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,
        1.0,
      ],
      num_inliers: 0,
    }
  }
}

///|
/// 初始化 SLAM 系统
pub fn initialize_slam(config : SLAMConfig) -> SLAMState {
  ignore(config)
  let initial_pose = CameraPose::{
    position: [0.0, 0.0, 0.0],
    rotation: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0],
    timestamp: 0.0,
    confidence: 1.0,
  }

  SLAMState::{
    current_pose: initial_pose,
    trajectory: [initial_pose],
    map_points: [],
    keyframes: [],
    num_frames_processed: 0,
    num_lost_frames: 0,
    is_tracking: true,
    is_mapping: true,
    processing_time_ms: 0.0,
  }
}

///|
/// 处理单帧图像
pub fn process_frame_slam(
  state : SLAMState,
  img : @types.Image,
  config : SLAMConfig,
) -> SLAMState {
  // 检测特征点
  let features = detect_features_slam(
    img,
    config.max_features,
    config.feature_threshold,
  )

  // 如果有关键帧，进行特征跟踪
  if state.keyframes.length() > 0 {
    let last_kf = state.keyframes[state.keyframes.length() - 1]
    let tracking_result = track_features_slam(
      last_kf.image,
      img,
      last_kf.features,
      16,
    )

    // 估计位姿
    let new_pose = estimate_camera_pose(
      tracking_result,
      config,
      state.current_pose,
    )

    // 如果是关键帧，添加到地图
    if tracking_result.is_keyframe {
      let new_kf = KeyFrame::{
        pose: new_pose,
        image: img,
        features,
        descriptors: [],
        map_points: [],
        timestamp: new_pose.timestamp,
        is_loop_closure: false,
      }

      // 三角化新的地图点
      let new_points = triangulate_points(
        features,
        last_kf.pose,
        new_pose,
        config,
      )

      let new_map_points = state.map_points
      for p in new_points {
        new_map_points.push(p)
      }

      let new_keyframes = state.keyframes
      new_keyframes.push(new_kf)

      let new_trajectory = state.trajectory
      new_trajectory.push(new_pose)

      SLAMState::{
        current_pose: new_pose,
        trajectory: new_trajectory,
        map_points: new_map_points,
        keyframes: new_keyframes,
        num_frames_processed: state.num_frames_processed + 1,
        num_lost_frames: state.num_lost_frames,
        is_tracking: tracking_result.num_tracked > 20,
        is_mapping: true,
        processing_time_ms: 0.0,
      }
    } else {
      let new_trajectory = state.trajectory
      new_trajectory.push(new_pose)

      SLAMState::{
        current_pose: new_pose,
        trajectory: new_trajectory,
        map_points: state.map_points,
        keyframes: state.keyframes,
        num_frames_processed: state.num_frames_processed + 1,
        num_lost_frames: if tracking_result.num_tracked < 20 {
          state.num_lost_frames + 1
        } else {
          state.num_lost_frames
        },
        is_tracking: tracking_result.num_tracked > 20,
        is_mapping: true,
        processing_time_ms: 0.0,
      }
    }
  } else {
    // 第一帧：初始化为关键帧
    let initial_kf = KeyFrame::{
      pose: state.current_pose,
      image: img,
      features,
      descriptors: [],
      map_points: [],
      timestamp: 0.0,
      is_loop_closure: false,
    }

    SLAMState::{
      current_pose: state.current_pose,
      trajectory: state.trajectory,
      map_points: state.map_points,
      keyframes: [initial_kf],
      num_frames_processed: state.num_frames_processed + 1,
      num_lost_frames: state.num_lost_frames,
      is_tracking: true,
      is_mapping: true,
      processing_time_ms: 0.0,
    }
  }
}

///|
/// 生成 SLAM 报告
pub fn slam_report_to_string(state : SLAMState) -> String {
  let mut report = "## SLAM 系统报告\n\n"
  report = report + "### 系统状态\n\n"
  report = report + "| 指标 | 值 |\n|------|------|\n"
  report = report +
    "| 处理帧数 | " +
    state.num_frames_processed.to_string() +
    " |\n"
  report = report +
    "| 丢失帧数 | " +
    state.num_lost_frames.to_string() +
    " |\n"
  report = report +
    "| 关键帧数 | " +
    state.keyframes.length().to_string() +
    " |\n"
  report = report +
    "| 地图点数 | " +
    state.map_points.length().to_string() +
    " |\n"
  report = report +
    "| 轨迹长度 | " +
    state.trajectory.length().to_string() +
    " |\n"
  report = report +
    "| 跟踪状态 | " +
    (if state.is_tracking { "跟踪中" } else { "丢失" }) +
    " |\n"
  report = report +
    "| 建图状态 | " +
    (if state.is_mapping { "建图中" } else { "暂停" }) +
    " |\n\n"

  report = report + "### 当前位姿\n\n"
  report = report + "| 指标 | 值 |\n|------|------|\n"
  report = report +
    "| 位置 X | " +
    state.current_pose.position[0].to_string() +
    " |\n"
  report = report +
    "| 位置 Y | " +
    state.current_pose.position[1].to_string() +
    " |\n"
  report = report +
    "| 位置 Z | " +
    state.current_pose.position[2].to_string() +
    " |\n"
  report = report +
    "| 置信度 | " +
    (state.current_pose.confidence * 100.0).to_string() +
    "% |\n"
  report = report +
    "| 时间戳 | " +
    state.current_pose.timestamp.to_string() +
    "s |\n"

  report
}
src\process\slam\slam_system.mbt

82.29%
