slam典型应用手搓

nice-wyh

已于 2024-09-26 22:05:34 修改

阅读量262

点赞数 1

分类专栏： slam 文章标签：算法

于 2024-09-25 22:14:58 首次发布

本文链接：https://blog.csdn.net/qq_46454669/article/details/142534420

版权

slam 专栏收录该内容

4 篇文章 0 订阅

订阅专栏

一.雅可比矩阵的计算

假设我们有一个三维状态向量 [x, y, z],并且定义一个新的观测模型。输出两个观测值[u, v],其中

u = x^2 + y^2 + z, v = sin(x) + cos(y) + e^z，演示雅可比矩阵的计算

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

/*
我们可以使用一个更复杂的例子来演示雅可比矩阵的计算。假设我们有一个三维状态向量 [x, y, z],并且定义一个新的观测模型。输出两个观测值[u, v],其中
u = x^2 + y^2 + z, v = sin(x) + cos(y) + e^z
*/


// 状态向量:[x, y, theta]
Eigen::Vector3d state; 

// 观测模型
Eigen::Vector2d observationModel(const Eigen::Vector3d& state){
    // 假设观测为一个3D点
    double x = state[0];
    double y = state[1];
    double z = state[2];

    double u = x * x + y * y + z;
    double v = std::sin(x) + std::cos(y) + std::exp(z);

    return Eigen::Vector2d(x, y);
}

// 计算雅可比矩阵
Eigen::Matrix<double, 2 ,3> computeJacobian(const Eigen::Vector3d& state){
    Eigen::Matrix<double, 2, 3> J;

    double x = state[0];
    double y = state[1];
    double z = state[2];

    // 对于x和y的偏导数
    J(0, 0) = 2 * x;    // u对x的偏导
    J(0, 1) = 2 * y;    // u对y的偏导
    J(0, 2) = 1;        // u对z的偏导

    J(1, 0) = std::cos(x);     // v对x的偏导
    J(1, 1) = -std::sin(y);    // v对y的偏导
    J(1, 2) = std::exp(z);     // v对z的偏导

    return J;
}


int main(int argc, char *argv[])
{
    // 示例状态
    state << 2, 3, 0;         // x = 2, y = 3, theta = 0

    // 计算观测
    Eigen::Vector2d z = observationModel(state);
    std::cout << "观测Observation: " << z.transpose() << std::endl;

    // 计算雅可比矩阵
    Eigen::Matrix<double, 2, 3> J = computeJacobian(state);
    std::cout << "Jaconbian Matrix :\n" << J <<std::endl;
    
    return 0;
}

二.里程计

编写一个运动模型的实现，如里程计模型，给定控制输入预测下一个状态

#include <iostream>
#include <Eigen/Dense>

/*
编写一个运动模型的实现，如里程计模型，给定控制输入预测下一个状态
*/

// 状态向量[x, y, theta]
Eigen::Vector3d state;

// 运动模型
Eigen::Vector3d motionModel(const Eigen::Vector3d& state, double v, double omega, double dt){
    double x = state[0];
    double y = state[1];
    double theta = state[2];

    // 更新状态
    double newX = x + v * std::cos(theta) * dt;
    double newY = y + v * std::sin(theta) * dt;
    double newTheta = theta + omega * dt;

    // 返回新的状态
    return Eigen::Vector3d(newX, newY, newTheta);
}

int main(){
    // 初始状态
    state << 0, 0, 0;

    // 控制输入:线速度和角速度
    double v = 1.0;        // 1.0m/s
    double omega = 0.1;    // 0.1rad/s
    double dt = 1.0;       // 时间间隔 1s

    // 计算新的状态
    Eigen::Vector3d newState = motionModel(state, v, omega, dt);
    std::cout << "新的状态: " << newState.transpose() << std::endl;

    return 0;
}

三.卡尔曼滤波

实现一个二维卡尔曼滤波器，使用二维状态向量，假设我们要跟踪物体在平面上的位置和速度。状态向量 [x, y, vx, vy].包括位置(x, y),速度(vx, vy)

#include <iostream>
#include <Eigen/Dense>


/*
实现一个二维卡尔曼滤波器，使用二维状态向量，假设我们要跟踪物体在平面上的位置和速度。状态向量 [x, y, vx, vy].包括位置(x, y),速度(vx, vy)
*/


