for Robot Artificial Inteligence

3. visualize Geometry practice

02 Jun 2020 | Visual SLAM

eigenGeometry.cpp

#include <iostream>
#include <cmath>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

// 本程序演示了 Eigen 几何模块的使用方法

int main(int argc, char **argv) {

  // Eigen/Geometry 模块提供了各种旋转和平移的表示
  // 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
  Matrix3d rotation_matrix = Matrix3d::Identity();
  // 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
  AngleAxisd rotation_vector(M_PI / 4, Vector3d(0, 0, 1));     //沿 Z 轴旋转 45 度
  cout.precision(3);
  cout << "rotation matrix =\n" << rotation_vector.matrix() << endl;   //用matrix()转换成矩阵
  // 也可以直接赋值
  rotation_matrix = rotation_vector.toRotationMatrix();
  // 用 AngleAxis 可以进行坐标变换
  Vector3d v(1, 0, 0);
  Vector3d v_rotated = rotation_vector * v;
  cout << "(1,0,0) after rotation (by angle axis) = " << v_rotated.transpose() << endl;
  // 或者用旋转矩阵
  v_rotated = rotation_matrix * v;
  cout << "(1,0,0) after rotation (by matrix) = " << v_rotated.transpose() << endl;

  // 欧拉角: 可以将旋转矩阵直接转换成欧拉角
  Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0); // ZYX顺序，即roll pitch yaw顺序
  cout << "yaw pitch roll = " << euler_angles.transpose() << endl;

  // 欧氏变换矩阵使用 Eigen::Isometry (homogenious)
  Isometry3d T = Isometry3d::Identity();                // 虽然称为3d，实质上是4＊4的矩阵
  T.rotate(rotation_vector);                                     // 按照rotation_vector进行旋转
  T.pretranslate(Vector3d(1, 3, 4));                     // 把平移向量设成(1,3,4)
  cout << "Transform matrix = \n" << T.matrix() << endl;

  // 用变换矩阵进行坐标变换
  Vector3d v_transformed = T * v;                              // 相当于R*v+t
  cout << "v tranformed = " << v_transformed.transpose() << endl;

  // 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

  // 四元数
  // 可以直接把AngleAxis赋值给四元数，反之亦然
  Quaterniond q = Quaterniond(rotation_vector);
  cout << "quaternion from rotation vector = " << q.coeffs().transpose()
       << endl;   // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
  // 也可以把旋转矩阵赋给它
  q = Quaterniond(rotation_matrix);
  cout << "quaternion from rotation matrix = " << q.coeffs().transpose() << endl;
  // 使用四元数旋转一个向量，使用重载的乘法即可
  v_rotated = q * v; // 注意数学上是qvq^{-1}
  cout << "(1,0,0) after rotation = " << v_rotated.transpose() << endl;
  // 用常规向量乘法表示，则应该如下计算
  cout << "should be equal to " << (q * Quaterniond(0, 1, 0, 0) * q.inverse()).coeffs().transpose() << endl;

  return 0;
}

visualize Geometry.cpp

#include <iostream>
#include <iomanip>

using namespace std;

#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

#include <pangolin/pangolin.h>

struct RotationMatrix {
  Matrix3d matrix = Matrix3d::Identity();
};

ostream &operator<<(ostream &out, const RotationMatrix &r) {
  out.setf(ios::fixed);
  Matrix3d matrix = r.matrix;
  out << '=';
  out << "[" << setprecision(2) << matrix(0, 0) << "," << matrix(0, 1) << "," << matrix(0, 2) << "],"
      << "[" << matrix(1, 0) << "," << matrix(1, 1) << "," << matrix(1, 2) << "],"
      << "[" << matrix(2, 0) << "," << matrix(2, 1) << "," << matrix(2, 2) << "]";
  return out;
}

istream &operator>>(istream &in, RotationMatrix &r) {
  return in;
}

struct TranslationVector {
  Vector3d trans = Vector3d(0, 0, 0);
};

ostream &operator<<(ostream &out, const TranslationVector &t) {
  out << "=[" << t.trans(0) << ',' << t.trans(1) << ',' << t.trans(2) << "]";
  return out;
}

istream &operator>>(istream &in, TranslationVector &t) {
  return in;
}

