Lecture de papier Étalonnage sans cible du système LiDAR-IMU basé sur l'optimisation par lots en temps continu (y compris l'interprétation du code)

Lien original :  https://arxiv.org/pdf/2007.14759v1.pdf

Adresse de code GitHub - APRIL-ZJU/lidar_IMU_calib : étalonnage sans cible du système LiDAR-IMU basé sur l'estimation de lots en temps continu

Résumé : L'étalonnage des capteurs est le module de base de la fusion multi-capteurs. Cet article élimine un système d'étalonnage LIDAR-IMU précis pour calibrer les paramètres externes à 6 degrés de liberté du LIDAR 3D et de l'IMU. Compte tenu de la fréquence élevée des données LIDAR et IMU, LI-Calib adopte l'équation de trajectoire en temps continu basée sur B-Spline . Par rapport à la méthode basée sur le temps discret, cette méthode est plus adaptée à l'intégration de la synchronisation haute fréquence et non temporelle. systèmes. LI-Calib décompose l'espace en cellules puis identifie les segments planaires dans l'association de données. Cela structure le problème d'étalonnage en un problème bien contraint, qui est également applicable dans des environnements sans marqueurs artificiels. La méthode proposée a été vérifiée à la fois dans des données de simulation et dans des données environnementales réelles, et les résultats ont montré une grande précision et une bonne répétabilité dans un environnement de fourgonnette général.

je cite

        La fusion multicapteurs constitue une direction de développement importante dans le domaine de la robotique. Le LIDAR est largement utilisé en raison de sa grande précision, de sa robustesse et de sa fiabilité, notamment pour le positionnement, la cartographie sémantique, ainsi que le suivi et la détection de cibles. Cependant, une image de points LiDAR 3D est obtenue à des moments différents, ce qui entraîne une distorsion du mouvement. Afin de supprimer la distorsion, l'IMU en tant qu'estimation d'auto-pose à haute fréquence est largement utilisée. De manière générale, les systèmes LiDAR-IMU sont plus fiables pour différents environnements.

        La précision de l'auto-évaluation, de la localisation, de la cartographie et de la navigation dépend fortement de la précision de l'étalonnage entre les capteurs. La mesure manuelle de la rotation relative et de la translation n’est ni assez précise ni pratique. Pour le problème d'étalonnage LiDAR-IMu, il convient de noter que les points de retard dans une image de balayage laser sont échantillonnés à différents moments et que leurs poses exactes dans le système de coordonnées LiDAR sont liées à la position LiDAR au moment de leur réception. L'estimation de la pose pour chaque point est nécessaire pour supprimer la distorsion. Les méthodes basées sur le temps discret utilisent l'interpolation entre les poses d'images clés, ce qui sacrifie la précision des scènes très dynamiques. [7] ont utilisé la régression du processus gaussien pour interpoler les observations IMU, résolvant partiellement le problème des points IMU de sous-précision et LiDAR haute fréquence. Le système caméra-IMU-LiDAR proposé par [8] utilise une méthode en deux étapes basée sur une estimation par lots en temps continu. Le système caméra-IMU est d'abord calibré à l'aide d'un échiquier, puis le lidar à faisceau unique est calibré sur la base du système caméra-IMU.

        Inspirés de [8], nous proposons une méthode d'étalonnage LiDAR-IMU précise utilisant un laps de temps continu, qui est également une extension de nos travaux précédents [9]. Les trajectoires sont paramétrées à l'aide de fonctions de base temporelle. Cette méthode basée sur le temps continu permet d'obtenir la position à tout moment sur la trajectoire, ce qui est très adapté à l'étalonnage des capteurs d'observation haute fréquence. De plus, étant donné que les mesures de vitesse angulaire et d'accélération de l'IMU sont obtenues par échantillonnage différentiel de la fonction de base de temps, la coopération entre cette équation de temps continu et l'IMU est également naturelle. Pour résumer les contributions :

        1) Proposer un système d'étalonnage LiDAR-IMU de haute précision et reproductible basé sur une optimisation par lots en temps continu sans avoir besoin de capteurs supplémentaires ou de marqueurs spécialement conçus.

        2) Proposer une nouvelle méthode pour le problème d'étalonnage LiDAR-IMU basée sur des trajectoires en temps continu . Les résidus introduits par les données IMU originales et la distance entre les points LiDAR et les bacs sont minimisés dans l'équation d'optimisation par lots en temps continu.

        3) Les données de simulation et les données minières réelles ont obtenu de bons résultats. Et le système est open source, qui est également le premier outil d’étalonnage 3D LiDAR-IMU open source.

