ceres::Manifold定义pose流形参数

已于 2024-05-16 19:15:10 修改
阅读量282 收藏 5
点赞数 1
文章标签： 学习 线性代数 c++
于 2024-05-16 19:12:30 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_37423490/article/details/138972028
版权

在看lio-mapping源码时发现，其定义的pose参数根据ceres::LocalParameterization，新版本ceres-solver库编译报错，因此改写该类。
ceres位姿参数类型，包含位置（三维向量）跟姿态（四元数），ceres-2.1以前自定义参数需要集成ceres::LocalParameterization类并重载类中的纯虚函数，ceres-solver-2.2以后取消了ceres::LocalParameterization类，并用ceres::Manifold代替，两者的具体区别可参考链接，下面给出使用ceres::Manifold定义的流形位姿参数。

头文件：

//
// Created by liuhuan on 24-5-16.
//

#ifndef SRC_POSEMANIFOLD_H
#define SRC_POSEMANIFOLD_H

#include <ceres/ceres.h>
#include <ceres/manifold.h>
#include <Eigen/Core>

namespace lio {

class PoseManifold : public ceres::Manifold{
public:
    int AmbientSize() const override { return 7; }
    int TangentSize() const override { return 6; }

    bool Plus(const double* x,
              const double* delta,
              double* x_plus_delta) const override;
    bool PlusJacobian(const double* x, double* jacobian) const override;
    bool Minus(const double* y,
               const double* x,
               double* y_minus_x) const override;
    bool MinusJacobian(const double* x, double* jacobian) const override;
    };

} // lio

#endif //SRC_POSEMANIFOLD_H

源文件：

//
// Created by liuhuan on 24-5-16.
//

#include "PoseManifold.h"

namespace lio {

    namespace {

        struct CeresQuaternionOrder {
            static constexpr int kW = 0;
            static constexpr int kX = 1;
            static constexpr int kY = 2;
            static constexpr int kZ = 3;
        };

        struct EigenQuaternionOrder {
            static constexpr int kW = 3;
            static constexpr int kX = 0;
            static constexpr int kY = 1;
            static constexpr int kZ = 2;
        };

        template<typename Order>
        inline void QuaternionPlusImpl(const double *x,
                                       const double *delta,
                                       double *x_plus_delta) {
            // x_plus_delta = QuaternionProduct(q_delta, x), where q_delta is the
            // quaternion constructed from delta.
            const double norm_delta = std::hypot(delta[0], delta[1], delta[2]);

            if (std::fpclassify(norm_delta) == FP_ZERO) {
                // No change in rotation: return the quaternion as is.
                std::copy_n(x, 4, x_plus_delta);
                return;
            }

            const double sin_delta_by_delta = (std::sin(norm_delta) / norm_delta);
            double q_delta[4];
            q_delta[Order::kW] = std::cos(norm_delta);
            q_delta[Order::kX] = sin_delta_by_delta * delta[0];
            q_delta[Order::kY] = sin_delta_by_delta * delta[1];
            q_delta[Order::kZ] = sin_delta_by_delta * delta[2];

            x_plus_delta[Order::kW] =
                    q_delta[Order::kW] * x[Order::kW] - q_delta[Order::kX] * x[Order::kX] -
                    q_delta[Order::kY] * x[Order::kY] - q_delta[Order::kZ] * x[Order::kZ];
            x_plus_delta[Order::kX] =
                    q_delta[Order::kW] * x[Order::kX] + q_delta[Order::kX] * x[Order::kW] +
                    q_delta[Order::kY] * x[Order::kZ] - q_delta[Order::kZ] * x[Order::kY];
            x_plus_delta[Order::kY] =
                    q_delta[Order::kW] * x[Order::kY] - q_delta[Order::kX] * x[Order::kZ] +
                    q_delta[Order::kY] * x[Order::kW] + q_delta[Order::kZ] * x[Order::kX];
            x_plus_delta[Order::kZ] =
                    q_delta[Order::kW] * x[Order::kZ] + q_delta[Order::kX] * x[Order::kY] -
                    q_delta[Order::kY] * x[Order::kX] + q_delta[Order::kZ] * x[Order::kW];
        }

