for Robot Artificial Inteligence

31. Sorting Technique 1

|

Intro

Bubble Sort

#include <iostream>

using namespace std;

template<class T>
void Print(T& A, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i < n; i++)
    {
        cout<<A[i]<<flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]" <<endl;
}
void swap_(int* x, int* y)
{
    int temp = * x;
    * x = * y;
    * y = temp;
}
void BubbleSort(int* A, int n)
{
    int flag = 0;
    for(int i=0; i<n-1; i++)
    {
        for(int j=0; j<n-1-i;j++)
        {
            if(A[j]>A[j+1])
            {
                swap_(&A[j],&A[j+1]);
                flag = 1;
            }
        }
        if(flag == 0)
        {
            return;
        }
    }

}
int main()
{
    int A[] = {3,7,9,10,6,5,12,4,11,2};
    int n = sizeof(A)/sizeof(A[0]);
    Print(A,n,"A");
    BubbleSort(A,n);
    Print(A,n,"A");
    return 0;
}

Insertion Sort

#include <iostream>

using namespace std;

template<class T>
void Print(T& A, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i < n; i++)
    {
        cout<<A[i]<<flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]" <<endl;
}
void insertionSort(int* A, int n)
{
    for(int i = 1; i<n; i++)
    {
        int j = i - 1;
        int x = A[i];
        while(j>-1 && A[j]>x)
        {
            A[j+1] = A[j];
            j--;
        }
        A[j+1] = x;
    }
}
int main()
{
    int A[] = {3,7,9,10,6,5,12,4,11,2};
    int n = sizeof(A)/sizeof(A[0]);
    Print(A,n,"A");
    insertionSort(A, n);
    Print(A,n,"A");
    return 0;
}

Compare Bubble sort and Insertion sort

Selection Sort

#include <iostream>

using namespace std;

template<class T>
void Print(T& A, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i < n; i++)
    {
        cout<<A[i]<<flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]" <<endl;
}
void Swap(int * x, int * y)
{
    int temp;
    temp = * x;
    * x = * y;
    * y = temp;
}
void SelectionSort(int* A, int n)
{
    for(int i =0; i<n-1; i++)
    {
        int j;
        int k;
        for(j=k=i; j<n; j++)
        {
            if(A[j]<A[k])
                k = j;
        }
        Swap(&A[i], &A[k]);
    }
}
int main()
{
    int A[] = {3,7,9,10,6,5,12,4,11,2};
    int n = sizeof(A)/sizeof(A[0]);
    Print(A,n,"A");
    SelectionSort(A, n);
    Print(A,n,"A");
    return 0;
}

Quick Sort

#include <iostream>

using namespace std;

template<class T>
void Print(T& A, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i < n; i++)
    {
        cout<<A[i]<<flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]" <<endl;
}
void Swap(int* x, int* y)
{
    int temp;
    temp = * x;
    * x = * y;
    * y = temp;
}
// first element pivot
int Partition(int* A, int low, int high)
{
    int pivot = A[low];
    int i = low+1;
    int j = high;

    while(true)
    {
        while(i<=j&&A[i]<=pivot)
        {
            i++;
        }
        while(A[j]>=pivot && j>=i)
        {
            j--;
        }
        if(j<i)
        {
            break;
        }
        else
        {
            // 위에 조건을 맞녹하지 않을 경우 즉 A[i]값이 피벗 보다 크고 A[j]가 피벗보다 작을경우
            Swap(&A[i], &A[j]);
        }

    }
    Swap(&A[low], &A[j]);
    return j;
}
// first pivot
void QuickSort(int* A, int low, int high)
{
    if(low<high)
    {
        int p = Partition(A,low,high);
        QuickSort(A,low, p-1);
        QuickSort(A,p+1, high);
    }

}

