Lidar_imu自动标定源码阅读（四）——optimize部分

已于 2022-08-30 10:18:56 修改
阅读量463 收藏 2
点赞数 1
分类专栏： slam 文章标签： c++
于 2022-08-16 22:04:35 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/m0_48748418/article/details/126372434
版权

源码阅读，能力有限，如有某处理解错误，请指出，谢谢。

optimize.hpp：主要是利用ceres完成非线性优化，但是在这个cpp中只有代价函数的构造和残差的定义，以及使用自动求导，没有针对一个具体的问题展开求解

残差定义：residual=normal/transform-average

使用 Ceres Solver 求解非线性优化问题，主要包括以下几部分：

  • 【STEP 1】构建优化问题(ceres::Problem)
  • 【STEP 2】构建代价函数(ceres::CostFunction) 或残差(residual)
  • 【STEP 3】配置代价函数和损失函数(ceres::Problem::AddResidualBlock)：通过 AddResidualBlock 添加代价函数(cost function)、损失函数(loss function) 和待优化状态量
  • 【STEP 4】配置求解器(ceres::Solver::Options)
  • 【STEP 5】运行求解器(ceres::Solve(options, &problem, &summary))
#pragma once

#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <eigen3/Eigen/Dense>
#include <math.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>

定义BalmVoxelEnergy1类，实现的功能：构造一个代价函数，定义残差，利用ceres的自动求导
struct BalmVoxelEnergy1 {
  //构造函数，传入参数，并完成传递
  BalmVoxelEnergy1(const Eigen::Vector3d &current_laser_point,
                   const Eigen::Vector3d &point_average,
                   const Eigen::Vector3d &normal,
                   const Eigen::Matrix4d &imu_tran)
      : current_laser_point_(current_laser_point),
        point_average_(point_average), normal_(normal), imu_tran_(imu_tran) {}
  //析构函数
  ~BalmVoxelEnergy1() {}

  //ceres代价函数需要用模板构造仿函数实现
  template <typename T>
  bool operator()(const T *const q, const T *const t, T *residual) const {
    T cur_p[3];
    cur_p[0] = T(current_laser_point_(0));
    cur_p[1] = T(current_laser_point_(1));
    cur_p[2] = T(current_laser_point_(2));
    T world_p[3];
    //给出旋转向量q，由cur_p得到world_p
    ceres::QuaternionRotatePoint(q, cur_p, world_p);
    // ceres::AngleAxisRotatePoint(q, cur_p, world_p);
    //加上平移向量，得到完整的世界坐标系下的坐标
    world_p[0] += t[0];
    world_p[1] += t[1];
    // world_p[2] += t[2];
    T tran_p[3];

    // n * (p - p_aver)
    //定义一个残差
    tran_p[0] = imu_tran_(0, 0) * world_p[0] + imu_tran_(0, 1) * world_p[1] +
                imu_tran_(0, 2) * world_p[2] + imu_tran_(0, 3) -
                point_average_[0];
    tran_p[1] = imu_tran_(1, 0) * world_p[0] + imu_tran_(1, 1) * world_p[1] +
                imu_tran_(1, 2) * world_p[2] + imu_tran_(1, 3) -
                point_average_[1];
    tran_p[2] = imu_tran_(2, 0) * world_p[0] + imu_tran_(2, 1) * world_p[1] +
                imu_tran_(2, 2) * world_p[2] + imu_tran_(2, 3) -
                point_average_[2];

    residual[0] = normal_[0] * tran_p[0] + normal_[1] * tran_p[1] +
                  normal_[2] * tran_p[2];

    return true;
  }

  static ceres::CostFunction *Create(const Eigen::Vector3d &current_laser_point,
                                     const Eigen::Vector3d &point_average,
                                     const Eigen::Vector3d &normal,
                                     const Eigen::Matrix4d &imu_tran) {
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个是输入维度，即待寻优参数x的维度
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelEnergy1, 1, 4, 3>(
            new BalmVoxelEnergy1(current_laser_point, point_average, normal,
                                 imu_tran));
    return cost_function;
  }

  const Eigen::Vector3d current_laser_point_;
  const Eigen::Vector3d point_average_;
  const Eigen::Vector3d normal_;
  const Eigen::Matrix4d imu_tran_;
};

定义BalmVoxelEnergy2类，实现的功能：构造一个代价函数，定义残差，利用ceres的自动求导
struct BalmVoxelEnergy2 {
  //构造函数，传入参数，并完成传递
  BalmVoxelEnergy2(const Eigen::Vector3d &current_laser_point,
                   const Eigen::Vector3d &point_average,
                   const Eigen::Vector3d &normal,
                   const Eigen::Matrix4d &imu_tran,
                   const Eigen::Matrix4d &imu_tran_orig)
      : current_laser_point_(current_laser_point),
        point_average_(point_average), normal_(normal), imu_tran_(imu_tran),
        imu_tran_orig_(imu_tran_orig) {}
  //析构函数
  ~BalmVoxelEnergy2() {}

