非线性优化求解ICP实例

最新推荐文章于 2024-07-12 10:45:15 发布

Enochzhu

最新推荐文章于 2024-07-12 10:45:15 发布

阅读量906

点赞数

分类专栏： SLAM C++ 文章标签：算法

本文链接：https://blog.csdn.net/Enochzhu/article/details/126232639

版权

C++ 同时被 2 个专栏收录

14 篇文章 2 订阅

订阅专栏

SLAM

7 篇文章 0 订阅

订阅专栏

1. 非线性优化求解ICP实例

给出一个基于手动jacobian的P-P ICP，重点在于优化参数化子类以及Factor类的编写。

1.1 优化状态推导及参数化

对于target点云 $\left \{ p_1, \cdots , p_n \right \}$ , source点云 $\left \{ p'_1, \cdots , p'_n \right \}$ ，

在旋转矩阵表示位姿时，构建最小二乘问题，求使得误差的平方和最小的变换矩阵T{R，t}
$\begin{array}{c} \min\limits_{R, t} J=\frac{1}{2} \sum_{i=1}^{n}\left\|p_{i}-\left(R p_{i}^{\prime}+t\right)\right\|_{2}^{2} \end{array}$
只看残差项
在李代数表示位姿时，目标函数可以写成
$\begin{array}{c} \min\limits_{\xi }J =\frac{1}{2} \sum_{i=1}^{n}\left\|p_{i}-exp(\xi ^\wedge )p'_i)\right\|_{2}^{2} \\ \end{array}$
优化函数 $J=\left\| f(T) \right\|^2_2, T$ 为优化的对象，在增量优化时我们应该求解一个se3而非T。
增量优化的求解思路梳理如下，采用扰动法，先给T一个左乘扰动，扰动后的函数为： $f(T\oplus\delta{T})=f(\delta{T}T)=f(exp([\delta\xi]^\wedge)T)$

其中 $\delta{\xi}$ 为左扰动对应的se3李代数。优化的目标即找到一个合适的δ ξ \delta{\xi}δξ使得代价函数尽可能缩小，根据高斯牛顿优化，我们需要将一个扰动后的函数一阶泰勒展开为 $f(X\oplus\delta{x})=f(X)+J\delta{x}$ 的形式, 但是由于姿态矩阵T是流形，因此对于 $f (T)$ 函数，若将其在T处展开，是不能得到我们要的形式的，如下：
但是，若将f ff看作δ ξ \delta\xiδξ的函数，则可以很好的解决，如下：
通过迭代优化方法求解出增量δ ξ \delta{\xi}δξ即可，求解出se3增量后，接着需要用这个增量对我们的POSE进行更新，因为我们的POSE是SE3，所以首先将se3转换为SE3，即： $\mathrm{f}(\mathrm{T} \oplus \delta \xi)=\mathrm{f}\left(\exp \left([\delta \xi]^{\wedge}\right) \mathrm{T}\right)=\mathrm{f}(\mathrm{T})+\frac{\partial \mathrm{f}}{\partial \mathrm{T}}(\delta \mathrm{T} \mathrm{T}-\mathrm{T}) \neq \mathrm{f}(\mathrm{T})+\frac{\partial \mathrm{f}}{\partial \mathrm{T}} \delta \mathrm{T}$
其中
由于我们的POSE中姿态用四元数表示，因此将so3-δ ϕ \delta\phiδϕ转换为四元数，
],当扰动很小时，近似为： $\mathrm{f}\left(\exp \left([\delta \xi]^{\wedge}\right) \mathrm{T}\right)=\mathrm{F}(\delta \xi)=\mathrm{F}(0)+\mathrm{J} \delta \xi \\\mathrm{J}=\left.\frac{\partial \mathrm{F}(\delta \zeta)}{\partial \delta \zeta}\right|_{\delta \xi=0}$

opt_ICP_CERES.cpp

#include "opt_ICP_CERES.h"
#include "lidarOptimization/lidarCeres.h"

namespace TESTICP
{
    opt_ICP_CERES::opt_ICP_CERES(const YAML::Node &node)
        : kdtree_flann(new pcl::KdTreeFLANN<PointType>)
    {

        max_iterations = node["max_iter"].as<int>();
        max_coresspoind_dis = node["max_corr_dist"].as<float>();
        trans_eps = node["trans_eps"].as<float>();
        euc_fitness_eps = node["euc_fitness_eps"].as<float>();
    }

    opt_ICP_CERES::~opt_ICP_CERES()
    {
    }

    bool opt_ICP_CERES::setTargetCloud(const CLOUD_PTR &target)
    {
        target_ptr = target;
        kdtree_flann->setInputCloud(target);
    }

