日記、メモ、その他

Visual SLAMでは、スケールの不定性があるため、スケールドリフトと呼ばれる問題が生じ、Loop Closureなどで蓄積誤差を解消するにあたって3次元の剛体変換ではうまく補正できない場合がある。このような場合に、3次元相似変換Sim3を用いて補正を行う方法が提案されている。 Scale Drift-Aware Large Scale Monocular SLAM http://roboticsproceedings.org/rss06/p10.pdf Sim3のExponential Mapは上記論文や以下の記事を参照してください。（書くのが面倒なので。。。） Sim(3) optimization の exponential map を計算してみた 3次元の相似変換では以下のように回転成分にスケール要素が掛かった形になる。 ここで、exp Sim(3) 、exp SO(3) はそれぞれSim(3)、SO(3)のExponential Mapを表す。 W の中身については、論文や上記記事を参照してください。 Sophusを用いた実装例は、以下のようになる。 #include <iostream> #include "sophus/sim3.hpp" #include "sophus/geometry.hpp" int main() { // 単位行列 Sophus::Sim3d T0; std::cout << "T0:" << std::endl; std::cout << T0.matrix() << std::endl; // 7次元のパラメータベクトル Sophus::Vector7d uws; uws << 10.0, 0.0, 0.0, Sophus::Constants ::pi(), 0.0, 0.0, 0.0; Sophus::Sim3d T1 = Sophus::Sim3d::exp(uws); // スケーリング double scale = 2.0; T1.setScale(scale); std::cout ...

3次元の回転（Exponential Map） ではSO(3)による回転表現について、簡単な説明とC++のライブラリSophusを用いた実装例について紹介した。今回は、併進を加えた3次元の剛体変換SE(3)について、簡単に紹介する。 SE(3)のExponential Mapは6次元のベクトル( u ω ) T を用いて以下のように表される。ここで、 ω はSO(3)の時と同様に回転の軸角表現となっている。 逆変換のLogarithm Mapは、まずSO(3)のLogarithm Mapを用いてωを算出し、その後、このωを用いて以下のように算出できる。 また、以下のような2つの変換行列T 1 ，T 2 の補間を行いたい場合は、次のように中間の剛体変換を算出することができる。 ここで、wは重みで0～１の値をとる。この補間方法を用いることで、2つのカメラ視点の中間視点などを算出できるようになる。 以下に、Sophusを用いたSE(3)の実装例を示す。 #include <iostream> #include "sophus/geometry.hpp" int main() { // 単位行列 Sophus::SE3d T0; std::cout << "T0:\n" << T0.matrix() << std::endl; // パラメータベクトルse3（後ろ3つが回転so3と対応） Sophus::Vector6d uw1; uw1 << 0.0, 0.0, 0.0, Sophus::Constants ::pi() / 3, 0.0, 0.0; // x軸周りに60度回転させる変換行列 Sophus::SE3d T1 = Sophus::SE3d::exp(uw1); std::cout << "Transform matrix" << std::endl; std::cout << T1.matrix() << std::endl; std::cout << std::endl; //変換行列2 So...

SLAMの論文を読んでいると三次元の回転表現としてExponential Mapを使った表現を用いているものが多くある。Exponential Mapを用いることで、回転行列や同時変換行列を加法的に閉じた空間へ写像することが可能になる。これにより、2点間の変換パラメータを線形に補間することが可能になる。この辺の話は、以下の文献に詳しく書かれている。 Lie Groups for 2D and 3D Transformations Exponential Mapを用いた3次元回転群SO(3)は3次元ベクトル ω （ベクトルの方向が回転軸、ノルムが回転量θ）を用いて以下のように表すことができる。 また、SO(3)のExponential Mapの逆変換であるLogarithm Mapは以下のように表される。 Exponential Mapを取り扱うことができるライブラリを探していたところ、C++のライブラリとしてSophusというものが比較的簡単に導入できそうなので試してみた。 Sophus SophusはHeader Onlyのライブラリであり、依存しているのもEigenのみなので、SophusとEigenをinclude directoryに指定するだけで利用可能になる。以下は、3次元回転群SO(3)をSophusを用いて利用する例である。 #include <iostream> #include "sophus/geometry.hpp" int main() {   // 単位行列   Sophus::SO3d R0;   std::cout << "R0:\n" << R0.matrix() << std::endl;   // x軸周りに180度回転   const double kPi = Sophus::Constants<double>::pi();   Sophus::Vector3d w(kPi, 0.0, 0.0);   Sophus::SO3d R_x_pi = Sophus::SO3d::exp(w);   std::cout << "R_x_pi:\n"...