  //ceres代价函数需要用模板构造仿函数实现
  template <typename T>
  bool operator()(const T *const q, const T *const t, T *residual) const {
    T cur_p[3];
    cur_p[0] = T(current_laser_point_(0));
    cur_p[1] = T(current_laser_point_(1));
    cur_p[2] = T(current_laser_point_(2));
    T world_p[3];
    //给出旋转向量q，由cur_p得到world_p
    ceres::QuaternionRotatePoint(q, cur_p, world_p);
    // ceres::AngleAxisRotatePoint(q, cur_p, world_p);
    world_p[0] += t[0];
    world_p[1] += t[1];
    // world_p[2] += t[2];
    T tran_p[3];

    // transform point average and surface normal变换点平均和曲面法线
    T tran_point_aver[3];//变换点平均值
    T tran_normal[3];//变换正常值
    T orig_point_aver[3];//原始点平均值
    T orig_normal[3];原始正常值
    orig_point_aver[0] = T(point_average_(0));
    orig_point_aver[1] = T(point_average_(1));
    orig_point_aver[2] = T(point_average_(2));
    orig_normal[0] = T(normal_(0));
    orig_normal[1] = T(normal_(1));
    orig_normal[2] = T(normal_(2));
    //给出旋转向量q，由orig_point_aver得到tran_point_aver
    ceres::QuaternionRotatePoint(q, orig_point_aver, tran_point_aver);
    tran_point_aver[0] += t[0];
    tran_point_aver[1] += t[1];
    // tran_point_aver[2] += t[2];
    //给出旋转向量q，由orig_normal得到tran_normal
    ceres::QuaternionRotatePoint(q, orig_normal, tran_normal);
    // imu pose
    T imu_point_aver[3];//imu点平均值
    T imu_normal[3];//imu正常值
    imu_point_aver[0] = imu_tran_orig_(0, 0) * tran_point_aver[0] +
                        imu_tran_orig_(0, 1) * tran_point_aver[1] +
                        imu_tran_orig_(0, 2) * tran_point_aver[2] +
                        imu_tran_orig_(0, 3);
    imu_point_aver[1] = imu_tran_orig_(1, 0) * tran_point_aver[0] +
                        imu_tran_orig_(1, 1) * tran_point_aver[1] +
                        imu_tran_orig_(1, 2) * tran_point_aver[2] +
                        imu_tran_orig_(1, 3);
    imu_point_aver[2] = imu_tran_orig_(2, 0) * tran_point_aver[0] +
                        imu_tran_orig_(2, 1) * tran_point_aver[1] +
                        imu_tran_orig_(2, 2) * tran_point_aver[2] +
                        imu_tran_orig_(2, 3);

    imu_normal[0] = imu_tran_orig_(0, 0) * tran_normal[0] +
                    imu_tran_orig_(0, 1) * tran_normal[1] +
                    imu_tran_orig_(0, 2) * tran_normal[2];
    imu_normal[1] = imu_tran_orig_(1, 0) * tran_normal[0] +
                    imu_tran_orig_(1, 1) * tran_normal[1] +
                    imu_tran_orig_(1, 2) * tran_normal[2];
    imu_normal[2] = imu_tran_orig_(2, 0) * tran_normal[0] +
                    imu_tran_orig_(2, 1) * tran_normal[1] +
                    imu_tran_orig_(2, 2) * tran_normal[2];

    // n * (p - p_aver)定义一个残差
    tran_p[0] = imu_tran_(0, 0) * world_p[0] + imu_tran_(0, 1) * world_p[1] +
                imu_tran_(0, 2) * world_p[2] + imu_tran_(0, 3) -
                imu_point_aver[0];
    tran_p[1] = imu_tran_(1, 0) * world_p[0] + imu_tran_(1, 1) * world_p[1] +
                imu_tran_(1, 2) * world_p[2] + imu_tran_(1, 3) -
                imu_point_aver[1];
    tran_p[2] = imu_tran_(2, 0) * world_p[0] + imu_tran_(2, 1) * world_p[1] +
                imu_tran_(2, 2) * world_p[2] + imu_tran_(2, 3) -
                imu_point_aver[2];

    residual[0] = imu_normal[0] * tran_p[0] + imu_normal[1] * tran_p[1] +
                  imu_normal[2] * tran_p[2];

    return true;
  }

  static ceres::CostFunction *Create(const Eigen::Vector3d &current_laser_point,
                                     const Eigen::Vector3d &point_average,
                                     const Eigen::Vector3d &normal,
                                     const Eigen::Matrix4d &imu_tran,
                                     const Eigen::Matrix4d &imu_tran_orig) {
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二4个是输入维度，即待寻优参数x的维度
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelEnergy2, 1, 4, 3>(
            new BalmVoxelEnergy2(current_laser_point, point_average, normal,
                                 imu_tran, imu_tran_orig));
    return cost_function;
  }

  const Eigen::Vector3d current_laser_point_;
  const Eigen::Vector3d point_average_;
  const Eigen::Vector3d normal_;
  const Eigen::Matrix4d imu_tran_;
  const Eigen::Matrix4d imu_tran_orig_;
};

定义BalmVoxelEnergy2_NOT类，实现的功能：构造一个代价函数，定义残差，利用ceres的自动求导
struct BalmVoxelEnergy2_NOT {
  //构造函数，传入参数，并完成传递
  BalmVoxelEnergy2_NOT(const Eigen::Vector3d &current_laser_point,
                       const Eigen::Vector3d &point_average,
                       const Eigen::Vector3d &normal,
                       const Eigen::Matrix4d &imu_tran,
                       const Eigen::Matrix4d &imu_tran_orig)
      : current_laser_point_(current_laser_point),
        point_average_(point_average), normal_(normal), imu_tran_(imu_tran),
        imu_tran_orig_(imu_tran_orig) {}
  //析构函数
  ~BalmVoxelEnergy2_NOT() {}

