526互联

通过数值求导的方式对VIO系统中的位姿和3D点的雅可比矩阵进行验证

发布时间 2023-03-26 13:22:40作者: weihao_ysgs

如何通过数值计算的方式对VIO系统中的求导结果进行验证

  • 验证位姿求导是否正确的C++代码
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <iostream>

#include "sophus/se3.hpp"
#include "sophus/so3.hpp"

using namespace std;
Eigen::Vector2d reprojectError(Eigen::Vector3d& P, Eigen::Quaterniond& q,
                               Eigen::Vector3d& t, Eigen::Matrix3d& K,
                               Eigen::Vector2d& obs)
{
  Sophus::SE3d pose(q, t);
  Eigen::Vector3d P_cam = pose * P;
  Eigen::Vector3d P_cam_ = q * P + t; // P_can is same as P_cam_
  P_cam = K * P_cam;
  Eigen::Vector3d P_cam_norm = P_cam / P_cam.z();
  Eigen::Vector2d proj = P_cam_norm.head<2>();
  return obs - proj;
}

// 计算相机位姿的雅可比矩阵
void computePoseJacobianEps(Eigen::Vector3d& P, Eigen::Quaterniond& q,
                            Eigen::Vector3d& t, Eigen::Matrix3d& K,
                            Eigen::Vector2d& obs,
                            Eigen::Matrix<double, 2, 6>& J)
{
  const double eps = 1e-5;
  J.setZero();

  Eigen::Quaterniond q_temp;
  Eigen::Vector3d t_temp;
  for (int i = 0; i < 6; ++i)
  {
    Eigen::Vector3d delta;
    delta.setZero();
    delta(i % 3) = eps;

    if (i < 3)
    {
      /// 通过 Sophus 对 q 扰动
      // Sophus::SO3d SO3_R(q);
      // Sophus::SO3d SO3_updated = Sophus::SO3d::exp(delta) * SO3_R;
      // q_temp = SO3_updated.unit_quaternion();

      /// 通过四元数对旋转进行扰动
      Eigen::Matrix<double, 3, 1> half_theta = delta;
      half_theta /= static_cast<double>(2.0);
      Eigen::Quaternion<double> dq(1.0, half_theta.x(),
                                   half_theta.y(), half_theta.z());
      q_temp = (q * dq);

      t_temp = t;
    }
    else
    {
      q_temp = q;
      t_temp = t + delta;
    }

    Eigen::Vector2d residual_perturbed, residual;
    residual_perturbed.setZero();
    residual.setZero();
    residual = reprojectError(P, q_temp, t_temp, K, obs);

    residual_perturbed = reprojectError(P, q, t, K, obs);
    if (i > 2)
    {
      J.col(i - 3) = (residual - residual_perturbed) / eps;
    }
    else
    {
      J.col(i + 3) = (residual - residual_perturbed) / eps;
    }
  }
}

void computePoseJacobian(Eigen::Vector3d& P_w, Eigen::Quaterniond& Q_w,
                         Eigen::Vector3d& t_w, Eigen::Matrix3d& K,
                         Eigen::Vector2d& obs, Eigen::Matrix<double, 2, 6>& J)
{
  double fx = K(0, 0);
  double fy = K(1, 1);
  Eigen::Vector3d pc = (Q_w * P_w + t_w);
  Eigen::Vector2d pt_cam_proj = ((K * pc) / pc.z()).head<2>();
  double inv_z = 1.0 / pc[2];
  double inv_z2 = inv_z * inv_z;

  J << -fx * inv_z, 0, fx * pc[0] * inv_z2, fx * pc[0] * pc[1] * inv_z2,
      -fx - fx * pc[0] * pc[0] * inv_z2, fx * pc[1] * inv_z, 0, -fy * inv_z,
      fy * pc[1] * inv_z2, fy + fy * pc[1] * pc[1] * inv_z2,
      -fy * pc[0] * pc[1] * inv_z2, -fy * pc[0] * inv_z;
}