最近、以下の本を読みながら3次元の回転について復習している。 3次元回転:パラメータ計算とリー代数による最適化 金谷先生の本は、各式の展開や意味など丁寧に説明してあり非常に分かりやすい。 SLAMなどの理論を理解し、実装するためには必須の項目なのでこれを機にきっちりと理解したい。 3次元回転のオイラー角による表現については、高校数学で習ったことがあるかもしれない。ただし、オイラー角には各軸周りの回転をどの順番で掛け合わせるのか自由度があるという問題やジンバルロックの問題があることが知られている。 なので、今回は上記問題が無い回転軸と回転角によって3次元の回転を表現する方法を紹介する。回転軸と回転角によって表現するロドリゲス（ロドリーグ）の式を紹介する。ロドリゲスの式では3つのパラメータK=[k 1 , k 2 , k 3 ]を用いて以下のように3次元の回転を表すことができる。 ここで、ロドリゲスの式では回転軸（x, y, z）と回転角（θ）の4つのパラメータがあるように見えるが、回転軸を表すベクトルのノルムは1という制約があるため、この方法による3次元回転の自由度は3となっている。 OpenCVやEigenなどには、ロドリゲスの式を使った3次元回転を取り扱うための方法がすでに実装されているので、それらを用いると簡単に3次元回転をプログラム上で実装することができる。以下は、Eigenを用いた場合の例である。x軸(1, 0, 0)周りに180度(π)回転させる例となっている。 #define _USE_MATH_DEFINES #include <cmath> #include <iostream> #include "Eigen/Core" #include "Eigen/Geometry" int main() {   Eigen::Vector3d rotation_axis;   rotation_axis << 1.0, 0.0, 0.0;   double angle = M_PI;   Eigen::AngleAxis<double> angle_axis(M_PI, rotation_axis);   std::co...

CまたはC++で半正定値計画問題を取り扱うことのできるライブラリを探していたところ以下のライブラリが見つかった。 ・CSDP https://github.com/coin-or/Csdp WindowsのVisual Studioで利用するためには少し作業が必要だったので軽くメモをしておく。基本的には、以下のサイトを見ながらライブラリを生成した。ただ、バージョンの違いなのか、いくつか修正する点があった。 ・CSDPのVisual Studio2010でのコンパイル方法 http://buaiso.blogspot.com/2013/01/csdpvisual-studio2010.html CLAPACKの準備 上記サイトに書かれているように内部でLAPACKが使用されているため、まずはCLAPACKを準備する。上記サイトとは異なり、CMake版をダウンロードして用いた。 http://www.netlib.org/clapack/ よりclapack-3.2.1-CMAKE.tgzをダウンロードし解凍する。 CMakeを使ってVisual Studioのプロジェクトを生成する。ただし、今回はx64環境でライブラリを使いたいのでx64でプロジェクトを生成する。（ConfigureするといくつかWarningが出るが無視） 出来上がったプロジェクトを開きビルドする。 Debug, Release各々でlapack.lib, blas.lib, libf2c.libが生成されていればOK。 CSDPのビルド GitHubからCSDPのコードをCloneする。 https://github.com/coin-or/Csdp ここ から Csdp-6.1.1_vcproj.zipをダウンロードして解凍する。（誰がアップしているんだろう。。。） 解凍するとvcbuildというフォルダができるので、これをCSDP直下にコピーする。※参考にしたウェブではlibsというフォルダができるとあるが私の環境では生成されませんでした。 CSDP直下にlibsフォルダを作成する。 vcbuild内にあるvcbuild.slnを開く。Visual Studioのバージョンによってはアップグレードのダイアログが出てくるので、とりあえずOK...

