9. Non Linear Least Square

17 Jun 2020 | Visual SLAM

Least Square

Rotation을 최적화 하기 어렵 -> Manifold 최적화(리대수로 최적화)

First Order & Second Order

테일러 근사값을 이용하여 최적화 값 구하기

Gauss Newton

1차 Talor 전개, Jacobian을 활용하여 최적화 값 구하기

Levenberg-Marquardt

Levenberg ul 에 Converge가 안될 수 도 있으므로

커지면 Gradient 작아지면 Newton 방법

D는 Diagonal 이다.

Reference

SLAM KR

Comment Read more

8. Lie Group and Lie Algebra Practice

17 Jun 2020 | Visual SLAM

Li Algebra Library : Sophus

1. install

git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff #非模板

2. Cmake

# 为使用 sophus，您需要使用find_package命令找到它
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )

set(CMAKE_CXX_STANDARD 11)

add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES} )

target_link_libraries(useSophus Sophus)

build

mkdir build
cd build
cmake ..
make

3. SO(3) and SE(3) algorithm using Sophus library(how to use)

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "sophus/se3.h"
#include "sophus/so3.h"


using namespace std;
using namespace Eigen;


/// 本程序演示sophus的基本用法

int main(int argc, char **argv)
{
  // 沿Z轴转90度的旋转矩阵
  Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();

  // 或者四元数
  // SO는 Sepcial Orthogonal로 transformation matrix 형태가 아닌 rotation matrix 형태이다.
  Quaterniond q(R);
  Sophus::SO3 SO3_R(R);              // Sophus::SO3d可以直接从旋转矩阵构造
  Sophus::SO3 SO3_q(q);              // 也可以通过四元数构造

  // 二者是等价的
  cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
  cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
  cout << "they are equal" << endl;

  // 使用对数映射获得它的李代数
  Vector3d so3 = SO3_R.log();
  cout << "so3 = " << so3.transpose() << endl;
  // hat 为向量到反对称矩阵 change to matrix
  cout << "so3 hat=\n" << Sophus::SO3::hat(so3) << endl;
  // 相对的，vee为反对称到向量 change to vector
  cout << "so3 hat vee= " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;

  // 增量扰动模型的更新
  Vector3d update_so3(1e-4, 0, 0); //假设更新量为这么多, 즉 현재 pose에서 옮겨지는 위치
  Sophus::SO3 SO3_updated = Sophus::SO3::exp(update_so3) * SO3_R; // 이 translate vector를 exponential(exp)를 하여 Matrix(리군)로, 그리고 가지고 있는 SO(리군값)값 곱하기
  cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

  cout << "*******************************" << endl;

  // 对 SE(3)操作大同小异
  // Speical ecludiean으로 transformation값이다.
  Vector3d t(1, 0, 0);           // 沿X轴平移1
  Sophus::SE3 SE3_Rt(R, t);           // 从R,t构造SE(3)
  Sophus::SE3 SE3_qt(q, t);            // 从q,t构造SE(3)
  cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
  cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;

  // 李代数se(3) 是一个六维向量，方便起见先typedef一下
  typedef Eigen::Matrix<double, 6, 1> Vector6d;

  Vector6d se3 = SE3_Rt.log(); // 리군 값을 리대수값으로 변경 log
  cout << "se3 = " << se3.transpose() << endl;

  // 观察输出，会发现在Sophus中，se(3) 的平移在前，旋转在后.
  // 同样的，有hat和vee两个算符
  cout << "se3 hat = \n" << Sophus::SE3::hat(se3) << endl;
  cout << "se3 hat vee = " << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl;

  // 最后，演示一下更新
  Vector6d update_se3; //更新量
  update_se3.setZero(); // 0000000

  // cout << "se3?" << update_se3.setZero() << endl;
  update_se3(0, 0) = 1e-4d;
  Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3) * SE3_Rt; // 리군으로 변환
  cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl; // 리군 Matrix 표현

  return 0;
}

Real Example

CMAKE

cmake_minimum_required(VERSION 2.8)
project(example)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11")
find_package(Pangolin REQUIRED)
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
include_directories(${Pangolin_INCLUDE_DIRS})
add_executable(trajectoryError trajectoryError.cpp)
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES})
target_link_libraries(trajectoryError ${Sophus_LIBRARIES})

TrajectoryError(comparing Estimated, visual odometer, ground truth )

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.h>


using namespace Sophus;
using namespace std;

string groundtruth_file = "/home/chan/Desktop/slambook2/ch4/example/groundtruth.txt";
string estimated_file = "/home/chan/Desktop/slambook2/ch4/example/estimated.txt";

typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType; // this is trajectory type
// data structure is 8 elements

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti); // comparing estimated pose and ground truth

TrajectoryType ReadTrajectory(const string &path) {
  ifstream fin(path);
  TrajectoryType trajectory;
  if (!fin) {
    cerr << "trajectory " << path << " not found." << endl;
    return trajectory;
  }

  while (!fin.eof()) {
    double time, tx, ty, tz, qx, qy, qz, qw;
    fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    Sophus::SE3 p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
    trajectory.push_back(p1);
  }
  return trajectory;
}

int main(int argc, char **argv)
{
  TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);
  TrajectoryType estimated = ReadTrajectory(estimated_file);
  assert(!groundtruth.empty() && !estimated.empty()); //만약 false면 프로그램 종료
  assert(groundtruth.size() == estimated.size()); //만약 false면 프로그램 종료

  // compute rmse
  double rmse = 0;
  for (size_t i = 0; i < estimated.size(); i++)
  {
    Sophus::SE3 p1 = estimated[i];
    Sophus::SE3 p1 = groundtruth[i];
    double error = (p2.inverse() * p1).log().norm();
    rmse += error * error;
  }
  rmse = rmse / double(estimated.size());
  rmse = sqrt(rmse);
  cout << "RMSE = " << rmse << endl;

  DrawTrajectory(groundtruth, estimated);
  return 0;

}

void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {
  // create pangolin window and plot the trajectory
  pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
  glEnable(GL_DEPTH_TEST);
  glEnable(GL_BLEND);
  glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

  pangolin::OpenGlRenderState s_cam(
      pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
      pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
  );

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


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

    d_cam.Activate(s_cam);
    glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

    glLineWidth(2);
    for (size_t i = 0; i < gt.size() - 1; i++) {
      glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truth
      glBegin(GL_LINES);
      auto p1 = gt[i], p2 = gt[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }

    for (size_t i = 0; i < esti.size() - 1; i++) {
      glColor3f(1.0f, 0.0f, 0.0f);  // red for estimated
      glBegin(GL_LINES);
      auto p1 = esti[i], p2 = esti[i + 1];
      glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
      glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
      glEnd();
    }
    pangolin::FinishFrame();
    usleep(5000);   // sleep 5 ms
  }
}

Reference

SLAM KR

Comment Read more

8. Lie Group and Lie Algebra 3 (Derivative of Lie Algebra)

17 Jun 2020 | Visual SLAM

Introduction

Lie Group Vs Lie Algebra

Lie Group

Lie group that is a differentiable manifold

Lie Algebra

The only tangent space for the Lie Group

Vector space that satisfies the lie bracket

The relationship between Lie Group and Lie algebra for SO(3) and SE(3)

Backer-Campbell-Hausdorff

Derivative of Lie Algebra

Perturbation model (left Jacobian)

Lie Algebra in SE(3)

Pseudo-transformers and Lie algebra

섭동 모델 Jacobian을 이용하여 최적화

Reference

SLAM KR

Comment Read more

7. Lie Group and Lie Algebra 2

17 Jun 2020 | Visual SLAM

Introduction

Reasons to Convert Lie Groups and Lie Algebra

Study on Rigid body motion in 3D world such as rotation matrix, rotation vector, Euler angle, quaternion, etc. => Content limited to the expression of rotation

SLAM needs to find the optimal value while updating the camera pose values including rotation and movement. In general, it is solved by solving the optimal R after minimizing t and minimizing the error after configuring the optimization problem.

in the case of a rotation matrix, the complexity increases because there is a constraint that the determinant is an orthogonal matrix.

When the relationship between the Lie group and the Lie algebra is used, the problem of estimating the pose of the camera is converted into an unconstrained optimization problem, which is simplified and accessible.
Sum up

Easy access by using conversion relationship between lie group and lie algebra(as Rotation matrix and translation vector)

Example of Lie Group and Lie ALgebra

3D rotation matrix SO(3), transformation matrix SE(3)

When there are two arbitrary rotation matrices R1 and R2, if the definition of matrix addition is followed, it is no longer a rotation matrix.

The multiplication operation corresponds to the synthesis of a rotation matrix and a transformation matrix, and the product of two rotation matrices means that two rotations are made in succession.

Called Group only for one of these working sets

Sum up

Rotation about z-axis in 2-dimensional x,y plane

Lie Group

Lie group is an algebraic structure consisting of sets and operations

Ex) When a set is called A and some operations are marked with ·, the group is represented by G = (A, ·)