  //ceres代价函数需要用模板构造仿函数实现
  template <typename T> bool operator()(const T *const q, T *residual) const {
    T cur_p[3];
    cur_p[0] = T(current_laser_point_(0));
    cur_p[1] = T(current_laser_point_(1));
    cur_p[2] = T(current_laser_point_(2));
    T world_p[3];
    ceres::QuaternionRotatePoint(q, cur_p, world_p);
    // ceres::AngleAxisRotatePoint(q, cur_p, world_p);
    T tran_p[3];

    // transform point average and surface normal变换点平均和曲面法线
    T tran_point_aver[3];//变换点平均值
    T tran_normal[3];//变换正常
    T orig_point_aver[3];//原始点平均值
    T orig_normal[3];//原始正常
    orig_point_aver[0] = T(point_average_(0));
    orig_point_aver[1] = T(point_average_(1));
    orig_point_aver[2] = T(point_average_(2));
    orig_normal[0] = T(normal_(0));
    orig_normal[1] = T(normal_(1));
    orig_normal[2] = T(normal_(2));
    //给出旋转向量q，由orig_point_aver得到tran_point_aver
    ceres::QuaternionRotatePoint(q, orig_point_aver, tran_point_aver);
    //给出旋转向量q，由orig_normal得到tran_normal
    ceres::QuaternionRotatePoint(q, orig_normal, tran_normal);
    // imu pose
    T imu_point_aver[3];//imu点平均值
    T imu_normal[3];//imu正常值
    
    imu_point_aver[0] = imu_tran_orig_(0, 0) * tran_point_aver[0] +
                        imu_tran_orig_(0, 1) * tran_point_aver[1] +
                        imu_tran_orig_(0, 2) * tran_point_aver[2] +
                        imu_tran_orig_(0, 3);
    imu_point_aver[1] = imu_tran_orig_(1, 0) * tran_point_aver[0] +
                        imu_tran_orig_(1, 1) * tran_point_aver[1] +
                        imu_tran_orig_(1, 2) * tran_point_aver[2] +
                        imu_tran_orig_(1, 3);
    imu_point_aver[2] = imu_tran_orig_(2, 0) * tran_point_aver[0] +
                        imu_tran_orig_(2, 1) * tran_point_aver[1] +
                        imu_tran_orig_(2, 2) * tran_point_aver[2] +
                        imu_tran_orig_(2, 3);

    imu_normal[0] = imu_tran_orig_(0, 0) * tran_normal[0] +
                    imu_tran_orig_(0, 1) * tran_normal[1] +
                    imu_tran_orig_(0, 2) * tran_normal[2];
    imu_normal[1] = imu_tran_orig_(1, 0) * tran_normal[0] +
                    imu_tran_orig_(1, 1) * tran_normal[1] +
                    imu_tran_orig_(1, 2) * tran_normal[2];
    imu_normal[2] = imu_tran_orig_(2, 0) * tran_normal[0] +
                    imu_tran_orig_(2, 1) * tran_normal[1] +
                    imu_tran_orig_(2, 2) * tran_normal[2];

    // n * (p - p_aver)定义残差
    tran_p[0] = imu_tran_(0, 0) * world_p[0] + imu_tran_(0, 1) * world_p[1] +
                imu_tran_(0, 2) * world_p[2] + imu_tran_(0, 3) -
                imu_point_aver[0];
    tran_p[1] = imu_tran_(1, 0) * world_p[0] + imu_tran_(1, 1) * world_p[1] +
                imu_tran_(1, 2) * world_p[2] + imu_tran_(1, 3) -
                imu_point_aver[1];
    tran_p[2] = imu_tran_(2, 0) * world_p[0] + imu_tran_(2, 1) * world_p[1] +
                imu_tran_(2, 2) * world_p[2] + imu_tran_(2, 3) -
                imu_point_aver[2];

    residual[0] = imu_normal[0] * tran_p[0] + imu_normal[1] * tran_p[1] +
                  imu_normal[2] * tran_p[2];

    return true;
  }

  static ceres::CostFunction *Create(const Eigen::Vector3d &current_laser_point,
                                     const Eigen::Vector3d &point_average,
                                     const Eigen::Vector3d &normal,
                                     const Eigen::Matrix4d &imu_tran,
                                     const Eigen::Matrix4d &imu_tran_orig) {
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个是输入维度，即待寻优参数x的维度
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelEnergy2_NOT, 1, 4>(
            new BalmVoxelEnergy2_NOT(current_laser_point, point_average, normal,
                                     imu_tran, imu_tran_orig));
    return cost_function;
  }

  const Eigen::Vector3d current_laser_point_;
  const Eigen::Vector3d point_average_;
  const Eigen::Vector3d normal_;
  const Eigen::Matrix4d imu_tran_;
  const Eigen::Matrix4d imu_tran_orig_;
};