III Méthode de représentation de trajectoire et indices

        {I} représente le système IMU, {L} représente le système LiDAR et {M} représente le système cartographique, qui est le même que le système de coordonnées LiDAR de la première image {L0} lors de l'étalonnage. Représente la conversion LiDAR en IMU.

        Nous utilisons B-Spline pour paramétrer les trajectoires car il fournit une différenciation analytique de forme fermée, ce qui peut simplifier la fusion de données d'observation à haute fréquence lors de l'estimation d'état. B-Spline a également une bonne contrôlabilité locale, ce qui signifie que la mise à jour d'un seul point de contrôle n'affecte que plusieurs éléments adjacents sur la spline. Cette propriété produit des systèmes clairsemés utilisant des points de contrôle limités. Il existe plusieurs façons d'utiliser B-Spline pour représenter la régression du mouvement d'un corps rigide. [21][22] utilisent une spline pour paramétrer la pose sur SE(3), et [23][24] utilisent deux splines pour représenter respectivement la translation et la rotation sur SE(3) . [24] ont conclu que la forme de l'articulation peut être à couple minimal, mais qu'il est difficile de contrôler la forme de la courbe de translation car la translation et la rotation sont étroitement couplées. Et la rotation et la translation couplées sont difficiles à résoudre dans notre problème de calibrage, nous choisissons donc de paramétrer la trajectoire en représentant séparément la translation et la rotation .

        La formule récursive de Cox-de Boor [25] définit l'équation de base de B-Spline, [26][27] utilise commodément des équations matricielles pour représenter les splines. Lorsque les nœuds du B-Spline sont uniformément répartis, le B-Spline peut synchroniser son degré pour être entièrement défini. En particulier, pour une B-Spline uniforme de d degrés, la translation dans la période de temps est contrôlée par le point de contrôle correspondant . La forme matricielle de la B-Spline uniforme peut être exprimée comme

                                                

                                             

        La vitesse et l'accélération sont

         Le code correspondant dans kontiki est lidar_IMU_calib/thirdparty/Kontiki/include/kontiki/trajectories/uniform_r3_spline_trajectory.h

Result Evaluate(T t, int flags) const override {
    auto result = std::make_unique<TrajectoryEvaluation<T>>(flags);

    int i0;
    T u;
    this->CalculateIndexAndInterpolationAmount(t, i0, u);
    // std::cout << "t=" << t << " i0=" << i0 << " u=" << u << std::endl;

    const size_t N = this->NumKnots();
    if ((N < 4) || (i0 < 0) || (i0 > (N - 4))) {
      std::stringstream ss;
      ss << "t=" << t << " i0=" << i0 << " is out of range for spline with ncp=" << N;
      throw std::range_error(ss.str());
    }

    Vector4 Up, Uv, Ua; // 插值函数的u矩阵
    Vector4 Bp, Bv, Ba; //u^T*M矩阵，第i个元素代表u^T和M矩阵的第i列相乘的结果
    T u2, u3;
    Vector3 &p = result->position;
    Vector3 &v = result->velocity;
    Vector3 &a = result->acceleration;
    T dt_inv = T(1)/this->dt();

    if (result->needs.Position() || result->needs.Velocity())
      u2 = ceres::pow(u, 2);
    if (flags & EvalPosition)
      u3 = ceres::pow(u, 3);

    if (result->needs.Position()) {
      Up = Vector4(T(1), u, u2, u3); 
      Bp = Up.transpose()*M.cast<T>();
      p.setZero();
    }

    if (result->needs.Velocity()) {
      Uv = dt_inv*Vector4(T(0), T(1), T(2)*u, T(3)*u2); // (u^T)'/t
      Bv = Uv.transpose()*M.cast<T>();
      v.setZero();
    }

    if (result->needs.Acceleration()) {
      Ua = ceres::pow(dt_inv, 2)*Vector4(T(0), T(0), T(2), T(6)*u); // (u^T)''/(t^2)
      Ba = Ua.transpose()*M.cast<T>();
      a.setZero();
    }

    // 4个控制点
    for (int i = i0; i < i0 + 4; ++i) {
      Vector3Map cp = this->ControlPoint(i); // 每个控制点位置

      if (flags & EvalPosition)
        p += Bp(i - i0)*cp; // 对位置加权

      if (flags & EvalVelocity)
        v += Bv(i - i0)*cp; // 对速度加权

      if (flags & EvalAcceleration)
        a += Ba(i - i0)*cp;

    }

    // This trajectory is not concerned with orientations, so just return identity/zero if requested
    if (result->needs.Orientation())
      result->orientation.setIdentity();
    if (result->needs.AngularVelocity())
      result->angular_velocity.setZero();

    return result;
  }

        Représente la jième colonne de la spline , qui dépend uniquement du degré de la B-Spline moyenne. La B-spline cubique est utilisée dans l'article, puis                

                                                

        Transformez (1) pour obtenir

                                

        Alors la matrice correspondante

                                                

        Compréhension de (1)(2)(3)(4) : algorithme d'ajustement de courbe B-spline cubique_Blog de Yiying丶-blog CSDN_ajustement de courbe b-spline

        On constate que les résultats matriciels sont les mêmes que ceux du  blog

         De la même manière, la forme cumulative de (3) est souvent utilisée pour paramétrer SO(3) [23, 24, 27] [28], initialement proposé en utilisant les quaternions unitaires comme points de contrôle. Nous pouvons constater que la forme est très similaire à (3).

        

        La vitesse angulaire de la courbe SO (3) peut être obtenue à partir de [27]. Le code dans konkiti est idar_IMU_calib/thirdparty/Kontiki/include/kontiki/trajectories/uniform_so3_spline_trajectory.h

(La dérivation de cette formule semble un peu compliquée)

