Ceres Solver の使い方１〜イントロダクション
久々の投稿です．やっぱ２月は短いですね．．．２月の目標の半分くらいしか行ってません．本にかじりついて消費ページ数を必死で増やしてるんですが，もはや「スキルを上げるために勉強するのか？」，「消費ページを増やすために本を読むのか？」わけわかんなくなってきました．でも読了した本が積み上がっていくのって気持ちいいんですよね（笑）
とはいうもののやっぱり理解しながら進めていかないと意味ないので手を動かすネタを探してたんですが，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を使った直線のフィッティング
まずは手始めに一番シンプルな直線のフィッティングを行います．問題設定のイメージは下記のとおりです．
（上記の図を見ていただくとわかりますが，この問題は厳密な意味では最適化ではないです．この問題設定ではサンプル点と直線間の距離はy座標の差だけを取り上げてますが，厳密な最適化問題の場合はサンプル点と直線の距離は点と直線の距離を使うべきです．（もしくはもっとちゃんとするならマハラノビス距離）ただ，まあ今回はこの問題設定でこれで行くとします．）
で，最適化する側（Ceres Solver）の観点でこの問題を見ると，下記のようになります．
1. 観測したサンプリング点座標は固定値．
2. 最適化する側が調整できる変数は，この問題の場合は直線の傾き "a" と切片 "b" のみ．
ということで，数学的には難しいことをいろいろやっていても，結局は
「 (a, b) をいろいろと変えてみて，評価関数が極小になるところを探す」
ということが最適化パッケージのやっていることになります．
Ceres Solver の API 観点で見た直線のフィティング問題
ということで，上記のフィッティング問題を Ceres Solver の API の観点で考えてみます．
Ceres Solver を使う上ではっきり意識しておかないといけないことは，下記の二点です．
1. Ceres に調整してほしい変数は何か？
2. 調整対象となる変数によって，全体のコストがどう変わるか？
で，この問題では．．．．
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 の変数２つ． 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()); }
上記のコードでは大きく２つのことをやってます．まず，
１．CostFunctionの作成
観測点毎にコスト関数を作ってあげます．このコスト関数は AutoDiffCostFuctionの他にも微分の仕方やパラメータブロックの生成方法によって DynamicAutoDiffCostFunction, NumericDiffCostFunction, DynamicNumericDiffCostFunction といくつかの種類があるのですが，ここでは一番基本的な AutoDiffCostFunction を使っています．で，AutoDiffCostFunction のテンプレート引数に注目していただきたいのですが，
/* AutoDiffCostFunction<コストを定義した自作関数オブジェクト, Residualの次元数, パラメータの次元数．．．> */ AutoDiffCostFunction<LinearCostFunctor, 1, 2>
今回のケースでは，コスト関数オブジェクトは LinearCostFunctor, Residual の次元数は r = (ax + b) - y なので１次元，パラメータは (a, b) なので２次元となっています．また，コードの最後で 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 でフィティングした直線になります．
ということで，次回は２次，３次曲線のフィッティングをやってみます．
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; }
