for Robot Artificial Inteligence

17. Linked List 2

|

Searching in 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;
}
Node* Lsearch(Node* p, int key)
{
    Node* q;
    while(p!=NULL)
    {
        if (key == p->data)
        {
            q->data = p->data;
            return q;
        }
        q=p;
        p=p->next;
    }
    return NULL;
}
Node* Rsearch(Node* p, int key)
{
    if(p==NULL)
        return NULL;
    if(key==p->data)
        return p;
    return Rsearch(p->next,key);
}
int main()
{
    int A[]={3,8,7,10,25,8,32,2};
    Node* temp; // 새로 new, heap에 생성할 필요없이 카피할 데이터는 포인터만 하면 된다.
    Node* result;
    temp = create(A,sizeof(A)/sizeof(A[0]));
    result = Lsearch(temp, 8);
    cout<<result->data<<endl;
    return 0;

}

Insert for 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;
}
Node* insert_(Node* p, int index, int x)
{
    Node* temp;
    int i;
    if(index<0 || index>8)
        return p;
    temp = new Node;
    temp->data = x;
    if(index == 0)
    {
        temp->next = p;
        p = temp;
    }
    else
    {
        for(i=0;i<index-1;i++)
            p=p->next;
        temp->next = p->next;
        p->next = temp;
    }
    return p;
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int main()
{
    int A[]={3,8,7,10,25,8,32,2};
    Node* temp;
    temp = create(A,sizeof(A)/sizeof(A[0]));
    insert_(temp,2,5);
    Display(temp);
    return 0;
}

  • min O(1)
  • max O(n)

Insert for Linked list (Last input)

#include <bits/stdc++.h>
#include <stdio.h>
using namespace std;
class Node
{
public:
    int data;
    Node* next;
};

void insert_(Node* p, int index, int x)
{
    Node* temp;
    int i;
    if(index<0 || index>8)
        return;
    temp = new Node;
    temp->data = x;
    if(index == 0)
    {
        temp->next = p->next;
        p->next = temp;
    }
    else
    {
        for(i=0;i<index-1;i++)
            p=p->next;
        temp->next = p->next;
        p->next = temp;
    }
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int main()
{
    Node* head;
    head = new Node;
    insert_(head,0,15);
    insert_(head,0,8);
    insert_(head,0,9);
    insert_(head,2,10);
    Display(head);
    return 0;
}

Sorted Input

#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 sorted_insert(Node* p, int x)
{
    Node* temp,*q=NULL;
    Node* head;
    temp = new Node;
    temp->data = x;
    temp->next =NULL;
    head = p;
    if(head==NULL)
    {
        head=temp;
    }
    else
    {
        while(p&&p->data<x)
        {
            q=p;
            p=p->next;
        }
        if(p==head)
        {
            temp->next = head;
            head = temp;
        }
        else
        {
            temp->next = q->next;
            q->next = temp;
        }
    }
}
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);
    sorted_insert(head,15);
    Display(head);
    return 0;
}

Remove Duplicates from 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 RemoveDuplicate(Node* p)
{
    Node* q;
    q = p->next;
    while(q!=NULL)
    {
        if(p->data!=q->data)
        {
            p=q;
            q=q->next;
        }
        else
        {
            //dupicated
            p->next = q->next;
            free(q);
            q = p->next;
        }
    }
}
void Display(Node* p)
{
    while(p!=NULL)
    {
        cout<<p->data<<" ";
        p=p->next;
    }
}
int main()
{
    int A[]={10,20,20,40,50,50,50,60};
    Node* head;
    head = create(A,8);
    RemoveDuplicate(head);
    Display(head);
    return 0;
}

Comment  Read more

6. Lie Group and Lie Algebra 1

|

Exponential and logarithmic mapping

  • In mathematics, a manifold is a topological space that locally resembles Euclidean space near each point.
    • a topological space may be defined as a set of points, along with a set of neighbourhoods for each point, satisfying a set of axioms relating points and neighbourhoods.
  • The Earth on which we live is round like (Spherical) [Lie Group]
  • 내가 현재 발을 딛고 땅(locally) 에서는 평면(Plane) 처럼 보인다 [Lie Algebra]
  • 즉 Lie Algebra와 Lie Group은 로봇이 현재 보고 있는 Frame의 Feature pointer들을 Map으로 Represented 하기를 도와주는 것이다.