Result Evaluate(T t, int flags) const override {
  auto result = std::make_unique<TrajectoryEvaluation<T>>(flags);

  // 因为SO3曲线无法获取位置，速度，加速度，所以置为0
  if (result->needs.Position())
    result->position.setZero();
  if (result->needs.Velocity())
    result->velocity.setZero();
  if (result->needs.Acceleration())
    result->acceleration.setZero();

  // Early return if we shouldn't calculate rotation components
  if (!result->needs.AnyRotation()) {
    return result;
  }

  // Since angular velocity computations implicitly requires orientation computations
  // we can henceforth assume do_orientation=true

  int i0; // 相距最近的前一个结点
  T u; // 与相距最近的前一个结点的距离
  this->CalculateIndexAndInterpolationAmount(t, i0, u);

  const size_t N = this->NumKnots();
  if ((N < 4) || (i0 < 0) || (i0 > (N - 4))) {
    std::stringstream ss;
    ss << "t=" << t << " i0=" << i0 << " is out of range for spline with ncp=" << N;
    throw std::range_error(ss.str());
  }

  Vector4 U, dU;
  Vector4 B, dB;
  T u2 = ceres::pow(u, 2);
  T u3 = ceres::pow(u, 3);
  T dt_inv = T(1) / this->dt();

  U = Vector4(T(1), u, u2, u3);
  B = U.transpose() * M_cumul.cast<T>();

  if (result->needs.AngularVelocity()) {
    dU = dt_inv * Vector4(T(0), T(1), T(2) * u, T(3) * u2);
    dB = dU.transpose() * M_cumul.cast<T>();
  }

  Quaternion &q = result->orientation;

  // These parts are updated when computing the derivative
  Quaternion dq_parts[3] = {
   
   {T(1), T(0), T(0), T(0)},
                            {T(1), T(0), T(0), T(0)},
                            {T(1), T(0), T(0), T(0)}};

  q = this->ControlPoint(i0);
  const int K = (i0 + 4);
  for (int i=(i0 + 1); i < K; ++i) {
    // Orientation
    QuaternionMap qa = this->ControlPoint(i - 1);
    QuaternionMap qb = this->ControlPoint(i);
    Quaternion omega = math::logq(qa.conjugate() * qb);
    Quaternion eomegab = math::expq(Quaternion(omega.coeffs() * B(i-i0)));
    q *= eomegab;

    // Angular velocity
    // This iterative scheme follows from the product rule of derivatives
    if (result->needs.AngularVelocity()) {
      for (int j = (i0 + 1); j < K; ++j) {
        const int m = j - i0 - 1;
        if (i==j) {
          dq_parts[m] *= Quaternion(omega.coeffs()*dB(i-i0));
        }
        dq_parts[m] *= eomegab;
      }
    }
  }

  if (result->needs.AngularVelocity()) {
    Quaternion dq = this->ControlPoint(i0) * Quaternion(dq_parts[0].coeffs() + dq_parts[1].coeffs() + dq_parts[2].coeffs());
    result->angular_velocity = math::angular_velocity(q, dq);
  }

  return result;
}

        Dans ce système d'étalonnage, nous utilisons les équations (1) (5) pour paramétrer la pose continue de l'IMU 6-DOF . La dérivée de la courbe spline par rapport au temps est facile à trouver [27] , alors l'accélération linéaire et la vitesse angulaire dans le système de coordonnées IMU local sont exprimées comme suit :

         

        

        Dans l’équation ci-dessus, et sont identiques. Nous utilisons le système de coordonnées IMU de la première image comme système de référence de la trajectoire IMU comme vecteur de gravité dans le système de coordonnées IMU.

Méthode IV

        Le pipeline de l'ensemble du système est présenté sur la Fig.2. Tout d’abord, les paramètres externes de rotation du système lidar au système IMU sont initialisés en alignant la rotation de l’IMU sur la rotation du LiDAR (voir IV-A), où la rotation du LiDAR est obtenue par l’odométrie basée sur les CND. Une fois l'initialisation terminée, nous pouvons supprimer partiellement la distorsion de mouvement dans le balayage LiDAR et obtenir une meilleure estimation de la pose LiDAR à partir de l'odométrie LiDAR. La carte de casiers LiDAR utilise l'initialisation de pose LiDAR (voir IV-B), tout comme la correspondance point à casier. Ensuite, une optimisation en temps continu basée sur l'optimisation par lots est effectuée sur les mesures LiDAR et IMU pour estimer les quantités d'état (y compris les paramètres externes) (voir IV-C). Enfin, avec la meilleure estimation actuelle de l'optimisation, nous mettons à jour la carte des bacs, l'association de données point à plan et l'état de l'estimation de l'optimisation de zone (voir IV-D).

 A. Initialisation des paramètres externes tournants

        Inspirée de [30], notre porte initialise les paramètres extrinsèques de rotation en alignant les séquences de rotation du LiDAR et de l'IMU. Sachant qu'une série de mesures de vitesse angulaire sont obtenues à partir du capteur IMU , nous pouvons ajuster la courbe B-spline rotative basée sur l'équation (5) . Une série de points de contrôle de quaternions de la courbe B-spline ajustée peut être résolue en résolvant le minimum suivant Le problème du carré est résolu :

        

        où est la vitesse angulaire mesurée par l'IMU à ce moment-là. Il est à noter que l’optimisation est calée sur le quaternion unité. Dans l'équation (8), nous essayons d'ajuster la courbe B-spline de l'équation de rotation à travers les données IMU originales , plutôt que la pose relative obtenue par l'intégration IMU. Cette dernière n'est pas assez précise et est facilement affectée par le bruit et le zéro. biais de l’IMU. . 

        Afin d'apprendre à utiliser kontiki, j'ai séparé l'ajustement de la trajectoire IMU (y compris la rotation et la traduction) dans un module distinct du code source, et j'ai utilisé pangolin pour la visualisation. J'ai également utilisé yaml-cpp. Les amis intéressés peuvent y aller Jetez un oeil , l'adresse du code est kontiki_tutorial  . J'apprends encore l'ajout de l'observation lidar.

     En utilisant la correspondance scan2map basée sur l'enregistrement NDT, nous pouvons estimer la pose du scan LiDAR. Il est donc facile d'obtenir la rotation relative entre des images LiDAR consécutives . Dans le même temps, la rotation relative sous le système de coordonnées IMU au cours de la période de temps peut également être facilement obtenue à partir de la courbe B-spline ajustée, c'est-à-dire Alors la rotation relative des deux capteurs à tout k satisfait l'équation suivante (l'ordre de (9) est un peu déroutant) :

        

        En combinant plusieurs observations à différents moments dans une équation surdéterminée

        

        où représente le poids, qui est déterminé par une méthode heuristique pour réduire l'influence des points externes (le calcul dans le code utilise l'angle de rotation lorsque l'angle de l'axe est exprimé, et le seuil est de 1,0) :

        

        

        où est la partie réelle du quaternion, l'équation (10) peut être obtenue à partir du vecteur propre unité droit correspondant à la plus petite valeur singulière.

        Concernant l'ordre d'étalonnage œil-main, j'ai trouvé un morceau de code qui utilise deux temps lidar comme temps IMU, obtient la pose IMU à deux moments via la trajectoire d'ajustement de l'IMU, puis l'obtient via la matrice de transformation relative entre le lidar et l'IMU. La posture relative du lidar correspondant aux deux moments