struct QuaternionDraw {
  Quaterniond q;
};

ostream &operator<<(ostream &out, const QuaternionDraw quat) {
  auto c = quat.q.coeffs();
  out << "=[" << c[0] << "," << c[1] << "," << c[2] << "," << c[3] << "]";
  return out;
}

istream &operator>>(istream &in, const QuaternionDraw quat) {
  return in;
}

int main(int argc, char **argv) {
  pangolin::CreateWindowAndBind("visualize geometry", 1000, 600);
  glEnable(GL_DEPTH_TEST);
  pangolin::OpenGlRenderState s_cam(
    pangolin::ProjectionMatrix(1000, 600, 420, 420, 500, 300, 0.1, 1000),
    pangolin::ModelViewLookAt(3, 3, 3, 0, 0, 0, pangolin::AxizY)
  );

  const int UI_WIDTH = 500;

  pangolin::View &d_cam = pangolin::CreateDisplay().
    SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0, -1000.0f / 600.0f).
    SetHandler(new pangolin::Handler3D(s_cam));

  // ui
  pangolin::Var<RotationMatrix> rotation_matrix("ui.R", RotationMatrix());
  pangolin::Var<TranslationVector> translation_vector("ui.t", TranslationVector());
  pangolin::Var<TranslationVector> euler_angles("ui.rpy", TranslationVector());
  pangolin::Var<QuaternionDraw> quaternion("ui.q", QuaternionDraw());
  pangolin::CreatePanel("ui").SetBounds(0.0, 1.0, 0.0, pangolin::Attach::Pix(UI_WIDTH));

  while (!pangolin::ShouldQuit()) {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    d_cam.Activate(s_cam);

    pangolin::OpenGlMatrix matrix = s_cam.GetModelViewMatrix();
    Matrix<double, 4, 4> m = matrix;

    RotationMatrix R;
    for (int i = 0; i < 3; i++)
      for (int j = 0; j < 3; j++)
        R.matrix(i, j) = m(j, i);
    rotation_matrix = R;

    TranslationVector t;
    t.trans = Vector3d(m(0, 3), m(1, 3), m(2, 3));
    t.trans = -R.matrix * t.trans;
    translation_vector = t;

    TranslationVector euler;
    euler.trans = R.matrix.eulerAngles(2, 1, 0);
    euler_angles = euler;

    QuaternionDraw quat;
    quat.q = Quaterniond(R.matrix);
    quaternion = quat;

    glColor3f(1.0, 1.0, 1.0);

    pangolin::glDrawColouredCube();
    // draw the original axis
    glLineWidth(3);
    glColor3f(0.8f, 0.f, 0.f);
    glBegin(GL_LINES);
    glVertex3f(0, 0, 0);
    glVertex3f(10, 0, 0);
    glColor3f(0.f, 0.8f, 0.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 10, 0);
    glColor3f(0.2f, 0.2f, 1.f);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 10);
    glEnd();

    pangolin::FinishFrame();
  }
}

Exercise

Main.cpp

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace std;

int main(int agrc, char **argv){

// put xiaoluobo first data
    Eigen::Quaterniond q1 (0.35, 0.2, 0.3, 0.1);
    Eigen::Vector3d t1 (0.3,0.1,0.1);

    // put xiaoluobo second data
    Eigen::Quaterniond q2 (-0.5,0.4,-0.1,0.2);
    Eigen::Vector3d t2 (-0.1,0.5,0.3);

    // input xiaoluobo first observation point data
    Eigen::Vector3d p (0.5,0,0.2);
    Eigen::Vector3d o;

    // initialize eular matrix
    Eigen::Isometry3d T_1cw = Eigen::Isometry3d::Identity();
    Eigen::Isometry3d T_2cw = Eigen::Isometry3d::Identity();

    // start to solve problem
    // normalization
    q1.normalize();
    q2.normalize();

    // output data q1,q2 after normalization

    cout<<"q1="<<endl<<q1.x()<<endl<<q1.y()<<endl<<q1.z()<<endl<<q1.w()<<endl;
    cout<<"q2="<<endl<<q2.x()<<endl<<q2.y()<<endl<<q2.z()<<endl<<q2.w()<<endl;

    //  quaternion and displacement(homogenious matrix)

    T_1cw.rotate(q1);
    T_1cw.pretranslate(t1);

    T_1cw.rotate(q2);
    T_1cw.pretranslate(t2);

    cout << "T_1cw:" << endl << T_1cw.matrix() << endl;
    cout << "T_2cw:" << endl << T_2cw.matrix() << endl;

    // get o coordinate

    o = T_2cw * T_1cw.inverse() * p;

    // output o coordinate

    cout << "o= " << o.transpose() << endl;
    return 0;
}