Special Orthogonal(SO)

  • small letter so is Lie Algebra
  • Capital Letter SO is Lie Group

  • A is Square Matrix

  • a^a = 0 의 의미는 동일한 외점들을 취하면 0이 된다.

Special Euclidean(SE)

  • (pie^)는 Sketric matrix

  • block matrix

summary

  • so<->SO 관계는 리대수-> 리선형으로 하여 Rotation 값을 구할 수 있으며(반대로 리대수의 행렬값을 얻을 수 있음)
  • se<->SE 관계는 리대수->리선형으로 통하여 translation과 Rotation을 구할 수 있고(반대로 리대수 se 행렬값을 얻을 수 있다.)

    Reference

    SLAM KR

Comment  Read more

5. Stereo & RGB-D camera model and Image Information

|

Stereo Camera Model

Recap : Pinhole camera model

Intro

Stereo camera principle

  • b is Baseline
  • d is distance between left and right camera that looking at point of object.

RGB-D camera Model

  • 2 Types of RGB-D Camera
    • Structured Light RGB-D Camera
    • Time of Flight(TOF) RGB-D Camera

  • Structured light를 쏘고 Encoding pattern 으로 받고(Acquisition)
    • Encoding patters: 시간추로 봤을때 라인마다 코드가 있다 (Stripe ID)
  • Decoding하여서(stripe ID 매칭) 3D 구조화 한다.

  • 카메라 안에있는 셀(Emitor)을 통해서 방출과 받음을 반복하는 Frequency를 이용하여 3D 구현화 (Kinetic 2 )
  • Lidar와 다른점
    1. 적외선 사용
    2. Lidar Force처럼 촘촘하게 만드는것이 어려움

Limitation of RGB-D Camera

  • Active Emission & reception
  • Limited Range for depth
  • Not available for outside usage(interference of natural light and other sensor)
  • Cannot handle transparent materials
  • How to overcome? Deep Learning Based Approach

Image information

  • Def: Data obtained by converting 3D space information into 2D digital format through camera equipment

  • Useful information can be obtained by digital image processing

  • Intensity value is 0~255. (Expressed using 8-bit int)
    • Grayscale image -> 0 (dark) to 255 (light)
      • 1 pixel occupies 8 bits of storage space
    • RGB image -> 0,0,0 (dark) to 255,255,255 (bright)
      • 1 pixel occupies 8x3 = 24 bits of storage
  • In case of RGB-D image, it contains depth data (expressed using 16bit int)
    • 0~65536 (up to 65m can be expressed when displayed in meters)

Reference

SLAM KR

Comment  Read more

4. Camera Model & Camera Distortion Practice

|

1. Install Opencv

sudo apt-get install build-essential libgtk2.0-dev libvtk5-dev libjpeg-dev libtiff4-dev libjasper-dev libopenexr-dev libtbb-dev

2. building it in Cmake

project(imageBasics)

set(CMAKE_CXX_STANDARD 11)

find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})

add_executable(imageBasics imageBasics.cpp)
# 链接OpenCV库
target_link_libraries(imageBasics ${OpenCV_LIBS})

add_executable(undistortImage undistortImage.cpp)
# 链接OpenCV库
target_link_libraries(undistortImage ${OpenCV_LIBS})

mkdir build
cd build
cmake ..
make

3. Imagebasic.cpp