class kamlanFilter{
public:
    kamlanFilter(){
        // 初始化状态向量[x, y, v_x, v_y]
        state = Eigen::Vector4d(0, 0, 0, 0);

        // 初始化协方差矩阵
        covariance = Eigen::Matrix4d::Identity();

        // 状态转移矩阵
        F = Eigen::Matrix4d::Identity();
        F(0, 2) = 1;   //x = x + v_x * dt
        F(1, 3) = 1;   //y = y + v_y * dt

        // 观测矩阵
        H = Eigen::Matrix<double, 2, 4>::Zero();
        H(0, 0) = 1;    // 观测x
        H(1, 1) = 1;    // 观测y

        // 状态噪声协方差
        Q = Eigen::Matrix4d::Identity() * 0.1;
        // 观测噪声协方差
        R = Eigen::Matrix2d::Identity() * 1;
    }

    void predict(){
        // 状态预测
        state = F * state;
        // 协方差预测
        covariance = F * covariance * F.transpose() + Q;
    }

    void update(const Eigen::Vector2d& measurement){
        // 计算创新
        Eigen::Vector2d y = measurement - H * state;
        // 计算协方差
        Eigen::Matrix2d S = H * covariance * H.transpose() + R;
        // 计算卡尔曼增益
        Eigen::Matrix<double, 4, 2> K = covariance * H.transpose() * S.inverse();
        // 更新状态
        state += K * y;
        // 更新协方差
        covariance = (Eigen::Matrix4d::Identity() - K * H) * covariance;
    }

    Eigen::Vector4d getState() const{
        return state;
    }


private:
    Eigen::Vector4d state;              // 状态向量
    Eigen::Matrix4d covariance;         // 协方差矩阵
    Eigen::Matrix4d F;                  // 状态转移矩阵
    Eigen::Matrix<double, 2, 4> H;      // 观测矩阵
    Eigen::Matrix4d Q;                  // 状态噪声协方差
    Eigen::Matrix2d R;                  // 观测噪声协方差
};

int main(){
    kamlanFilter kf;

    // 模拟数据
    Eigen::Vector2d measurements;
    for(int i = 0; i < 10; i++){
        // 模拟测量量(假设真实值是线性运动，添加噪声)
        measurements(0) = i + ((double) rand() / RAND_MAX) * 0.5;   // 真实x添加噪声
        measurements(1) = i + ((double) rand() / RAND_MAX) * 0.5;   // 真实y添加噪声
        // 预测
        kf.predict();
        // 更新
        kf.update(measurements);

        std::cout << "当前状态估计: " << kf.getState().transpose() << std::endl;

    }

    return 0;
}

四.扩展卡尔曼滤波

要在现有的二维卡尔曼滤波器基础上实现扩展卡尔曼滤波（EKF），你需要以下几个步骤：

非线性状态转移模型：定义一个非线性状态转移函数。
非线性观测模型：定义一个非线性观测函数。
计算雅可比矩阵：在预测和更新步骤中，计算状态转移和观测的雅可比矩阵。
下面是修改后的代码示例，添加了扩展卡尔曼滤波的实现：

#include <iostream>
#include <Eigen/Dense>


/*
要在现有的二维卡尔曼滤波器基础上实现扩展卡尔曼滤波（EKF），你需要以下几个步骤：

非线性状态转移模型：定义一个非线性状态转移函数。
非线性观测模型：定义一个非线性观测函数。
计算雅可比矩阵：在预测和更新步骤中，计算状态转移和观测的雅可比矩阵。
下面是修改后的代码示例，添加了扩展卡尔曼滤波的实现：
*/


class ExtendedKalmanFilter {
public:
    ExtendedKalmanFilter() {
        // 初始化状态向量 [x, y, v_x, v_y]
        state = Eigen::Vector4d(0, 0, 0, 0);

        // 初始化协方差矩阵
        covariance = Eigen::Matrix4d::Identity();

        // 状态噪声协方差
        Q = Eigen::Matrix4d::Identity() * 0.1;
        // 观测噪声协方差
        R = Eigen::Matrix2d::Identity() * 1;
    }

    void predict(double dt) {
        // 使用非线性状态转移模型
        Eigen::Vector4d newState;
        newState(0) = state(0) + state(2) * dt; // x = x + v_x * dt
        newState(1) = state(1) + state(3) * dt; // y = y + v_y * dt
        newState(2) = state(2); // 速度保持不变
        newState(3) = state(3); // 速度保持不变

        // 更新状态
        state = newState;

        // 计算雅可比矩阵 F
        F = Eigen::Matrix4d::Identity();
        F(0, 2) = dt; // ∂x/∂v_x
        F(1, 3) = dt; // ∂y/∂v_y

        // 协方差预测
        covariance = F * covariance * F.transpose() + Q;
    }

