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自体はいろんな用途で使えそうだ。