SLAM14讲第七讲习题10:Ceres实现ICP优化

很好的学习ceres的习题,难度很低,容易入手.

ceres结构体构造:

struct ICPCeres
{
    /**
     * @brief Construct a new ICPCeres object
     * 
     * @param uvw:当前帧3d点
     * @param xyz:上一帧3d点
     */
    ICPCeres(cv::Point3f uvw, cv::Point3f xyz) : _uvw(uvw), _xyz(xyz) {}
    template <typename T>
    /**
     * @brief 
     * 
     * @param camera 
     * @param residual 
     * @return true 
     * @return false 
     */
    bool operator()(const T *const camera,
                    T *residual) const
    {
        T p[3];
        T point[3];
        point[0] = T(_xyz.x);
        point[1] = T(_xyz.y);
        point[2] = T(_xyz.z);
        AngleAxisRotatePoint(camera, point, p);
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];
        //计算误差 e=p-（Rp‘+t）
        residual[0] = T(_uvw.x) - p[0];
        residual[1] = T(_uvw.y) - p[1];
        residual[2] = T(_uvw.z) - p[2];
        return true;
    }

    const cv::Point3f _uvw;
    const cv::Point3f _xyz;
};

其中AngleAxisRotatePoint在rotation.h中,作用是投影两帧之前的点,得到两帧之间camera的Rotation和position

rotation.h

#ifndef ROTATION_H
#define ROTATION_H

#include <algorithm>
#include <cmath>
#include <limits>

//
// math functions needed for rotation conversion.

// dot and cross production

template <typename T>
inline T DotProduct(const T x[3], const T y[3])
{
  return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}

template <typename T>
inline void CrossProduct(const T x[3], const T y[3], T result[3])
{
  result[0] = x[1] * y[2] - x[2] * y[1];
  result[1] = x[2] * y[0] - x[0] * y[2];
  result[2] = x[0] * y[1] - x[1] * y[0];
}

//

// Converts from a angle anxis to quaternion :
template <typename T>
inline void AngleAxisToQuaternion(const T *angle_axis, T *quaternion)
{
  const T &a0 = angle_axis[0];
  const T &a1 = angle_axis[1];
  const T &a2 = angle_axis[2];
  const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;

  if (theta_squared > T(std::numeric_limits<double>::epsilon()))
  {
    const T theta = sqrt(theta_squared);
    const T half_theta = theta * T(0.5);
    const T k = sin(half_theta) / theta;
    quaternion[0] = cos(half_theta);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  }
  else
  { // in case if theta_squared is zero
    const T k(0.5);
    quaternion[0] = T(1.0);
    quaternion[1] = a0 * k;
    quaternion[2] = a1 * k;
    quaternion[3] = a2 * k;
  }
}

template <typename T>
inline void QuaternionToAngleAxis(const T *quaternion, T *angle_axis)
{
  const T &q1 = quaternion[1];
  const T &q2 = quaternion[2];
  const T &q3 = quaternion[3];
  const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;

  // For quaternions representing non-zero rotation, the conversion
  // is numercially stable
  if (sin_squared_theta > T(std::numeric_limits<double>::epsilon()))
  {
    const T sin_theta = sqrt(sin_squared_theta);
    const T &cos_theta = quaternion[0];

    // If cos_theta is negative, theta is greater than pi/2, which
    // means that angle for the angle_axis vector which is 2 * theta
    // would be greater than pi...

    const T two_theta = T(2.0) * ((cos_theta < 0.0)
                                      ? atan2(-sin_theta, -cos_theta)
                                      : atan2(sin_theta, cos_theta));
    const T k = two_theta / sin_theta;

    angle_axis[0] = q1 * k;
    angle_axis[1] = q2 * k;
    angle_axis[2] = q3 * k;
  }
  else
  {
    // For zero rotation, sqrt() will produce NaN in derivative since
    // the argument is zero. By approximating with a Taylor series,
    // and truncating at one term, the value and first derivatives will be
    // computed correctly when Jets are used..
    const T k(2.0);
    angle_axis[0] = q1 * k;
    angle_axis[1] = q2 * k;
    angle_axis[2] = q3 * k;
  }
}