// Quick Sort using INT_MAX or INFINITY
int Partition_infinity(int A[], int low, int high)
{
    int pivot = A[low];
    int i = low;
    int j = high;
    do{
        // increment i as long as elements are smaller/equal to pivot
        do{i++;}while(A[i]<=pivot);

        // Decrement j as long as elements are larger than pivot
        do {j--;} while (A[j] > pivot);

        if(i<j)
        {
            Swap(&A[i], &A[j]);
        }
    }while(i<j);

    Swap(&A[low], &A[j]);
    return j;
}
// using inifinity
void QuickSort_infinity(int* A, int low, int high)
{
    if(low<high)
    {
        int p = Partition_infinity(A,low,high);
        QuickSort_infinity(A,low, p);
        QuickSort_infinity(A,p+1, high);
    }

}


// Last Element is Pivot + without using INT_MAX or INFINITY
int partitionLast(int* A, int low, int high)
{
    int pivot = A[high];
    int i = low -1;
    for(int j= low; j<=high-1;j++)
    {
        if(A[j]<pivot)
        {
            i++;
            Swap(&A[i],&A[j]);
        }
    }
    Swap(&A[i+1], &A[high]);
    return i+1;
}
void QuicksortLast(int* A, int low, int high)
{
    if(low<high)
    {
        int p = partitionLast(A, low, high); // A[p] in sorted position
        QuicksortLast(A,low,p-1);
        QuicksortLast(A,p+1,high);
    }
}
int main()
{
    int A[] = {3,7,9,10,6,5,12,4,11,2};
    int n = sizeof(A)/sizeof(A[0]);
    Print(A,n,"A");


    // First pivot
    QuickSort(A, 0,n-1);
    Print(A,n,"A");

    cout << "Last Element as Pivot + w/o INT_MAX or Infinity" << endl;
    int B[] = {11, 13, 7, 12, 16, 9, 24, 5, 10, 3};
    Print(B, sizeof(B)/sizeof(B[0]), "\t\tB");

    // last pivot
    QuicksortLast(B, 0, sizeof(B)/sizeof(B[0])-1);
    Print(B, sizeof(B)/sizeof(B[0]), " Sorted B");
    cout<<endl;

    cout << "First Element as Pivot + w/o INT_MAX or Infinity" << endl;
    int C[] = {11, 13, 7, 12, 16, 9, 24, 5, 10, 3};
    Print(C, sizeof(C)/sizeof(C[0]), "\t\tC");

    // int max or infinity
    QuickSort_infinity(C, 0, sizeof(C)/sizeof(C[0])-1);
    Print(C, sizeof(C)/sizeof(C[0]), "\t\tC");

    return 0;
}

Merging

#include <iostream>

using namespace std;

template<class T>
void Print(T& A, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i < n; i++)
    {
        cout<<A[i]<<flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]" <<endl;
}
void Swap(int* x, int* y)
{
    int temp;
    temp = * x;
    * x = * y;
    * y = temp;
}

void Merge(int x[], int y[], int z[], int m, int n)
{
    int i = 0;
    int j = 0;
    int k = 0;
    while(i<m && j<n)
    {
        if(x[i]<y[j])
        {
            z[k++] = x[i++];
        }
        else
        {
            z[k++] = y[j++];
        }
    }
    while(i<m)
    {
        z[k++] = x[i++];
    }
    while(j<n)
    {
        z[k++] = y[j++];
    }

}
void MergeSingle(int* A, int low, int mid, int high)
{
    int i = low;
    int j = mid+1;
    int k = low;
    int B[high+1];
    while(i<mid && j<high)
    {
        if(A[i]<A[j])
        {
            B[k++] = A[i++];
        }
        else
        {
            B[k++] = A[j++];
        }
    }
    while(i<mid)
    {
        B[k++] = A[i++];
    }
    while(j<high)
    {
        B[k++] = A[j++];
    }
    for(int i = low; i<=high; i++)
    {
        A[i] = B[i];
    }
}

int main()
{
    int A[] = {2, 10, 18, 20, 23};
    int m = sizeof(A)/sizeof(A[0]);
    Print(A, m, "\t A");

    int B[] = {4, 9, 19, 25};
    int n = sizeof(B)/sizeof(B[0]);
    Print(B, n, "\t B");

    int r = m+n;
    int C[r];
    Merge(A, B, C, m, n);

    // Print function does not work for variable length array C
    cout << "Sorted" << ": [" << flush;
    for (int i=0; i<r; i++){
        cout << C[i] << flush;
        if (i < r-1){
            cout << ", " << flush;
        }
    }
    cout << "]" << endl;
    cout << endl;

    int D[] = {2, 5, 8, 12, 3, 6, 7, 10};
    Print(D, sizeof(D)/sizeof(D[0]), "\t\tD");
    MergeSingle(D, 0, 3, 7);
    Print(D, sizeof(D)/sizeof(D[0]), " Sorted D");
    return 0;
}