射影行列を求める方法として、以前DLTアルゴリズムを紹介した（ 2D-3Dの対応から射影行列を求める方法（DLTアルゴリズム） ）。この方法は、線形演算で高速に射影行列を求めることができるという特長を持つが、ノイズに弱いという欠点がある。 より高精度な推定手法として、再投影誤差を最小化することにより射影行列を求める方法がある。再投影誤差とは、三次元点\(X_{i}\)を射影行列\(P\)を用いて投影した点\(\hat{x_{i}}\)と観測座標\(x_{i}\)との差\(d\left(x_{i}, \hat{x_{i}}\right)\)である。再投影誤差最小化による手法では、この差を最小とする射影行列を求める方法である。 具体的には、以下の誤差関数を最小化する。 \[E=\sum_{i}d\left(x_{i}, \hat{x_{i}}\right)\] ただし、再投影誤差の最小化は非線形な問題であるため、Levenberg-Marquardt法などの非線形最小化アルゴリズムを利用する必要がある。ここでは、以前紹介した非線形最小化ライブラリCeres-Solverを用いた実装方法について紹介する。（ 非線形最小化ライブラリCeres Solverの導入方法（Windows+Visual Studio 2012） ） 以下のプログラムでは、非線形最小化の前にDLTアルゴリズムなどによって初期解が得られていることを前提としている。 まず、Ceres-Solverを利用するために以下のヘッダファイルの指定とnamespaceの利用を宣言しておく。 #include <ceres/ceres.h> using ceres::AutoDiffCostFunction; using ceres::CostFunction; using ceres::Problem; using ceres::Solver; using ceres::Solve; 次に、誤差関数を以下のように定義する。 struct ErrorFunction{   ErrorFunction(cv::Point2d ip, cv::Point3d wp){     p2d[0] = ip.x; p2d[1] = ip.y; ...

射影行列\(P\)はカメラの内部パラメータ行列\(K\)とカメラの外部パラメータ行列\(\left[R|t\right]\)を掛けあわせたもので構成されている。ここでは、射影行列から、カメラの内部パラメータ行列\(K\)とカメラの姿勢\(R\)を求める方法について紹介する。（射影行列の推定方法については、 2D-3Dの対応から射影行列を求める方法（DLTアルゴリズム） を参照） 3行4列の射影行列\(P\)は左側の3×3の行列と右端の列ベクトルに分けることができ、それぞれ内部パラメータ\(K\)、カメラ姿勢\(R\)、カメラ位置\(C\)を用いて以下のように書ける。 \[P=\left[M|-MC\right]=K\left[R|-RC\right]\] ここから、内部パラメータ\(K\)は上三角行列、カメラ姿勢を表す\(R\)は直交行列なので、行列\(M\)をQR分解により上三角行列と直交行列に分解することで\(K\)、\(R\)を算出できることがわかる。OpenCVを用いる場合、 cv::RQDecomp3x3 を用いてQR分解を行うことができる。 以下は、3行4列の射影行列projectionMatを分解して内部パラメータ\(K\)、カメラ姿勢\(R\)を求めるOpenCVのコードである。 //射影行列の3x3要素を抽出 cv::Mat M = projectionMat.colRange(cv::Range(0, 3)); std::cout << M << std::endl; cv::Mat R, Q; cv::RQDecomp3x3(M, R, Q); std::cout << "Intrinsic Camera Parameters:\n" << R << std::endl; std::cout << "Rotation matrix:\n" << Q << std::endl; ただし、OpenCVには、射影行列を分解して、内部パラメータ\(K\)、カメラ姿勢\(R\)、カメラ位置\(C\)を求める cv::decomposeProjectionMatrix 関数が用意されており、上記と...