// 两个时刻LiDAR的相对位姿获取，LiDAR的相对旋转是由IMU的相对旋转和标定矩阵获得
bool TrajectoryManager::evaluateLidarRelativeRotation(double lidar_time1,
        double lidar_time2, Eigen::Quaterniond &q_L2toL1) const {
  assert(lidar_time1 <= lidar_time2
         && "[evaluateRelativeRotation] : lidar_time1 > lidar_time2");

  double traj_time1 = lidar_time1 + lidar_->time_offset();
  double traj_time2 = lidar_time2 + lidar_->time_offset();

  if (traj_->MinTime() > traj_time1 || traj_->MaxTime() <= traj_time2)
    return false;

  Result result1 = traj_->Evaluate(traj_time1, EvalOrientation);
  Result result2 = traj_->Evaluate(traj_time2, EvalOrientation);
  Eigen::Quaterniond q_I2toI1 = result1->orientation.conjugate()*result2->orientation; // 左乘：result1*q_I2toI1 = result2

  q_L2toL1 = calib_param_manager->q_LtoI.conjugate() * q_I2toI1 * calib_param_manager->q_LtoI; // 手眼标定公式
  return true;
}

Il est très difficile         d'initialiser la traduction . Premièrement, l’accélération subie par Yotsuya est couplée au vecteur gravité et à la rotation. Par conséquent, l’erreur d’alignement dans la B-spline pivotée affectera la précision de la B-spline traduite. Et la deuxième différentielle de la courbe B-spline introduira deux éléments 0 dans le vecteur u de l'équation (1), ce qui rend également le point de contrôle insoluble. Par conséquent, l’utilisation de la valeur mesurée de l’IMU pour initialiser la traduction n’est pas fiable, nous ignorons donc l’initialisation des paramètres externes de traduction, qui auront un faible impact sur les résultats d’étalonnage des tests expérimentaux.

        Interprétation du code d'initialisation : principalement les équations (9) (10)

bool InertialInitializer::EstimateRotation(
        TrajectoryManager::Ptr traj_manager,
        const Eigen::aligned_vector<LiDAROdometry::OdomData>& odom_data) {

  int flags = kontiki::trajectories::EvalOrientation;
  std::shared_ptr<kontiki::trajectories::SplitTrajectory> p_traj
          = traj_manager->getTrajectory(); // kontiki优化结果中获取轨迹

  Eigen::aligned_vector<Eigen::Matrix4d> A_vec;
  // 每次取出相邻两个数据
  for (size_t j = 1; j < odom_data.size(); ++j) {
    size_t i = j - 1;
    double ti = odom_data.at(i).timestamp;
    double tj = odom_data.at(j).timestamp;
    if (tj >= p_traj->MaxTime()) // 后一个里程计数据超出时间范围，则跳出
      break;
    auto result_i = p_traj->Evaluate(ti, flags); // 获取对应时刻IMU的ti时刻的姿态
    auto result_j = p_traj->Evaluate(tj, flags);
    Eigen::Quaterniond delta_qij_imu = result_i->orientation.conjugate()
                                       * result_j->orientation;

    Eigen::Matrix3d R_Si_toS0 = odom_data.at(i).pose.topLeftCorner<3,3>();
    Eigen::Matrix3d R_Sj_toS0 = odom_data.at(j).pose.topLeftCorner<3,3>();
    Eigen::Matrix3d delta_ij_sensor = R_Si_toS0.transpose() * R_Sj_toS0; // dT
    Eigen::Quaterniond delta_qij_sensor(delta_ij_sensor);

    // 求解超参
    Eigen::AngleAxisd R_vector1(delta_qij_sensor.toRotationMatrix()); // 转换为轴角形式
    Eigen::AngleAxisd R_vector2(delta_qij_imu.toRotationMatrix());
    double delta_angle = 180 / M_PI * std::fabs(R_vector1.angle() - R_vector2.angle()); // 旋转绝对角度的差值
    double huber = delta_angle > 1.0 ? 1.0/delta_angle : 1.0; // 两个旋转角度差的阈值1.0rad

    Eigen::Matrix4d lq_mat = mathutils::LeftQuatMatrix(delta_qij_sensor); // 构建左乘和右乘矩阵
    Eigen::Matrix4d rq_mat = mathutils::RightQuatMatrix(delta_qij_imu);
    A_vec.push_back(huber * (lq_mat - rq_mat));
  }
  size_t valid_size = A_vec.size();
  if (valid_size < 15) {
    return false;
  }
  Eigen::MatrixXd A(valid_size * 4, 4);
  for (size_t i = 0; i < valid_size; ++i)
    A.block<4, 4>(i * 4, 0) = A_vec.at(i);

  Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen:: | Eigen::ComputeFullV); // 奇异值从大到小排列
  Eigen::Matrix<double, 4, 1> x = svd.matrixV().col(3);
  Eigen::Quaterniond q_ItoS_est(x); // vector4d初始化，实部在后方式一：4个标量

  // Quaterniond q1(1, 2, 3, 4);   // 第一种方式  实部为1 ，虚部234
  // Quaterniond q2(Vector4d(1, 2, 3, 4));  // 第二种方式Vector4d  实部为4 ，虚部123

  Eigen::Vector4d cov = svd.singularValues();

  if (cov(2) > 0.25) { //最小的奇异值>0.25
    q_ItoS_est_ = q_ItoS_est;
    rotaion_initialized_ = true;
    return true;
  } else {
    return false;
  }
}

        Ce qu'il faut noter dans le code, c'est que la méthode de représentation des quaternions est que la partie imaginaire est la première et la partie réelle est la dernière, donc les fonctions de génération matricielle gauche et droite de q sont différentes de la méthode de représentation des quaternions avec la partie réelle en premier. , et les deux sont simplement échangés en diagonale.