Reference

SLAM KR 视觉SLAM书

Cmake Tutorial : https://github.com/TheErk/Cmake-tutorial.

Wiki

Comment  Read more

6. Recursion exercise

02 Jun 2020 | Algorithm

Sum of Natural Number using Recursion

#include <stdio.h>
int sum(int n) // recursive method
{
 if(n==0)
 return 0;
 return sum(n-1)+n; // n
}
int Isum(int n) // for loop method
{
 int s=0,i; // 1
 for(i=1;i<=n;i++) // n+1
 s=s+i; // n

 return s; // 1
}
int main()
{
 int r=sum(5);
 printf("%d ",r);
 return 0;
}
  • sum(n) = 1+2+3+4+…(n-1) +n
  • sum(n) = (n-1)+n
  • 조건
    • n=0, sum(n) = 0
    • n>0, sum(n) = (n-1)+n
  • Time Complexity = O(n)

Factorial

#include <stdio.h>
int fact(int n)
{
 if(n==0)
 return 1;
 return fact(n-1) * n;
}
int Ifact(int n)
{
 int f=1,i;
 for(i=1;i<=n;i++)
 f=f * i;

 return f;
}
int main()
{
 int r=fact(5);
 printf("%d ",r);
 return 0;
}
  • Sum 하고 똑같은 방식이다.

Power(Exponent)

#include <stdio.h>
int power(int m,int n)
{
 if(n==0)
 return 1;
 return power(m,n-1) * m;
}
int power1(int m,int n)
{
 if(n==0)
 return 1;
 if(n%2==0)
 return power1(m*m,n/2);
 return m * power1(m*m,(n-1)/2);
}
int main()
{
 int r=power1(9,3);
 printf("%d ",r);
 return 0;
}
  • pow(m,n) = (mM….* (n-1)) * m
  • pow(m,n) = pow(m,n-1) * m
  • 조건
    • n = 0, p(m,n) = 1;
    • n>0, p(m,n) = p(m,n-1)* m

Talyor Series using static variable

#include <stdio.h>
double e(int x, int n)
{
 static double p=1,f=1;
 double r;

 if(n==0)
 return 1;
 r=e(x,n-1);
 p=p*x;
 f=f*n;
 return r+p/f;
}
int main()
{
 printf("%lf \n",e(4,4));
 return 0;
}

  • time complexity = n(n+1) = O(n)

Talyor Series Horner’s Rule

double e(int x, int n)
{
 static double s;
 if(n==0)
 return s;
 s=1+x*s/n;
 return e(x,n-1);

}
int main()
{
 printf("%lf \n",e(2,10));
 return 0;
}
  • tail recursive 방법을 써서 구하는 방식, 즉 전에 방법하고 반대로 되는 방식이다.

Taylor Series Iterative

#include <stdio.h>
double e(int x, int n)
{
 double s=1;
 int i;
 double num=1;
 double den=1;

 for(i=1;i<=n;i++)
 {
 num*=x;
 den*=i;
 s+=num/den;
 }
 return s;
}
int main()
{
 printf("%lf \n",e(1,10));
 return 0;
}
  • for loop를 사용하여 구현하는 방법

Fibonacci Series

  • Fibonacci Series 개념
    • fin(n)=(n-2)+(n-1)
    • 즉, 0 1 1 2 3 5 8 13 18 31 …. 이런 식으로 늘려나가는 것