void test1()
{
  Eigen::Quaterniond q(1.0, 0, 0, 0);
  Eigen::Vector3d pts_w(-0.0374123267, -0.830815672, 2.74480009);
  Eigen::Vector3d t(0, 0, 0);
  Eigen::Vector2d obs_measure(323, 109);
  //  J_real << -189.777, 0, -2.58671, 2.14908, -520.997, -157.67, 0, -189.813,
  //      -57.4541, 568.734, -2.14949, 7.10136;
  Eigen::Matrix3d K;
  K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  Eigen::Matrix<double, 2, 6> J1;
  computePoseJacobian(pts_w, q, t, K, obs_measure, J1);
  cout << "test1 J1:\n" << J1 << "\n";

  Eigen::Matrix<double, 2, 6> J2;
  computePoseJacobianEps(pts_w, q, t, K, obs_measure, J2);
  cout << "test1 J2:\n" << J2 << "\n";
}

void test2()
{
  Eigen::Quaterniond q(0.99942872200188759, -0.01300752799707,
                       0.018957781400434373, 0.024771684994214886);
  Eigen::Vector3d t(-0.1201287342, -0.0084115707, 0.0647927311);

  Eigen::Vector3d pts_w(-0.0374123267, -0.830815672, 2.74480009);
  Eigen::Vector2d obs_measure(323, 109);
  //  -184.132         0  -0.890295  0.681803  -520.912  -141.011
  //      0     -184.167  -49.8553    559.18 -0.681934   2.51909
  Eigen::Matrix3d K;
  K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  Eigen::Matrix<double, 2, 6> J1;
  computePoseJacobian(pts_w, q, t, K, obs_measure, J1);
  cout << "test2 J1:\n" << J1 << "\n";

  Eigen::Matrix<double, 2, 6> J2;
  computePoseJacobianEps(pts_w, q, t, K, obs_measure, J2);
  cout << "test2 J2:\n" << J2 << "\n";
}

void test3()
{
  Eigen::Quaterniond q(0.99937854227215139, -0.013252554243367793,
                       0.020307888833076233, 0.025582976823657653);
  Eigen::Vector3d t(-0.12722598797280898, -0.0075073261527069694,
                    0.06138584950958844);

  Eigen::Vector3d pts_w(-0.627753, 0.160186, 1.3396);
  Eigen::Vector2d obs_measure(323, 109);
  //  -366.511        0 -182.696 -28.7796 -650.331  57.7355
  //      0 -366.581  40.6311  527.401  28.7851  259.704

  // pts_2d (65 308)
  Eigen::Matrix3d K;
  K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  Eigen::Matrix<double, 2, 6> J1;
  computePoseJacobian(pts_w, q, t, K, obs_measure, J1);
  cout << "test3 J1:\n" << J1 << "\n";

  Eigen::Matrix<double, 2, 6> J2;
  computePoseJacobianEps(pts_w, q, t, K, obs_measure, J2);
  cout << "test3 J2:\n" << J2 << "\n";
}

int main(int argc, char const* argv[])
{
  test1();
  test2();
  test3();
  return 0;
}
  • 理论上解析求导的结果应该和数值求导的结果一样
test1 J1:
-189.777        0 -2.58671  2.14908 -520.997  -157.67
       0 -189.813 -57.4541  568.734 -2.14949  7.10136
test1 J2:
-189.777        0  -2.5867  2.14912 -520.997  -157.67
       0 -189.813 -57.4539  568.735  -2.1487  7.10057
test2 J1:
 -184.132         0 -0.890295  0.681803  -520.912  -141.011
        0  -184.167  -49.8553    559.18 -0.681934   2.51909
test2 J2:
 -184.132         0 -0.890292  -18.8983  -504.617  -152.999
        0  -184.167  -49.8551   546.781   -21.555  0.927323
test3 J1:
-366.511        0 -182.696 -28.7795 -650.331  57.7354
       0 -366.581  40.6311    527.4  28.7851  259.704
