2019年4月やること!

ということで,2019年新年度!今月やることです.今月末から GW が始まるので,GW の自由研究に向けての下準備を中心にしていきます!

今月(~GW前後)のテーマ

2019年やりたいこと!で宣言した中で,今月(から GW にかけての)のテーマは下記です(゚Д゚)ノ

3次元復元プロジェクト再開

・最適化・バンドル調整の理解・コードいじり.
・画像特徴量(Fast, SIFT etc...)の理解・コードいじり.
・小さな SfM のサンプルコード作成.

4月は GW の自由研究ネタである「3次元復元」に向けた勉強・コード作成をします.「3次元復元」といっても,解こうとする問題のサイズによって難易度がだいぶ変わってくると思うので,4月はまず手持ちのカメラで撮った10枚程度の写真を復元するところから始めたいと思います.一年半くらい前に "Structure from Motion 事始め" と題して(全然始まらなかったんですが(笑))アップした下記の記事があるんですが,ここで紹介した下記の本に SfM の簡単なサンプルコードがあります.すごいシンプルな実装なので,あんまりきれいに復元できないみたいなんですが,ちょっとここから始めます. OpenCV に実装されてあるE行列計算の関数とかをそのまま使っているので,必要に応じて部分的に違うアルゴリズムを使ってみたりして,どのくらい復元できるようになるかやってみます.
daily-tech.hatenablog.com

SfMのサンプルコードを載せてくれてある本.

1.画像処理

ということで,画像処理に関しては「必要なとこだけ読み」になってしまいますが,,,

4章の Exploring Structure from Motion Using OpenCVを!

E行列,F行列の計算方法を確認.

3次元コンピュータビジョン計算ハンドブック

3次元コンピュータビジョン計算ハンドブック

難易度は高そうですが,,,必要に応じて....

Multiple View Geometry in Computer Vision

Multiple View Geometry in Computer Vision

2.ソフトウェア

数式に疲れたら,,,

今月はテンプレートの月にします!

C++ テンプレート完全ガイド (Programmer’s SELECTION)

C++ テンプレート完全ガイド (Programmer’s SELECTION)

ということで,My Small SFMプログラムができたら,今月は花丸君とします(゚Д゚)ノ

2019年2・3月の釣果

ブログ更新,前回の更新から1か月半近くたってしまいました...さぼりました...

でもあれですね...やっぱり欲張りすぎるとだめですね.自分の本読み速度が遅いことと相まって,本当に本読みで必死になってました(笑)
ここからは,GWの自由研究ネタと11月のつくばチャレンジに向けて手を動かすことに重点を移していこうと思います.詳しくは次のエントリで書きます.

2月の読了図書

Modern Effective C++
C++11の壁を超える (゚Д゚)ノ とこまではまだ行ってませんが,だいぶ景色が見えてきました.ただ使いこなすのは練習が必要そうですね...

Effective Modern C++ ―C++11/14プログラムを進化させる42項目

Effective Modern C++ ―C++11/14プログラムを進化させる42項目

Effective STL
あとは実際に書く中で使う量を増やすのみ!

Effective STL―STLを効果的に使いこなす50の鉄則

Effective STL―STLを効果的に使いこなす50の鉄則

  • 作者: スコットメイヤーズ,Scott Meyers,細谷昭
  • 出版社/メーカー: ピアソンエデュケーション
  • 発売日: 2002/01
  • メディア: 単行本
  • 購入: 9人 クリック: 155回
  • この商品を含むブログ (95件) を見る

SLAM入門
一通り読了しました.スキャンマッチングのやり方(サンプリングを等間隔にするとこ)とか,目からうろこでした.

SLAM入門: ロボットの自己位置推定と地図構築の技術

SLAM入門: ロボットの自己位置推定と地図構築の技術

3月の読了図書

アジャイルソフトウェア開発の奥義 第2版 オブジェクト指向開発の神髄と匠の技
一通り読了です!が,この手の本は繰り返し読んで,実際に自分でデザインする行為を繰り返し読まないとモノにはならないですね...

アジャイルソフトウェア開発の奥義 第2版 オブジェクト指向開発の神髄と匠の技

アジャイルソフトウェア開発の奥義 第2版 オブジェクト指向開発の神髄と匠の技