#include <iostream>
#include <chrono> // 시간 측정할때 쓰임

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

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

  // 读取argv[1]指定的图像
  cv::Mat image;
  image = cv::imread(argv[1]); //cv::imread函数读取指定路径下的图像

  // 判断图像文件是否正确读取
  if (image.data == nullptr) { //数据不存在,可能是文件不存在
    cerr << "文件" << argv[1] << "不存在." << endl;
    return 0;
  }

  // 文件顺利读取, 首先输出一些基本信息
  cout << "图像宽为" << image.cols << ",高为" << image.rows << ",通道数为" << image.channels() << endl;
  cv::imshow("image", image);      // 用cv::imshow显示图像
  cv::waitKey(0);                  // 暂停程序,等待一个按键输入

  // 判断image的类型
  if (image.type() != CV_8UC1 && image.type() != CV_8UC3) {
    // 图像类型不符合要求
    cout << "请输入一张彩色图或灰度图." << endl;
    return 0;
  }

  // 遍历(traversal)图像, 请注意以下遍历方式亦可使用于随机像素访问
  // 使用 std::chrono 来给算法计时
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (size_t y = 0; y < image.rows; y++) {
    // 用cv::Mat::ptr获得图像的行指针
    unsigned char * row_ptr = image.ptr<unsigned char>(y);  // row_ptr是第y行的头指针
    for (size_t x = 0; x < image.cols; x++) {
      // 访问位于 x,y 处的像素
      unsigned char * data_ptr = &row_ptr[x * image.channels()]; // data_ptr 指向待访问的像素数据
      // 输出该像素的每个通道,如果是灰度图就只有一个通道
      for (int c = 0; c != image.channels(); c++) {
        unsigned char data = data_ptr[c]; // data为I(x,y)第c个通道的值
      }
    }
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast < chrono::duration < double >> (t2 - t1);
  out << "遍历图像用时:" << time_used.count() << " 秒。" << endl;

  // 关于 cv::Mat 的拷贝
  // 直接赋值并不会拷贝数据

  cv::Mat image_another = image;

  // 修改 image_another 会导致 image 发生变化
  image_another(cv::Rect(0, 0, 100, 100)).setTo(0); // 将左上角100*100的块置零
  cv::imshow("image", image);
  cv::waitKey(0);

  // 使用clone函数来拷贝数据
  cv::Mat image_clone = image.clone();
  image_clone(cv::Rect(0, 0, 100, 100)).setTo(255);
  cv::imshow("image", image);
  cv::imshow("image_clone", image_clone);
  cv::waitKey(0);
  // 对于图像还有很多基本的操作,如剪切,旋转,缩放等,限于篇幅就不一一介绍了,请参看OpenCV官方文档查询每个函数的调用方法.
  cv::destroyAllWindows();
  return 0;
}
build/image_basics ubuntu.png

4. undistortImage.cpp

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "/home/chan/Desktop/slambook2/ch5/imageBasics/distorted.png";   // 请确保路径正确

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

  // 本程序实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变,但自己实现一遍有助于理解。

  // 畸变参数
  double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
  // 内参
  double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

  cv::Mat image = cv::imread(image_file);   // 图像是灰度图,CV_8UC1
  int rows = image.rows, cols = image.cols;
  cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸(ji)变以后的图

  // 计算去畸变后图像的内容
  for (int v = 0; v < rows; v++) {
    for (int u = 0; u < cols; u++) {
      // 按照公式,计算点(u,v)对应到畸变图像中的坐标(u_distorted, v_distorted)
      double x = (u - cx) / fx, y = (v - cy) / fy;
      double r = sqrt(x * x + y * y);
      double x_distorted = x * (1 + k1 * r * r + k2 * r * r * r * r) + 2 * p1 * x * y + p2 * (r * r + 2 * x * x);
      double y_distorted = y * (1 + k1 * r * r + k2 * r * r * r * r) + p1 * (r * r + 2 * y * y) + 2 * p2 * x * y;
      double u_distorted = fx * x_distorted + cx;
      double v_distorted = fy * y_distorted + cy;

      // 赋值(fuzhi)(value assignment) (最近邻插值)
      if (u_distorted >= 0 && v_distorted >= 0 && u_distorted < cols && v_distorted < rows) {
        image_undistort.at<uchar>(v, u) = image.at<uchar>((int) v_distorted, (int) u_distorted);
      } else {
        image_undistort.at<uchar>(v, u) = 0;
        }
      }
    }

    // 画图去畸变后图像
    cv::imshow("distorted", image);
    cv::imshow("undistorted", image_undistort);
    cv::waitKey();
    return 0;
}

5. Stereo Vision.cpp(Point Cloud)

#include <opencv2/opencv.hpp>
#include <vector>
#include <string>
#include <Eigen/Core>
#include <pangolin/pangolin.h>
#include <unistd.h>

using namespace std;
using namespace Eigen;

// 文件路径
string left_file = "./left.png";
string right_file = "./right.png";

// 在pangolin中画图,已写好,无需调整
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud);