template <typename T>
inline void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3])
{
  const T theta2 = DotProduct(angle_axis, angle_axis);
  if (theta2 > T(std::numeric_limits<double>::epsilon()))
  {
    // Away from zero, use the rodriguez formula
    //
    //   result = pt costheta +
    //            (w x pt) * sintheta +
    //            w (w . pt) (1 - costheta)
    //
    // We want to be careful to only evaluate the square root if the
    // norm of the angle_axis vector is greater than zero. Otherwise
    // we get a division by zero.
    //
    const T theta = sqrt(theta2);
    const T costheta = cos(theta);
    const T sintheta = sin(theta);
    const T theta_inverse = 1.0 / theta;

    const T w[3] = {angle_axis[0] * theta_inverse,
                    angle_axis[1] * theta_inverse,
                    angle_axis[2] * theta_inverse};

    // Explicitly inlined evaluation of the cross product for
    // performance reasons.
    /*const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
                              w[2] * pt[0] - w[0] * pt[2],
                              w[0] * pt[1] - w[1] * pt[0] };*/
    T w_cross_pt[3];
    CrossProduct(w, pt, w_cross_pt);

    const T tmp = DotProduct(w, pt) * (T(1.0) - costheta);
    //    (w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);

    result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
    result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
    result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
  }
  else
  {
    // Near zero, the first order Taylor approximation of the rotation
    // matrix R corresponding to a vector w and angle w is
    //
    //   R = I + hat(w) * sin(theta)
    //
    // But sintheta ~ theta and theta * w = angle_axis, which gives us
    //
    //  R = I + hat(w)
    //
    // and actually performing multiplication with the point pt, gives us
    // R * pt = pt + w x pt.
    //
    // Switching to the Taylor expansion near zero provides meaningful
    // derivatives when evaluated using Jets.
    //
    // Explicitly inlined evaluation of the cross product for
    // performance reasons.
    /*const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
                              angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
                              angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };*/
    T w_cross_pt[3];
    CrossProduct(angle_axis, pt, w_cross_pt);

    result[0] = pt[0] + w_cross_pt[0];
    result[1] = pt[1] + w_cross_pt[1];
    result[2] = pt[2] + w_cross_pt[2];
  }
}

#endif // rotation.h

整体代码如下

ceres_icp.cpp

#include <iostream>
#include <chrono>
#include <ceres/ceres.h>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include "rotation.h"

struct ICPCeres
{
    /**
     * @brief Construct a new ICPCeres object
     * 
     * @param uvw:当前帧3d点
     * @param xyz:上一帧3d点
     */
    ICPCeres(cv::Point3f uvw, cv::Point3f xyz) : _uvw(uvw), _xyz(xyz) {}
    template <typename T>
    /**
     * @brief 
     * 
     * @param camera 
     * @param residual 
     * @return true 
     * @return false 
     */
    bool operator()(const T *const camera,
                    T *residual) const
    {
        T p[3];
        T point[3];
        point[0] = T(_xyz.x);
        point[1] = T(_xyz.y);
        point[2] = T(_xyz.z);
        AngleAxisRotatePoint(camera, point, p);
        p[0] += camera[3];
        p[1] += camera[4];
        p[2] += camera[5];
        //计算误差 e=p-（Rp‘+t）
        residual[0] = T(_uvw.x) - p[0];
        residual[1] = T(_uvw.y) - p[1];
        residual[2] = T(_uvw.z) - p[2];
        return true;
    }

    const cv::Point3f _uvw;
    const cv::Point3f _xyz;
};

void find_feature_matches(
    const cv::Mat &img_1, const cv::Mat &img_2,
    std::vector<cv::KeyPoint> &keypoints_1,
    std::vector<cv::KeyPoint> &keypoints_2,
    std::vector<cv::DMatch> &matches);

// 像素坐标转相机归一化坐标
cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K);

void pose_estimation_3d3d(
    const std::vector<cv::Point3f> &pts1,
    const std::vector<cv::Point3d> &pts2,
    cv::Mat &R, cv::Mat &t);