Comment  Read more

25. Voxel Hashing

|

Volumetric Representation

  • Volumetric representation using a hash lookup
  • Data structures and operations(공간 효율적으로 맵핑)
    • Voxel
    • Voxel Block hash
    • Hash table and hashing function
    • Hash table Operations
  • Voxel
    • A value on a regular grid in 3D space, the extended concept of 2D pixel
    • Widely used for Realistic rendering 3D object in computer graphics.
    • Data
      • Truncated signed distance function (TSDF) value
      • TSDF Weight
      • Color value
      • Color Weight
  • Truncated signed distance function (TSDF)
    • Predefined 3D volume is subdivided uniformly into a 3D grid of voxels
    • These values are positive in-front of the surface and negative behind
    • Zero-crossing point means the surface of the object

  • Voxel block array
    • Majority of data stored in the regular voxel grid is marked either as
      • Free space
      • Unobserved space
    • Only store the surface data by efficient hashing scheme(표면 정보만 저장)
    • Grouped voxels in blocks of predefined size (ex. 8x8x8)
    • Data
      • Positions of the corner of the 8x8x8 voxel block
      • Offset in the excess list
      • Pointer to the voxel block array

  • Unordered entries는 행렬의 overide를 예방하기 위해

  • Hash table

    • To quickly and efficiently find the position of a certain voxel block in the voxel block array

  • Hash table operations
    1. Given a target 3D voxel location in world coordinates
    2. Compute its corresponding voxel block location by dividing the voxel location by the size of the voxel block array
    3. Call the hashing function to compute the index of the bucket from the ordered part of the hash table

    4. Return
      • Returns the voxel stored at the target location within the block addressed by the hash entry
    5. Insertion
      • Reserves a block inside the voxel block array

Hashing Operation

Reference

SLAM KR

Comment  Read more

24. Mapping TSDF

|

3D Reconstruction

  • From (RGB –)D images to 3D voxel grid

Transformations between the different coordinate systems

TSDF

  • Truncated Signed Distance Function (TSDF)
    • 3D 모델링 할떄 사용
    • Signed distance function
      • Distance of the closest zero crossing (surface)
        • 멀어지면 + , 사물 안에는 -
    • Truncated signed distance function
      • Subtract it from the distance of the voxel itself and divide by the truncation threshold
        • 즉, “0”에서 멀어지는 값들은 잘라낸다.

  • Build TSDF

Overview

  • From (RGB –)D images to 3D voxel grid

Raycasting

  • TSDF Voxel 맵으로 부터 Normal Vectors 구하기

Results

  • Volumetric TSDF Fusion of RGB D Images in Python (2018)
    • https github.com/andyzeng/tsdf fusion python
    • GPU (CUDA)
  • CHISEL: Real Time Large Scale 3D Reconstruction Onboard a Mobile Device using Spatially Hashed Signed Distance Fields (RSS 2015)
    • https:// github.com/personalrobotics/OpenChisel
    • CPU

Reference

SLAM KR

Comment  Read more

23. Mapping Octomap

|

Introduction

  • Octomap Definition
    • Efficient probabilistic 3D spatial mapping algorithm using Octree, data structure
  • what?
    • Octree
    • probabilistic
    • 3D Mappoint(Mapping)
  • Data representation method in 3D space : Voxel
  • Probabilistic data representation : Occupancy Grid Map
  • Efficient 3D Map data representation : Octree

Data representation method in 3D space

>

>

  • 2D 공간의 한 점을 정의한 그래픽 정보 -> Pixel
  • 3D 공간의 한 점을 정의한 그래픽 정보 -> Voxel

>

  • By using these voxels, it can save space from data.