定义BalmVoxelEnergy3类，实现的功能：构造一个代价函数，定义残差，利用ceres的自动求导
struct BalmVoxelEnergy3 {
  //构造函数，传入参数，完成赋值
  BalmVoxelEnergy3(
      const Eigen::Vector3d &cur_laser_point,
      const Eigen::Matrix4d &cur_imu_tran,
      const std::vector<std::vector<Eigen::Vector3d> *> &orig_laser_points,
      const std::vector<pcl::PointXYZINormal> &ap_centor_origns,
      const std::vector<Eigen::Matrix4d> &imu_trans)
      : cur_laser_point_(cur_laser_point), cur_imu_tran_(cur_imu_tran),
        orig_laser_points_(orig_laser_points),
        ap_centor_origns_(ap_centor_origns), imu_trans_(imu_trans) {}
  //析构函数
  ~BalmVoxelEnergy3() {}
  //ceres代价函数需要用利用模板构建仿函数实现
  template <typename T>
  bool operator()(const T *const q, const T *const t, T *residual) const {
    T real_window_size = T(0);//实际窗口大小
    T overall_point_aver[3] = {T(0), T(0), T(0)};//全部点平均
    T overall_normal[3] = {T(0), T(0), T(0)};//全部正常
    // transform point average and surface normal变换点平均和曲面法线
    //遍历每个原始激光雷达点数据
    for (int i = 0; i < orig_laser_points_.size(); i++) {
      if (orig_laser_points_[i]->size() <= 0)
        continue;
      real_window_size += T(1);
      // int asize = current_laser_points_[i] -> size();
      T tran_point_aver[3];//变换点平均值
      T tran_normal[3];//变换正常
      T orig_point_aver[3];//原始点平均值
      T orig_normal[3];//原始正常
      orig_point_aver[0] = T(ap_centor_origns_[i].x);
      orig_point_aver[1] = T(ap_centor_origns_[i].y);
      orig_point_aver[2] = T(ap_centor_origns_[i].z);
      orig_normal[0] = T(ap_centor_origns_[i].normal_x);
      orig_normal[1] = T(ap_centor_origns_[i].normal_y);
      orig_normal[2] = T(ap_centor_origns_[i].normal_z);
      //给出旋转向量q，由orig_point_aver得到tran_point_aver
      ceres::QuaternionRotatePoint(q, orig_point_aver, tran_point_aver);
      tran_point_aver[0] += t[0];
      tran_point_aver[1] += t[1];
      // tran_point_aver[2] += t[2];
      //给出旋转向量q，由orig_normal得到tran_normal
      ceres::QuaternionRotatePoint(q, orig_normal, tran_normal);
      Eigen::Matrix4d imu_tran_orign = imu_trans_[i];

      // imu pose
      T imu_point_aver[3];//imu点平均值
      T imu_normal[3];//imu正常
      imu_point_aver[0] = imu_tran_orign(0, 0) * tran_point_aver[0] +
                          imu_tran_orign(0, 1) * tran_point_aver[1] +
                          imu_tran_orign(0, 2) * tran_point_aver[2] +
                          imu_tran_orign(0, 3);
      imu_point_aver[1] = imu_tran_orign(1, 0) * tran_point_aver[0] +
                          imu_tran_orign(1, 1) * tran_point_aver[1] +
                          imu_tran_orign(1, 2) * tran_point_aver[2] +
                          imu_tran_orign(1, 3);
      imu_point_aver[2] = imu_tran_orign(2, 0) * tran_point_aver[0] +
                          imu_tran_orign(2, 1) * tran_point_aver[1] +
                          imu_tran_orign(2, 2) * tran_point_aver[2] +
                          imu_tran_orign(2, 3);

      imu_normal[0] = imu_tran_orign(0, 0) * tran_normal[0] +
                      imu_tran_orign(0, 1) * tran_normal[1] +
                      imu_tran_orign(0, 2) * tran_normal[2];
      imu_normal[1] = imu_tran_orign(1, 0) * tran_normal[0] +
                      imu_tran_orign(1, 1) * tran_normal[1] +
                      imu_tran_orign(1, 2) * tran_normal[2];
      imu_normal[2] = imu_tran_orign(2, 0) * tran_normal[0] +
                      imu_tran_orign(2, 1) * tran_normal[1] +
                      imu_tran_orign(2, 2) * tran_normal[2];
      // add to overall point average
      overall_point_aver[0] += imu_point_aver[0];
      overall_point_aver[1] += imu_point_aver[1];
      overall_point_aver[2] += imu_point_aver[2];
      overall_normal[0] += imu_normal[0];
      overall_normal[1] += imu_normal[1];
      overall_normal[2] += imu_normal[2];
    }
    //得到全部激光雷达点云数据的均值
    overall_point_aver[0] /= real_window_size;
    overall_point_aver[1] /= real_window_size;
    overall_point_aver[2] /= real_window_size;
    overall_normal[0] /= real_window_size;
    overall_normal[1] /= real_window_size;
    overall_normal[2] /= real_window_size;

    // add residuals
    T cur_p[3];
    cur_p[0] = T(cur_laser_point_(0));
    cur_p[1] = T(cur_laser_point_(1));
    cur_p[2] = T(cur_laser_point_(2));
    T world_p[3];
    //给出旋转四元数q，由cur_p得到world_p
    ceres::QuaternionRotatePoint(q, cur_p, world_p);
    // ceres::AngleAxisRotatePoint(q, cur_p, world_p);
    world_p[0] += t[0];
    world_p[1] += t[1];
    // world_p[2] += t[2];
    T tran_p[3];

    // n * (p - p_aver)定义残差
    tran_p[0] = cur_imu_tran_(0, 0) * world_p[0] +
                cur_imu_tran_(0, 1) * world_p[1] +
                cur_imu_tran_(0, 2) * world_p[2] + cur_imu_tran_(0, 3) -
                overall_point_aver[0];
    tran_p[1] = cur_imu_tran_(1, 0) * world_p[0] +
                cur_imu_tran_(1, 1) * world_p[1] +
                cur_imu_tran_(1, 2) * world_p[2] + cur_imu_tran_(1, 3) -
                overall_point_aver[1];
    tran_p[2] = cur_imu_tran_(2, 0) * world_p[0] +
                cur_imu_tran_(2, 1) * world_p[1] +
                cur_imu_tran_(2, 2) * world_p[2] + cur_imu_tran_(2, 3) -
                overall_point_aver[2];

    residual[0] = overall_normal[0] * tran_p[0] +
                  overall_normal[1] * tran_p[1] + overall_normal[2] * tran_p[2];
    return true;
  }

