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

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;
}
相关推荐
yi.Ist22 分钟前
数据结构 —— 键值对 map
数据结构·算法
s1533525 分钟前
数据结构-顺序表-猜数字
数据结构·算法·leetcode
Coding小公仔28 分钟前
LeetCode 8. 字符串转换整数 (atoi)
算法·leetcode·职场和发展
GEEK零零七34 分钟前
Leetcode 393. UTF-8 编码验证
算法·leetcode·职场和发展·二进制运算
DoraBigHead2 小时前
小哆啦解题记——异位词界的社交网络
算法
木头左3 小时前
逻辑回归的Python实现与优化
python·算法·逻辑回归
lifallen7 小时前
Paimon LSM Tree Compaction 策略
java·大数据·数据结构·数据库·算法·lsm-tree
web_Hsir9 小时前
vue3.2 前端动态分页算法
前端·算法
地平线开发者11 小时前
征程 6M 部署 Omnidet 感知模型
算法·自动驾驶
秋说12 小时前
【PTA数据结构 | C语言版】线性表循环右移
c语言·数据结构·算法