牛顿迭代法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