// 四元数虚部在前，实部在后，于Quaternion kinematics 的顺序不一样
template<typename T>
inline Eigen::Matrix<T, 4, 4> LeftQuatMatrix(const Eigen::Matrix<T, 4, 1> &q) {
  Eigen::Matrix<T, 4, 4> m;
  Eigen::Matrix<T, 3, 1> vq{q.x(), q.y(), q.z()};
  T q4 = q.w();
  m.block(0, 0, 3, 3) << q4 * I3x3 + SkewSymmetric(vq);
  m.block(3, 0, 1, 3) << -vq.transpose();
  m.block(0, 3, 3, 1) << vq;
  m(3, 3) = q4;
  return m;
}

template<typename T>
inline Eigen::Matrix<T, 4, 4> RightQuatMatrix(const Eigen::Matrix<T, 4, 1> &p) {
  Eigen::Matrix<T, 4, 4> m;
  Eigen::Matrix<T, 3, 1> vp{p.x(), p.y(), p.z()};
  T p4 = p.w();
  m.block(0, 0, 3, 3) << p4 * I3x3 - SkewSymmetric(vp);
  m.block(3, 0, 1, 3) << -vp.transpose();
  m.block(0, 3, 3, 1) << vp;
  m(3, 3) = p4;
  return m;
}

        Les valeurs singulières des résultats svd d'Eigen sont classées du plus grand au plus petit .

        Faites attention à la différence dans la méthode d'initialisation d'Eigen::Quaternion. Le code utilise l'initialisation Vector4d, et la partie réelle du quaternion qu'il représente est à la fin.

Quaterniond q1(1, 2, 3, 4); // La première façon, la partie réelle est 1, la partie imaginaire est 234
Quaterniond q2(Vector4d(1, 2, 3, 4)); // La deuxième façon, la partie réelle de Vector4d est 4, la partie imaginaire est 123

Eigen::Matrix<double, 4, 1> x = svd.matrixV().col(3);
Eigen::Quaterniond q_ItoS_est(x); // vector4d初始化，实部在后方式一：4个标量

        Le jugement du quaternion à résoudre utilise des valeurs singulières. Le seuil gauche de 0,25 emprunté ici est une valeur empirique, qui est la même que VINS-Mono. Généralement, la stabilité des résultats de la solution pour les équations linéaires est liée au numéro de condition de la matrice. Plus le numéro de condition est grand, plus l'influence du bruit sur la solution est grande et plus elle sera instable. Le numéro de condition est le rapport entre la plus grande valeur singulière et la plus petite valeur singulière. (Voir Méthodes de calcul numérique de l'Université Tsinghua)

