Ceres Solver で遊んでみる

仕事に空きができたので、面白そうだけどフォローできていなかったライブラリやツールの調査をしている。
で、Googleがオープンソースで公開している"Ceres Solver"が凄そうだ。非線形の最小二乗法ソルバーでgoogle社内の様々なプロジェクトでも利用されているようだ。
ただwebを探しても情報が結構少なく、チュートリアルもものすごく単純なケースか、Bundler Adjustmentのような複雑な例しかない。以下のような2D図形でゴニョゴニョするサンプルコードなんかがあるといいんだけど見つからない。


そこで、習作としてひとまず2Dポリゴンの位置合わせなんかに使えないかなと実験してみた。

アルゴリズムは単純なものにした。位置合わせ先と元の2Dポリゴンの境界線上で適当にサンプリングして、得られた点群同士を 単純なICPのちょっと変形版で位置合わせを行うというものだ。
パラメータの自由度は 平行移動(x,y)、スケール(x,y)、xy平面での回転の5つ。

誤差関数は以下のような感じ。

誤差関数

template<typename T>
void Transform(T x, T y, T tx, T ty, T r, T sx, T sy, T& xNew, T& yNew) {
    xNew = sx * (x * cos(r) - y * sin(r)) + tx;
    yNew = sy * (x * sin(r) + y * cos(r)) + ty;
}

struct PointResidual {
    PointResidual(const Point& model, const Point& data)
        : model_(model), data_(data) {}

    template <typename T>
    bool operator()(const T* const t, const T* r, const T* s, T* residual) const {
        T x, y;
        Transform(T(data_.x()), T(data_.y()), t[0], t[1], r[0], s[0], s[1], x, y);

        T dx = T(model_.x()) - x;
        T dy = T(model_.y()) - y;
        residual[0] = sqrt(dx*dx + dy*dy);
        return true;
    }

private:
    Point model_;
    Point data_;
};

自動微分を使うために、operator()内の演算をすべてジェネリック型で行ったが、数値微分とあまり結果が変わらなかった。何か間違っているのだろうか?
そして、自動微分なんて技術があるなんて初めて知った。用意された演算子や関数を使って関数書くだけで厳密な微分を行ってくれる。ceresだと普通にdoubleで計算してるみたいなコードで書ける。すごい。

データ群の設定は以下の様な感じ。
ICPに詳しくないのでここの論文を参考にしてみたが、論文では変位した変換元の点に対して、変換先の最近点を対にするだけで良いようだがこれではスケール成分が0になって変換先の1点に収束する現象が生じてしまった。
仕方ないので逆方向の変換先の点に対して、変位した変換元の点も対として用いるようにした。一般的にはどうやって対応しているんだろう?

データ入力

    while (true) {
        ...

        Problem problem;
        std::vector<Point> dataT;
        for (auto ptD : data.outer()) {
            // 変換元の点を現在の変換行列で変位
            double x, y;
            Transform(ptD.x(), ptD.y(), param[0], param[1], param[2], param[3], param[4], x, y);
            Point ptT(x, y);
            dataT.push_back(ptT);

            // 変換先の点群から最も近い点を検索する
            size_t iNearest;
            FindNeareset(ptT, model, iNearest);

            // コスト関数に登録
            CostFunction* cost_function =
                new AutoDiffCostFunction<PointResidual, 1, 2, 1, 2>(
                    new PointResidual(model.outer()[iNearest], ptD));
            problem.AddResidualBlock(cost_function, NULL, param, &param[2], &param[3]);
        }

        for (auto ptM : model.outer()) {
            // 変換先の点に対し、変位した変換元の点群の中で最も近い点を検索する
            size_t iNearest = 0;
            FindNeareset(ptM, dataT, iNearest);

            // コスト関数に登録
            CostFunction* cost_function =
                new AutoDiffCostFunction<PointResidual, 1, 2, 1, 2>(
                    new PointResidual(ptM, data.outer()[iNearest]));
            problem.AddResidualBlock(cost_function, NULL, param, &param[2], &param[3]);
        }

        // 解いてみる
        Solver::Options options;
        options.linear_solver_type = ceres::DENSE_QR;
        options.minimizer_progress_to_stdout = false;
        Solver::Summary summary;
        Solve(options, &problem, &summary);

        // 変位行列が前回と変わらなかったら終了, そうでなければ繰り返し
        ...
    }

結果は以下のようになった。太い灰色線が求めるべき変換先のポリゴン、緑色線が変換元のポリゴン、赤い線がceresで求めた変換行列を緑色ポリゴンに施したもの。サンプリング点数は数百点ほど。

例1

真値: 平行移動=(-20,12), 回転角=30, スケール=(3,0.8)
計算値: 平行移動=(-20.1121,11.9887), 回転角=29.8778, スケール=(2.99513,0.801453)

例2

真値: 平行移動=(-20,10), 回転角=30, スケール=(3,0.8)
計算値: 平行移動=(-20.3742,10.0149), 回転角=29.8033, スケール=(3.01447,0.79894)

結構単純な方法だが結構良い精度で求まっているようだ。
ただしサンプリング点数を少なくすると、やはり変なところに収束してしまう。例2のサンプリング点数を50点ほどに少なくすると以下のようになる。

例3

ICPなんかをもっと勉強しないと実用するのは厳しそうか。
Ceres Solver自体はいろんな用途で使えそうだ。

UE4のビルドでハマった

