Ceres Solver の使い方3〜曲面のフィッティング

引き続き Ceres Solver のサンプルです.前回の3次関数フィッティングより少し問題を複雑化して解いてみます.

1.問題設定(曲面のフィッティング)

前回とほぼ同じですが,曲面のフィッティングをやってみます.曲面なので,推定するパラメータが前回よりだいぶ増えることと,入力が x だけでなく, (x, y) の二つになります.

2.曲面のフィティングと Ceres の問題設定

ということで,前回のエントリでも書いた下記の2点を明確にします.

f:id:rkoichi2001:20190226052647j:plain
曲面のフィティングと Ceres Solver での問題設定

1. Ceres に調整してほしい変数は何か?
> 3次曲面のパラメータ3つ.(c0, c1, c2, c3, c4, c5, c6, c7, c8, c9, c10)

2. 調整対象となる変数によって,全体のコストがどう変わるか?
> 上図の一つ一つの Residual Block の総和が全体のコストを定義します.

3.直線のフィティングとの差分

3次関数フィティング問題の時からの差分ですが,Ceres Solver のコードに直接的に関係する差分は下記の2点だけです.ほかは全く同じ用に問題を解けます.

コスト関数の差分

当然ながらコストを計算する関数が曲面対応になっています.

struct CubicCostFunctor
{
  CubicCostFunctor(double x, double y, double z) : m_x(x), m_y(y), m_z(z) {}

  template <typename T>
  bool operator()(const T *const x, T *residual) const
  {
    std::vector<T> c{x[0], x[1], x[2], x[3], x[4],
                     x[5], x[6], x[7], x[8], x[9]};
    CubicFunctor<T> cFunc(c);
    residual[0] = m_z - cFunc(static_cast<T>(m_x), static_cast<T>(m_y));
    return true;
  }

private:
  const double m_x, m_y, m_z;
};
CostFunctionのパラメータ設定

調整パラメータの数が4つから10個に増えているのでそれをテンプレートパラメータとして指定します.

    ceres::Problem problem;
    for (size_t j = 0; j < y_elems.size(); j++)
    {
      for (size_t i = 0; i < x_elems.size(); i++)
      {
        ceres::CostFunction *cost_function =
            new ceres::AutoDiffCostFunction<CubicCostFunctor, 1, 10>(
                new CubicCostFunctor(x_elems[j][i], y_elems[j][i],
                                     z_n_elems[j][i]));
        problem.AddResidualBlock(cost_function, nullptr, co.data());
      }
    }

4.実行結果

下記,実行結果です.matplotlib-cppを使って,c++からPythonを読んでます.下図の青い曲線がもともとの直線,点がノイズを含む点,そして緑の曲線が Ceres でフィティングした直線になります.

f:id:rkoichi2001:20190226051449p:plain
元の曲面

f:id:rkoichi2001:20190226051518p:plain
推定する点群から構成される曲面

f:id:rkoichi2001:20190226051546p:plain
最小二乗法で推定したパラメータを使って復元した曲面

5.コード

一応全コードを下記に載せておきます.自分のGithubにも上げているので,何かの役に立てば...該当コードは curve_fitting3.cpp というコードです.
github.com

// Standard Lib
#include <iostream>
#include <random>

// STL
#include <algorithm>
#include <iterator>
#include <vector>

// Ceres
#include <ceres/ceres.h>
#include <glog/logging.h>

// Matplot Lib
#include <matplotlibcpp.h>

using namespace std;
namespace plt = matplotlibcpp;

template <typename T>
struct CubicFunctor : public std::binary_function<T, T, T>
{
  CubicFunctor(std::vector<T> &c) : m_c(c) {}
  T operator()(T x, T y)
  {

    // 3rd order terms
    T x3 = m_c[0] * x * x * x;
    T y1x2 = m_c[1] * y * x * x;
    T y2x1 = m_c[2] * y * y * x;
    T y3 = m_c[3] * y * y * y;

    // 2nd order terms
    T x2 = m_c[4] * x * x;
    T y1x1 = m_c[5] * y * x;
    T y2 = m_c[6] * y * y;

    // 1st order terms
    T x1 = m_c[7] * x;
    T y1 = m_c[8] * y;

    // 0th order tems
    T x0y0 = m_c[9] * 1.0;

    return x3 + y1x2 + y2x1 + y3 + x2 + y1x1 + y2 + x1 + y1 + x0y0;
  }

private:
  std::vector<T> m_c;
};

template <typename T>
struct NoiseAddingFunctor : public unary_function<T, T>
{
  NoiseAddingFunctor(T mean, T stdv) : gen(), dist(mean, stdv)
  {
    random_device rd;
    gen.seed(rd());
  }
  NoiseAddingFunctor(const NoiseAddingFunctor &obj)
  {
    dist = obj.dist;
    gen = obj.gen;
    random_device rd;
    gen.seed(rd());
  }
  T operator()(T x) { return x + dist(gen); }

private:
  mt19937_64 gen;
  normal_distribution<> dist;
};

