class AdaptiveReprojectionError {
public:
    AdaptiveReprojectionError(const Eigen::Vector2d& observed_p, 
                             double point_quality,
                             double huber_threshold)
        : observed_p_(observed_p), 
          point_quality_(point_quality), 
          huber_threshold_(huber_threshold) {}
    
    template 
    bool operator()(const T* const position,
                   const T* const quaternion,
                   const T* const point,
                   T* residuals) const {
        // 坐标变换
        T p[3];
        ceres::QuaternionRotatePoint(quaternion, point, p);
        p[0] += position[0];
        p[1] += position[1];
        p[2] += position[2];
        
        if (p[2] <= T(0.1)) return false;
        
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];
        
        T error_x = xp - T(observed_p_[0]);
        T error_y = yp - T(observed_p_[1]);
        
        // 点质量加权
        T weight = T(point_quality_);
        
        // 自适应Huber核
        T squared_error = error_x * error_x + error_y * error_y;
        T huber_th = T(huber_threshold_);
        
        if (squared_error > huber_th * huber_th) {
            T norm = ceres::sqrt(squared_error);
            residuals[0] = weight * huber_th * (error_x / norm);
            residuals[1] = weight * huber_th * (error_y / norm);
        } else {
            residuals[0] = weight * error_x;
            residuals[1] = weight * error_y;
        }
        
        return true;
    }
};

class InformationWeightedPoseGraphError {
public:
    InformationWeightedPoseGraphError(
        const Eigen::Vector3d& t_ij,
        const Eigen::Quaterniond& q_ij,
        const Eigen::Matrix& info,
        double base_weight)
        : t_ij_(t_ij), q_ij_(q_ij), base_weight_(base_weight) {
        Eigen::LLT> llt(info);
        sqrt_info_ = llt.matrixL().transpose();
    }
    
    template 
    bool operator()(const T* const pos_i,
                   const T* const quat_i,
                   const T* const pos_j,
                   const T* const quat_j,
                   T* residuals) const {
        // 计算相对旋转和平移误差
        // 应用信息矩阵加权
        T weight = T(base_weight_);
        for (int i = 0; i < 6; ++i) {
            residuals[i] = T(0);
            for (int j = 0; j < 6; ++j) {
                residuals[i] += weight * T(sqrt_info_(i, j)) * combined[j];
            }
        }
        return true;
    }
};

SLAMOptimizer::SLAMOptimizer() {
    options_.linear_solver_type = ceres::SPARSE_SCHUR;
    options_.trust_region_strategy_type = ceres::LEVENBERG_MARQUARDT;
    options_.max_num_iterations = 200;
    options_.function_tolerance = 1e-4;
    options_.gradient_tolerance = 1e-6;
    options_.parameter_tolerance = 1e-4;
    options_.num_threads = std::thread::hardware_concurrency();
    options_.use_nonmonotonic_steps = true;
    
    // 梯度检查
    options_.check_gradients = true;
    options_.gradient_check_relative_precision = 1e-4;
}

bool SLAMOptimizer::checkConvergence(const std::vector& cost_history) {
    if (cost_history.size() < 3) return false;
    
    double recent_improvement = (cost_history[cost_history.size()-3] - 
                                 cost_history.back()) / cost_history.back();
    
    if (std::abs(recent_improvement) < config_.convergence_rate_threshold * 10) {
        convergence_streak_++;
    } else {
        convergence_streak_ = 0;
    }
    
    return convergence_streak_ >= config_.convergence_patience;
}

cmake_minimum_required(VERSION 3.10)
project(ceres_slam_v4)

set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

find_package(Ceres REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
find_package(Threads REQUIRED)

include_directories(
    ${CMAKE_CURRENT_SOURCE_DIR}/include
    ${EIGEN3_INCLUDE_DIRS}
    ${CERES_INCLUDE_DIRS}
    ${OpenCV_INCLUDE_DIRS}
)

set(SOURCES
    src/main.cpp
    src/residuals.cpp
    src/optimizer.cpp
    src/utils.cpp
)

add_executable(ceres_slam_complete ${SOURCES})

target_link_libraries(ceres_slam_complete
    ${CERES_LIBRARIES}
    ${OpenCV_LIBS}
    Threads::Threads
)

if(CMAKE_BUILD_TYPE STREQUAL "Release")
    target_compile_options(ceres_slam_complete PRIVATE -O3 -march=native)
endif()

mkdir build && cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j$(nproc)
./ceres_slam_complete --keyframes 30 --points 150

// 自动检测CPU核心数
options_.num_threads = std::thread::hardware_concurrency();

// 或手动指定
options_.num_threads = 8;  // 根据实际CPU核心数调整