    bool opt_ICP_CERES::scanMatch(const CLOUD_PTR &source, const Eigen::Matrix4f &predict_pose,
                                  CLOUD_PTR &transformed_source_ptr, Eigen::Matrix4f &result_pose)
    {
        source_ptr = source;
        CLOUD_PTR transform_cloud(new CLOUD());
        Eigen::Matrix4d T = predict_pose.cast<double>();
        q_w_curr = Eigen::Quaterniond(T.block<3, 3>(0, 0));
        t_w_curr = T.block<3, 1>(0, 3);

        for (int i = 0; i < max_iterations; ++i)
        {
            pcl::transformPointCloud(*source_ptr, *transform_cloud, T);
            ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1);
            ceres::Problem::Options problem_options;
            ceres::Problem problem(problem_options);

            problem.AddParameterBlock(parameters, 7, new test_ceres::PoseSE3Parameterization());

            // std::cout << "------------ " << i << "------------" << std::endl;
            for (int j = 0; j < transform_cloud->size(); ++j)
            {
                const PointType &origin_pt = source_ptr->points[j];
                if (!pcl::isFinite(origin_pt))
                    continue;

                const PointType &transform_pt = transform_cloud->at(j);
                std::vector<float> res_dis;
                std::vector<int> indices;
                kdtree_flann->nearestKSearch(transform_pt, 1, indices, res_dis);
                if (res_dis.front() > max_coresspoind_dis)
                    continue;

                Eigen::Vector3d nearest_pt = Eigen::Vector3d(target_ptr->at(indices.front()).x,
                                                             target_ptr->at(indices.front()).y,
                                                             target_ptr->at(indices.front()).z);

                Eigen::Vector3d origin_eigen(origin_pt.x, origin_pt.y, origin_pt.z);

                ceres::CostFunction *cost_function = new test_ceres::EdgeAnalyticCostFuntion(origin_eigen, nearest_pt);
                problem.AddResidualBlock(cost_function, loss_function, parameters);
            }
            ceres::Solver::Options options;
            options.linear_solver_type = ceres::DENSE_QR;
            options.max_num_iterations = 10;
            options.minimizer_progress_to_stdout = false;
            options.check_gradients = false;
            options.gradient_check_relative_precision = 1e-4;

            ceres::Solver::Summary summary;
            ceres::Solve(options, &problem, &summary);

            T.setIdentity();
            T.block<3, 1>(0, 3) = t_w_curr;
            T.block<3, 3>(0, 0) = q_w_curr.toRotationMatrix();

            // std::cout << "T\n"
            //           << T << std::endl;
        }

        final_pose = T.cast<float>();
        result_pose = T.cast<float>();
        pcl::transformPointCloud(*source_ptr, *transformed_source_ptr, result_pose);
        return true;
    }

    float opt_ICP_CERES::getFitnessScore()
    {
        float max_range = std::numeric_limits<float>::max();
        float score = 0.f;

        CLOUD_PTR transform_cloud(new CLOUD());
        pcl::transformPointCloud(*source_ptr, *transform_cloud, final_pose);
        std::vector<int> nn_indices(1);
        std::vector<float> nn_dists(1);

        int nr = 0;

        for (size_t i = 0; i < transform_cloud->size(); ++i)
        {
            kdtree_flann->nearestKSearch(transform_cloud->points[i], 1, nn_indices, nn_dists);
            if (nn_dists.front() <= max_range)
            {
                score += nn_dists.front();
                nr++;
            }
        }
        if (nr > 0)
            return score / static_cast<float>(nr);
        else
            return (std::numeric_limits<float>::max());
    }
}

opt_ICP_CERES.h

#pragma once
#include <eigen3/Eigen/Core>
#include <pcl/common/transforms.h>
#include <pcl/kdtree/kdtree_flann.h>
#include "registration_interface.hpp"

namespace TESTICP
{
    class opt_ICP_CERES : public RegistrationInterface
    {

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        opt_ICP_CERES(const YAML::Node &node);
        ~opt_ICP_CERES();
        bool setTargetCloud(const CLOUD_PTR &target) override;
        bool scanMatch(const CLOUD_PTR &source, const Eigen::Matrix4f &predict_pose,
                       CLOUD_PTR &transformed_source_ptr, Eigen::Matrix4f &result_pose) override;

        float getFitnessScore();

    private:
        CLOUD_PTR target_ptr, source_ptr;
        Eigen::Matrix4f final_pose;
        int max_iterations;
        float max_coresspoind_dis;
        float trans_eps;
        float euc_fitness_eps;

        double parameters[7] = {0, 0, 0, 1, 0, 0, 0};
        Eigen::Map<Eigen::Quaterniond> q_w_curr = Eigen::Map<Eigen::Quaterniond>(parameters);
        Eigen::Map<Eigen::Vector3d> t_w_curr = Eigen::Map<Eigen::Vector3d>(parameters + 4);

        pcl::KdTreeFLANN<PointType>::Ptr kdtree_flann;
    };
}

lidarCeres.cpp

#include <lidarOptimization/lidarCeres.h>

namespace test_ceres
{
    EdgeAnalyticCostFuntion::EdgeAnalyticCostFuntion(Eigen::Vector3d cur_pt_, Eigen::Vector3d near_pt_)
        : cur_pt(cur_pt_), near_pt(near_pt_) {}

