F-LOAM中的点线，点面残差手动推导

FivPluSeven

已于 2023-11-30 16:34:02 修改

阅读量161

点赞数

文章标签： linux

于 2023-11-29 15:34:06 首次发布

本文链接：https://blog.csdn.net/weixin_45205745/article/details/134691396

版权

F-LOAM中的点线，点面残差手动推导

文章目录

F-LOAM中的点线，点面残差手动推导

F-LOAM中使用ceres进行优化，残差通过evaluate函数手动推导。现学习一下：

点线残差：

代码：

bool EdgeAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    //求解残差
    Eigen::Map<const Eigen::Quaterniond> q_last_curr(parameters[0]);
    Eigen::Map<const Eigen::Vector3d> t_last_curr(parameters[0] + 4);
    Eigen::Vector3d lp;
    lp = q_last_curr * curr_point + t_last_curr; 

    Eigen::Vector3d nu = (lp - last_point_a).cross(lp - last_point_b);
    Eigen::Vector3d de = last_point_a - last_point_b;
    double de_norm = de.norm();
    residuals[0] = nu.norm()/de_norm;
	    
		//求解雅各比矩阵
    if(jacobians != NULL)
    {
        if(jacobians[0] != NULL)
        {                     
            Eigen::Matrix3d skew_lp = skew(lp);
            Eigen::Matrix<double, 3, 6> dp_by_se3;
            dp_by_se3.block<3,3>(0,0) = -skew_lp;
            (dp_by_se3.block<3,3>(0, 3)).setIdentity();//SE3的左扰动模型，I不应该在前面三行么？
            Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);//雅各比矩阵
            J_se3.setZero();
            Eigen::Matrix3d skew_de = skew(de);
           
            J_se3.block<1,6>(0,0) = - nu.transpose() / nu.norm() * skew_de * dp_by_se3/de_norm;
      
        }
    }
    return true;
 
}

雅各比推导：

在这里插入图片描述

残差计算：

$d_{\epsilon}=\frac{||(x^w-a) \times (x^w-b)||}{||a-b||} = \frac{||\vec{n}||}{||a-b||}$

$x^w=R\cdot x^l +t=Tx^l$

$R$ 为四元数存储：

$\frac{\partial x^w}{\xi}=\begin{bmatrix}I& -{x^w}^{\wedge} \end{bmatrix} (note1）$

雅各比计算：

$\frac{\partial d_{\partial\epsilon}}{\xi}=\frac{\partial d_{\epsilon}}{\partial {\vec{n}}} \cdot \frac{\partial\vec{n}}{\partial x^w} \cdot \frac{\partial x^w}{\partial\xi}$

其中：

$\frac{\partial{d_{\epsilon}}}{\partial\vec{n}} = \frac{\vec{n}}{||a-b||\cdot||\vec{n}||} (note 2)$

$\frac{\partial \vec{n}}{\partial{x^w}} = -(a-b)^{\wedge} (note3)$

点面残差：

代码:

bool SurfNormAnalyticCostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians) const
{
    Eigen::Map<const Eigen::Quaterniond> q_w_curr(parameters[0]);
    Eigen::Map<const Eigen::Vector3d> t_w_curr(parameters[0] + 4);
    Eigen::Vector3d point_w = q_w_curr * curr_point + t_w_curr;
    residuals[0] = plane_unit_norm.dot(point_w) + negative_OA_dot_norm;

    if(jacobians != NULL)
    {
        if(jacobians[0] != NULL)
        {
            Eigen::Matrix3d skew_point_w = skew(point_w);
            Eigen::Matrix<double, 3, 6> dp_by_se3;
            dp_by_se3.block<3,3>(0,0) = -skew_point_w;
            (dp_by_se3.block<3,3>(0, 3)).setIdentity();
            Eigen::Map<Eigen::Matrix<double, 1, 7, Eigen::RowMajor> > J_se3(jacobians[0]);
            J_se3.setZero();
            J_se3.block<1,6>(0,0) = plane_unit_norm.transpose() * dp_by_se3;
   
        }
    }
    return true;
}

雅各比推导：

在这里插入图片描述

平面方程为:

$A X + B Y + CZ + 1 = 0$

令： $\vec{n}=\{A,B,C\}, norm=1/\sqrt{A^2+B^2+C^2}$

残差：

$d_s = \vec{n} \cdot x^w + norm$

求导：

$\frac{\partial d_s}{\xi} = \frac{\partial d_s}{\partial x^w}\cdot \frac{\partial x^w}{\partial\xi} = \vec{n}^T \cdot \frac{\partial x^w}{\partial\xi}$

Note

先验知识：

在这里插入图片描述

$n o t e 1 :$

$SE 3 的左扰动模型（视觉 S L A M 十四讲 4.3.5 ）$

$n o t e 2 :$

$\frac {\partial{||n||}}{\partial n}=\frac{n^T}{||n||}$

$e g :$

$n=\begin {bmatrix} a&b&c \end{bmatrix}^T$

$||n||=\sqrt{a^2+b^2+c^2}$

$\frac{\partial||n||}{\partial n} = \begin{bmatrix} \frac{\partial{||n||}}{a} & \frac{\partial{||n||}}{b} & \frac{\partial{||n||}}{c} \end{bmatrix} = \frac{1}{||n||} \begin{bmatrix} a&b&c \end{bmatrix} = \frac{n^T}{||n||}$

$n o t e 3 :$

$\frac{\partial (x-a)\times (x-b)}{\partial x} = -(a-b)^{\wedge}$

$(x-a)\times(x-b)=\begin{bmatrix} (x_2-a_2)(x_3-b_3)-(x_3-a_3)(x_2-b_2)\\ (x_3-a_3)(x_1-b_1)-(x_1-a_1)(x_3-b_3) \\ (x_1-a_1)(x_2-b_2)-(x_1-b_1)(x_2-a_2)\end{bmatrix}$

$\frac{\partial(x-a)\times(x-b)}{\partial x}=\begin{bmatrix}\frac{\partial (x-a)\times(x-b)}{\partial x_1} &\frac{\partial (x-a)\times(x-b)}{\partial x_2}&\frac{\partial (x-a)\times(x-b)}{\partial x_3}\end{bmatrix}=\begin{bmatrix} 0 & a_3-b_3 &-a_2+b_2 \\ -a_3+b_3 &0 & a_1-b_1 \\ a_2-b_2 &-a_1+b_1 *& 0\end{bmatrix}=-(a-b)^{\wedge}$