#include <stdio.h>
int fib(int n) // for loop 방법
{
 int t0=0,t1=1,s=0,i;

 if(n<=1) return n;

 for(i=2;i<=n;i++)
 {
 s=t0+t1;
 t0=t1;
 t1=s;
 }

 return s;
}
int rfib(int n) // recursive 방법
{
 if(n<=1)return n;
 return rfib(n-2)+rfib(n-1);
}
int F[10];
int mfib(int n)
{
 if(n<=1)
 {
 F[n]=n;
 return n;
 }
 else
 {
 if(F[n-2]==-1)
 F[n-2]=mfib(n-2);
 if(F[n-1]==-1)
 F[n-1]=mfib(n-1);
 F[n]=F[n-2]+F[n-1];
 return F[n-2]+F[n-1];
 }
}
int main()
{
 int i;
 for(i=0;i<10;i++)
 F[i]=-1;

 printf("%d \n",mfib(5));
 return 0;
}
  • 조건
    • n=0, fin(n) = 0;
    • n=1, fin(n) = 1;
    • n>1, fin(n) = fin(n-2) + fib(n-1)

  • Time Complexity = O(2^n)

nCr using Recursion(Combination Formular)

#include <stdio.h>
int fact(int n)
{
 if(n==0)return 1;
 return fact(n-1) * n;
}
int nCr(int n,int r)
{
 int num,den;

 num=fact(n);
 den=fact(r) * fact(n-r);

 return num/den;
}
int NCR(int n,int r)
{
 if(n==r || r==0)
 return 1;
 return NCR(n-1,r-1)+NCR(n-1,r);

}
int main()
{
 printf("%d \n",NCR(5,3));
 return 0;
}
  • Time Complexity O(n)

Tower of Hanoi

  • “1” is from
  • “2” is waiting
  • “3” is to

#include <stdio.h>
void TOH(int n,int A,int B,int C)
{
 if(n>0)
 {
 TOH(n-1,A,C,B);
 printf("(%d,%d)\n",A,C);
 TOH(n-1,B,A,C);
 }
}
int main()
{
 TOH(3,1,2,3);
 return 0;
}

  • Time Complexity = 1 + 2 + 2^2 + … +2^n = 2^(n+1) - 1 = O(2^(n+1))

Comment  Read more

3. Represent 3D Rigid Body Transform using Quternion

02 Jun 2020 | Visual SLAM

Representation of the Rotation in 3D

  • 3차원 공간상에서의 회전의 표현 방법
    • Rotation Matrix
      • 좌표계 간의 회전 변환 관계를 나타내는 행렬
      • 3 DOF를 표현하기 위해 9개의 값을 사용하는 중복성 존재
      • 행렬식이 1인 orthogonal matrices이라는 제약 조건에 따라 최적화 문제에 대한 복잡도 증가
    • Euler angle(r,p,y로 표현)
      • 세 축을 중심으로 하는 순차적 회전을 통한 매우 직관적인 표현 방법
      • 특정 상황에서 1 자유도를 잃어버리는 짐벌락(Gimbal rock) 문제가 발생(예로, x축 = y축이 같아지는 현상)
      • 보간과 반복에 적합하지 않아 주로 결과의 신속한 가시화를 위해 사용
    • Quaternion
      • 4개의 값으로 이루어진 확장된 복소수 체계를 이용해 3차원 회전을 표현
      • 행렬에 비해 연산 속도가 빠르고, 차지하는 메모리 양도 적으며, 최단호(shortest arc) 보간으로 오류 발생률이 적다는 장점을 지님

Quaternion

  • definition of the Quaternion

  • 3차원 회전을 표현하기 위하여 확장된 복소수인 사원수(Quaternion)을 사용
  • 3차원 공간을 다루기 위해서는 4차원 공간이 필요
  • 실수부/스칼라부 Re(q) 와 허수부/벡터부 Im(q) 로 구성

  • Quaternion의 basis 간의 곱

Quaternion 특징

  • Quaternion을 이용한 회전
    • 3차원 공간 상의 한 점 p 에 대하여
    • p = [0,x,y,z]
    • n 벡터를 축으로 theta만큼 회전을 나타내는 단위 Quternion을 이용 하면

  • 3차원 공간상에서 회전된 점 p’ 을 계산 가능

  • 계산 결과 실수부는 0이 되고, 허수부의 세 요소는 회전 후의 3차원 점의 좌표로 표현

  • 회전 벡터(Rotation vector)와 Quaternion 간의 변환