    void update(const Eigen::Vector2d& measurement) {
        // 使用非线性观测模型
        Eigen::Vector2d z_pred;
        z_pred(0) = state(0); // 观测 x
        z_pred(1) = state(1); // 观测 y

        // 计算创新
        Eigen::Vector2d y = measurement - z_pred;

        // 计算雅可比矩阵 H
        H = Eigen::Matrix<double, 2, 4>::Zero();
        H(0, 0) = 1; // ∂z/∂x
        H(1, 1) = 1; // ∂z/∂y

        // 计算协方差
        Eigen::Matrix2d S = H * covariance * H.transpose() + R;
        // 计算卡尔曼增益
        Eigen::Matrix<double, 4, 2> K = covariance * H.transpose() * S.inverse();
        // 更新状态
        state += K * y;
        // 更新协方差
        covariance = (Eigen::Matrix4d::Identity() - K * H) * covariance;
    }

    Eigen::Vector4d getState() const {
        return state;
    }

private:
    Eigen::Vector4d state;              // 状态向量
    Eigen::Matrix4d covariance;         // 协方差矩阵
    Eigen::Matrix4d F;                  // 状态转移雅可比矩阵
    Eigen::Matrix<double, 2, 4> H;      // 观测雅可比矩阵
    Eigen::Matrix4d Q;                  // 状态噪声协方差
    Eigen::Matrix2d R;                  // 观测噪声协方差
};

int main() {
    ExtendedKalmanFilter ekf;

    // 模拟数据
    Eigen::Vector2d measurements;
    double dt = 1.0; // 时间步长
    for (int i = 0; i < 10; i++) {
        // 模拟测量量（假设真实值是线性运动，添加噪声）
        measurements(0) = i + ((double)rand() / RAND_MAX) * 0.5; // 真实 x 加噪声
        measurements(1) = i + ((double)rand() / RAND_MAX) * 0.5; // 真实 y 加噪声
        // 预测
        ekf.predict(dt);
        // 更新
        ekf.update(measurements);

        std::cout << "当前状态估计: " << ekf.getState().transpose() << std::endl;
    }

    return 0;
}

五.g2o

使用g2o优化曲线

#include <iostream>
#include <g2o/core/g2o_core_api.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
#include <chrono>

using namespace std;

// 曲线模型的顶点，模板参数：优化变量维度和数据类型
class CurveFittingVertex : public g2o::BaseVertex<3, Eigen::Vector3d> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW //这是一个宏，用于在Eigen库中创建对齐的new操作符。这可以确保在分配内存时，对象的大小与编译器指定的对齐要求相匹配

  // 重置
  virtual void setToOriginImpl() override {    //用于将顶点的位置重置为原点
    _estimate << 0, 0, 0;
  }

  // 更新
  virtual void oplusImpl(const double *update) override {    //用于更新顶点的位置
    _estimate += Eigen::Vector3d(update);
  }

  // 存盘和读盘：留空
  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}
};

// 误差模型 模板参数：观测值维度，类型，连接顶点类型
class CurveFittingEdge : public g2o::BaseUnaryEdge<1, double, CurveFittingVertex> {
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  CurveFittingEdge(double x) : BaseUnaryEdge(), _x(x) {}

  // 计算曲线模型误差
  virtual void computeError() override {    //用于计算曲线模型的误差
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    _error(0, 0) = _measurement - std::exp(abc(0, 0) * _x * _x + abc(1, 0) * _x + abc(2, 0));
  }

  // 计算雅可比矩阵
  virtual void linearizeOplus() override {      //用于计算雅可比矩阵
    const CurveFittingVertex *v = static_cast<const CurveFittingVertex *> (_vertices[0]);
    const Eigen::Vector3d abc = v->estimate();
    double y = exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
    _jacobianOplusXi[0] = -_x * _x * y;
    _jacobianOplusXi[1] = -_x * y;
    _jacobianOplusXi[2] = -y;
  }

  virtual bool read(istream &in) {}

  virtual bool write(ostream &out) const {}

public:
  double _x;  // x 值， y 值为 _measurement
};