test3 J2:
-366.511        0 -182.695 -63.1713 -603.564  42.5683
  • 验证3D点的求导是否正确
//
// Created by weihao on 23-3-23.
//
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"
#include "iostream"
#include "sophus/se3.hpp"
#include "sophus/so3.hpp"
using namespace std;

Eigen::Vector2d ReprojectError(Eigen::Vector3d& P, Eigen::Quaterniond& q,
                               Eigen::Vector3d& t, Eigen::Matrix3d& K,
                               Eigen::Vector2d& obs)
{
  Sophus::SE3d pose(q, t);
  Eigen::Vector3d P_cam = pose * P;
  P_cam = K * P_cam;
  Eigen::Vector3d P_cam_norm = P_cam / P_cam.z();
  Eigen::Vector2d proj = P_cam_norm.head<2>();
  return obs - proj;
}
void computePoseJacobianNumeric(Eigen::Vector3d& P_w, Eigen::Quaterniond& Q_w,
                                Eigen::Vector3d& t_w, Eigen::Matrix3d& K,
                                Eigen::Vector2d& obs,
                                Eigen::Matrix<double, 2, 3>& jacobian_Pi)
{
  Eigen::Vector3d dx = Eigen::Vector3d::Zero();
  double eps = 1e-6;
  Eigen::Matrix3d K_identity = Eigen::Matrix3d::Identity();
  Eigen::Vector3d pc = (Q_w * P_w + t_w);
  pc = K_identity * pc;
  Eigen::Vector2d uv = (pc / pc.z()).head<2>();

  Eigen::Vector2d residual_perturbed =
      ReprojectError(P_w, Q_w, t_w, K_identity, uv);

  for (int i = 0; i < 3; i++)
  {
    Eigen::Vector3d update_pw = P_w;
    update_pw[i] += eps;
    Eigen::Vector2d residual =
        ReprojectError(update_pw, Q_w, t_w, K_identity, uv);
    jacobian_Pi.col(i) = (residual_perturbed - residual) / eps;
  }
}

void computePoseJacobian(Eigen::Vector3d& P_w, Eigen::Quaterniond& Q_w,
                         Eigen::Vector3d& t_w, Eigen::Matrix3d& K,
                         Eigen::Vector2d& obs,
                         Eigen::Matrix<double, 2, 3>& jacobian_Pi)
{
  double fx = 1;
  double fy = 1;
  Eigen::Vector3d pc = (Q_w * P_w + t_w);
  double x = pc.x();
  double y = pc.y();
  double z = pc.z();
  double z_2 = z * z;
  Eigen::Matrix<double, 2, 3> jacobian_uv_Pc;
  jacobian_uv_Pc << fx / z, 0, -x * fx / z_2, 0, fy / z, -y * fy / z_2;
  jacobian_Pi = jacobian_uv_Pc * Q_w.toRotationMatrix().inverse();
  //  0.11851264208338291
  //  0
  //  0
  //  0.11851264208338291
  //  0.041401140286611439
  //  0.0046461526527604444
}

void test1()
{
  Eigen::Vector3d p_w(-2.9476976980899146, -0.33079894381424158,
                      8.4379183724249582);
  Eigen::Quaterniond q_wc(1, 0, 0, 0);
  Eigen::Vector3d t_w(0, 0, 0);
  Eigen::Matrix3d K;
  K << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1;
  Eigen::Matrix<double, 2, 3> J1;
  Eigen::Vector2d obs(0, 0);
  computePoseJacobian(p_w, q_wc, t_w, K, obs, J1);
  cout << "J1: \n" << J1 << endl;

  Eigen::Matrix<double, 2, 3> J2;
  computePoseJacobianNumeric(p_w, q_wc, t_w, K, obs, J2);
  cout << "J2: \n" << J2 << endl;
}

int main(int argc, char** argv)
{
  test1();
  return 0;
}
  • 输出结果
J1: 
  0.118513          0  0.0414011
         0   0.118513 0.00464615
J2: 
  0.118513          0  0.0414011
         0   0.118513 0.00464615