        template<typename Order>
        inline void QuaternionPlusJacobianImpl(const double *x, double *jacobian_ptr) {
            Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian(
                    jacobian_ptr);
            jacobian.setZero();

            jacobian(Order::kW+3, 0+3) = -x[Order::kX];
            jacobian(Order::kW+3, 1+3) = -x[Order::kY];
            jacobian(Order::kW+3, 2+3) = -x[Order::kZ];
            jacobian(Order::kX+3, 0+3) = x[Order::kW];
            jacobian(Order::kX+3, 1+3) = x[Order::kZ];
            jacobian(Order::kX+3, 2+3) = -x[Order::kY];
            jacobian(Order::kY+3, 0+3) = -x[Order::kZ];
            jacobian(Order::kY+3, 1+3) = x[Order::kW];
            jacobian(Order::kY+3, 2+3) = x[Order::kX];
            jacobian(Order::kZ+3, 0+3) = x[Order::kY];
            jacobian(Order::kZ+3, 1+3) = -x[Order::kX];
            jacobian(Order::kZ+3, 2+3) = x[Order::kW];
        }

        template<typename Order>
        inline void QuaternionMinusImpl(const double *y,
                                        const double *x,
                                        double *y_minus_x) {
            // ambient_y_minus_x = QuaternionProduct(y, -x) where -x is the conjugate of
            // x.
            double ambient_y_minus_x[4];
            ambient_y_minus_x[Order::kW] =
                    y[Order::kW] * x[Order::kW] + y[Order::kX] * x[Order::kX] +
                    y[Order::kY] * x[Order::kY] + y[Order::kZ] * x[Order::kZ];
            ambient_y_minus_x[Order::kX] =
                    -y[Order::kW] * x[Order::kX] + y[Order::kX] * x[Order::kW] -
                    y[Order::kY] * x[Order::kZ] + y[Order::kZ] * x[Order::kY];
            ambient_y_minus_x[Order::kY] =
                    -y[Order::kW] * x[Order::kY] + y[Order::kX] * x[Order::kZ] +
                    y[Order::kY] * x[Order::kW] - y[Order::kZ] * x[Order::kX];
            ambient_y_minus_x[Order::kZ] =
                    -y[Order::kW] * x[Order::kZ] - y[Order::kX] * x[Order::kY] +
                    y[Order::kY] * x[Order::kX] + y[Order::kZ] * x[Order::kW];

            const double u_norm = std::hypot(ambient_y_minus_x[Order::kX],
                                             ambient_y_minus_x[Order::kY],
                                             ambient_y_minus_x[Order::kZ]);
            if (std::fpclassify(u_norm) != FP_ZERO) {
                const double theta = std::atan2(u_norm, ambient_y_minus_x[Order::kW]);
                y_minus_x[0] = theta * ambient_y_minus_x[Order::kX] / u_norm;
                y_minus_x[1] = theta * ambient_y_minus_x[Order::kY] / u_norm;
                y_minus_x[2] = theta * ambient_y_minus_x[Order::kZ] / u_norm;
            } else {
                std::fill_n(y_minus_x, 3, 0.0);
            }
        }

        template<typename Order>
        inline void QuaternionMinusJacobianImpl(const double *x, double *jacobian_ptr) {
            Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>> jacobian(
                    jacobian_ptr);

            jacobian.setZero();
            jacobian(0+3, Order::kW+3) = -x[Order::kX];
            jacobian(0+3, Order::kX+3) = x[Order::kW];
            jacobian(0+3, Order::kY+3) = -x[Order::kZ];
            jacobian(0+3, Order::kZ+3) = x[Order::kY];
            jacobian(1+3, Order::kW+3) = -x[Order::kY];
            jacobian(1+3, Order::kX+3) = x[Order::kZ];
            jacobian(1+3, Order::kY+3) = x[Order::kW];
            jacobian(1+3, Order::kZ+3) = -x[Order::kX];
            jacobian(2+3, Order::kW+3) = -x[Order::kZ];
            jacobian(2+3, Order::kX+3) = -x[Order::kY];
            jacobian(2+3, Order::kY+3) = x[Order::kX];
            jacobian(2+3, Order::kZ+3) = x[Order::kW];
        }

        
        using AmbientVector = Eigen::Matrix<double, 3, 1>;
        using MatrixJacobian = Eigen::Matrix<double, 3, 3, Eigen::RowMajor>;
        void PosPlus(const double *x_ptr,
                     const double *delta_ptr,
                     double *x_plus_delta_ptr) {
            Eigen::Map<const AmbientVector> x(x_ptr, 3);
            Eigen::Map<const AmbientVector> delta(delta_ptr, 3);
            Eigen::Map<AmbientVector> x_plus_delta(x_plus_delta_ptr, 3);
            x_plus_delta = x + delta;
        }

        void PosPlusJacobian(const double *x_ptr, double *jacobian_ptr) {
            Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, 3, 3);
            jacobian.setIdentity();
        }