if (cov(2) > 0.25) { //最小的奇异值>0.25
    q_ItoS_est_ = q_ItoS_est;
    rotaion_initialized_ = true;
    return true;
  } else {
    return false;

          La solution d’estimation optimale pour Ax=0 :

Carte B Bin et association de données

        Grâce à la courbe initiale et à la courbe de rotation, l'IMU peut fournir des prévisions de rotation pour les données d'analyse LiDAR, qui peuvent être utilisées pour supprimer la distorsion de rotation, ce qui améliore la précision de l'odométrie LiDAR et la qualité des cartes de nuages ​​de points LiDAR. Par conséquent, nous allons discrétiser la carte du nuage de points en cellules 3D et calculer l’impulsion du premier et du deuxième ordre des points dans chaque cellule. L'algorithme ndt_omp utilisé dans le code peut obtenir directement la cellule, l'impulsion, les valeurs propres et les vecteurs propres du nuage de points.

plane_lambda_ = 0.7;
surfel_association_->setPlaneLambda(plane_lambda_);
auto ndt_omp = LiDAROdometry::ndtInit(ndt_resolution_);
ndt_omp->setInputTarget(scan_undistortion_->get_map_cloud()); // 将点云输入ndt_omp，借助ndt_omp来计算每个cell的性质
surfel_association_->setSurfelMap(ndt_omp, map_time_);



// 每个格子拟合平面，并且给不同cell的内点不同颜色
void SurfelAssociation::setSurfelMap(
        const pclomp::NormalDistributionsTransform<VPoint, VPoint>::Ptr& ndtPtr,
        double timestamp) {
  clearSurfelMap();
  map_timestamp_ = timestamp;

  //mCellSize = ndtPtr->getTargetCells().getLeafSize()(0);

  // check each cell
  Eigen::Vector3i counter(0,0,0);
  for (const auto &v : ndtPtr->getTargetCells().getLeaves()) { //leaves报保存的是map<int,leaf> leaf保存当前cell的信息
    auto leaf = v.second;

    if (leaf.nr_points < 10) // cell点数要>10
      continue;
    // leaf直接可以获取
    int plane_type = checkPlaneType(leaf.getEvals(), leaf.getEvecs(), p_lambda_); //cell里的特征值和特征向量，用于判断是否为平面
    if (plane_type < 0)
      continue;

    Eigen::Vector4d surfCoeff;
    VPointCloud::Ptr cloud_inliers = VPointCloud::Ptr(new VPointCloud);
    if (!fitPlane(leaf.pointList_.makeShared(), surfCoeff, cloud_inliers)) // cell里的点RANSAC拟合平面，平面参数保存到surfCoeff中，平面内点保存到cloud_inliers中，inlier点数必须大与20
      continue;

    counter(plane_type) += 1; // -1 0 1 2
    // 平面信息保存
    SurfelPlane surfplane;
    surfplane.cloud = leaf.pointList_;
    surfplane.cloud_inlier = *cloud_inliers;
    surfplane.p4 = surfCoeff;
    surfplane.Pi = -surfCoeff(3) * surfCoeff.head<3>();

    // 平面包围框
    VPoint min, max;
    pcl::getMinMax3D(surfplane.cloud, min, max);
    surfplane.boxMin = Eigen::Vector3d(min.x, min.y, min.z);
    surfplane.boxMax = Eigen::Vector3d(max.x, max.y, max.z);

    surfel_planes_.push_back(surfplane);
  }

  spoint_per_surfel_.resize(surfel_planes_.size());

  std::cout << "Plane type  :" << counter.transpose()
            << "; Plane number: " << surfel_planes_.size() << std::endl;

  // 给平面上色
  surfels_map_.clear();
  {
    int idx = 0;
    for (const auto &v : surfel_planes_) {
      colorPointCloudT cloud_rgb;
      pcl::copyPointCloud(v.cloud_inlier, cloud_rgb);

      size_t colorType = (idx++) % color_list_.size(); // 点云颜色
      for (auto & p : cloud_rgb) {
        p.rgba = color_list_[colorType]; // 每个点给不同颜色
      }
      surfels_map_ += cloud_rgb;
    }
  }
}

Les tensioactifs peuvent être identifiés à l’aide des paramètres de similarité plane suivants [32] :

        

        où ce sont les valeurs propres de la matrice de quantité de mouvement du second ordre. Pour les cellules distribuées dans le plan, . Ainsi, si le paramètre de similarité des plans dépasse un seuil (0,6 avant la première optimisation et 0,7 après la première optimisation), on considère les points de cette cellule comme des plans.

        Code correspondant :

int SurfelAssociation::checkPlaneType(const Eigen::Vector3d& eigen_value,
                                      const Eigen::Matrix3d& eigen_vector,
                                      const double& p_lambda) {
  Eigen::Vector3d sorted_vec;
  Eigen::Vector3i ind;
  Eigen::sort_vec(eigen_value, sorted_vec, ind); //sorted_vec由大到小排排列

  double p = 2*(sorted_vec[1] - sorted_vec[2]) /
             (sorted_vec[2] + sorted_vec[1] + sorted_vec[0]);

  if (p < p_lambda) { // 小于阈值，不是平面
    return -1;
  }

  int min_idx = ind[2]; // 最小特征值对应列的特征向量为平面的法向量
  Eigen::Vector3d plane_normal = eigen_vector.block<3,1>(0, min_idx); // 对应列
  plane_normal = plane_normal.array().abs();

  Eigen::sort_vec(plane_normal, sorted_vec, ind);
  return ind[2];
}

        Lorsqu'il est déterminé qu'il s'agit d'un plan, l'algorithme RANSAC est utilisé pour ajuster le plan, et il est obtenu .

// cloud RANSAC拟合平面，平面参数保存到coeffs中，平面内点保存到cloud_inliers中，inlier点数大与20
bool SurfelAssociation::fitPlane(const VPointCloud::Ptr& cloud,
                                 Eigen::Vector4d &coeffs,
                                 VPointCloud::Ptr cloud_inliers) {
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::SACSegmentation<VPoint> seg;    /// Create the segmentation object
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.05);

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () < 20) {
    return false;
  }

  for(int i = 0; i < 4; i++) {
    coeffs(i) = coefficients->values[i];
  }

  pcl::copyPointCloud<VPoint> (*cloud, *inliers, *cloud_inliers);
  return true;
}