>

  • In other words, Octomap is a method of expressing data in a three-dimensional space using the concept of Voxel.

probabilistic

>

  • The sensor that acquires the information of the object may acquire uncertain data.

  • Let’s express the existence of the object by probabilistic modeling the uncertainty of these sensors!

    • Object exists. (Occupied, OCC)
    • Object does not exist. (Non-occupied, EMP)

>

>

  • There are various ways to create an Occupancy Grid Map. (Various modeling methods.)
  • See the blog below for more information.
    • http://jinyongjeong.github.io/2017/02/21/lec10_Grid_map/
  • As a way of expressing the existence of data in Octomap, Occupancy Estimation (the basis for creating an Occupancy Grid Map) is used.

Efficient 3D Map data representation : Octree

>

>

>

Summary

  • Octomap includes all three features described above.
  1. The concept of Voxel is needed to represent the data that exists in the 3D space.

  2. Octree data structure is adopted for efficient data representation.

  3. Since uncertainty exists in the data acquired by the actual sensor, the existence of the object is judged using the occupancy estimation method.

Reference

SLAM KR

Comment  Read more

30. Self balancing tree using Heap

|

Binary Heap

Insert in Binary Heap

<a

>

>

>

#include <iostream>
#include <vector>
using namespace std;

//Lecture Based
void InsertA(int* A, int n)
{
    int i=n; // i = 9
    int temp= A[n]; // temp = 50
    while(i>0&&temp>A[i%2 == 0 ? (i/2)-1:i/2])
    {
        // if odd, A[i/2]
        // if even A[i/2+1]
        A[i] = A[i%2 == 0 ? (i/2)-1:i/2];
        // first, A[9] = A[5] = 10;
        i = i%2 == 0 ? (i/2)-1:i/2; // 4;
    }
    A[i] = temp;
    // result 50,35,45,30,15,12,6,5,20,10
}
void Insert(vector<int>& vec, int key)
{
    // insert key at the end
    auto i = vec.size();
    vec.emplace_back(key);

    //Rearrange elements: indices are not DRY
    while(i>0 && key>vec[i%2 == 0 ? (i/2)-1:i/2])
    {
        vec[i]= vec[i%2 == 0 ? (i/2)-1:i/2];
        i = i%2 == 0 ? (i/2)-1:i/2;
    }
    vec[i] = key;
}
template <class T>
void Print(T& vec,int n)
{
    cout<<"Max Heap:[ " <<flush;
    for(int i=0; i<n; i++)
    {
        cout<<vec[i]<<flush;
        if(i<n-1)
        {
            cout<<" , " <<flush;
        }
    }
    cout<<" ] " <<endl;
}
int main()
{
    int a[] = {45, 35, 15, 30, 10, 12, 6, 5, 20, 50};
    int size_ = sizeof(a)/sizeof(a[0]);
    InsertA(a, 9);
    cout<< "size = " << size_ << endl;
    // InsertA(a,9);
    Print(a,size_);
    cout<<endl;

    // STL Based
    vector<int> v = {45, 35, 15, 30, 10, 12, 6, 5, 20}; // heap memeory
    Print(v,size_);
    v.reserve(15);  // Reserve space for 15 elements

    Insert(v,50);
    Print(v,size_);
    return 0;
}

Create Heap using STL

#include <iostream>
#include <vector>

using namespace std;

template<class T>
void Print(T& vec, int n, char c)
{
    cout<< c << ":[" <<flush;
    for(int i = 0; i<n; i++)
    {
        cout << vec[i]<< flush;
        if(i<n-1)
        {
            cout <<"," <<flush;
        }
    }
    cout << "] " <<endl;
}

