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

已于 2022-08-30 10:18:56 修改
阅读量506 收藏 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外参标定工具lidar_align
qq_33287871的博客
07-26 1519
多传感融合 多传感融合是目前移动机器人、无人驾驶、带Marker AR等前沿人工智能技术的重要解决方案和发展方向之一。由于上述技术都不可避免地涉及在欧式空间定位这一基本感知问题，本系列涉及的传感器就包括了惯性位姿传感器imu/gps/ins、测地激光雷达lidar、摄像机camera等等传感器，会介绍一些标定各传感器内外参数的工具。 多数工具会提供其Github链接，并对其适用情况和局限性进行评述。 传感器内参与外参 标定传感器时必须要遵守的一大原则是：用精度高的测量规范对精度低的测量规范进行校正，例如：量
lidar_imu_calib:自动校准3D激光雷达和IMU外在性
04-01
lidar_imu_calib 概述 在基于3D激光雷达的slam开发中，我们经常使用imu为匹配算法（icp，ndt）提供先验，因此需要校准激光雷达和imu之间的变换。对于匹配算法，transfom中的姿态比变换中的位置更重要，并且通常将位置设置为0。因此，此回购专注于在激光雷达和imu之间的转换中校准姿态分量。 先决条件 将ndt_omp安装到系统目录 git clone https://github.com/koide3/ndt_omp.git cd ndt_omp mkdir build cd build cmake .. make sudo make install 步 使用rosbag工具记录imu和激光雷达数据 rosbag record /imu /lidar_points 配置启动文件 lidar_topic: lidar data topic name imu_to
Lidar_imu自动标定
m0_48748418的博客
07-24 2612
Lidar imu间的外参标定
LiDAR-IMU标定一把搞定！开源GRIL-Calib：妙用地平面运动约束
最新发布
CV_Autobot的博客
05-20 358
作者|hlhsugoi 编辑| 3D视觉之心点击下方卡片，关注“自动驾驶之心”公众号戳我-> 领取自动驾驶近15个方向学习路线>>点击进入→自动驾驶之心『传感器标定』技术交流群本文只做学术分享，如有侵权，联系删文写在前面摘要：随着IMU‑LiDAR融合系统重要性的增加，无目标IMU‑LiDAR外在校准方法越来越受到关注。现有的校准方法需要在所有轴上完全运动的情况下推导校准...
Lidar_imu自动标定源码阅读（二）——calibration部分
m0_48748418的博客
08-15 996
Lidar_imu自动标定源码阅读
关于Lidar-IMU标定
u011906844的博客
12-13 732
1.【学习总结】Lidar与IMU标定_larrydong的博客-CSDN博客_imu lidar
lidar_IMU_calib:基于连续时间批量估计的LiDAR-IMU系统的无目标标定
03-21
LI-Calib是用于校准6DoF刚性变换和3D LiDAR与IMU之间的时间偏移的工具包。它基于连续时间的批次优化。基于IMU的成本和LiDAR点到surfel距离共同最小化，这使得校准问题在一般情况下受到了很好的约束。 先决条件 ...
浙大开源的Lidar IMU标定工具，适配了自己的数据集。.zip
12-29
从压缩包子文件的文件名称“lidar_IMU_calib-main”来看，这可能是项目的主代码库或者主要执行文件，可能包含以下组件： 1. **标定算法**：这部分代码实现LiDAR与IMU之间的参数标定，可能包括特征点匹配、误差模型...
lidar_align标定代码
06-15
"lidar_align标定代码"是由瑞士苏黎世联邦理工大学（ETH Zurich）的自动驾驶实验室开发的一个工具，用于实现LiDAR（Light Detection and Ranging）与其它传感器，如相机或IMU（惯性测量单元）之间的几何对齐。...
multi_lidar_calibrator.zip
08-09
多激光雷达标定，配准。基于NDT算法的多激光雷达外参标定的方法和实践。 工程代码是在ROS中间件下完成的。 内涵详细的算法说明和编码流程。 使用的编码语言为C++. 关于激光雷达标定的其他资源: [毫米波雷达与摄像头...
eigen和ceres中的四元数
qq_42731705的博客
04-19 2017
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 \
ceres学习总结
dna葡萄糖的博客
06-22 1156
ceres是用来求解优化问题的工具库。 使用时至少需要构建目标函数，优化变量。 如果自动求导困难，则需要给出雅克比矩阵。（一般是采用了第三方的库，比如eigen和sophus的一些运算） 如果优化变量不对加法封闭，则需要给出优化变量的更新方法。 ============================================================================== 对于简单优化问题总体流程：（可以采用自动求导） 1、构建代价函数模型，描述如何计算残差 2、构
Ceres使用
小红鱼的博客
11-09 4278
定义：ceres是一款非线性优化问题的数值求解器。 1. CostFunction 输入为自变量，输出为残差。首先要定义一个用于求解残差的struct，并在内定义重载函数operator()： template bool operator() (const T* const x, T* residual ) const 2. LocalParameterization 输入为全局参数，输出...
激光雷达和IMU联合标定并运行LIOSAM
cyx610481953的博客
03-27 2万+
标定激光雷达和IMU并运行LIOSAM 激光雷达和IMU（我用的是Velodyne和xsense传感器）标定这块我尝试了许多方法，钻研了好几天包括： （1）浙江大学开源的方案。 （2）通过我之前对激光雷达和相机联合标定得到的变换矩阵和相机与IMU联合标定得到的变换矩阵相乘 （3）lidar-align（里面代码需要自己改写） 但是最后还是通过lidar-align标定成功了，并且将获得参数用到LIOSAM中（通过自己录制数据集跑出效果）。下面我一一介绍我标定过程中遇到的问题，希望对你有帮助。 1.浙江大学开
pycocotools库的pip安装（windows10+ubuntu18.04）
重点记录机器人和自动驾驶相关领域的技术，尤其是计算机视觉，视觉SLAM等，不仅有前言文献或必备算法的学习，也会有实际应用的点滴经验
09-04 4867
文章目录1.bug2. 解决办法 1.bug 今天想在windows训练Faster_RCNN网络，用到的数据集是coco2012数据集，结果出现bug： 没有安装pycocotools库，于是想cmd中直接输入： pip install pycocotools 安装失败，也试了： pip install pycocotools-win 安装也失败，这下就尴尬了。。。 2. 解决办法 网上说要先装vc++ 14.0及以上，而这个我确信装了（已安装vs2017），应该不是这个问题，于是尝试cmd中输入：
ceres UnitQuaternionRotatePoint 利用单位四元数旋转向量的函数算法原理推导
大橙子的博客
05-15 891
背景 算法效率的提升需要扣细节，每个函数，每个过程都需知道其流程 算法原理推导 改进 原算法 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外参标定工具lidar_align
热门推荐
miracle629的博客
02-21 2万+
多传感融合内外参标定工具介绍（一）：lidar-imu外参标定工具lidar_align多传感融合传感器内参与外参lidar-imu外参标定工具lidar_align外参标定与laser slam的关系使用方法与局限挖坑 多传感融合 多传感融合是目前移动机器人、无人驾驶、带Marker AR等前沿人工智能技术的重要解决方案和发展方向之一。由于上述技术都不可避免地涉及在欧式空间定位这一基本感知问题，...
激光雷达和IMU标定
lpj822的专栏
01-12 1万+
激光雷达和IMU标定激光雷达简介IMU简介算法的数学原理四元数ENU坐标系三维旋转欧拉角(Euler Angle)与旋转矩阵(Rotation Matrix)四元数(Quaternion)与旋转矩阵四元数与欧拉角三维空间的位姿描述和齐次变换优化器标定方法标定流程图 激光雷达简介 激光雷达，即LiDAR（Light Detection and Ranging），是激光探测及测距系统的简称，另外也称Laser Radar或LADAR（Laser Detection and Ranging）。激光雷达可以探测目标
激光雷达与组合惯导联合标定--方案二（matlab）
xx970829的博客
03-22 4458
目录原理matlab代码 【说明】：该方法是自己通过坐标系转换关系建立方程并用matlab求解的，精度不一定满足要求，可能有很多不足之处，仅供参考。 方案一：https://blog.csdn.net/xx970829/article/details/115072158 原理 按照方案一，在实际采集数据时比较困难，纸箱或车等标志物在点云中都不够明显，若要良好效果需要以一栋小平房的一角来做角点，于是提出了方案二：通过地面或台阶一条明显的点云线在地图坐标系中建立一条空间直线方程，在不同时刻，不同帧之间对应雷达坐
lidar_imu_calib标定过程
06-28
这个问题翻译成中文是：b'lidar_imu_calib标定过程'。 回答：lidar_imu_calib是指激光雷达和惯性测量单元（IMU）之间的标定过程，通过该过程可以解决激光雷达和IMU在安装中的相对姿态和时间同步问题。该过程需要...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值