詳解 OpenCV 3 ―コンピュータビジョンライブラリを使った画像処理・認識
分厚かった..ですが一通り読了しました.細かい理解はできてないですが,OpenCV API カタログは頭の中にできたと思うので,こっからガンガン使っていきます!

詳解 OpenCV 3 ―コンピュータビジョンライブラリを使った画像処理・認識

詳解 OpenCV 3 ―コンピュータビジョンライブラリを使った画像処理・認識

目指せ今年の読了30冊!

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;
}

Ceres Solver の使い方2〜3次関数のフィッティング

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

daily-tech.hatenablog.com

3次関数のフィッティング

1.問題設定(3次関数のフィッティング)

前回とほぼ同じですが,3次関数のフィッティングをやってみます.3次関数なので,推定するパラメータが2つ増えています.

問題設定は下記のとおりです.

f:id:rkoichi2001:20190226050513j:plain
最小二乗法を用いた3次関数のフィッティング

2.3次関数のフィティングと Ceres の問題設定

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

f:id:rkoichi2001:20190226050547j:plain
3次関数のフィティングと Ceres Solver での問題設定

1. Ceres に調整してほしい変数は何か?
> 2次関数のパラメータ3つ.(a, b, c)

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

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

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

コスト関数の差分

当然ながらコストを計算する関数が3次関数になっています.

struct CostFunctor {
  CostFunctor(double x, double y) : m_x(x), m_y(y) {}

  // コスト計算部を3次関数に.
  template <typename T> bool operator()(const T *const x, T *residual) const {
    T val = x[0] * std::pow(m_x, 3.0) + x[1] * std::pow(m_x, 2.0) + x[2] * m_x +
            x[3];
    residual[0] = val - m_y;
    return true;
  }

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

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

    ceres::Problem problem;
    for (size_t i = 0; i < x_vec.size(); i++) {
      ceres::CostFunction *cost_function =
          new ceres::AutoDiffCostFunction<CostFunctor, 1, 4>(     // Residual の次元は1,調整パラメータは4つなので,4.
              new CostFunctor(x_vec[i], y_vec_noise[i]));
      problem.AddResidualBlock(cost_function, nullptr, co_est.data());
    }

4.実行結果

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

f:id:rkoichi2001:20190226044949p:plain
Ceres Solver を使って解いた3次関数フィッテイング

5.コード

一応全コードを下記に載せておきます.自分のGithubにも上げているので,何かの役に立てば...該当コードは curve_fitting2.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 unary_function<T, T> {
  CubicFunctor(vector<double> &c) : m_c(c) {}
  T operator()(T x) {
    T x3 = x * x * x;
    T x2 = x * x;
    T x1 = x;
    return m_c[0] * x3 + m_c[1] * x2 + m_c[2] * x1 + m_c[3];
  }

private:
  vector<T> m_c;
};

template <typename T> struct NoiseAddingFunctor : public unary_function<T, T> {
  NoiseAddingFunctor(T mean, T stdv) : engine(), dist(mean, stdv) {}
  T operator()(T x) { return x + dist(engine); }

private:
  default_random_engine engine;
  normal_distribution<> dist;
};

struct CostFunctor {
  CostFunctor(double x, double y) : m_x(x), m_y(y) {}