void Insert(vector<int>& vec, int key)
{
    //insert Key at the end, if there is nothing, size is zero
    auto i = vec.size();
    vec.emplace_back(key);

    // Rearrange elements: O(log n)
    while(i>0 && key>vec[i%2==0?(i/2)-1:i/2])
    {
        vec[i] = vec[i%2==0?(i/2)-1:i/2];
        i = i%2==0?(i/2)-1:i/2;
    }
    vec[i] = key;
}
void CreateHeap(vector<int>& vec, int A[], int n)
{
    //O(n logn)
    for(int i=0; i<n; i++)
    {
        Insert(vec,A[i]);
    }
}
void insertinplace(int A[],int n)
{
    int i = n;
    int temp = A[n];
    while(i>0 && temp>A[i%2==0?(i/2)-1:i/2])
    {
        A[i]=A[i%2==0?(i/2)-1:i/2];
        i = i%2==0?(i/2)-1:i/2;
    }
    A[n] = temp;
}
void createHeap(int A[], int n)
{
    for(int i=0; i<n; i++)
    {
        insertinplace(A,i);
    }
}


int main()
{
    cout << "Create Heap" << endl;
    int b[] = {10, 20, 30, 25, 5, 40, 35};
    Print(b, sizeof(b)/sizeof(b[0]), 'b');

    vector<int> v;
    CreateHeap(v,b,sizeof(b)/sizeof(b[0]));
    Print(v, sizeof(b)/sizeof(b[0]), 'v');

    cout << "Inplace Insert" << endl;
    createHeap(b, sizeof(b)/sizeof(b[0]));
    Print(b, sizeof(b)/sizeof(b[0]), 'b');

    return 0;
}

Deleting From heap and Heap sort

>

>

#include <stdio.h>

void Insert(int A[], int n)
{
    int i = n;
    int temp = A[i];
    while(i>0 && temp>A[i/2])
    {
        A[i]=A[i/2];
        i=i/2;
    }
    A[i] = temp;
}

void Delete(int A[], int n)
{
    int i,j,x,temp, val;
    val = A[1];
    x = A[n];
    A[1] = A[n];
    A[n] = val;
    i=1;
    j=i*2;
    while(j<=n-1)
    {
        if(j<n-1 && A[j+1] > A[j])
        {
            j=j+1;
        }
        if(A[i]<A[j])
        {
            temp = A[i];
            A[i] = A[j];
            A[j] = temp;;
            i = j;
            j = 2*j;
        }
        else
            break;
    }
}

int main()
{
    int H[] = {0,14,15,5,20,30,8,40};
    int i;
    for(i=2;i<=7;i++)
    {
        Insert(H,i);
    }
    for(i=1;i<=7;i++)
    {
        printf("%d ", H[i]);
    }
    printf("\n");
    for(i=7;i>1;i--)
    {
        Delete(H,i);
    }
    for(i=1;i<=7;i++)
    {
        printf("%d ", H[i]);
    }
    printf("\n"); // result : 0 5 8 14 15 20 30
    return 0;
}

>

Faster Method for Creating Heap(Heapify)

>

>

>

>

>

#include <iostream>

using namespace std;

template<class T>
void Print(T& vec, int n, string c)
{
    cout<< c << " : [" <<flush;
    for(int i =0; i<n;i++)
    {
        cout<< vec[i] << flush;
        if(i<n-1)
        {
            cout<<" ,"<<flush;
        }
    }
    cout<<"]"<<endl;
}
void Swap(int A[], int i, int j)
{
    int temp = A[i];
    A[i] = A[j];
    A[j] = temp;
}
void Heapify(int A[], int n)
{
    // # of leaf elements: (n+1)/2, index of last leaf elements parent = (n/2)-1
    for(int i = (n/2) -1 ; i>=0; i-- )
    {
        int j = 2*i +1; // left child for current i, at first n ;

        while(j<n-1)
        {
            //Compare left and right children of current i ;
            if(A[j]<A[j+1])
            {
                j = j+1;
            }

            // Compare Parent and largest child
            if(A[i]<A[j])
            {
                Swap(A,i,j);
                i = j;
                j= 2*i + 1;
            }
            else
                break;
        }

    }
}
int main()
{
    int A[] = {5, 10, 30, 20, 35, 40, 15};
    Print(A, sizeof(A)/sizeof(A[0]),"A");

    Heapify(A,sizeof(A)/sizeof(A[0]));
    cout<<endl;
    Print(A, sizeof(A)/sizeof(A[0]),"A");
    return 0;
}

Binary Heap as Priority Queue

>

Comment  Read more