Il est important de noter que toutes les cartes planaires et LiDAR sont relatives au système de coordonnées . L'extraction de plans à partir de petites cellules nous permet d'utiliser pleinement toutes les zones planes de l'environnement pour fournir des contraintes plus fiables pour l'optimisation des lots. Afin d'être plus robuste, lorsque la distance du point au plan est supérieure à un seuil, nous écarterons cette contrainte. Afin de réduire la complexité informatique, nous sous-échantillonnons le LiDAR original.

C Optimisation des lots en temps continu

        La quantité d’état estimée du système est

        où et représentent respectivement les points de contrôle de rotation et de translation de la trajectoire temporelle continue décrite en III. est une matrice de rotation bidimensionnelle qui peut aligner l'axe z du système de référence IMU sur la direction du vecteur gravité. On suppose que le biais zéro du compteur de vitesse angulaire et du gyroscope est affecté par le bruit blanc gaussien. D’une manière générale, les problèmes de calibrage peuvent être formulés comme des problèmes d’optimisation de graphes. Étant donné une série de relations entre le point LiDAR L, l'observation d'accélération linéaire A, l'observation de vitesse angulaire W et l'hypothèse selon laquelle toutes les observations ci-dessus sont affectées par un bruit gaussien indépendant.

// add constraints
addGyroscopeMeasurements(estimator_split);
addAccelerometerMeasurement(estimator_split);
addSurfMeasurement(estimator_split, surfels_association);

        Le problème d'estimation de probabilité maximale p(x|L,A,W) peut être résolu en résolvant l'équation des moindres carrés :

        

        Qui représentent respectivement les résidus des observations de l'accéléromètre, du gyroscope et du LiDAR. Représentez respectivement la matrice de variance correspondante. En particulier, le résidu IMU est défini comme :

        

        

        où se trouvent les données de mesure brutes de l'IMU à ce moment-là. Pour le point associé , capturé à un instant donné et associé au plan, la distance du point au plan est :

             

        Nous optimisons l'équation (15) à l'aide de l'algorithme LM et estimons l'état de manière itérative.   

        Voici l'interprétation de la distance d'un point à la surface. Le code est dans lidar_surfel_point.h, comme suit :

        Cette partie est assez exigeante. Nous devons d’abord garder quelques points à l’esprit :

        1. Les courbes B-spline utilisées tout au long de cet article pour s'ajuster sont les trajectoires de l'IMU , et non les trajectoires du lidar. Les deux sont liées par nos paramètres externes d'étalonnage.

        2. L'heure initiale de la carte du nuage de points lidar n'est pas cohérente avec l'heure initiale de l'IMU, mais ce sont toutes deux des matrices unitaires au début, donc si nous obtenons l'heure de la carte map_time_ (l'heure de début de la construction de la carte, c'est l'heure de la carte). pose correspondante de la trajectoire de la carte comme matrice unitaire) et les coordonnées correspondantes sur la trajectoire IMU du temps du point laser (timestamp_). Vous devez également obtenir les coordonnées de la pose de la trajectoire IMU par rapport à timestamp_ à map_time_, afin de pouvoir puis reconvertissez-le pour obtenir les coordonnées du point laser dans le système de coordonnées de la carte.

        Vous trouverez ci-dessous une explication détaillée des étapes :

        Connus : un plan dans la carte du nuage de points, le point laser (système de coordonnées lidar) d'un repère lidar original associé à ce plan, et la trajectoire IMU ;

        Objectif : Obtenir les coordonnées de ce point laser sur la carte du nuage de points pour calculer la distance du point au plan.         

  // 误差函数 将当前点投影到地图中，计算点面误差，这个投影的姿态矩阵使用的是拟合的轨迹得到的位姿
  template<typename TrajectoryModel, typename T>
  Eigen::Matrix<T, 1, 1> reprojectPoint2map(const type::Trajectory<TrajectoryModel, T>& trajectory,
                                            const type::LiDAR<LiDARModel, T>& lidar,
                                            const T* plane_cp) const {
    int flags = trajectories::EvaluationFlags::EvalPosition | trajectories::EvaluationFlags::EvalOrientation;

    auto T_I0toG = trajectory.Evaluate(T(map_time_) + lidar.time_offset(), flags); // 当前激光地图时刻对应的IMU轨迹上的IMU数据的全局位姿，此时对于激光雷达轨迹而言，此时是单位矩阵，所以需要进行抵消
    auto T_IktoG = trajectory.Evaluate(T(timestamp_)+ lidar.time_offset(), flags); // 当前激光点时刻对应的IMU轨迹上的IMU数据的全局位姿

    const Eigen::Matrix<T, 3, 1> p_LinI = lidar.relative_position();
    const Eigen::Quaternion<T>   q_LtoI = lidar.relative_orientation();

    Eigen::Matrix<T, 3, 1> p_Lk = lidar_point_.cast<T>(); // 当前激光雷达的位姿，这里使用的是原始的激光雷达数据
    Eigen::Matrix<T, 3, 1> p_Ik = q_LtoI * p_Lk + p_LinI; // 得到激光雷达点在当前IMU坐标系下的坐标

    Eigen::Matrix<T, 3, 1> p_temp = T_I0toG->orientation.conjugate()*(T_IktoG->orientation * p_Ik + T_IktoG->position - T_I0toG->position); // 激光雷达点在Ik时刻IMU坐guiji1ssd标系下的坐标相对于I0时刻的相对位姿
    Eigen::Matrix<T, 3, 1> p_M = q_LtoI.conjugate() * (p_temp - p_LinI); // 激光雷达在激光雷达世界坐标系（激光雷达地图）下的坐标

#if 1
    Eigen::Matrix<T,3,1> Pi = Eigen::Map<const Eigen::Matrix<T,3,1>>(plane_cp);
    T plane_d = T(Pi.norm()); // plane_d = -d  // Pi = -surfCoeff(3) * surfCoeff.head<3>() = -d*(a,b,c)
    T plane_norm[3];
    plane_norm[0] = T(Pi[0])/plane_d; // a
    plane_norm[1] = T(Pi[1])/plane_d; // b
    plane_norm[2] = T(Pi[2])/plane_d; // c

    T dist = ceres::DotProduct(plane_norm, p_M.data()) - plane_d; // 使用点面距离
#else
    T plane_d = T(plane_[3]);
    T plane_norm[3];
    plane_norm[0] = T(plane_[0]);
    plane_norm[1] = T(plane_[1]);
    plane_norm[2] = T(plane_[2]);
    T dist = ceres::DotProduct(plane_norm, p_M.data()) + plane_d;
#endif

    Eigen::Matrix<T, 1, 1> error(dist);
  //  error.topLeftCorner(1,1) = T(dist);
  //  Vector3 error_vector = dist * p_L / p_L.norm();

    return error;
  }

                                     

 D Ajustement

       Après une optimisation continue des lots, l'état estimé (y compris les paramètres externes) sera plus précis. Par conséquent, nous utilisons l'estimation optimale actuelle pour supprimer la distorsion du scan LiDAR d'origine , reconstruire la carte de surface LiDAR et mettre à jour l'association de données point à surface. Notez que l'enregistrement CND basé sur l'odométrie LiDAR n'est utilisé que lors de l'initialisation de la pose LiDAR et de la carte LiDAR. Des cartes de casiers LiDAR typiques lors de l'optimisation du premier et du deuxième lot sont présentées sur la figure 3. La qualité de la carte a été grandement améliorée après une optimisation. Après seulement quelques itérations (4 dans l’expérience), l’algorithme proposé a convergé et peut donner un résultat d’étalonnage de haute précision.