  template <typename T> bool operator()(const T *const x, T *residual) const {
    T val = x[0] * std::pow(m_x, 3.0) + x[1] * std::pow(m_x, 2.0) + x[2] * m_x +
            x[3];
    residual[0] = val - m_y;
    return true;
  }

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

int main(int argc, char **argv) {
  std::cout << __FILE__ << std::endl;
  google::InitGoogleLogging(argv[0]);

  // 1. Setting Up Sample Points.
  double minx = -5.0, maxx = 5.0, reso = 0.1;
  int num = (maxx - minx) / reso + 1;
  // Determine Coefficient
  vector<double> coeff{0.5, -0.5, -10.5, 1.0};

  // Create X Vector
  vector<double> x_vec(num, 0.0);
  iota(x_vec.begin(), x_vec.end(), 0.0);
  transform(x_vec.begin(), x_vec.end(), x_vec.begin(),
            [minx, reso](double val) { return val * reso + minx; });

  // Create Y Vector
  vector<double> y_vec(num, 0.0);
  transform(x_vec.begin(), x_vec.end(), y_vec.begin(),
            CubicFunctor<double>(coeff));

  // Create Noised Y Vector
  vector<double> y_vec_noise(num, 0.0);
  transform(y_vec.begin(), y_vec.end(), y_vec_noise.begin(),
            NoiseAddingFunctor<double>(0.0, 5.0));

  // 2. Create Ceres Problem
  vector<double> co_est(4, 0.0);
  std::vector<double> y_est(x_vec);
  {
    ceres::Problem problem;
    for (size_t i = 0; i < x_vec.size(); i++) {
      ceres::CostFunction *cost_function =
          new ceres::AutoDiffCostFunction<CostFunctor, 1, 4>(
              new CostFunctor(x_vec[i], y_vec_noise[i]));
      problem.AddResidualBlock(cost_function, nullptr, co_est.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;
    {
      ostream_iterator<double> out(std::cout, ", ");
      copy(coeff.begin(), coeff.end(), out);
      std::cout << std::endl << std::flush;
    }

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

    // Generate Point on Estimated Curve.
    transform(x_vec.begin(), x_vec.end(), y_est.begin(),
              CubicFunctor<double>(co_est));
  }

  // 3. Plot Result
  {
    plt::xlim(minx - 5, maxx + 5);

    plt::plot(x_vec, y_vec);
    plt::title("Orignal vs Estimated");
    plt::scatter(x_vec, y_vec_noise);
    plt::plot(x_vec, y_est);

    plt::show();
  }

  return 0;
}

Ceres Solver の使い方1〜イントロダクション

久々の投稿です.やっぱ2月は短いですね...2月の目標の半分くらいしか行ってません.本にかじりついて消費ページ数を必死で増やしてるんですが,もはや「スキルを上げるために勉強するのか?」,「消費ページを増やすために本を読むのか?」わけわかんなくなってきました.でも読了した本が積み上がっていくのって気持ちいいんですよね(笑)

とはいうもののやっぱり理解しながら進めていかないと意味ないので手を動かすネタを探してたんですが,GoogleのCartographerを勉強していくなかで,Ceres Solverの使い方を勉強したのでそれをアップすることにしました.今後結構使いそうなので,割と気合を入れて勉強してます.

1. Ceres Solver とは?

Googleが開発した最適化計算用のパッケージ.Debianパッケージとして管理されていて,apt でもインストールできます.

sudo apt install libceres-dev

GitHubからソースダウンロードもできます.
github.com

2. サンプルコード

Ceres Solver の使い方ですが,Tutorialとして下記にわかりやすくまとめられています.
ceres-solver.org

あと,上記のTutorialで詳しく取り上げられてはいませんが,下記のGitにもサンプルコードがたくさんあります.サンプルコードといっても,本格的なバンドル調整とかのコードもあって,結構必見です.春に再トライしようと思っている三次元復元にも使えそうです.
ceres-solver.googlesource.com

3. 使い方

上記の Tutorial にわかりやすいサンプルを載せてくれているんですが,ちょっと使いこなせるようになりたかったので自分でもいくつか問題を説いてみます.

Ceres Solverを使った直線のフィッティング

まずは手始めに一番シンプルな直線のフィッティングを行います.問題設定のイメージは下記のとおりです.

f:id:rkoichi2001:20190224233448j:plain
最小二乗法を用いた直線のフィッティング
サンプリングした点がありますが,この点には通常ノイズが乗っているのでどの点も通るような直線を引くことは不可能です.なので一番つじつまが合っている直線を引く必要があるのですが,「つじつまが合っている」を数学的に記述してあげないといけません.で,今回の場合はサンプリング点のy座標と直線のy座標の差分の二乗和が最小になるように設定したということです.コスト関数自体は上図の二重線を引いた箇所になります.

(上記の図を見ていただくとわかりますが,この問題は厳密な意味では最適化ではないです.この問題設定ではサンプル点と直線間の距離はy座標の差だけを取り上げてますが,厳密な最適化問題の場合はサンプル点と直線の距離は点と直線の距離を使うべきです.(もしくはもっとちゃんとするならマハラノビス距離)ただ,まあ今回はこの問題設定でこれで行くとします.)

で,最適化する側(Ceres Solver)の観点でこの問題を見ると,下記のようになります.
1. 観測したサンプリング点座標は固定値.
2. 最適化する側が調整できる変数は,この問題の場合は直線の傾き "a" と切片 "b" のみ.
ということで,数学的には難しいことをいろいろやっていても,結局は

「 (a, b) をいろいろと変えてみて,評価関数が極小になるところを探す」

ということが最適化パッケージのやっていることになります.

Ceres Solver の API 観点で見た直線のフィティング問題

ということで,上記のフィッティング問題を Ceres Solver の API の観点で考えてみます.

Ceres Solver を使う上ではっきり意識しておかないといけないことは,下記の二点です.
1. Ceres に調整してほしい変数は何か?
2. 調整対象となる変数によって,全体のコストがどう変わるか?

で,この問題では....

f:id:rkoichi2001:20190224233547j:plain
直線フィティングとCeres Solverでの問題設定

1. Ceres に調整してほしい変数は何か?
> 直線の傾き "a" と切片 "b"
2. 調整対象となる変数によって,全体のコストがどう変わるか?
> 一つ一つの構成要素(Residual Block)の総和

それでは,上記の二つの点をコードと対応させて考えてみます.Ceresに調整してほしい変数 (a, b) を変えることによって全体のコストが変わりますが,この全体のコストは結局直線と一つのサンプル点の関係の総和に過ぎません.なので,まず一つのサンプル点と直線から決まるコストを定義してやる必要があります.これを関数オブジェクトとして定義します.この関数オブジェクトが計測点数分できるイメージになります.この関数オブジェクトがたった一点と直線の関係を表すというのは,privateのメンバ変数 m_x, m_y をみても解ると思います.

下記の関数オブジェクトでは,"const T *const x" として調整対象となるパラメータが配列の形でやってきます.ここでは,(a, b) = (x[0], x[1]) です.で,そのパラメータ設定だった時にこの関数オブジェクトが保持する点とのコストを residual[0] として返しています.

struct LinearCostFunctor {
  LinearCostFunctor(double x, double y) : m_x(x), m_y(y) {}
  template <typename T> bool operator()(const T *const x, T *residual) const {
    std::vector<T> c{x[0], x[1]};
    LinearFunctor<T> lFunc(c);
    residual[0] = static_cast<T>(m_y) - lFunc(static_cast<T>(m_x));
    return true;
  }

private:
  const double m_x, m_y;
};

上記の関数オブジェクトを,計測点数分作成して Ceres に渡します.ここで,ceres::Problem というクラスがひとつの最適化問題を表しています.こいつに対していろいろと条件を加えていきます.

    vector<double> co(2, 0.0); // <- こいつが調整したいパラメータを保持するためのメモリ領域.今回は (a, b) なので,double の変数2つ. 
    ceres::Problem problem;
    for (size_t i = 0; i < x_elems.size(); i++) {
      double x = x_elems[i];
      double y = y_n_elems[i];
      ceres::CostFunction *cost_function =
          new ceres::AutoDiffCostFunction<LinearCostFunctor, 1, 2>(
              new LinearCostFunctor(x, y));
      problem.AddResidualBlock(cost_function, nullptr, co.data());
    }

上記のコードでは大きく2つのことをやってます.まず,
1.CostFunctionの作成
観測点毎にコスト関数を作ってあげます.このコスト関数は AutoDiffCostFuctionの他にも微分の仕方やパラメータブロックの生成方法によって DynamicAutoDiffCostFunction, NumericDiffCostFunction, DynamicNumericDiffCostFunction といくつかの種類があるのですが,ここでは一番基本的な AutoDiffCostFunction を使っています.で,AutoDiffCostFunction のテンプレート引数に注目していただきたいのですが,

/* AutoDiffCostFunction<コストを定義した自作関数オブジェクト, Residualの次元数, パラメータの次元数...> */
AutoDiffCostFunction<LinearCostFunctor, 1, 2>

今回のケースでは,コスト関数オブジェクトは LinearCostFunctor, Residual の次元数は r = (ax + b) - y なので1次元,パラメータは (a, b) なので2次元となっています.また,コードの最後で AddResigualBlockという関数を読んでコスト関数を一つづつ登録しています.その関数の最後の引数 "co.data()" というのが Ceres に調整してほしいパラメータを保持する配列になります.上記の設定が終われば,問題設定は完了です.(API使っていて本当に思うんですが,やっぱ Google の人すごいですね....こんな設計絶対思いつかない...というか慣れるのだけでヒーヒー言っているので..)あとは,細かい設定をして実際に問題をときます.

    // 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;

4. 実行結果

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

f:id:rkoichi2001:20190225001627p:plain
Ceres Solver を使って解いた直線フィッテイング

ということで,次回は2次,3次曲線のフィッティングをやってみます.

5. コード

一応全コードを下記に載せておきます.自分のGithubにも上げているので,何かの役に立てば...該当コードは curve_fitting1.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 LinearFunctor : public std::unary_function<T, T> {
  LinearFunctor(std::vector<T> &c) : m_c(c) {}
  T operator()(T x) { return m_c[0] * x + m_c[1]; }

private:
  vector<T> m_c;
};

struct LinearCostFunctor {
  LinearCostFunctor(double x, double y) : m_x(x), m_y(y) {}
  template <typename T> bool operator()(const T *const x, T *residual) const {
    std::vector<T> c{x[0], x[1]};
    LinearFunctor<T> lFunc(c);
    residual[0] = static_cast<T>(m_y) - lFunc(static_cast<T>(m_x));
    return true;
  }

private:
  const double m_x, m_y;
};

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;
};

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.1;
  int num_x = (maxx - minx) / resox + 1;
  // Determine Coefficients
  vector<double> coeff{1.5, 2};

  // 2. Problem Formulation
  vector<double> x_elems(num_x, 0.0), y_elems(num_x, 0.0),
      y_n_elems(num_x, 0.0), y_est_elems(num_x, 0.0);
  {
    // Create X Vector
    iota(x_elems.begin(), x_elems.end(), 0.0);
    transform(x_elems.begin(), x_elems.end(), x_elems.begin(),
              [num_x, minx, resox](double val) { return val * resox + minx; });

    // Create Y Vector
    LinearFunctor<double> lFunc(coeff);
    transform(x_elems.begin(), x_elems.end(), y_elems.begin(),
              [&lFunc](double val) { return lFunc(val); });

    // Create Noised Y Vector
    NoiseAddingFunctor<double> nNoise(0.0, 3.0);
    transform(y_elems.begin(), y_elems.end(), y_n_elems.begin(),
              [&nNoise](double val) { return nNoise(val); });
  }

  // 3. Ceres Estimation
  vector<double> co(2, 0.0);
  {
    ceres::Problem problem;
    for (size_t i = 0; i < x_elems.size(); i++) {
      double x = x_elems[i];
      double y = y_n_elems[i];
      ceres::CostFunction *cost_function =
          new ceres::AutoDiffCostFunction<LinearCostFunctor, 1, 2>(
              new LinearCostFunctor(x, y));
      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 Y Vector
    LinearFunctor<double> lFunc(co);
    transform(x_elems.begin(), x_elems.end(), y_est_elems.begin(),
              [&lFunc](double val) { return lFunc(val); });
  }

  // 4. Plot Result
  {
    plt::title("Original vs Estimated");
    plt::plot(x_elems, y_elems);
    plt::scatter(x_elems, y_n_elems);
    plt::plot(x_elems, y_est_elems);
    plt::show();
  }

  return 0;
}

2019年2月やること!

ということで,2月の目標設定です!2月は短い&お休みがあんまりないので,どこまでできるかわからないですが,1月が割とうまくいったのでちょっと欲張って設定してしまいました.

今月のテーマ

2019年やりたいこと!で宣言した中で,今月のテーマは下記です (゚Д゚)ノ

1.SLAM のお勉強

Google Cartographer の理解とコードいじり.
 先月は openslam_gmapping のコード読みとリファクタリング&部品パクりをやってました(まだ終わってませんが...)が,今月は Google Cartographer で同じことをやってみます.Scan Matching のところとか,最適化計算のところとかいろいろと重要なトピックが出てくると思うので,ブログに何か残せれば.

 あと,ありがたく参照していただいた下記ブログで紹介されていた「SLAM入門」という本ですが,わかりやすそうだったので,今月の課題図書にしました.これと確率ロボティクスを読みながら,Cartographer に立ち向かいます(゚Д゚)ノ

ssk0109.hatenablog.com

 なんだか「SLAM入門」にしても,後述する「アジャイルソフトウェア開発の奥義」にしてもそうなんですが,「この本良いよ!」って言われたら,まず買ってみてから読むかどうか決めるみたいな感じなんですよね(笑)参考書ばっかり増えていく受験生みたいになってますが(実際そうでしたが(笑)),まあ良しとしましょう.

2.C++を(ある程度)マスター

 Scott Meyers の本ばかり読んでますが,せっかく2冊(Effective C++, More Effective C++)読んだのであとの2冊(Modern Effective C++, Effective STL)も読んでしまいます.C++11/14って意識して使ったことがないので,読むの時間かかりそうですが.
 あとは,先月の Clean Architecture に引き続き,アーキテクチャの本を一冊読んでみます.「アジャイルソフトウェア開発の奥義」って本がいいって聞いたので,またもや本棚に一冊追加してしまいました(笑).

今月の課題図書

ということで,今月の課題図書は下記の4冊です!

確率ロボティクス
11章を Google Cartographer のコードいじりとともに読みます.

確率ロボティクス (プレミアムブックス版)

確率ロボティクス (プレミアムブックス版)

SLAM入門
前述ブログでおすすめされてたので,買ってみました.いい感じにまとまっていて,わかりやすそうです.

SLAM入門: ロボットの自己位置推定と地図構築の技術

SLAM入門: ロボットの自己位置推定と地図構築の技術

Modern Effective C++
C++11の壁を越えます(゚Д゚)ノ

Effective Modern C++ ―C++11/14プログラムを進化させる42項目

Effective Modern C++ ―C++11/14プログラムを進化させる42項目

More Effective C++
目指せSTLマスター.

Effective STL―STLを効果的に使いこなす50の鉄則

Effective STL―STLを効果的に使いこなす50の鉄則

  • 作者: スコットメイヤーズ,Scott Meyers,細谷昭
  • 出版社/メーカー: ピアソンエデュケーション
  • 発売日: 2002/01
  • メディア: 単行本
  • 購入: 9人 クリック: 155回
  • この商品を含むブログ (95件) を見る

アジャイルソフトウェア開発の奥義
分厚いから,2月中には終わらないかも...

アジャイルソフトウェア開発の奥義 第2版 オブジェクト指向開発の神髄と匠の技

アジャイルソフトウェア開発の奥義 第2版 オブジェクト指向開発の神髄と匠の技

ということで,ちょっと欲張りすぎですが...今月の目標設定でした.
目指せ,今年の読了30冊!

2019年1月の釣果

ということで,年初に宣言した通り今月の成果です!
1月は冬休み時間があったこともあり,割といい感じに終わりました(゚Д゚)ノ

1.SLAM のお勉強

・確率ロボティクスの勉強.
に関しては,EKF, Particle Filter を使う自己位置推定,Particle Filter を用いた SLAM のサンプルコードができました.確率ロボティクスをかたっぱしから実装してやろうかとも思ったんですが,それは無理でした(笑)
github.com

・GMappingの理解とコードいじり.
ここが結構悩ましくて,OSSのコードってただ読むだけだといまいち本質的なとこまで理解できないんですよね.「フフーン( `ー´)ノ」って感じで理解したつもりなんですが,読み終わったらあんまわかってないみたいな.で,どう進めるのがいいかな...悩んだ結果,OSSのコードを読んで,少しずつ部品をリファクタ&パクっていって,自分のライブラリ化することにしました.これも公開したいんですが,ライセンス周りがいまいちわかっておらず,今はGithubのプライベートリポジトリでやってます.

2.C++を(ある程度)マスター

 こちらは今月の課題図書読了です!More Effective C++は二周くらい読んでだいぶ理解できてきましたが,やっぱり時間がたつと忘れてますね(笑)2月,3月とC++関連の本を読んで重ね塗りしていくことで,忘却曲線に立ち向かいます.

1月の読了図書!

ということで,積読本が3冊減りました!!

確率ロボティクス
※14,15,16章は除く...

確率ロボティクス (プレミアムブックス版)

確率ロボティクス (プレミアムブックス版)

Clean Architecture

Clean Architecture 達人に学ぶソフトウェアの構造と設計

Clean Architecture 達人に学ぶソフトウェアの構造と設計

More Effective C++

新訂版 More Effective C++ (AddisonーWesley professional co)

新訂版 More Effective C++ (AddisonーWesley professional co)

  • 作者: スコット・メイヤーズ,安村通晃,伊賀聡一郎,飯田朱美,永田周一
  • 出版社/メーカー: ピアソンエデュケーション
  • 発売日: 2007/06/29
  • メディア: 単行本(ソフトカバー)
  • 購入: 8人 クリック: 129回
  • この商品を含むブログ (44件) を見る