int main(int argc, char **argv) {
  // 内参(camera 내부 파라미터)
  double fx = 718.856, fy = 718.856, cx = 607.1928, cy = 185.2157;

  // 基线(Baseline)
  double b = 0.573;

  // 读取图像
  cv::Mat left = cv::imread(left_file, 0);
  cv::Mat right = cv::imread(right_file, 0);
  cv::Ptr<cv::StereoSGBM> sgbm = cv::StereoSGBM::create(0, 96, 9, 8 * 9 * 9, 32 * 9 * 9, 1, 63, 10, 100, 32);    // 神奇的参数
  cv::Mat disparity_sgbm, disparity; // disparity(差异)
  sgbm->compute(left, right, disparity_sgbm);
  disparity_sgbm.convertTo(disparity, CV_32F, 1.0 / 16.0f);

  // 生成点云
  vector<Vector4d, Eigen::aligned_allocator<Vector4d>> pointcloud;
  // 如果你的机器慢,请把后面的v++和u++改成v+=2, u+=2
  for (int v = 0; v < left.rows; v++)
  {
    for (int u = 0; u < left.cols; u++)
    {
      if (disparity.at<float>(v, u) <= 0.0 || disparity.at<float>(v, u) >= 96.0) continue;
      Vector4d point(0, 0, 0, left.at<uchar>(v, u) / 255.0); // 前三维为xyz,第四维为颜色
      // 根据双目模型计算 point 的位置
      double x = (u - cx) / fx;
      double y = (v - cy) / fy;
      double depth = fx * b / (disparity.at<float>(v, u));
      point[0] = x * depth;
      point[1] = y * depth;
      point[2] = depth;
      pointcloud.push_back(point);
    }
  }
  cv::imshow("disparity", disparity / 96.0);
  cv::waitKey(0);
  // 画出点云
  showPointCloud(pointcloud);
  return 0;
}
void showPointCloud(const vector<Vector4d, Eigen::aligned_allocator<Vector4d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud 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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3f(p[3], p[3], p[3]);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}
  • CMAKE
find_package(Pangolin REQUIRED)

add_executable(stereoVision stereoVision.cpp)
target_link_libraries(stereoVision ${OpenCV_LIBS} ${Pangolin_LIBRARIES})
mkdir build
cd build
cmake ..
make
./stereoVision

6. Pointcloud Using Depth Camera

  • 궁국적인 목적은 camera pose를 이용하여 카메라에서 적출된 observation point의 pose xyz를 구하는 것
#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/format.hpp>  // for formating strings
#include <sophus/se3.h>
#include <pangolin/pangolin.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <Eigen/Core>
// 稠密矩阵的代数运算(逆,特征值等)
#include <Eigen/Dense>

using namespace std;

typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType; // pointcloud pose
typedef Eigen::Matrix<double, 6, 1> Vector6d;

// 在pangolin中画图,已写好,无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);

int main(int argc, char **argv) {
  vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
  TrajectoryType poses;         // 相机位姿
  ifstream fin("./pose.txt"); //frame 별로 camerap pose가 있음
  if (!fin) {
    cerr << "请在有pose.txt的目录下运行此程序" << endl;
    return 1;
  }
  for (int i = 0; i < 5; i++) { // 5개 프레임
    boost::format fmt("./%s/%d.%s"); //图像文件格式
    colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
    depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像
    double data[7] = {0,0,0,0,0,0,0}; // double data[7] = {0}; same as above
    for (auto &d:data)
    {
      fin >> d; // pose 데이터 넣기 각 프레임의
    }
    Sophus::SE3 pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]), Eigen::Vector3d(data[0], data[1], data[2])) // Speical eclidea 을 써서 리군 변환 리대수 변환으로 쉽게 pose구함
    poses.push_back(pose);
    }

    // 计算点云并拼接
    // 相机内参
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud; //포인트 클라우드
    pointcloud.reserve(1000000); //포인터 클라우드의 개수 한량

    for (int i = 0; i < 5; i++) {
      cout << "转换图像中: " << i + 1 << endl;
      cv::Mat color = colorImgs[i]; // 이미지 불러오기
      cv::Mat depth = depthImgs[i];
      Sophus::SE3 T = poses[i]; // 이미지당 카메라 포즈 불러오기
      for (int v = 0; v < color.rows; v++)
      {
        for (int u = 0; u < color.cols; u++)
        {
          unsigned int d = depth.ptr<unsigned short>(v)[u]; // 深度值
          if (d == 0) continue; // 为0表示没有测量到
          Eigen::Vector3d point; // Depth로 통하여 현재 표현되는 값들을 모두 pointcloud로 변환하여 표현(octomap처럼, 아직까지는 특정점에대한 포인트 클라우드가 아니다.)
          point[2] = double(d) / depthScale;
          point[0] = (u - cx) * point[2] / fx;
          point[1] = (v - cy) * point[2] / fy;
          Eigen::Vector3d pointWorld = T * point; // point를 월드에 reprent

          Vector6d p;
          p.head<3>() = pointWorld; // giving pose
          p[5] = color.data[v * color.step + u * color.channels()];   // blue
          p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
          p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
          pointcloud.push_back(p);
        }
      }

    }
    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    return 0;
}
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {

    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }

    pangolin::CreateWindowAndBind("Point Cloud 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);

        glPointSize(2);
        glBegin(GL_POINTS);
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);
            glVertex3d(p[0], p[1], p[2]);
        }
        glEnd();
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
    return;
}

  • CMAKE
