利用平面进行位姿约束优化

cpp 复制代码
/**
 * 设位姿pose存在误差,利用观测到的平面进行位姿pose优化,只优化位姿pose的z轴平移和roll,pitch,yaw四个变量,要求优化后的pose,z轴与平面的z一致。请修改代码实现这个功能
 * 
 * 
 */
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/geometry/OrientedPlane3.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <boost/optional.hpp>
#include <iostream>

using namespace gtsam;

// 自定义因子类
class CustomOrientedPlane3Factor : public NoiseModelFactor1<Pose3> {
    OrientedPlane3 measured_;

public:
    CustomOrientedPlane3Factor(const OrientedPlane3& measured, const SharedNoiseModel& model, Key poseKey)
        : NoiseModelFactor1<Pose3>(model, poseKey), measured_(measured) {}

    // 误差评估函数
    Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = boost::none) const override {
        OrientedPlane3 transformedPlane = measured_.transform(pose, H); // 变换平面
        
        // 单独定义误差据向量
        double zError = pose.translation().z() - transformedPlane.planeCoefficients()(3);
        Vector3 normalError = transformedPlane.planeCoefficients().head<3>() - measured_.planeCoefficients().head<3>();

        Vector error(4); // 定义为4维向量
        error << normalError, zError; // 确保合并的正确顺序

        if (H) {
            *H = Matrix::Zero(4, 6); // 初始化 4x6 矩阵
            (*H)(3, 5) = 1.0; // 对z方向的偏导数
            // 这里需要自行计算剩余的angular部分根据误差函数的特定定义


        }

        return error; // 返回误差
    }
};

int main() {
    NonlinearFactorGraph graph;

    // 初始位姿,只允许z的平移以及旋转freedom
    Pose3 initialPose(Rot3::Ypr(0.1, 0.1, 0.1), Point3(10.5, 20.4, 1.2));
    Vector6 poseConstraint;
    poseConstraint << 0.0, 0.0, 1.0, 1.0, 1.0, 1.0; // 仅允许调整z, roll, pitch, yaw
    auto poseNoise = noiseModel::Diagonal::Sigmas(poseConstraint * 0.1);
    graph.add(PriorFactor<Pose3>(Symbol('x', 0), initialPose, poseNoise));
    // 输出初始位姿
    std::cout << "Initial Pose:\n" << initialPose << std::endl;

    // 观测到的平面
    // OrientedPlane3 observedPlane1(0, 0, 1, -1.0); // 假设z是我们的目标
    // 添加多个观测平面
    std::vector<OrientedPlane3> observedPlanes = {
        OrientedPlane3(0, 0, 1, -1.5),  // 平面 1
        OrientedPlane3(0, 0, 1, -2.0),  // 平面 2
        OrientedPlane3(0, 0, 1, -1.0),  // 平面 3
        OrientedPlane3(0, 0, 1, -0.5)   // 平面 4
        // 其他平面可根据需求添加
    };

    auto planeNoise = noiseModel::Isotropic::Sigma(4, 1e-3);

    // 添加因子
    // graph.add(boost::make_shared<CustomOrientedPlane3Factor>(observedPlane1, planeNoise, Symbol('x', 0)));
    // 为所有平面添加因子
    for (size_t i = 0; i < observedPlanes.size(); ++i) {
        graph.add(boost::make_shared<CustomOrientedPlane3Factor>(observedPlanes[i], planeNoise, Symbol('x', 0)));
    }

    Values initialEstimate;
    initialEstimate.insert(Symbol('x', 0), initialPose);

    GaussNewtonParams params;
    params.setMaxIterations(10);
    params.setRelativeErrorTol(1e-5);
    GaussNewtonOptimizer optimizer(graph, initialEstimate, params);
    Values result = optimizer.optimize();

    // 输出优化结果
    std::cout << "Optimized Pose:\n" << result.at<Pose3>(Symbol('x', 0)) << std::endl;

    return 0;
}
相关推荐
Pniubi14 小时前
力扣21合并两个有序链表
算法·leetcode·职场和发展
AIgorithmGEEK15 小时前
std::unordered_map 与 std::unordered_set:接口、算法技巧与权衡 (速查)
c++·算法·哈希表·负载因子·桶·unordered_map·unordered_set
上海潭龙东海科技有限公司16 小时前
智慧堆场动态模型三维实时孪生重构智能管控
大数据·网络·数据库·算法·重构
阿庆_AI研发工程师18 小时前
Stanford CS336 Lecture 10:大语言模型推理与性能优化
算法
用户73416810354818 小时前
基于MindSpore的深度残差收缩网络程序
算法
Pniubi19 小时前
力扣239.滑动窗口最大值(优先队列)
算法·leetcode·职场和发展
青少儿编程课堂20 小时前
第十八届蓝桥杯青少组图形化真题 + 全国青少年信息素养大赛 + 电子学会图形化考级:校园泡泡堂大挑战解析
c++·python·算法·bfs·信息学竞赛
超低空21 小时前
一文说清LRU算法
后端·算法·架构
青少儿编程课堂21 小时前
蓝桥杯·全国青少年信息素养大赛·电子学会Python/C++考级:Dinic最大流解析
c++·python·算法·bfs·信息学竞赛
开开心心就好1 天前
显示器关不掉怎么办,用小工具一键息屏
算法·智能手机·pdf·计算机外设·ocr·lua·1024程序员节