Quaternion 구면 선형 보간

Euler Transformation, similarity Transformation, affine transformation, projection transformation comparison

Reference

SLAM KR 视觉SLAM书

Cmake Tutorial : https://github.com/TheErk/Cmake-tutorial.

Wiki

Comment  Read more

2. 3D Rigid Body Transform practice(Eigen)

02 Jun 2020 | Visual SLAM

Eigen

  • Rotation Matrix와 Transformation Matrix를 계산하는데 사용되는 Library이다.
sudo apt-get install libeigen3-devl
  • 인스톨이 잘되어있는지 확인
sudo updatedb
locate eigen3

Practice

eigenMatrix.cpp

#include <iostream>
using namespace std;
#include <ctime>
#include <Eigen/Core> // Eigen 핵심
#include <Eigen/Dense> //稠密矩阵（Dense  Matrix 의 Algebra 계산(역, eigenvalue)

using namespace Eigen;

#define MATRIX_SIZE 50;
/*
Eigen 연습
*/
int main(int argc, char**argv)
{
  //Eigen에서 모든 벡터와 매트릭스는 Eigen::Matrix로 시작한다. Eigen의 앞에 3개 파라미터는 <데이터유형, 행, 열>이다.
  Matrix<float,2,3> matrix_23; // 2 x 3의 사이즈와 float 데이터 유형의 메트릭스

  //동시에, Eigen은 typedef를 사용하여 내부의 많은 파라미터를 제공하기 때문에 다양한 방법으로 표현할 수 있따. 예를 들어
  vector3d v_3d;
  Matrix<float, 3, 1> vd_3d;
  // 두 위에 방법은 똑같은 방법이다.

  // Matrix3d는 Eigen::Matrix<double, ,3 ,3>
  Matrix3d matrix_33 = Matrix3d::zero(); //initialize as zero
  // 만약 매트릭스의 사이즈를 확실히 정할수 없다면, Dynamic으로 크기를 정할 수 있다.
  Matrix<double, Dynamic, Dynamic> matrix_dynamic;
  // 더 간단하게
  MatirxXd matrix_x

  // 위에와 같이 Matrix 사이즈를 정하였다면, 아래와 같이 행렬별 element를 넣을 수 있다.

  matrix_23 << 1, 2, 3, 4, 5, 6;
  //output
  cout << "matrix 2x3 from 1 to 6: \n" << matrix_23 << endl;

  // output using for loop
  cout << "print matrix 2x3: " << endl;
  for (int i = 0; i < 2; i++) {
    for (int j = 0; j < 3; j++) cout << matrix_23(i, j) << "\t";
    cout << endl;
  }

  // Matrix와 벡터의 곱셈
  v_3d << 3, 2, 1;
  vd_3d << 4, 5, 6;

  // 주의할점은 곱셈을 할때 항상 같은 데이터 유형의 매트릭스와 벡터로 만 해야 한다.
  Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
  cout << "[1,2,3;4,5,6] * [3,2,1]=" << result.transpose() << endl;

  Matrix<float, 2, 1> result2 = matrix_23 * vd_3d;
  cout << "[1,2,3;4,5,6] * [4,5,6]: " << result2.transpose() << endl;
  return 0;
  // 또한 계산 결과의 Dimension을 잘못 지정하게 해서는 안된다.(Dimensionality program)

  //각 종 Matrix 표현 방법
  matrix_33 = Matrix3d::Random(); //3x3, element value 랜덤 행령
  cout << "random matrix: \n" << matrix_33 << endl;
  cout << "transpose: \n" << matrix_33.transpose() << endl;  // Transpose
  cout << "sum: " << matrix_33.sum() << endl;  // element sum
  cout << "trace: " << matrix_33.trace() << endl; // trace 값
  cout << "times 10: \n" << 10 * matrix_33 << endl; // multiply
  cout << "inverse: \n" << matrix_33.inverse() << endl;
  cout << "det: " << matrix_33.determinant() << endl;

  // Eigen Value와 Eigen Vector를 구하는 방법
  SelfAdjointEigenSolver<Matrix3d> eigen_solver(matrix_33.transpose() * matrix_33);
  cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
  cout << "Eigen vectors = \n" << eigen_solver.eigenvectors() << endl;

  // 방정식 문제 풀기
  // matirx_NN * x = v_Nd
  Matrix<double, MATRIX_SIZE, MATRIX_SIZE> matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
  // matrix_NN = matrix_NN * matrix_NN.transpose();  // guarantee positive define
  Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE,1);

  clock_t time_stt = clock(); // 시간
  //역행렬로 x 값 구하기
  Matrix<double, MATRIX_SIZE, 1> x = matrix_NN.inverse() * v_Nd;
  cout << "time of normal inverse is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  //QR comDeposition으로 x값 구하기
  time_stt = clock();
  x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
  cout << "time of Qr decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;

  // Cholesky Decomposition 으로 x값 구하기
  time_stt = clock();
  x = matrix_NN.ldlt().solve(v_Nd);
  cout << "time of ldlt decomposition is "
       << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
  cout << "x = " << x.transpose() << endl;
  return 0;
}
  • 프로그램을 다 짜게 되었으면 CMakelists.txt를 만든다.