UE4を githubから取得してVS2015でビルドしているのだが、ある日リポジトリから最新版を取得したところビルドが通らなくなった。
Oculus RiftがらみのDLLやIntelのライブラリがリンクできなくなって、いろいろ試行錯誤したが一向に治らない。
webで検索しても同じ症状の人がちらほら見つかるのだが、解決まで至っていない。

で、結局原因は単純なもので、UE4のルートフォルダのSetup.batとGenerateProjectFiles.batを再実行していないことだった。
ソースの更新内容によっては、これらの手順を再度踏まないといけないようだ。
最初にビルドしてから時間が空いてたから、そんな手順があったってことすっかり忘れてた。

Visual Studio 2015で gflagsを x64/Unicode文字セットのアプリケーションから利用する

google先生の作った gflagsというコマンドライン引数を扱うライブラリを タイトルの環境で使えるようにしたのだけど、とてもつかれた・・・ また同じ思いはしたくないのでメモ。ちなみにまだ簡単なサンプルでしか動作確認していないので注意。

[CMakeのインストール]
・cmakeがインストールされてなければ、CMake公式サイトから win32/x86のバイナリをダウンロードしてインストール

[gflagsのビルド]
1. githubから ソースを入手
2. VS2015の各種ツールのパスの通ったコンソールから、gflagsの CMakeLists.txt があるフォルダに移って、"cmake ."を実行
3. gflags.sln が生成されたはずなので、これをVS2015で開いてビルドすると、ひとまずx86でマルチバイト文字セットのライブラリができる

[gflagsをx64でビルド]
1. gflags.slnを開いた状態で、"ビルド->構成マネージャー"メニューを実行し、右上のアクティブソリューションプラットフォームの欄で新規作成を選び x64プラットフォームを作成する。
2. gflags_staticとgflags_nothred_staticプロジェクトのプロパティを開き、ライブラリアン->その他のオプションの項目で MachineX86オプションが明示的に指定されているのでこれらを除去。
3. これでx64のライブラリがビルドできるはず。

[gflagsをUnicode文字セットのプロジェクトからリンクする]
1. gflagsをUnicode文字セットでビルドするのはソースをガシガシ書き換える必要がありそうなので諦めて、gflags自体はマルチバイト文字セットのままにする。
2. Unicode文字セットのプロジェクトでは普通に gflagsをリンクし、google::ParseCommandLineFlags()で渡す引数をマルチバイトに変換して渡す。
3. これで利用できるはずだが、VS2015ではロケールの扱い周りでバグがあるらしく(?)、std::cout、std::wcoutにどちらかしか日本語出力ができなかったり、mbstowcs系の関数がうまく動かなかったりでハマった。 ひとまず Win32 APIの MultiByteToWideChar()を使ったり, coutへの出力は諦めて対応。コードはざっと以下のような感じ。

#include "../../../gflags/include/gflags/gflags.h"
#pragma comment(lib, "../../../gflags/x64/release/gflags_static.lib")
#pragma comment(lib, "shlwapi.lib") // __imp_PathMatchSpecAのリンクに必要

#include <iostream>
#include <vector>
#include <windows.h>

DEFINE_bool(big_menu, true, "Include 'advanced' options in the menu listing");
DEFINE_string(language, "japanese", "default language");

bool MBS2WCS(const std::string& s, std::wstring& sResult)
{
    int nBufSize = MultiByteToWideChar(CP_ACP, 0, s.c_str(), -1, NULL, 0);
    std::vector<wchar_t> vBuf(nBufSize);
    if (MultiByteToWideChar(CP_ACP, 0, s.c_str(), -1, &vBuf[0], nBufSize) == 0) {
        _ASSERTE(false);
        return false;
    }
    sResult = std::wstring(&vBuf[0]);
    return true;
}
bool WCS2MBS(const std::wstring& s, std::string& sResult)
{
    int nBufSize = WideCharToMultiByte(CP_ACP, 0, s.c_str(), -1, NULL, 0, NULL, NULL);
    std::vector<char> vBuf(nBufSize);
    if (WideCharToMultiByte(CP_ACP, 0, s.c_str(), -1, &vBuf[0], nBufSize, NULL, NULL) == 0) {
        _ASSERTE(false);
        return false;
    }
    sResult = std::string(&vBuf[0]);
    return true;
}

int _tmain(int argc, TCHAR* argv[], TCHAR* envp[])
{
    setlocale(LC_ALL, "");
    std::locale loc("");
    std::wcout.imbue(loc);

    std::vector<char*> argvA(argc);
    std::vector<std::vector<char>> buf(argc);
    for (int i = 0; i < argc; ++i) {
        std::string s;
        WCS2MBS(argv[i], s);
        buf[i].resize(s.size() + 1);
        strcpy_s(&buf[i][0], buf[i].size(), s.c_str());
        argvA[i] = &buf[i][0];
    }

    gflags::SetUsageMessage("gflagsライブラリのテスト");
    char** pA = &argvA[0];
    google::ParseCommandLineFlags(&argc, &pA, true);

    std::wcout << FLAGS_big_menu << std::endl;

    // coutに日本語が出力できないのでwcoutに出力
    std::wstring s;
    MBS2WCS(FLAGS_language, s);
    std::wcout << s << std::endl;

    return 0;
}

実行するとこんな感じ。

>gflag_test.exe -language 日本語
1
日本語

>gflag_test.exe -language English -nobig_menu
0
English