        void PosRightMultiplyByPlusJacobian(const double *x,
                                            const int num_rows,
                                            const double *ambient_matrix,
                                            double *tangent_matrix) {
            std::copy_n(ambient_matrix, num_rows * 3, tangent_matrix);
        }

        void PosMinus(const double *y_ptr,
                      const double *x_ptr,
                      double *y_minus_x_ptr) {
            Eigen::Map<const AmbientVector> x(x_ptr, 3);
            Eigen::Map<const AmbientVector> y(y_ptr, 3);
            Eigen::Map<AmbientVector> y_minus_x(y_minus_x_ptr, 3);
            y_minus_x = y - x;
        }

        void PosMinusJacobian(const double *x_ptr, double *jacobian_ptr) {
            Eigen::Map<MatrixJacobian> jacobian(jacobian_ptr, 3, 3);
            jacobian.setIdentity();
        }

    }  // namespace



    bool PoseManifold::Plus(const double* x,
                                       const double* delta,
                                       double* x_plus_delta) const {
        const double p_x[3] = {x[0], x[1], x[2]};
        const double p_delta[3] = {delta[0], delta[1], delta[2]};
        const double q_x[4] = {x[3], x[4], x[5], x[6]};
        const double q_delta[3] = {delta[3], delta[4], delta[5]};
        double* p_x_plus_delta; double* q_x_plus_delta;
        QuaternionPlusImpl<EigenQuaternionOrder>(q_x, q_delta, q_x_plus_delta);
        PosPlus(p_x, p_delta, p_x_plus_delta);

        Eigen::Map<Eigen::VectorXd> e_x_plus_delta(x_plus_delta, 7);
        e_x_plus_delta.segment<3>(0) = Eigen::Map<Eigen::Vector3d>(p_x_plus_delta, 3);
        e_x_plus_delta.segment<4>(3) = Eigen::Map<Eigen::Vector4d>(q_x_plus_delta, 4);

        return true;
    }

    bool PoseManifold::PlusJacobian(const double* x, double* jacobian) const {
        const double q_x[4] = {x[3], x[4], x[5], x[6]};

        QuaternionPlusJacobianImpl<EigenQuaternionOrder>(q_x, jacobian);
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> jacobian_map(jacobian);
        jacobian_map(0,0) = 1;
        jacobian_map(1,1) = 1;
        jacobian_map(2,2) = 1;
        return true;
    }

    bool PoseManifold::Minus(const double* y, const double* x, double* y_minus_x) const {
        const double p_x[3] = {x[0], x[1], x[2]};
        const double p_y[3] = {y[0], y[1], y[2]};
        const double q_x[4] = {x[3], x[4], x[5], x[6]};
        const double q_y[4] = {y[3], y[4], y[5], y[6]};

        double* p_y_minus_x;
        double* q_y_minus_x;

        QuaternionMinusImpl<EigenQuaternionOrder>(q_y, q_x, q_y_minus_x);
        PosMinus(p_y, p_x, p_y_minus_x);

        Eigen::Map<Eigen::VectorXd> e_y_minus_x(y_minus_x, 6);
        e_y_minus_x.segment<3>(0) = Eigen::Map<Eigen::Vector3d>(p_y_minus_x, 3);
        e_y_minus_x.segment<3>(3) = Eigen::Map<Eigen::Vector3d>(q_y_minus_x, 3);
        return true;
    }

    bool PoseManifold::MinusJacobian(const double* x, double* jacobian) const {
        const double q_x[4] = {x[3], x[4], x[5], x[6]};
        QuaternionMinusJacobianImpl<EigenQuaternionOrder>(q_x, jacobian);

        Eigen::Map<Eigen::Matrix<double, 6, 7, Eigen::RowMajor>> jacobian_map(jacobian);
        jacobian_map(0,0) = 1;
        jacobian_map(1,1) = 1;
        jacobian_map(2,2) = 1;
        return true;
    }
} // lio
刘乐去哪儿了
关注
  • 1
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
ceres：可分配的时间序列数据库（未积极维护）
02-04
谷神星 谷神星没有积极维护。 Ceres是，是可供使用的时间序列存储选项之一。 Ceres提供了一种文件格式，用于在从网络接收到传入指标时将其持久化。 又见 总览 Ceres是一种时间序列数据库格式，旨在替代作为的默认存储格式。 与Whisper相比，Ceres不是固定大小的数据库，而是旨在更好地支持任意固定大小分辨率的稀疏数据。 这使Graphite可以在多个服务器或安装之间分配各个时间序列。 Ceres本身未提供预期的功能，例如汇总汇总和数据过期，而是在作为维护插件实现。
ceres:极简的墨水测试环境！ 合同
03-16
谷神星 Ceres是用于墨水的跨平台桌面客户端！ 合同。 设计 - - - - - - - - - - - - - - - - - - | ink! contract | ---> | ceres | ---> | app | - - - - - - - - - - - - - - - - - - 归因 谷神蜡使用： 执照 麻省理工学院
ceres:加密电子真实交易系统
05-17
蜡膜 &Ccedil;ryptoËlectronicřEALËxchange的变体系
ceres:知道在科英布拉哪里可以找到有机食品
06-07
#Projecto Ceres Saiba onde encontrar produtos agricolas biológicos em Coimbra。 ##项目状态非常在制品。 您可以在试用该站点的开发版本。 对于测试 porpuses，管理员凭据是： 给我发电子邮件me@lfac.me 密码test123 ##Acessing 我们的数据数据可以通过端点/json/agricultores以 JSON 格式获得（在我们的测试实例中）。 ##设置开发环境 请注意，Windows 目前（正式）不支持 Meteor。 安装 安装 安装 执行mrt ##License 这个存储库中的代码是在 MIT 许可下获得许可的。
coq-ceres:Coq库，用于序列化为S表达式
05-20
塞雷斯 Cérès是一个Coq库，用于序列化为S表达式。 S表达式 S表达式是结构化数据的统一表示。 例如，它们是Haskell中的Show和Rust中的Debug所使用的普通字符串的替代。 S表达式更适合程序使用，避免了自定义解析器并启用了灵活的格式化策略。 例子 这个S表达式... (example (message "I'm a teapot") (code 418)) ...对应于Coq中的此sexp 。 Definition example : sexp := [ Atom "example" ; [ Atom "message" ; Atom (Str "I'm a teapot") ] ; [ Atom "code" ; Atom 418%Z ] ]. 文献资料 是快速入门的好地方。 简化的概述 该库提供了带有两个构造函数的sexp类型：
Ceres Manifolds/LocalParameterization 类
泠山的博客
01-10 1222
x在老版本中，想实现流形的建模，使用的是类。在 V2.1.0 版本时，新增了Manifolds类并在 V2.2.0 时，Ceres-Solver 官方将类删除了。两个类在用法上区别不大。但不得不说，老版本的命名方式确实不如新版，至少新版又短又一眼能看出其用途。对于四元数或者旋转矩阵这种使用过参数化表示旋转的方式，它们是。
优化库——ceres（二）深入探索ceres::Problem
雨落凡城的博客
08-03 2868
文章目录1-1 :bookmark: Problem类函数总览1-2 :bookmark: ceres::LocalParameterization1-2-1自定义LocalParameterization1-2-2 案例：QuaternionParameterization解析1-2-3 ceres::LocalParameterization的其他用法1-3 :bookmark: 关于添加残差块，参数块，设置参数化的区别和调用关系参考链接 1-1 ???? Problem类函数总览 整个Problem函
Ceres优化库使用
A_L_A_N
11-11 1740
自动求导 ： struct CostFunctor { template &lt;typename T&gt; bool operator()(const T* const x, T* residual) const { //拟函数 residual[0] = T(10.0) - x[0]; return true; } }; int main(i...
【SLAM】Ceres优化库官方实例详细解析（SLAM相关问题）
热门推荐
Yngz_Miao的博客
07-13 1万+
在ceres-solver官方教程中提供了三个SLAM相关的优化问题，分别为：bundle_adjuster、pose_graph_2d、pose_graph_3d。本文就这三个问题进行拆解和分析。在阅读本文之前，你需要确保已经知晓了Ceres的基础用法，这部分可以参考文章：【SLAM】Ceres优化库超详细解析。（BA、光束平差）问题是视觉SLAM中最基础最常用的优化方法和思路。给定一组测量的图像特征位置和对应关系，光束平差的目标是优化3D空间点和相机参数来最小化重投影误差。这个优化问题通常是被定义为一个
《视觉SLAM十四讲》示例程序编译报错处理（上）
MSysのCSDN
12-07 1971
Ubuntu 22.04，所有库采用最新版。这本书中的代码有一些因为版本的问题会报错，本文总结一下我遇到的问题。
SLAM本质剖析-Ceres
诗筱涵的博客
08-27 739
转载自：https://mp.weixin.qq.com/s/fKlG9LWlPI52wStUAv18iw SLAM本质剖析-Ceres 原创 lovely_yoshino 古月居 今天 在了解SLAM的原理、流程后，个人经常实时困惑该如何去从零开始去设计编写一套能够符合我们需求的SLAM框架。作者认为Ceres、Eigen、Sophus、G2O这几个函数库无法避免，尤其是Ceres函数库在激光SLAM和V-SLAM的优化中均有着大量的应用。所以作者从Ceres作为开端，来对手写SLAM开个头，来
slam优化库，优化方法，G2o Ceres的学习
A172494的博客
05-06 1613
优化库G2o Ceres的学习。
Ceres-Solver 官方文档-其一
泠山的博客
07-10 1362
具体来说，它创建了一个表示机器人姿态的节点图，以及表示两个节点之间的相对变换(delta position and orientation)的边。这种优化问题通常被表述为非线性最小二乘问题，其中误差是观测到的特征位置与相应的三维点在相机图像平面上的投影之差的平方。在某些情况下，我们无法定义一个模板代价拟函数，例如，当残差的求值涉及对无法控制的库函数的调用时。计算导数是迄今为止使用 Ceres 最复杂的部分，根据具体情况，用户可能需要更复杂的计算导数的方法。拟合随机分布在具有近似线段轮廓的椭圆上的点。
学习整理 docker
wonder_dog的博客
05-31 223
nexus。
书籍学习|基于SprinBoot+vue的书籍学习平台(源码+数据库+文档)
JIngJaneIL的博客
05-28 1200
首先,论文一开始便是清楚的论述了平台的研究内容。其次,剖析平台需求分析,弄明白“做什么”,分析包括业务分析和业务流程的分析以及用例分析,更进一步明确平台的需求。然后在明白了平台的需求基础上需要进一步地设计平台,主要包罗软件架构模式、整体功能模块、数据库设计。本项目软件架构选择B/S模式和java技术，总体功能模块运用自顶向下的分层思想。再然后就是实现平台并进行代码编写实现功能。论文的最后章节总结一下自己完成本论文和开发本项目的心得和总结。通过书籍学习平台将会使书籍学习各个方面的工作效率带来实质性的提升。
前端技术入门学习：探索数字世界的奇幻之旅
qwertyhn_0024的博客
05-27 229
HTML定义了网页的结构和内容，CSS负责样式和布局，而JavaScript则实现了网页的交互功能。对于初学者来说，前端技术既是一个充满魅力的新世界，也是一个充满挑战与困惑的领域。本文将引导您从四个方面、五个方面、六个方面和七个方面深入理解前端技术，开启您的学习之旅。在这个过程中，我们需要不断探索、学习和实践，才能逐步掌握这门技艺的精髓。同时，我们也需要保持敏锐的洞察力和开放的心态，不断适应和应对前端技术领域的变化和发展。同时，随着人工智能、大数据等新技术的不断涌现，前端技术也将面临更多的挑战和机遇。
超大功率光伏并网逆变器学习(三相) 一
最新发布
我的博客
05-31 319
1.超大功率用的IGBT开关频率通常很低,比如6KHz。2.线电压和相电压的关系。
Android学习之ION memory manager
我的博客
05-28 1057
ion_device是miscdevice的封装，buffers是存放ion_buffer对象的根结点，heaps为存放ion_heap对象的列表，custom_ioctl为自定义的操作函数，clients为客户端根结点。每一种类型的memory都对应一个ion_heap类型的对象，ion_heap的对象存放在dev中，内存的类型存放做type中，ops为内存的操作函数指针。ION 模块是可扩展的（API都是统一的），支持各种形式的内存分配方式，可以表述不同的硬件资源和他们的一些限制。
ceres::AutoDiffCostFunction的函数参数
06-09
ceres::AutoDiffCostFunction的函数参数包括： 1. 残差函数（Residual Function）：表示优化问题中每个观测值的误差，即实际观测值与模型预测值之间的差距。 2. 参数块（Parameter Block）：表示优化问题中需要优化的变量，可以是一个或多个。 3. 参数块大小（Parameter Block Size）：表示每个参数块中包含的元素个数。 4. 残差项大小（Residual Block Size）：表示每个残差项的元素个数，通常与参数块大小相同。 5. 自动微分类型（Auto Differentiation Type）：表示使用何种自动微分方法，包括数值微分（Numeric Differentiation）和符号微分（Symbolic Differentiation）两种。 6. 残差块的输出维度（Residual Block Output Dimension）：表示每个残差项的输出维度，通常为1，表示一个标量。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值