Lie group constraint

proving Lie Group

The following properties are satisfied when only the random rotation matrix is considered

R is a camera rotation that continuously changes with time and is expressed as a function of time R(t)

Since it is still a rotation matrix, the following properties are satisfied

Derivative of time on both sides of the equation (differentiation of the product) gives:

The ^ operator converts a vector to an symmetric skew matrix. This means that any counter symmetric matrix can find the corresponding vector. By putting v we can see:

즉 ^ 요것일 이용하여서 벡터를 symmetric skew matrix로 표현한다. v이것은 반대로 symeetric skew matrix를 vector로 바꾸는 것이다.

Multiply both sides of the spinning equation by R(t). Since R is an orthogonal matrix,

Formula induction process

If Phi(t) does not depend on time, Equation 4.8 can be written as follows to convert it to a linear differential equation.

The above formula is the differential equation for R, and the solution of the linear differential equation is

Approximate solution using Taylor series

Linear differential equation solution

국부 미분 관계 Local differential relationship

Definition of Lie Algebra

Each Lie group has a corresponding Lie Algebra

Lie Algebra is a local property of Lie Group

Lie algebra consists of a vector space V, a real set F, and a two-term operation[,].

Element notation of SO(3): (1) 3D vector and (2) 3x3 matrix can be expressed. The meanings of the two are the same, and ^ and v can be used to convert and notify matrix to vector using vector to matrix.

Element notation of SE(3) :(1)6 dimensional vector and (2)4x4 matrix can be expressed Translation and rotatio can be expressed as rho and phi The meaning of the two is the same, and using ^ and v to matrix from vector to matrix From vector to vector and notation

<a href=https://postimg.cc/G490Jw8M”>

Reference

SLAM KR

Comment Read more

18. Linked List 3

16 Jun 2020 | Algorithm