int main (int argc, char ** argv) {
    if (argc != 5)
    {
        std::cout << "usage: pose_estimation_3d3d img1 img2 depth1 depth2 !" << std::endl;
        return -1;
    }

    double camera[6] = {0, 1, 2, 0, 0, 0}; //初始位姿估计(R,t), 6维
    cv::Mat img_1 = cv::imread(argv[1], CV_LOAD_IMAGE_COLOR);
    cv::Mat img_2 = cv::imread(argv[2], CV_LOAD_IMAGE_COLOR);
    cv::Mat depth1 = cv::imread(argv[3], CV_LOAD_IMAGE_UNCHANGED);
    cv::Mat depth2 = cv::imread(argv[4], CV_LOAD_IMAGE_UNCHANGED);

    std::vector<cv::KeyPoint> keypoints_1, keypoints_2;
    std::vector<cv::DMatch> matches;
    find_feature_matches(img_1, img_2, keypoints_1, keypoints_2, matches);
    std::cout << "Find number of matched point: " << matches.size() << std::endl;

    cv::Mat K = (cv::Mat_<double>(3, 3) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1);
    std::vector<cv::Point3d> pts1, pts2;

    for (cv::DMatch m : matches) {
        ushort d1 = depth1.ptr<unsigned short>(int(keypoints_1[m.queryIdx].pt.y))[int(keypoints_1[m.queryIdx].pt.x)]; //提取特征点深度信息
        ushort d2 = depth2.ptr<unsigned short>(int(keypoints_2[m.trainIdx].pt.y))[int(keypoints_2[m.trainIdx].pt.x)];
        if (d1 == 0 || d2 == 0) { //深度无效点
            continue;
        }
        cv::Point2d p1 = pixel2cam(keypoints_1[m.queryIdx].pt, K);
        cv::Point2d p2 = pixel2cam(keypoints_2[m.trainIdx].pt, K);
        float dd1 = float(d1) / 5000.0; //相机的参数,(和奥比中光一样必须要除,有点坑)
        float dd2 = float(d2) / 5000.0;
        pts1.push_back(cv::Point3f(p1.x * dd1, p1.y * dd1, dd1));
        pts2.push_back(cv::Point3f(p2.x * dd2, p2.y * dd2, dd2));
    }
    std::cout << "3d-3d pairs: " << pts1.size() << std::endl;
    cv::Mat R, t;

    ceres::Problem problem;
    for (int i = 0; i < pts2.size();  ++i) 
    {
        ceres::CostFunction *cost_function = new ceres::AutoDiffCostFunction<ICPCeres, 3, 6>//3为输出残差维数, x,y,z的差值. 6为输入维数R(3维)t(3维)
(new ICPCeres(pts2[i], pts1[i]));
        problem.AddResidualBlock(cost_function, nullptr, camera);
    }
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summery;
    ceres::Solve(options, &problem, &summery);
    std::cout << summery.FullReport() << "\n";

    cv::Mat R_vec = (cv::Mat_<double>(3, 1) << camera[0], camera[1], camera[2]); // 数组转cv向量
    cv::Mat R_cvest;
    Rodrigues(R_vec, R_cvest); // 罗德里格斯公式，旋转向量转旋转矩阵
    std::cout << "R_cvest=" << R_cvest << std::endl;
    Eigen::Matrix<double, 3, 3> R_est;
    cv2eigen(R_cvest, R_est); // cv矩阵转eigen矩阵
    std::cout << "R_est=" << R_est << std::endl;
    Eigen::Vector3d t_est(camera[3], camera[4], camera[5]);
    std::cout << "t_est=" << t_est << std::endl;
    Eigen::Isometry3d T(R_est); // 构造变换矩阵与输出
    T.pretranslate(t_est);

    std::cout << T.matrix() << std::endl;
    return 0;
}

/**
 * @brief 调用cv::ORB返回匹配的特征点
 * 
 * @param[in] img_1 
 * @param[in] img_2 
 * @param[out] keypoints_1 
 * @param[out] keypoints_2 
 * @param[out] matches 
 */
void find_feature_matches(
    const cv::Mat &img_1, const cv::Mat &img_2,
    std::vector<cv::KeyPoint> &keypoints_1,
    std::vector<cv::KeyPoint> &keypoints_2,
    std::vector<cv::DMatch> &matches)
{
    //-- 初始化
    cv::Mat descriptors_1, descriptors_2;
    // used in OpenCV3
    cv::Ptr<cv::FeatureDetector> detector = cv::ORB::create();
    cv::Ptr<cv::DescriptorExtractor> descriptor = cv::ORB::create();
    // use this if you are in OpenCV2
    // Ptr<FeatureDetector> detector = FeatureDetector::create ( "ORB" );
    // Ptr<DescriptorExtractor> descriptor = DescriptorExtractor::create ( "ORB" );
    cv::Ptr<cv::DescriptorMatcher> matcher = cv::DescriptorMatcher::create("BruteForce-Hamming");
    //-- 第一步:检测 Oriented FAST 角点位置
    detector->detect(img_1, keypoints_1);
    detector->detect(img_2, keypoints_2);

    //-- 第二步:根据角点位置计算 BRIEF 描述子
    descriptor->compute(img_1, keypoints_1, descriptors_1);
    descriptor->compute(img_2, keypoints_2, descriptors_2);

    //-- 第三步:对两幅图像中的BRIEF描述子进行匹配，使用 Hamming 距离
    std::vector<cv::DMatch> match;
    // BFMatcher matcher ( NORM_HAMMING );
    matcher->match(descriptors_1, descriptors_2, match);

    //-- 第四步:匹配点对筛选
    double min_dist = 10000, max_dist = 0;

    // 找出所有匹配之间的最小距离和最大距离, 即是最相似的和最不相似的两组点之间的距离
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        double dist = match[i].distance;
        if (dist < min_dist)
            min_dist = dist;
        if (dist > max_dist)
            max_dist = dist;
    }

    printf("-- Max dist : %f \n", max_dist);
    printf("-- Min dist : %f \n", min_dist);

    // 当描述子之间的距离大于两倍的最小距离时,即认为匹配有误.但有时候最小距离会非常小,设置一个经验值30作为下限.
    for (int i = 0; i < descriptors_1.rows; i++)
    {
        if (match[i].distance <= std::max(2 * min_dist, 30.0))
        {
            matches.push_back(match[i]);
        }
    }
}