cmake_minimum_required(VERSION 2.8)
project(useEigen)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-O3")

include_directories("/usr/include/eigen3")
add_executable(eigenMatrix eigenMatrix.cpp)
  • build 폴더에다가 compile을 시킨다.
mkdir build
cd build
cmake ..
make
  • 실행
./eigenMatrix

Reference

SLAM KR 视觉SLAM书

Cmake Tutorial : https://github.com/TheErk/Cmake-tutorial.

Wiki

Comment  Read more

2. 3D Rigid Body Transform

02 Jun 2020 | Visual SLAM

강체 변환

  • 강체(Rigid Body)
    • 강체 : 딱딱한 물체 -> 물체의 형태가 변하지 않음을 말함.

좌표계

  • 왼손 좌표계 & 오른손 좌표계(3차원 공간)
    • 위치나 방향을 나타내려고 할때 기준점과 기준 방향이 필요
    • 두 가지의 방식의 좌표계가 존재 => 왼손 & 오른손 좌표계
      • 두 좌표계는 아무리 회전을 해도 절대 동일해 지지 않음
      • 나머지 소위 좌표계라고 착각되는 것들은 모두 회전 변환으로 상호 변환이 가능함

  • 책인용 : “왼손 시스템의 세 번째 축은 오른손 시스템과 반대방향이다.”

벡터 및 스칼라

  • Scalar(스칼라)
    • 양 (하나의 값) 및 크기를 말한다(Scale)
  • Vector
    • 방향과 크기를 가짐
    • 위치는 없음
    • 벡터와 벡터간 연산 : 덧셈, 뺄셈, 외적(결과는 벡터), 내적(결과는 스칼라)
    • 벡터와 스칼라 연산: 곱셈(결과는 벡터)
  • Position
    • 위치만 가짐
    • 위치와 위치간 연산 : 뺄셈 ( 결과는 벡터 )
    • 벡터와 위치 연산 : 덧셈 ( 결과는 위치 )
  • 3D 벡터와 3D 위치의 공통점
    • X, Y, Z 값으로 표시 가능
    • 하지만 둘의 차이점을 아는 것은 중요

내적

  • 벡터들간의 Projection 관계를 나타낸다(projection 길이를 구할 수 있다)

좌표계

  • e는 선형성을가지고 있는 공간상의 bias, a는 벡터
  • 좌표축간에 회전을 알 수 있다.

외적

  • 3D 강체 운동에서 외적은 Rotation을 구할 수 있다.
  • Skew-symmetric, 부호 ^