Delete on Linked list

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};
Node* create(int A[],int n)
{
    int i;
    Node* t, *last;
    Node* head;
    head = new Node;
    head->data = A[0];
    head->next = NULL;
    last = head;
    for(i=1;i<n;i++)
    {
        t = new Node;
        t->data = A[i];
        t->next =NULL;
        last->next = t;
        last = t;
    }
    return head;
}
void Delete(Node* p, int index)
{
    Node* q = NULL;
    Node* head;
    head = p;
    int i;
    if(index<1 || index>5)
        return;
    if(index==1)
    {
        q=head;
        head = head->next;
        free(q);
    }
    else
    {
        for(i=0;i<index-1;i++)
        {
            q=p;
            p=p->next;
        }
        q->next = p->next;
        free(p);
    }
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int main()
{
    int A[]={10,20,30,40,50};
    Node* head;
    head = create(A,5);
    Delete(head,2);
    Display(head);
    return 0;
}

check if a Linked List is sorted

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};
Node* create(int A[],int n)
{
    int i;
    Node* t, *last;
    Node* head;
    head = new Node;
    head->data = A[0];
    head->next = NULL;
    last = head;
    for(i=1;i<n;i++)
    {
        t = new Node;
        t->data = A[i];
        t->next =NULL;
        last->next = t;
        last = t;
    }
    return head;
}
int isSorted(Node* p)
{
    int x=-65536;
    while(p!=NULL)
    {
        if(p->data < x)
            return 0;
        x=p->data;
        p=p->next;
    }
    return 1;
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int main()
{
    int A[]={10,20,30,40,50};
    Node* head;
    head = create(A,5);
    isSorted(head);
    return 0;
}

Reverse a Linked List

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};
Node* create(int A[],int n)
{
    int i;
    Node* t, *last;
    Node* head;
    head = new Node;
    head->data = A[0];
    head->next = NULL;
    last = head;
    for(i=1;i<n;i++)
    {
        t = new Node;
        t->data = A[i];
        t->next =NULL;
        last->next = t;
        last = t;
    }
    return head;
}
int isSorted(Node* p)
{
    int x=-65536;
    while(p!=NULL)
    {
        if(p->data < x)
            return 0;
        x=p->data;
        p=p->next;
    }
    return 1;
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
void Reverse1(Node* p)
{
    int* A;
    int i=0;
    Node* q = p;
    A = new int[5];
    while(q!=NULL)
    {
        A[i]=q->data;
        q= q->next;
        i++;
    }
    q=p;
    i--;
    while(q!=NULL)
    {
        q->data = A[i];
        q = q->next;
        i--;
    }
}
void Reverse2(Node* p)
{
    Node* q=NULL, * r=NULL;
    while(p!=NULL)
    {
        r=q;
        q=p;
        p=p->next;
        q->next=r;
    }
}
void Reverse3(Node* q, Node* p)
{
    if(p)
    {
        Reverse3(p,p->next);
        p->next = q;
    }
    else
        q=p;
}
int main()
{
    int A[]={10,20,30,40,50};
    Node* head;
    head = create(A,5);
    Reverse1(head);


    Display(head);
    return 0;
}

Concatenate and Merge Linked lists

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};
Node* create(int A[],int n)
{
    int i;
    Node* t, *last;
    Node* head;
    head = new Node;
    head->data = A[0];
    head->next = NULL;
    last = head;
    for(i=1;i<n;i++)
    {
        t = new Node;
        t->data = A[i];
        t->next =NULL;
        last->next = t;
        last = t;
    }
    return head;
}
int isSorted(Node* p)
{
    int x=-65536;
    while(p!=NULL)
    {
        if(p->data < x)
            return 0;
        x=p->data;
        p=p->next;
    }
    return 1;
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
Node* Merge(Node* p, Node* q)
{
    Node* last;
    Node* Merged;
    if(p->data < q->data)
    {
        Merged = last = p;
        p=p->next;
        Merged->next=NULL;
    }
    else
    {
        Merged = last = q;
        q = q->next;
        Merged->next = NULL;
    }
    while(p && q)
    {
        if(p->data < q->data)
        {
            last->next = p;
            last = p;
            p=p->next;
            last->next = NULL;
        }
        else
        {
            last->next = q;
            last = q;
            q = q->next;
            last->next = NULL;
        }
        if(p)
            last->next = p;
        if(q)
            last->next = q;
    }
    return Merged;
}
int main()
{
    Node* head_A;
    Node* head_B;
    Node* Merged;
    int A[]={10,20,30,40,50};
    int B[]={15,18,25,30,55};
    head_A = create(A,5);
    head_B = create(B,5);
    Merged = Merge(head_A,head_B);

    Display(Merged);
    return 0;
}

Check Loop

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};
Node* create(int A[],int n)
{
    int i;
    Node* t, *last;
    Node* head;
    head = new Node;
    head->data = A[0];
    head->next = NULL;
    last = head;
    for(i=1;i<n;i++)
    {
        t = new Node;
        t->data = A[i];
        t->next =NULL;
        last->next = t;
        last = t;
    }
    return head;
}
int isSorted(Node* p)
{
    int x=-65536;
    while(p!=NULL)
    {
        if(p->data < x)
            return 0;
        x=p->data;
        p=p->next;
    }
    return 1;
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int isLoop(Node* f)
{
    Node* p,* q;
    p=q=f;
    do
    {
        p=p->next;
        q=q->next; //여기서 부터  q 체크(약 두번씩 점프)
        q=q?q->next:q; // if(q) q->next else q;
    }while(p && q && p=!q);

    // 사이클에서 같아버리면 나가게 된다.

    if(p==q)
        return 1;
    else
        return 0;
}
int main()
{
    Node* head_A;
    Node* t1, * t2;
    int A[]={10,20,30,40,50};
    head_A = create(A,5);
    t1=head_A->next->next;
    t2=head_A->next->next->next->next;
    t2->next = t1;
    cout<<isLoop(head_A);
    return 0;
}

<a href=https://postimg.cc/SYn8ZwVL”> source: imgur.com

<a href=https://postimg.cc/dkyw46rk”> source: imgur.com

Comment Read more

Older Newer

for Robot Artificial Inteligence

9. Non Linear Least Square

Least Square

First Order & Second Order

Gauss Newton

Levenberg-Marquardt

Reference

8. Lie Group and Lie Algebra Practice

Li Algebra Library : Sophus

1. install

2. Cmake

3. SO(3) and SE(3) algorithm using Sophus library(how to use)

Real Example

TrajectoryError(comparing Estimated, visual odometer, ground truth )

Reference

8. Lie Group and Lie Algebra 3 (Derivative of Lie Algebra)

Introduction

Backer-Campbell-Hausdorff

Derivative of Lie Algebra

Perturbation model (left Jacobian)

Lie Algebra in SE(3)

Pseudo-transformers and Lie algebra

Reference

7. Lie Group and Lie Algebra 2

Introduction

Sum up

Example of Lie Group and Lie ALgebra

Sum up

Lie Group

proving Lie Group

Definition of Lie Algebra

Reference

18. Linked List 3

Delete on Linked list

check if a Linked List is sorted

Reverse a Linked List

Concatenate and Merge Linked lists

Check Loop