cv::Point2d pixel2cam(const cv::Point2d &p, const cv::Mat &K)
{
    return cv::Point2d(
        (p.x - K.at<double>(0, 2)) / K.at<double>(0, 0),
        (p.y - K.at<double>(1, 2)) / K.at<double>(1, 1));
}

CMakeLists.txt如下

cmake_minimum_required(VERSION 3.5)
project(ceres_icp)

set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g -ggdb")
set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall")
set(CMAKE_CXX_STANDARD 14)

find_package(OpenCV 3 REQUIRED)
find_package(Ceres REQUIRED)

include_directories(
  /usr/local/include
  include
)

link_directories(
  /usr/local/lib
)

add_executable(ceres_icp
  src/ceres_icp.cpp
)

target_link_libraries(ceres_icp
  ${OpenCV_LIBRARIES}
  ${CERES_LIBRARIES}
)

结果如下

-- Max dist : 95.000000 
-- Min dist : 7.000000 
Find number of matched point: 81
3d-3d pairs: 75
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  1.187529e+02    0.00e+00    1.49e+02   0.00e+00   0.00e+00  1.00e+04        0    3.80e-03    3.86e-03
   1  3.468083e+01    8.41e+01    5.94e+01   1.29e+00   8.95e-01  1.98e+04        1    3.80e-03    7.68e-03
   2  6.366072e+00    2.83e+01    3.48e+01   1.47e+00   1.06e+00  5.93e+04        1    3.82e-03    1.15e-02
   3  2.387209e+00    3.98e+00    1.63e+01   7.92e-01   8.04e-01  7.66e+04        1    3.93e-03    1.55e-02
   4  9.092359e-01    1.48e+00    3.68e-01   2.88e-01   1.00e+00  2.30e+05        1    3.83e-03    1.93e-02
   5  9.076072e-01    1.63e-03    1.65e-02   1.80e-02   1.05e+00  6.89e+05        1    3.73e-03    2.31e-02
   6  9.075968e-01    1.05e-05    2.11e-03   2.14e-03   1.13e+00  2.07e+06        1    3.95e-03    2.70e-02

Solver Summary (v 2.0.0-eigen-(3.3.90)-lapack-suitesparse-(5.7.1)-cxsparse-(3.2.0)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            1                        1
Parameters                                  6                        6
Residual blocks                            75                       75
Residuals                                 225                      225

Minimizer                        TRUST_REGION

Dense linear algebra library            EIGEN
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver                        DENSE_QR                 DENSE_QR
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        1

Cost:
Initial                          1.187529e+02
Final                            9.075968e-01
Change                           1.178453e+02

Minimizer iterations                        7
Successful steps                            7
Unsuccessful steps                          0

Time (in seconds):
Preprocessor                         0.000067

  Residual only evaluation           0.000095 (7)
  Jacobian & residual evaluation     0.026609 (7)
  Linear solver                      0.000119 (7)
Minimizer                            0.027029

Postprocessor                        0.000004
Total                                0.027100

Termination:                      CONVERGENCE (Function tolerance reached. |cost_change|/cost: 1.878852e-07 <= 1.000000e-06)

R_cvest=[0.9972083771125694, -0.0578448212458617, 0.0472168325022967;
 0.05830249021928781, 0.9982638507212928, -0.008372811793325411;
 -0.04665053323109484, 0.0111022969754419, 0.9988495716328478]
R_est=   0.997208  -0.0578448   0.0472168
  0.0583025    0.998264 -0.00837281
 -0.0466505   0.0111023     0.99885
t_est=-0.139986
0.0568282
0.0369365
   0.997208  -0.0578448   0.0472168   -0.139986
  0.0583025    0.998264 -0.00837281   0.0568282
 -0.0466505   0.0111023     0.99885   0.0369365
          0           0           0           1

和用g2o结果差不多

欢迎交流