  static ceres::CostFunction *
  Create(const Eigen::Vector3d &cur_laser_point,
         const Eigen::Matrix4d &cur_imu_tran,
         const std::vector<std::vector<Eigen::Vector3d> *> &orig_laser_points,
         const std::vector<pcl::PointXYZINormal> &ap_centor_origns,
         const std::vector<Eigen::Matrix4d> &imu_trans) {
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个是输入维度，即待寻优参数x的维度
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelEnergy3, 1, 4, 3>(
            new BalmVoxelEnergy3(cur_laser_point, cur_imu_tran,
                                 orig_laser_points, ap_centor_origns,
                                 imu_trans));
    // cost_function.SetNumResiduals(point_size);
    return cost_function;
  }

  const Eigen::Vector3d cur_laser_point_;
  const Eigen::Matrix4d cur_imu_tran_;
  const std::vector<std::vector<Eigen::Vector3d> *> orig_laser_points_;
  const std::vector<pcl::PointXYZINormal> ap_centor_origns_;
  const std::vector<Eigen::Matrix4d> imu_trans_;
};

struct BalmVoxelTransEnergy {
  //构造函数，传递参数，完成赋值
  BalmVoxelTransEnergy(const Eigen::Vector3d &current_laser_point,
                       const Eigen::Vector3d &point_average,
                       const Eigen::Vector3d &normal,
                       const Eigen::Matrix4d &imu_tran,
                       const Eigen::Matrix4d &imu_tran_orig,
                       const Eigen::Matrix4d &refined_calib_mat)
      : current_laser_point_(current_laser_point),
        point_average_(point_average), normal_(normal), imu_tran_(imu_tran),
        imu_tran_orig_(imu_tran_orig), refined_calib_mat_(refined_calib_mat) {}
  //析构函数
  ~BalmVoxelTransEnergy() {}
  //ceres构造代价函数需要借助模板搭建仿函数实现
  template <typename T> bool operator()(const T *const t, T *residual) const {
    T cur_p[3];
    cur_p[0] = T(current_laser_point_(0));
    cur_p[1] = T(current_laser_point_(1));
    cur_p[2] = T(current_laser_point_(2));
    T world_p[3];
    world_p[0] = refined_calib_mat_(0, 0) * cur_p[0] +
                 refined_calib_mat_(0, 1) * cur_p[1] +
                 refined_calib_mat_(0, 2) * cur_p[2] + refined_calib_mat_(0, 3);
    world_p[1] = refined_calib_mat_(1, 0) * cur_p[0] +
                 refined_calib_mat_(1, 1) * cur_p[1] +
                 refined_calib_mat_(1, 2) * cur_p[2] + refined_calib_mat_(1, 3);
    world_p[2] = refined_calib_mat_(2, 0) * cur_p[0] +
                 refined_calib_mat_(2, 1) * cur_p[1] +
                 refined_calib_mat_(2, 2) * cur_p[2] + t[2];
    T tran_p[3];

    // transform point average and surface normal变换点平均和曲面法线
    T tran_point_aver[3];//变换点平均
    T tran_normal[3];//变换正常
    T orig_point_aver[3];//原始点平均
    T orig_normal[3];//原始正常
    orig_point_aver[0] = T(point_average_(0));
    orig_point_aver[1] = T(point_average_(1));
    orig_point_aver[2] = T(point_average_(2));
    orig_normal[0] = T(normal_(0));
    orig_normal[1] = T(normal_(1));
    orig_normal[2] = T(normal_(2));
    tran_point_aver[0] = refined_calib_mat_(0, 0) * orig_point_aver[0] +
                         refined_calib_mat_(0, 1) * orig_point_aver[1] +
                         refined_calib_mat_(0, 2) * orig_point_aver[2] +
                         refined_calib_mat_(0, 3);
    tran_point_aver[1] = refined_calib_mat_(1, 0) * orig_point_aver[0] +
                         refined_calib_mat_(1, 1) * orig_point_aver[1] +
                         refined_calib_mat_(1, 2) * orig_point_aver[2] +
                         refined_calib_mat_(1, 3);
    tran_point_aver[2] = refined_calib_mat_(2, 0) * orig_point_aver[0] +
                         refined_calib_mat_(2, 1) * orig_point_aver[1] +
                         refined_calib_mat_(2, 2) * orig_point_aver[2] + t[2];
    tran_normal[0] = refined_calib_mat_(0, 0) * orig_normal[0] +
                     refined_calib_mat_(0, 1) * orig_normal[1] +
                     refined_calib_mat_(0, 2) * orig_normal[2];
    tran_normal[1] = refined_calib_mat_(1, 0) * orig_normal[0] +
                     refined_calib_mat_(1, 1) * orig_normal[1] +
                     refined_calib_mat_(1, 2) * orig_normal[2];
    tran_normal[2] = refined_calib_mat_(2, 0) * orig_normal[0] +
                     refined_calib_mat_(2, 1) * orig_normal[1] +
                     refined_calib_mat_(2, 2) * orig_normal[2];
    // imu pose
    T imu_point_aver[3];//imu点平均
    T imu_normal[3];//imu正常
    imu_point_aver[0] = imu_tran_orig_(0, 0) * tran_point_aver[0] +
                        imu_tran_orig_(0, 1) * tran_point_aver[1] +
                        imu_tran_orig_(0, 2) * tran_point_aver[2] +
                        imu_tran_orig_(0, 3);
    imu_point_aver[1] = imu_tran_orig_(1, 0) * tran_point_aver[0] +
                        imu_tran_orig_(1, 1) * tran_point_aver[1] +
                        imu_tran_orig_(1, 2) * tran_point_aver[2] +
                        imu_tran_orig_(1, 3);
    imu_point_aver[2] = imu_tran_orig_(2, 0) * tran_point_aver[0] +
                        imu_tran_orig_(2, 1) * tran_point_aver[1] +
                        imu_tran_orig_(2, 2) * tran_point_aver[2] +
                        imu_tran_orig_(2, 3);

    imu_normal[0] = imu_tran_orig_(0, 0) * tran_normal[0] +
                    imu_tran_orig_(0, 1) * tran_normal[1] +
                    imu_tran_orig_(0, 2) * tran_normal[2];
    imu_normal[1] = imu_tran_orig_(1, 0) * tran_normal[0] +
                    imu_tran_orig_(1, 1) * tran_normal[1] +
                    imu_tran_orig_(1, 2) * tran_normal[2];
    imu_normal[2] = imu_tran_orig_(2, 0) * tran_normal[0] +
                    imu_tran_orig_(2, 1) * tran_normal[1] +
                    imu_tran_orig_(2, 2) * tran_normal[2];

    // n * (p - p_aver)定义残差
    tran_p[0] = imu_tran_(0, 0) * world_p[0] + imu_tran_(0, 1) * world_p[1] +
                imu_tran_(0, 2) * world_p[2] + imu_tran_(0, 3) -
                imu_point_aver[0];
    tran_p[1] = imu_tran_(1, 0) * world_p[0] + imu_tran_(1, 1) * world_p[1] +
                imu_tran_(1, 2) * world_p[2] + imu_tran_(1, 3) -
                imu_point_aver[1];
    tran_p[2] = imu_tran_(2, 0) * world_p[0] + imu_tran_(2, 1) * world_p[1] +
                imu_tran_(2, 2) * world_p[2] + imu_tran_(2, 3) -
                imu_point_aver[2];

    residual[0] = imu_normal[0] * tran_p[0] + imu_normal[1] * tran_p[1] +
                  imu_normal[2] * tran_p[2];

    return true;
  }