cmake_minimum_required(VERSION 2.8)

project(jointmap)

set(CMAKE_CXX_STANDARD 11)
# opencv
find_package( OpenCV REQUIRED)
include_directories( ${OpenCV_INCLUDE_DIRS})

# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})

# Pangolin
find_package(Pangolin REQUIRED)

# eigen
include_directories("/usr/include/eigen3/")

# pcl
find_package( PCL REQUIRED COMPONENT common io)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})

list(REMOVE_ITEM PCL_LIBRARIES vtkproj4)

#execute
add_executable(joinMap joinMap.cpp)
target_link_libraries(joinMap ${OpenCV_LIBS} ${Pangolin_LIBRARIES} ${PCL_LIBRARIES} )
./jointmap

7. Pointcloud Using Depth Camera 2 with pointcloud library

#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>  // for formating strings
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

int main( int argc, char** argv )
{
  /*彩色图和灰度图各5张,所以用容器来存储*/
  vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图

  /*vector有两个参数,后面的参数一般是默认的,这里用适合Eigen库的对齐方式来初始化容器,总共有5张图片 所以对应着5个位姿矩阵*/
  vector<Eigen::Isometry3d,Eigen::aligned_allocator<Eigen::Isometry3d>> poses;   // 相机位姿

  ifstream fin("../pose.txt"); // pose route
  if (fin.bad())//如果没有打开 那么提示错误!
  {
    cerr<<"请在有pose.txt的目录下运行此程序"<<endl;
    return 1;
  }

  /*循环读取图像*/
  for ( int i=0; i<5; i++ )
  {
    /*用boost中的format格式类,来循环读取图片,否则单张读取图片就会有问题*/
    /* 当在命令行中执行的时候这里必须要为../  在当前ide中执行的时候要修改为*/
    boost::format fmt( "../%s/%d.%s" ); //图像文件格式   "../%s/%d.%s"   ../ 表示可执行文件在build中,图像在上一个目录,所以用../
    /* 这里的%对应. color对应%s  下面的符号就是与上面一致对应的 * /
    colorImgs.push_back( cv::imread( (fmt%"color"%(i+1)%"png").str() ));
    depthImgs.push_back( cv::imread( (fmt%"depth"%(i+1)%"pgm").str(), -1 )); // 使用-1读取原始图像

    /*基于范围的for循环,表示从data数组的第一项开始 循环遍历  auto表示自动根据后面的元素 获得符合要求的类型*/
    double data[7] = {0};
    for ( auto& d:data )//auto自动类型转换
    {
      fin>>d;//文件流类型的变量fin将pose.txt中的数据给了d数组
    }
    Eigen::Quaterniond q( data[6], data[3], data[4], data[5] ); //四元数 data[6]是实数 但是coeffis输出的是先虚数后实数
    Eigen::Isometry3d T(q);                                     //变换矩阵初始化旋转部分,
    T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));//变换矩阵初始化平移向量部分
    poses.push_back( T );   //存储变换矩阵到位姿数组中

  }
  // 计算点云并拼接
  // 相机内参
  double cx = 325.5;
  double cy = 253.5;
  double fx = 518.0;
  double fy = 519.0;
  double depthScale = 1000.0;//
  // 定义点云使用的格式:这里用的是XYZRGB
  typedef pcl::PointXYZRGB PointT;
  typedef pcl::PointCloud<PointT> PointCloud;
  // 新建一个点云//PointCoud::Ptr是一个智能指针类 通过构造函数初始化指针指向的申请的空间
  /*Ptr是一个智能指针,返回一个PointCloud<PointT> 其中PointT是pcl::PointXYZRGB类型。它重载了->  返回了指向PointCloud<PointT>的指针
   *Ptr是下面类型 boost::shared_ptr<PointCloud<PointT> > */
  /*pointCloud 是一个智能指针类型的对象 具体可以参考http://blog.csdn.net/worldwindjp/article/details/18843087*/
  PointCloud::Ptr pointCloud( new PointCloud );
