牛顿迭代法c++代码_事件相机的光流计算-代码

之前在文章中提到，会用c++实现里面的光流估计的核心算法Algorithm 1

huhupy：事件相机的光流计算​zhuanlan.zhihu.com

这里面的主要的问题是从时空的局部估计一个平面的表达式，算法1使用的是类似于Ransac的求解方式，不同的地方在于Ransac方法是从数据集合中随机抽取3点(三点确定平面)来估计平面参数，然后在所有数据上对参数进行评估，统计inlier的数目，重复进行多次迭代选择最好的参数。理论上只要是不超过50%的outlier都能使用这样的方法估计出参数。而算法1采用的方法是先用全部点集用最小二乘去求解平面参数，然后再利用参数进行outlier的筛选，然后用剩余的inlier集合估计平面参数，重复进行以上过程直至参数收敛。可以明显的发现算法1的性能受outlier数目的影响较大，试想第一次如果把一些真正的inlier过滤而outlier保留的话，算法很有可能最后并不能收敛。好处是这样的算法在outlier数目不多的情况能够快速收敛，但我认为Ransac方法也能达到同样的效率甚至更快(因为只用3个点估计平面参数，ps. 大家知道怎么由三个点计算平面参数吗[狗头])。

C++的代码实现如下

#pragma once

#include <array>
#include <vector>

#include <ceres/ceres.h>

struct Event{int x, y; double timestamp;};
struct PlaneFit {

    PlaneFit(double xx, double yy, double zz) : x(xx), y(yy), z(zz) {}

    template <typename T> bool operator()(const T *const abc, T *residual) const {

        residual[0] = T(z) - abc[0] * T(x) - abc[1] * T(y) - abc[2];

        return true;
    }

    double x, y, z;
};

namespace event {

/**
 *
 * Algorithm is based on the method presented in “Event-Based Visual Flow,”
 * ref{Algorithm 1}
 *
 * It uses the local properties of events' spatiotemporal neighborhood to estimate
 * a plane parameter.
 *
 * */

class LocalPlanesFlow {
private:
    // half of the search window
    const int L = 1;

    // threshold of updated plane parameter magnitude difference to the last estimate
    const float th1 = 1e-5f;

    // when an event in the spatiotemporal is far away from the estimated plane,
    // it will be discarded
    const float th2 = 5e-2f;

    // Events must occur in this time interval before curernt event to be
    // considered. */
    const double max_dt_threshold = 1e-3;

    // {a, b, c} && z = a * x + b * y + c
    double old_estimate[3], new_estimate[3];

    // Map of input event times, surface of events(SAE)
    double *sae0, *sae1, *sae;

    // image resolution
    int w, h;

    // current event location on the image plane
    int cur_x, cur_y;

    // current event timestamp
    double cur_t;

    double difference() {
        double sum = 0.;

        for (int i = 0; i < 3; ++i)
            sum += (old_estimate[i] - new_estimate[i]) * (old_estimate[i] - new_estimate[i]);

        return std::sqrt(sum);
    }

    double distance(const std::array<double, 3> &point, double *plane){
        double e = point[2] - plane[0] * point[0] - plane[1] * point[1] - plane[2];
        return std::abs(e) / std::sqrt(plane[0] * plane[0] + plane[1] * plane[1] + 1);
    }

    void neighborhood(std::vector<std::array<double, 3>> &neighbors) {

        neighbors.clear();

        for (int i = -L; i <= L; ++i) {
            for (int j = -L; j <= L; ++j) {
                double t = sae[cur_x + i + (cur_y + j) * w];

                if (std::abs(t - cur_t) < max_dt_threshold) {
                    neighbors.push_back({(double)cur_x + i, (double)cur_y + j, t});
                }
            }
        }
    }

    void estimateParameter(const std::vector<std::array<double, 3>> &neighbors, double *parameter) {
        ceres::Problem problem;

        for (const auto &data : neighbors)
            problem.AddResidualBlock(new ceres::AutoDiffCostFunction<PlaneFit, 1, 3>(
                                         new PlaneFit(data[0], data[1], data[2])),
                                     nullptr, parameter);

        ceres::Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = true;

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

public:
    LocalPlanesFlow(int ww, int hh) : w(ww), h(hh) {
        sae0 = new double[w * h];
        sae1 = new double[w * h];

        memset(sae0, 0, w * h * sizeof(double));
        memset(sae1, 0, w * h * sizeof(double));
    }

    ~LocalPlanesFlow() {
        delete sae0;
        delete sae1;
    }

    std::array<double, 2> opticalVelocity(const Event &curr_event) {

        sae = curr_event.polarity ? sae1 : sae0;
        cur_x = curr_event.x;
        cur_y = curr_event.y;
        cur_t = curr_event.timestamp;

        sae[cur_x + cur_y * w] = cur_t;

        // skip boundary pixels
        if (cur_x < L || cur_y < L || cur_x > w - L - 1 || cur_y > h - L - 1)
            return {0., 0.};

        std::vector<std::array<double, 3>> neighbors;
        neighborhood(neighbors);

        // too less pixel to estimate the plane
        if (neighbors.size() < 3) {
            return {0., 0.};
        }

        estimateParameter(neighbors, new_estimate);

        do {
            
            memcpy(old_estimate, new_estimate, 3 * sizeof(double));

            decltype(neighbors) new_neighbors;

            for (const auto &data : neighbors) {
                if(distance(data, old_estimate) < th2){
                    new_neighbors.push_back(data);
                }
            }

            // if(new_neighbors.size() == new_neighbors.size())
            //     break;

            estimateParameter(new_neighbors, new_estimate);

            neighbors = new_neighbors;

        } while (difference() > th1);


        // since we get the optimal plane parameter
        // we have that
        // $frac{partial z}{partial x} = a, frac{partial z}{partial y} = b$

        return {1. / new_estimate[0], 1. / new_estimate[0]};
    }
};

} // namespace event

使用需要安装好ceres求解器，大家也可以直接使用Eigen求解这个线性最小二乘问题(

)，没有必要用迭代法去解。