  static ceres::CostFunction *Create(const Eigen::Vector3d &current_laser_point,
                                     const Eigen::Vector3d &point_average,
                                     const Eigen::Vector3d &normal,
                                     const Eigen::Matrix4d &imu_tran,
                                     const Eigen::Matrix4d &imu_tran_orig,
                                     const Eigen::Matrix4d &refined_calib_mat) {
    //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个是输入维度，即待寻优参数x的维度
    auto cost_function =
        new ceres::AutoDiffCostFunction<BalmVoxelTransEnergy, 1, 3>(
            new BalmVoxelTransEnergy(current_laser_point, point_average, normal,
                                     imu_tran, imu_tran_orig,
                                     refined_calib_mat));
    return cost_function;
  }

  const Eigen::Vector3d current_laser_point_;
  const Eigen::Vector3d point_average_;
  const Eigen::Vector3d normal_;
  const Eigen::Matrix4d imu_tran_;
  const Eigen::Matrix4d imu_tran_orig_;
  const Eigen::Matrix4d refined_calib_mat_;
};

参考连接：

参考系列2：优化库——ceres（一）快速概览_贵在坚持，不忘初心的博客-CSDN博客_ceres库1 ???? ceres快速概览基本概念对于任何一个优化问题，我们首先需要对问题进行建模，之后采用合适的优化方法，进行求解。在求解的过程中，往往需要进行梯度下降求取最优，这里涉及了导数计算。所以在代码中使用Ceres进行优化时，需要包含基本内容：建模、优化、求导方法。1.1 ????问题建模和求解1.1.1 问题建模对于形如 下 问题来说：​​这样便完成了一个优化问题的建模 ！在写代码时，我们构建问题之后需要先定义代价函数，定义时需要指明求导方式，之后将残差块加入到https://blog.csdn.net/hltt3838/article/details/119774758?ops_request_misc=&request_id=&biz_id=102&utm_term=%20%20%20%20auto%20cost_function%20=%20%20%20%20%20%20&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduweb~default-1-119774758.142%5Ev41%5Econtrol,185%5Ev2%5Econtrol&spm=1018.2226.3001.4187