//    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloud( new pcl::PointCloud<pcl::PointXYZRGB> );
  /*将5张图片 像素坐标转换到相机坐标 之后转换到世界坐标存储到点云格式的变量中 for循环之后用pcl的相关函数将点云转换到pcl能够显示的格式*/
  for ( int i=0; i<5; i++ )//转换5张图像
  {
    cout<<"转换图像中: "<<i+1<<endl;
    cv::Mat color = colorImgs[i];
    cv::Mat depth = depthImgs[i];
    Eigen::Isometry3d T = poses[i];

    /*           插入部分
     * //color.at<cv::Vec3b>(471,537)[0] = 12;//修改图像上的对应像素位置的值
    //color.ptr<cv::Vec3b>(471)[537][0] = 12;//与上面的效果一样

    //测试像素的输出效果,这里无法通过cout<<color.at<cv::Vec3b>(471,537)[0] 这种方式来输出第一个通道的值,因为每个通道的像素占了8位而unsigned char
    // 表示ascii码 所以输出的时候不是正确的数字,可以通过下面的方式强制转化为int类型(或者用自带的类型转换方式进行显示转换),就可以看到内部的值了
    //需要注意的一点是 cout页无法输出char类型的变量的地址,也是需要强制转换成void *类型的指针才能正常输出char类型变量的地址信息。
    if(colorImgs[i].channels() == 3) {
         std::cout << "测试1结果 " << color.ptr<cv::Vec3b>(471)[537] << "正确的结果:  "
                   << (char) color.at<cv::Vec3b>(471, 537)[0] << std::endl;
         std::cout << depth.ptr<unsigned short>(471)[537] << std::endl;
         std::cout << colorImgs[i].at<cv::Vec3b>(471, 537) << std::endl;
    }
                 插入部分结束
     */

    /*对图像像素进行坐标转换,将图像的坐标通过内参矩阵K转换为相机坐标系下的坐标,之后通过外参矩阵T 转化为世界坐标系下的坐标*/
    for ( int v=0; v<color.rows; v++ )
    {
      /*通过用Mat中的ptr模板函数 返回一个unsigned short类型的指针。v表示行 根据内部计算返回data头指针 + 偏移量来计算v行的头指针
       * 图像为单通道的   depth.ptr<unsigned short> ( v ) 来获取行指针*/
      unsigned int d = depth.ptr<unsigned short> ( v )[u]; // 深度值16位存储 一般color图像像素中每一个通道是8位
      /*             单通道遍历图像的方式总结:
      *  注意深度图像的像素占16位 与普通图片每个通道的像素为8位不同
      * 1、同样是用上面的双层for循环,遍历图像 用at方式
      * for ( int v=0; v<color.rows; v++ )
      *      for ( int u=0; u<color.cols; u++ )
      *          unsigned int d = depth.at<unsigned short >(v,u);
      *
      *
      * 2、使用迭代器进行图像的遍历
      * 不是基于for循环了
      * cv::MatIterator_<unsigned short > begin,end;
      * for( begin =depth.begin<unsigned short >(), end = depth.end<unsigned short >(); begin != end; ){}
      *
      * 3、使用指针的方式 如本实验的结果
      * * /

     //迭代器的参数是通道数,因为深度图是单通道的,每个像素的值是unsigned short,所以参数是unsigned short
     //begin代表像素的开始地方
     if ( d==0 ) continue;                // 为0表示没有测量到 然后继续进行for循环那么跳过这个像素继续执行 在后面形成点云时需要设置is_dense为假
     Eigen::Vector3d point;
     // pixel coordinate (u,v,d) ->camera coordinate p_c(x,y,z)
     point[2] = double(d)/depthScale;    //对实际尺度的一个缩放 z
     point[0] = (u-cx)* point[2]/fx;      //根据书上5.5式子---86页  x
     point[1] = (v-cy)* point[2]/fy;  //y
     Eigen::Vector3d pointWorld = T*point;   //将相机坐标系转换为世界坐标系

     PointT p ;
     p.x = pointWorld[0];    //将世界坐标系下的坐标用pcl专门的点云格式存储起来
     p.y = pointWorld[1];
     p.z = pointWorld[2];
     /*  color.step 虽然是一个类,但是它内部有一个转换操作符 operator size_t() const;
      * 此时的color.size编译器就会把它当做size_t类型的变量,这个值的大小是1920 这个是随着图像的读入MAT类中会有自动转换然后存储的buf[]中 * /
     p.b = color.data[ v* color.step+u*color.channels() ];
     p.g = color.data[ v*color.step+u*color.channels()+ 1 ];
     p.r = color.data[ v*color.step+u*color.channels()+ 2 ];

     /*  -> 是智能指针重载的 然后返回的类型就是输入的类型 可以看上面Ptr的解释说明 * /
     pointCloud->points.push_back( p );//存储格式好的点
    }
  }
  std::cout<<"点云的列和行为 : "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;
  //这里有可能深度图中某些像素没有深度信息,那么就是包含无效的像素,所以先置为假,但是如果设置成true的话 也没有看出来有什么不一样的地方
  pointCloud->is_dense = false;
  std::cout<<"点云的列和行为 : "<<pointCloud->width<<" "<<pointCloud->height<<std::endl;

  cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
  pcl::io::savePCDFileBinary("map.pcd", *pointCloud );//获取pointCloud指向的对象 这个就当做获取普通指针指向的对象来理解,这个对象是在定义的时候new出来的一段内存空间。
  return 0;
}
/*                                            备注: 3通道的图像的遍历方式总结
 * 对于单通道来说 每个像素占8位 3通道则是每个矩阵元素是一个Vec3b 即一个三维的向量 向量内部元素为8位数的unsigned char类型
 * 1、使用at遍历图像
 * for(v)row
 *  for(u)col
 *      image.at<Vec3b>(v,u)[0] 表示第一个通道的像素的值
 *      image.at<Vec3b>(v,u)[1]
 *      image.at<Vec3b>(v,u)[2]
 * 2、使用迭代器方式 (实际上就是一个指针指向了 cv::Mat矩阵元素)
 * cv::MatIterator_<Vec3b>begin,end;
 * for( begin = image.begin<Vec3b>(), end = image.end<Vec3b>() ; begin != end;  )
 *      (*begin)[0] = ...
 *      (*begin)[1] = ...
 *      (*begin)[2] = ...
 *
 * 3、用指针的方式操作
 * for(v)
 *  for(u)
 *      image.ptr<Vec3b>(v)[u][0] 表示第一个通道
 *      image.ptr<Vec3b>(v)[u][0] 表示第二通道
 *              .
 *              .
 *              .
 * */
./jointmap
pcl_viewer map.pcd

8. Monocular Camera Calibration

https://blog.csdn.net//heyijia0327/article/details/43538695

9. RGB-D Camera Calibration

https://blog.csdn.net//Siyuada/article/details/78981555

Reference

SLAM KR

Comment  Read more

4. Camera Model & Camera Distortion

|

Camera and Image

  • Visual SLAM(Camera-based)
    • Robot uses camera images as a means to observe the outside world(机器人使用相机图像作为观察外界的手段)
    • Camera records real-world color and brightness information in pixels(相机以像素为单位记录现实世界的颜色和亮度信息)
      1. In a three-dimensional world, light is reflected and emitted by objects(在三维世界中,光被物体反射和发射。)
      2. Light received through the lens is projected onto the camera’s image sensor (e.g., CCD, CMOS)(通过镜头接收的光会投射到相机的图像传感器(例如CCD,CMOS)上)
      3. Photodiode reacts to convert the detected light intensity and position into digital information(光电二极管起反应,将检测到的光强度和位置转换为数字信息)
      4. Reconstructed/stored as a digital image through an image processing engine (通过图像处理引擎重建/存储为数字图像)
    • For Visual SLAM, a geometric model of the process in which a point in 3D space is projected onto a 2D image plane(对于Visual SLAM,是将3D空间中的点投影到2D图像平面上的过程的几何模型)
  • Word Coordinate to image plane
    • A process in which a point in 3D space is projected onto a 2D image plane(将3D空间中的点投影到2D图像平面上的过程)
      1. World coord to a point 상의 in 3D space. Expressed in coordinates 𝑷_w(世界坐标到3D空间中的point点。 以坐标𝑷_w表示)

Pinhole Camera Model

  • The mathematical relationship between the coordinates of a point in three-dimensional space and its projection onto the image plane of an ideal pinhole camera, where the camera aperture is described as a point and no lenses are used to focus light.(三维空间中的点的坐标与其在理想针孔相机的图像平面上的投影之间的数学关系,其中相机光圈被描述为一个点,并且没有使用透镜来聚焦光线。)
  • Projection of a 3D point onto the image plane(将3D点投影到图像平面上)
    • Relationship between the point 𝑷_c and in the three-dimensional space expressed in the camera coordinate system and the image plane(相机坐标系中表示的三维空间中的点𝑷_c与像平面之间的关系)

  • Physical image plane to uv image plane(物理图像平面到uv图像平面)
    • Sampling and quantization are performed in the process of converting the light detected by the physical imaging sensor into pixels on the digital image plane.(在将由物理成像传感器检测到的光转换为数字图像平面上的像素的过程中执行采样和量化)

Camera Distortion

  • Distortion correction plays a very important role in the field of image processing and image (image) utilization.(失真校正在图像处理和图像(图像)利用领域中起着非常重要的作用。)

  • Image distortion is a serious problem in guessing real-world information from images acquired by a camera(在从相机获取的图像中猜测真实信息时,图像失真是一个严重的问题。)

  • As shown in the figure, the degree of distortion in grasping the shape of the road and building from the image or estimating the location is linked to the error level of the estimated result.(如图所示,从图像中把握道路和建筑物的形状或估计位置时的变形程度与估计结果的误差水平相关。)

Definition of Distortion

  • 현실(3차원)세계의 정보를 이미지(2차원)로 투영하는 과정에서 렌즈의 형상(구면)이 Image Plane(평면)에 빛을 올바르게 전달하지 못하여 수차(Aberration) 발생

  • 이미지의 왜곡은 크게 두 가지로 구분한다.
    • 왜곡(Distortion) : 상(Plane)의 가장자리에 있는 직선을 휘어져 보이게 만드는 렌즈의 수차
    • 원근 왜곡(Perspective Distortion) : 렌즈가 피사체에 아주 가깝게 접근했을 때(광각 효과), 또는 아주 멀리 떨어져 있을 때(망원효과) 발생하는 원근감의 변화
  • 왜곡은 다시 크게 방사왜곡(Radial Distortion)과 접선왜곡(Tangential Distortion)으로 구분한다
    • 방사왜곡 : 카메라에 사용된 볼록렌즈의 굴절률에 의한 왜곡
    • 접선왜곡 : 카메라 이미지 센서와 렌즈의 수평이 맞지 않아서 생기는 왜곡
    • 이러한 왜곡을 렌즈왜곡(Lens Distortion)이라고도 부른다
  • 일반적으로 이미지의 왜곡은 Normalized Image Plane에서 정의됩니다
    • 같은 카메라여도 내부 파라미터의 값이 다르기 때문에, 이러한 영향을 제거한 영역에서 정의가 필요

Radial Distortion & Tangential Distortion

  • 방사왜곡은 왜곡의 형태에 따라 술통형왜곡(Barrel Distortion)과 바늘꽂이형 왜곡(Pincushion Distortion)으로 부른다.

Radial Distortion

Tangential Distortion

Distortion represented by a mathematical model

  • M 은 즉 켈레브레이션을 할떄 필요한 형상 네모나 동그라미 같은 것을 가르킨다.

Reference

SLAM KR

Comment  Read more