int main(int argc, char **argv) {
  double ar = 1.0, br = 2.0, cr = 1.0;         // 真实参数值
  double ae = 2.0, be = -1.0, ce = 5.0;        // 估计参数值
  int N = 200;                                 // 数据点
  double w_sigma = 1.0;                        // 噪声Sigma值
  double inv_sigma = 1.0 / w_sigma;
  cv::RNG rng;                                 // OpenCV随机数产生器

  vector<double> x_data, y_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i / 100.0;
    x_data.push_back(x);
    y_data.push_back(exp(ar * x * x + br * x + cr) + rng.gaussian(w_sigma * w_sigma));
  }

  // 构建图优化，先设定g2o
  typedef g2o::BlockSolver<g2o::BlockSolverTraits<3, 1>> BlockSolverType;  // 每个误差项优化变量维度为3，误差值维度为1
  typedef g2o::LinearSolverDense<BlockSolverType::PoseMatrixType> LinearSolverType; // 线性求解器类型

  // 梯度下降方法，可以从GN, LM, DogLeg 中选
  auto solver = new g2o::OptimizationAlgorithmGaussNewton(
    g2o::make_unique<BlockSolverType>(g2o::make_unique<LinearSolverType>()));
  g2o::SparseOptimizer optimizer;     // 图模型
  optimizer.setAlgorithm(solver);   // 设置求解器
  optimizer.setVerbose(true);       // 打开调试输出

  // 往图中增加顶点
  CurveFittingVertex *v = new CurveFittingVertex();
  v->setEstimate(Eigen::Vector3d(ae, be, ce));
  v->setId(0);
  optimizer.addVertex(v);

  // 往图中增加边
  for (int i = 0; i < N; i++) {
    CurveFittingEdge *edge = new CurveFittingEdge(x_data[i]);
    edge->setId(i);
    edge->setVertex(0, v);                // 设置连接的顶点
    edge->setMeasurement(y_data[i]);      // 观测数值
    edge->setInformation(Eigen::Matrix<double, 1, 1>::Identity() * 1 / (w_sigma * w_sigma)); // 信息矩阵：协方差矩阵之逆
    optimizer.addEdge(edge);
  }

  // 执行优化
  cout << "start optimization" << endl;
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  optimizer.initializeOptimization();
  optimizer.optimize(10);
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

  // 输出优化值
  Eigen::Vector3d abc_estimate = v->estimate();
  cout << "estimated model: " << abc_estimate.transpose() << endl;

  return 0;
}

六. 给定一组3D点，计算它投影到图像上的2D点坐标

要将一组3D点投影到图像上的2D点坐标，通常需要使用相机的内参和外参进行投影。以下是实现的基本步骤：

基本步骤：

1.定义相机内参K

2.定义相机外参

包括旋转矩阵R和平移向量t，将3D点从世界坐标系转换到相机坐标系

3.投影公式

给定一个3D点P =(X,Y,Z)^T,首先将其转换到相机坐标系，Pcamera=R*P+t,然后使用内参矩阵投影到2D平面：[u, v, 1] = K*Pcamera,最后计算2D坐标u' = u / z, v' = v / z。

#include <iostream>
#include <Eigen/Dense>
#include <vector>

using namespace Eigen;

// 定义3D点投影到2D的函数
std::vector<Vector2d> projectPoints(const std::vector<Vector3d>& points3D, const Matrix3d& K, const Matrix3d& R, const Vector3d& t) {
    std::vector<Vector2d> points2D;

    for (const auto& point3D : points3D) {
        // 将3D点转换到相机坐标系
        Vector3d pointCamera = R * point3D + t;

        // 进行投影
        Vector3d point2DHomogeneous = K * pointCamera;

        // 将齐次坐标转换为2D坐标
        double z = point2DHomogeneous(2);
        if (z != 0) {
            Vector2d point2D(point2DHomogeneous(0) / z, point2DHomogeneous(1) / z);
            points2D.push_back(point2D);
        }
    }

    return points2D;
}

int main() {
    // 相机内参
    Matrix3d K;
    K << 800, 0, 320,   // fx, 0, cx
         0, 800, 240,   // 0, fy, cy
         0, 0, 1;       // 0, 0, 1

    // 相机外参 (旋转和平移)
    Matrix3d R = Matrix3d::Identity(); // 假设无旋转
    Vector3d t(0, 0, 0); // 假设无平移

    // 定义3D点
    std::vector<Vector3d> points3D = {
        {1, 1, 5},
        {2, 2, 5},
        {3, 3, 5}
    };

    // 投影到2D
    auto points2D = projectPoints(points3D, K, R, t);

    // 输出结果
    for (const auto& point : points2D) {
        std::cout << "Projected 2D Point: (" << point(0) << ", " << point(1) << ")\n";
    }

    return 0;
}