struct CubicCostFunctor
{
  CubicCostFunctor(double x, double y, double z) : m_x(x), m_y(y), m_z(z) {}

  template <typename T>
  bool operator()(const T *const x, T *residual) const
  {
    std::vector<T> c{x[0], x[1], x[2], x[3], x[4],
                     x[5], x[6], x[7], x[8], x[9]};
    CubicFunctor<T> cFunc(c);
    residual[0] = m_z - cFunc(static_cast<T>(m_x), static_cast<T>(m_y));
    return true;
  }

private:
  const double m_x, m_y, m_z;
};

int main(int argc, char **argv)
{
  std::cout << __FILE__ << std::endl;

  // 1. Setting Up Sample Points.
  double minx = -5.0, maxx = 5.0, resox = 0.5;
  double miny = -5.0, maxy = 5.0, resoy = 0.5;
  int num_x = (maxx - minx) / resox + 1;
  int num_y = (maxy - miny) / resoy + 1;
  // Determine Coefficient
  std::vector<double> coeff{-0.2, 1.0, 1.0 - 0.2, 0.5, 1.0,
                            0.5, -1.0, -1.0, 1.0};

  // 2. Problem Formulation
  vector<vector<double>> x_elems(num_y), y_elems(num_y), z_elems(num_y),
      z_n_elems(num_y), z_est_elems(num_y);
  {
    // Create X Vector
    transform(x_elems.begin(), x_elems.end(), x_elems.begin(),
              [num_x, minx, resox](const vector<double> &v) {
                vector<double> vnew(num_x, 0.0);
                iota(vnew.begin(), vnew.end(), 0.0);
                transform(
                    vnew.begin(), vnew.end(), vnew.begin(),
                    [minx, resox](double val) { return val * resox + minx; });
                return vnew;
              });

    // Create Y Vector
    transform(y_elems.begin(), y_elems.end(), y_elems.begin(),
              [num_x, miny, resoy](const vector<double> &v) {
                static int cnt = 0;
                double val = resoy * cnt + miny;
                cnt++;
                vector<double> vnew(num_x, val);
                return vnew;
              });

    // Create Z Vector
    CubicFunctor<double> cFunc(coeff);
    for (int j = 0; j < num_y; j++)
    {
      for (int i = 0; i < num_x; i++)
      {
        double x = x_elems[j][i];
        double y = y_elems[j][i];
        z_elems[j].push_back(cFunc(x, y));
      }
    }

    z_n_elems = z_elems;
    NoiseAddingFunctor<double> nFunc(0.0, 10.0);
    for_each(z_n_elems.begin(), z_n_elems.end(), [&nFunc](vector<double> &v) {
      transform(v.begin(), v.end(), v.begin(), nFunc);
    });
  }

  // 3. Ceres Estimation
  vector<double> co(10, 0.0);
  {
    ceres::Problem problem;
    for (size_t j = 0; j < y_elems.size(); j++)
    {
      for (size_t i = 0; i < x_elems.size(); i++)
      {
        ceres::CostFunction *cost_function =
            new ceres::AutoDiffCostFunction<CubicCostFunctor, 1, 10>(
                new CubicCostFunctor(x_elems[j][i], y_elems[j][i],
                                     z_n_elems[j][i]));
        problem.AddResidualBlock(cost_function, nullptr, co.data());
      }
    }

    // Run the solver.
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

    // Status Report!
    std::cout << summary.BriefReport() << std::endl;

    std::cout << "Original Parameters" << std::endl;
    {
      std::ostream_iterator<double> out(std::cout, ", ");
      copy(coeff.begin(), coeff.end(), out);
      std::cout << std::endl
                << std::flush;
    }

    std::cout << "Estimated Parameters" << std::endl;
    {
      std::ostream_iterator<double> out(std::cout, ", ");
      copy(co.begin(), co.end(), out);
      std::cout << std::endl
                << std::flush;
    }

    // Create Estimated Z Vector
    CubicFunctor<double> cFuncEst(co);
    for (int j = 0; j < num_y; j++)
    {
      for (int i = 0; i < num_x; i++)
      {
        double x = x_elems[j][i];
        double y = y_elems[j][i];
        z_est_elems[j].push_back(cFuncEst(x, y));
      }
    }
  }

  // 4. Plot Result
  {
    plt::plot_surface(x_elems, y_elems, z_elems);
    plt::title("Original Surface");

    plt::plot_surface(x_elems, y_elems, z_n_elems);
    plt::title("Noised Surface");

    plt::plot_surface(x_elems, y_elems, z_est_elems);
    plt::title("Estimated Surface");

    plt::show();
  }
  return 0;
}