有了个相册
关注
  • 1
    点赞
  • 踩
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
lidar_imu_calib:自动校准3D激光雷达和IMU外在性
04-01
lidar_imu_calib 概述 在基于3D激光雷达的slam开发中，我们经常使用imu为匹配算法（icp，ndt）提供先验，因此需要校准激光雷达和imu之间的变换。对于匹配算法，transfom中的姿态比变换中的位置更重要，并且通常将...
VINS-FUSION GPS融合坐标转换细节分析
BuLianJianDeWenHua的博客
04-02 4071
先贴两个对GPS融合的代码结构分析的博文，感谢博主们的分享。 https://zhuanlan.zhihu.com/p/75492883 https://blog.csdn.net/weixin_41843971/article/details/86748719 希望读者先看看整体的代码结构，本文不再赘述，本文主要想把想到的GPS融合时坐标转换的一些细节分享给大家。 GPS坐标系和vio坐...
Ceres使用
小红鱼的博客
11-09 4129
定义：ceres是一款非线性优化问题的数值求解器。 1. CostFunction 输入为自变量，输出为残差。首先要定义一个用于求解残差的struct，并在内定义重载函数operator()： template bool operator() (const T* const x, T* residual ) const 2. LocalParameterization 输入为全局参数，输出...
ceres优化参数的传递、残差构建时状态量的传递、evaluate函数的使用
weixin_38216573的博客
12-09 6457
误差函数中的参数包括： 已知参数和待优化参数两部分， 1. 添加待优化参数 ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization(); problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization); 2. 添加残差及需要对待优化参数变来嗯进行雅可比矩阵的计算（解析求导） 先加入残差项 IMU残差项 I
vins-mono中的初始化详解（2）
weixin_44587828的博客
11-01 130
本文详细介绍了在vins-mono初始化阶段如何使用ceres solver求解工具对其中的状态量进行优化求解。
ceres手动求导
bkhyv的博客
10-08 86
ceres手动求导
eigen和ceres中的四元数
qq_42731705的博客
04-19 1911
ceres中定义了ceres::QuaternionRotatePoint(quantion, p, result )函数，就是实现四元数对点的乘法，其内部调用的是UnitQuaternionRotatePoint函数进行计算。 四元数乘法： q=(w,x,y,z),q−1=(w,−x,−y,−z),p=(0,a,b,c)p′=q⊗p⊗q−1q = (w, x, y, z), \quad q^{-1} = (w, -x, -y, -z), \quad p = (0, a, b, c) \\ p' = q \
VINS全局优化残差函数分析
fb_help的专栏
07-24 848
VINS全局优化残差函数分析 VINS的全局优化包含有与GPS低耦合的优化．即GPS作为一个观测(边)加入到整体的优化中去． 其优化的依据是： 帧与帧之间相对平移和旋转在局部是准确的 含有gps的帧其位置应与gps重合． 第一个约束对应的是RelativeRTError,第二个约束对应的是TError 总体的优化由于包括姿态，因此为非线性优化． 参数是：每一帧的位置和姿态 观测是：某帧的gps...
ceres学习总结
dna葡萄糖的博客
06-22 1118
ceres是用来求解优化问题的工具库。 使用时至少需要构建目标函数，优化变量。 如果自动求导困难，则需要给出雅克比矩阵。（一般是采用了第三方的库，比如eigen和sophus的一些运算） 如果优化变量不对加法封闭，则需要给出优化变量的更新方法。 ============================================================================== 对于简单优化问题总体流程：（可以采用自动求导） 1、构建代价函数模型，描述如何计算残差 2、构
VINS_FUSION入门系列---GPS与VIO融合
pj_find的博客
06-05 3017
参考的博客:https://blog.csdn.net/subiluo/article/details/105429471 http://www.luyixian.cn/news_show_313718.aspx 1.代码的输入输出 输入输出的定义都在globalOptNode.cpp这个文件里面 ros::Subscriber sub_GPS = n.subscribe("/gps", 100, GPS_callback); ros::Subscriber sub_vio = n.subs
多传感器标定
Shilong的博客
10-18 1715
多传感器标定
lidar_IMU_calib:基于连续时间批量估计的LiDAR-IMU系统的无目标标定
03-21
LI-Calib是用于校准6DoF刚性变换和3D LiDAR与IMU之间的时间偏移的工具包。它基于连续时间的批次优化。基于IMU的成本和LiDAR点到surfel距离共同最小化，这使得校准问题在一般情况下受到了很好的约束。 先决条件 ...
lidar_align标定代码
06-15
由瑞士苏黎世联邦理工大学自动驾驶实验室开发的lidar_align标定
multi_lidar_calibrator.zip
08-09
多激光雷达标定，配准。基于NDT算法的多激光雷达外参标定的方法和实践。 工程代码是在ROS中间件下完成的。 内涵详细的算法说明和编码流程。 使用的编码语言为C++. 关于激光雷达标定的其他资源: [毫米波雷达与摄像头...
传感器(camera,lidar,IMU,radar)外参标定及在线标定(源码)-多传感器融合感知.pdf
最新发布
04-30
传感器(camera,lidar,IMU,radar)外参标定及在线标定(源码)-多传感器融合感知 ● IMU/GNSS与车体坐标的外参 ● Camera 与 Camera 外参标定 ● Lidar和Camera的外参： ● Lidar和Lidar的外参 ● Lidar和IMU/GNSS的外参...
VINS 初始化 ceres计算重投影误差
JH_233的博客
05-03 3685
详细分析一下VINS在初始化的时候对计算出的相机位姿和特征点的3D坐标用ceres进行最小化重投影误差的操作。参考资料：【1】https://www.jianshu.com/p/e5b03cf22c80【2】http://www.ceres-solver.org/index.html【3】http://www.cnblogs.com/decade-dnbc66/p/5347088.html推荐先看...
ceres UnitQuaternionRotatePoint 利用单位四元数旋转向量的函数算法原理推导
大橙子的博客
05-15 853
背景 算法效率的提升需要扣细节，每个函数，每个过程都需知道其流程 算法原理推导 改进 原算法 template <typename T> inline void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) { const T t2 = q[0] * q[1]; const T t3 = q[0] * q[2]; const T t4 = q[0] * q[3]; const T
Lidar_imu自动标定
m0_48748418的博客
07-24 2458
Lidar imu间的外参标定
Lidar与IMU标定——lidar_imu_calib
Laney_Midory的博客
08-31 4254
我是文章的搬运工 详细介绍以及编译报错解决方法在这篇文章 这里贴一下我遇到的问题： 定义冲突问题 error: conflicting declaration ‘typedef struct LZ4_stream_t LZ4_stream_t’ typedef struct { long long table[LZ4_STREAMSIZE_U64]; } LZ4_stream_t; 这个原因是ROS版本下有两个头文件定义发生冲突，github的issue中给出了两种解决办法，之一是重命名头文件避免冲突： s
lidar_imu_calib标定过程
06-28
### 回答1： 这个问题翻译成中文是：b'lidar_imu_calib标定过程'。 回答：lidar_imu_calib是指激光雷达和惯性测量单元（IMU）之间的标定过程，通过该过程可以解决激光雷达和IMU在安装中的相对姿态和时间同步问题。该过程需要收集一些基准数据，例如IMU的加速度计和陀螺仪数据，以及激光雷达的点云数据。然后将这些数据引入标定算法中，得到激光雷达和IMU之间的相对位姿和时间偏差，最后将它们纠正并同步，从而使系统达到更高的精度。 ### 回答2： Lidar_imu_calib是一种激光雷达和惯性测量单元的联合标定方法。它旨在从激光雷达的数据和惯性传感器的数据中获取相机、激光雷达和车辆的位姿（即位置和姿态）信息。 lidar_imu_calib标定过程可以分为以下几步： 1.采集数据：首先需要采集车辆在各种不同的姿态和运动条件下的数据，包括车速变化、车辆俯仰和横滚角变化等。同时，需要记录激光雷达和惯性传感器的输出数据。 2.匹配点云和IMU数据：利用系统时间戳进行点云数据和IMU数据的对齐，通过坐标系变换将两者的数据进行匹配。 3.计算位姿：根据匹配后的数据，计算车辆的位姿，包括车辆位置和姿态（即旋转角度），这是通过解决非线性优化问题来完成的。 4.评估误差：标定结果需要进行评估，通过比较计算出的车辆真实姿态和标定结果之间的差异来确定标定的准确性。 5.优化标定结果：根据评估结果进行标定结果的优化，即根据误差来调整标定结果，以提高标定准确性。 总之，lidar_imu_calib标定是激光雷达与惯性测量单元联合标定的方法，通过匹配点云和IMU数据，计算位姿，评估误差和优化标定结果等步骤，得到车辆的位姿信息，从而提高自动驾驶车辆的安全性和性能。 ### 回答3： Lidar_imu_calib是一种遥感设备，由激光雷达和惯性测量单元(IMU)组成，它的目的是3D空间中的可视化地图构建。Lidar可以测量环境中物体的位置和形状，而IMU可以测量设备的位置和运动状态。因此，Lidar_imu_calib的精度和准确性对于它的应用非常重要。为此，在使用Lidar_imu_calib之前，必须进行标定。 Lidar_imu_calib标定过程试图确定几何和姿态转换矩阵，这个矩阵用于将Lidar和IMU测量结果在同一个坐标系下进行配置。其中，几何变换矩阵被用来纠正从Lidar和IMU获得的点云数据中发生的误差，姿态变换矩阵则用于纠正导致视角变化的角度问题。 标定过程的首要步骤是采集数据，包括Lidar和IMU的原始数据以及因为设备不同而引起的差异。通过在一定时间内在多个场景下对数据进行采集，可以获得更加丰富的数据，并确保标定能够在多种条件下表示准确。 数据采集之后，接下来需要进行数据处理。主要是通过使用非线性最小二乘法以最小化两个矩阵的几何和姿态误差。 这个过程需要大量的计算能力和优秀的算法以最大化标定的准确度。最终的标定参数是由几何和姿态矩阵的组合产生的，并被应用到Lidar_imu_calib设备以及期望的应用程序中。 总之，Lidar_imu_calib标定是一项复杂的过程，它需要充分的数据采集、数据处理和优秀的算法来确保标定结果的准确性和精度。

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

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

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值