Rotation Matrix

  • p 는 카메라의 벡터
    • 카메라 운동은 강체 운동이다, 이것은 똑같은 벡터와 각종 좌표계의 길이와 각도가 변환이 되지 않는 것을 보장한다.
    • 즉, 이것을 오일러 변환이라고 말한다(euler transformation)
  • 예를 들어 (e_1, e_2, e_3)를 가지고 있는 어떤 단위가 1번의 Transformation을 거치면, (e_1’, e_2’, e_3’)가 된었다고 할때, 똑같은 벡터 a 를 이용하여 두 좌표계 [a_1,a_2,a_3]^T 와 [a_1’,a_2’,a_3’]^T 를 만들수 있다.
  • 좌표계의 정의에 근거하면

  • 두 관계를 묘사하였고, (e_1, e_2, e_3)를 반대편에다 대입하였을 경우, 아래와 같은 단위 매트릭스를 구할 수 있게 된다

  • 여기서 R은 Rotation Matrix라고 불린다.
  • Rotation Matrix의 특징은 Determinant가 항상 1이라는 것이다. 그렇기 때문에 아래와 같이 정의할 수 있다.

  • SO(n)은 Special Orthogonal Group이라고 하여 Li group이라고 말한다.
    • n Demension의 Rotation Matix의 집합으로부터 구성이 되어 있다.
    • 특히, SO(3) 은 3차원 공간의 Rotation을 말한다.
  • Rotation Matrix는 Camera의 Rotatation으로 묘사할 수 있다.

오일러 변환(euler Transformation)

  • 오일로 변환중에 Rotation Matrix를 제외한 이동인(transportation) T가 있다.
  • 만약 World 좌표계에서의 벡터 a, 그리고 한번의 Rotation과 이동 T가 있었다면, 아래와 같이 a’ 를 얻을 수 있다.

Transformation Matrix and Homogeneous coordinate

  • 방금 위에서 공부한 것들을 이용하여서 아래와 같이 방정식으로 쉽게 만들 수 있다.

  • T는 Transformation이라고 불린다.
  • 이와 같은 표현 방법을 Homogeneous Coordinate 불리는데 그래픽스 또는 3D 비전 분야에서 많이 사용 되는 좌표계의 한 종류이다.
    • 인간이 보고 있는 좌표계는 Inhmogeous 이다.
    • Homogeneous를 이용한다면 무한대의 점을 표현하는데 유리하다
  • Homogeneous 좌표계를 사용하면 Affine 변환과 Perspective 변환이 하나의 행렬로 정의 할 수 있다.
  • 선의 Homogeneous coordinate 표현
    • 직선의 방정식 ax+by+c = 0 을 ax+by+cw=0로 표현한다.
    • 이때 ax+by+c = 0 상의 점으로 투영되는 점 (x,y,w)들은 ax+by+cw를 만족해야 한다.
  • 점의 Homogeneous coordinate 표현
    • 임의 상수 w에 대해 점 P(x,y)의 좌표를 P(wx,wy,w)로 표현하는 방식이다.
    • 2D 이미지에서 한 점으로 투영되는 모든 점(같은 ray 상의 점)들을 표현 할 수 있다(Up to Scale)
    • 원래 좌표를 구하고 싶다면 w=1로 만들면 된다. P(x,y,1)
  • 이와 같은 Transformation 매트릭스를 Special Euclidean Group(SE)으로 불린다.

  • SO(3) 는 Rotation Matrix를 묘사하고, SE는 Transformation을 묘사한다.

하지만 오일러 변환을 사용하는 경우 짐벌락(black hole처럼 singularity에 빠지는 경우) 문제 발생

  • 오일러 변환을 사용하는 경우 짐벌락 문제 Blcak hole 처럼 Signgularity에 빠지는 경우가 나타나게 됨
  • 이는 Full Matrix를 사용하여 SO 구하게 되면, 많은 Computational을 제공할 뿐만 아니라 Singularity에 빠지기 쉽다.

Axis-Angle, Rodriguerese Formular

  • 그렇기 떄문에 SO(3)를 더 간편하게 구하기 위해 Rodriguerese Formular를 쓴다.
  • 주어진 축(Axis)와 Angle of Rotation으로 Rotation Vector값으로 3D rotation을 구할 수있다.
  • 구해진 Rotatation vector값을 Rodrigues 수식에 따라 행렬로 변환한다.
  • By extension, this can be used to transform all three basis vectors to compute a rotation matrix in SO(3)
  • the Rodrigues’ formula provides an algorithm to compute the exponential map from so(3), the Lie algebra of SO(3), to SO(3) without actually computing the full matrix exponential.

Reference

SLAM KR 视觉SLAM书

Cmake Tutorial : https://github.com/TheErk/Cmake-tutorial.

Wiki

Comment  Read more