非線形最小化問題を解くためのライブラリとしてGoogleのCeres Solverがある。このライブラリは、比較的使い勝手がよく、また、バンドル調整のような大規模な疎行列を扱うような問題にも対応している。 ここでは、Windows+Visual Studio 2012での導入方法について個人的なメモも兼ねて記載しておく。 基本的には、 このページ の手順にしたがって進めていくと問題なくコンパイルできました。 以下のフィアルをダウンロード Ceres Solverのソースコード（ http://code.google.com/p/ceres-solver/downloads/list ） gflags（ http://code.google.com/p/gflags/downloads/list ） glogs（ http://code.google.com/p/google-glog/downloads/list ） CXSparse（ https://github.com/PetterS/CXSparse ） Eigen（ http://eigen.tuxfamily.org/index.php?title=Main_Page ） gflags, glogs, CXSparseのライブラリを生成 ダウンロードしたファイルを展開すると、それぞれのフォルダにVisual Studioのソリューションファイルがあるので、これを開いてビルド。参考にしたページに書かれているようなソースコードの修正は必要ありませんでした。 CMakeをつかってCeres Solverのソリューションファイルを生成 ソースコードの位置をceres/ceres-solver-1.8.0に設定し、バイナリ生成のためのフォルダをceres/ceres-solver/buildのように設定する。設定後、gflags, glogs, CXSparse, Eigen3のヘッダファイルの場所、ライブラリを以下のように指定し、configure, generateするとソリューションファイルが生成される。 EIGEN_INCLUDE_DIRにEigenへのパス GFLAGS_INCLUDE_DIRにgflags-2.0\src\window...

座標変換を行う際には、ある点の座標を同次座標で表現したほうが計算が便利な場合がある。 OpenCVでは、ユークリッド座標から同次座標、同次座標からユークリッド座標へ変換するための関数が用意されている。 cv::convertPointsToHomogeneous 同次座標系への変換。n次元の座標をn+1次元の同次座標へ変換する。 cv::convertPointsFromHomogeneous ユークリッド座標系への変換。n+1次元の同次座標をn次元のユークリッド座標へ変換する。 cv::convertPointsHomogeneous 自動でユークリッド座標系 -> 同次座標系、ユークリッド座標系 <- 同次座標系を判断し変換する。内部で、cv::convertPointsToHomogeneousまたはcv::convertPointsFromHomogeneousを呼び出している。 以下に使用例を示す。 #include <iostream> #include <vector> #include <opencv2/opencv.hpp> int main(int argc, char** argv) {   std::vector<cv::Point2d> points2D;   //適当に二次元座標を生成   for (int i = 0; i < 10; ++i)   {     cv::Point2d p;     p.x = rand();     p.y = rand();     points2D.push_back( p );   }   std::vector<cv::Point3d> points3D;   //ユークリッド座標 -> 同次座標系への変換   cv::convertPointsToHomogeneous(points2D, points3D);   std::cout << "Euclidean to Homogeneous" <...

\(AX=0\)かつ\(X\neq0\)の時の解は行列\(A\)を特異値分解し、最小特異値に対応する右特異ベクトルとして得ることが出来る。 OpenCVにはこれを行ってくれる関数SVD::solveZが用意されている。 使い方は簡単で、以下のように入力の行列と結果を格納するための行列を引数として渡すことで計算結果を得ることが出来る。 このプログラムでは、以下の同次線形方程式を解いている。 \[\left\{ \begin{array}{@{}1} 2x-y+4z=0\\ -6x+3y-12z=0\\ 4x-2y+8z=0 \end{array} \right. \] #include <iostream> #include <opencv2/opencv.hpp> int main(int argc, char** argv) { cv::Mat A = (cv::Mat_<double>(3, 3) << 2, -1, 4, -6, 3, -12, 4, -2, 8); cv::Mat X; cv::SVD::solveZ(A, X); std::cout << X << std::endl; return 0; } 結果 [-0.2445084883048524; -0.9623980449408547; -0.1183452670827875]