V expérience

        Nous avons conçu des expériences avec des capteurs simulés et réels. En pratique, LiDAR et IMU utilisent la synchronisation matérielle [33], et nous ne nous soucions que des résultats d'étalonnage spatial des deux capteurs. Dans le code, nous utilisons Kontiki [34] pour l'optimisation des lots en temps continu.

        Le décalage temporel est toujours estimé dans le code, c'est-à-dire qu'en ajoutant la distance résiduelle du point à la surface, et en trouvant la pose du point correspondant au temps, un décalage est ajouté au temps. Il est entendu ici que B-Spline rend la trajectoire différentiable, puis le gradient peut être calculé, qui peut ensuite être optimisé. Ici, je dois expliquer les résultats de mon expérience d'étalonnage du retard : j'ai utilisé les données de synchronisation temporelle fournies et ajouté manuellement un décalage de 50 ms. Cependant, après trois itérations, le décalage temporel calibré n'était que de 6 ms. Je ne sais pas si c'est le cas. . Il y a un problème avec mon fonctionnement, ou le retard calibré par cette méthode n'est pas très précis. J'espère que les étudiants intéressés pourront le partager ensemble.

auto T_I0toG = trajectory.Evaluate(T(map_time_) + lidar.time_offset(), flags); // 当前激光地图时刻对应的IMU轨迹上的IMU数据的全局位姿，此时对于激光雷达轨迹而言，此时是单位矩阵，所以需要进行抵消
auto T_IktoG = trajectory.Evaluate(T(timestamp_)+ lidar.time_offset(), flags); // 当前激光点时刻对应的IMU轨迹上的IMU数据的全局位姿

        Lors de la collecte réelle de données, la vitesse angulaire absolue moyenne de toutes les séquences de données est de 47,39 degrés/s et l'accélération est de 0,628 . Afin d'obtenir des résultats précis, nous devons nous assurer qu'il y a suffisamment d'accélération linéaire et d'excitation de vitesse angulaire dans toutes les données , et que la distribution spatiale des nœuds de la spline est définie sur 0,02 seconde pour tenir compte d'un mouvement hautement dynamique. Évaluation de trajectoire à l'aide d'ATE

Organiser certains numéros sur VI github

        1. Ce code peut-il être utilisé pour calibrer le lidar 2D et l'IMU ?

            Oui, actuellement seules les étapes d'initialisation et d'association des données utilisent l'odométrie CND, elle ne peut pas être utilisée pour les radars 2D. Cependant, le raffinement n'a rien à voir avec le nombre de lignes lidar, il est donc recommandé de modifier l'odométrie lidar.

        2. Analyse d'observabilité

            sur la base de l'observabilité de l'étalonnage extrinsèque IMU-LiDAR [1,2], au moins deux rotations autour d'axes différents sont nécessaires. Pendant ce temps, les grandes accélérations et vitesses angulaires rendent l'estimation plus précise, mais vous devez vous assurer que l'erreur de l'odométrie LiDAR se situe dans une certaine plage sous ce mouvement sévère.

        [1] LIC-Fusion 2.0  https://arxiv.org/pdf/2008.07196.pdf  (Section IV. ANALYSE D'OBSERVABILITÉ)
        [2] Hesch, Joel. (2016). Analyse de cohérence et amélioration de la navigation inertielle assistée par vision. Récupéré de l'Université