    bool EdgeAnalyticCostFuntion::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 = q_last_curr * cur_pt + t_last_curr;
        residuals[0] = lp.x() - near_pt.x();
        residuals[1] = lp.y() - near_pt.y();
        residuals[2] = lp.z() - near_pt.z();

        if (jacobians != NULL)
        {
            Eigen::Matrix3d skew_lp = skew(cur_pt);
            Eigen::Matrix<double, 3, 6> dp_by_se3;
            // dp_by_se3.block<3, 3>(0, 0) = -q_last_curr.matrix() * skew_lp;  //  右乘扰动
            dp_by_se3.block<3, 3>(0, 0) = -skew(q_last_curr * cur_pt);
            (dp_by_se3.block<3, 3>(0, 3)).setIdentity();
            Eigen::Map<Eigen::Matrix<double, 3, 7, Eigen::RowMajor>> J_se3(jacobians[0]);
            J_se3.setZero();
            J_se3.block<3, 6>(0, 0) = dp_by_se3;
        }

        return true;
    }

    bool PoseSE3Parameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
    {
        Eigen::Map<const Eigen::Vector3d> trans(x + 4);

        Eigen::Quaterniond delta_q;
        Eigen::Vector3d delta_t;
        getTransformFromSe3(Eigen::Map<const Eigen::Matrix<double, 6, 1>>(delta), delta_q, delta_t);
        Eigen::Map<const Eigen::Quaterniond> quater(x);
        Eigen::Map<Eigen::Quaterniond> quater_plus(x_plus_delta);
        Eigen::Map<Eigen::Vector3d> trans_plus(x_plus_delta + 4);

        quater_plus = delta_q * quater;
        trans_plus = delta_q * trans + delta_t;
        return true;
    }

    bool PoseSE3Parameterization::ComputeJacobian(const double *x, double *jacobian) const
    {
        Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
        (j.topRows(6)).setIdentity();
        (j.bottomRows(1)).setZero();
        return true;
    }

    Eigen::Matrix<double, 3, 3> skew(const Eigen::Vector3d &mat)
    {
        Eigen::Matrix<double, 3, 3> skew_mat;
        skew_mat.setZero();
        skew_mat(0, 1) = -mat(2);
        skew_mat(0, 2) = mat(1);
        skew_mat(1, 2) = -mat(0);
        skew_mat(1, 0) = mat(2);
        skew_mat(2, 0) = -mat(1);
        skew_mat(2, 1) = mat(0);
        return skew_mat;
    }

    void getTransformFromSe3(const Eigen::Matrix<double, 6, 1> &se3, Eigen::Quaterniond &q, Eigen::Vector3d &t)
    {
        Eigen::Vector3d omega(se3.data());
        Eigen::Vector3d upsilon(se3.data() + 3);
        Eigen::Matrix3d Omega = skew(omega);

        double theta = omega.norm();
        double half_theta = 0.5 * theta;

        double imag_factor;
        double real_factor = std::cos(half_theta);
        if (theta < 1e-10)
        {
            double theta_sq = theta * theta;
            double theta_po4 = theta_sq * theta_sq;
            imag_factor = 0.5 - 0.0208333 * theta_sq + 0.000260417 * theta_po4;
        }
        else
        {
            double sin_half_theta = sin(half_theta);
            imag_factor = sin_half_theta / theta;
        }
        q = Eigen::Quaterniond(real_factor, imag_factor * omega.x(), imag_factor * omega.y(), imag_factor * omega.z());

        Eigen::Matrix3d J;
        if (theta < 1e-10)
            J = q.matrix();
        else
        {
            Eigen::Matrix3d Omega2 = Omega * Omega;
            J = (Eigen::Matrix3d::Identity() + (1 - std::cos(theta)) / (theta * theta) * Omega + (theta - std::sin(theta)) / (std::pow(theta, 3)) * Omega2);
        }
        t = J * upsilon;
    }

}

lidarCeres.h

#pragma once

#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>

namespace test_ceres
{

    Eigen::Matrix3d skew(const Eigen::Vector3d &mat);

    void getTransformFromSe3(const Eigen::Matrix<double, 6, 1> &se3, Eigen::Quaterniond &q, Eigen::Vector3d &t);

    class EdgeAnalyticCostFuntion : public ceres::SizedCostFunction<3, 7>
    {
    public:
        EdgeAnalyticCostFuntion(Eigen::Vector3d cur_pt_, Eigen::Vector3d near_pt_);
        virtual ~EdgeAnalyticCostFuntion() {}
        virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const;

        Eigen::Vector3d cur_pt, near_pt;
    };

    class PoseSE3Parameterization : public ceres::LocalParameterization
    {
    public:
        PoseSE3Parameterization(){};
        virtual ~PoseSE3Parameterization(){};
        virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
        virtual bool ComputeJacobian(const double *x, double *jacobian) const;
        virtual int GlobalSize() const { return 7; }
        virtual int LocalSize() const { return 6; }
    };
}