Quick Point Cloud Search in C++ using unordered_multimap

 The main idea of the algorithm as explained to one of my previous post is:
- At first, the points are imported into a grid.
- Then, for every point of our interest
    - find the square the point lies in
    - loop through all the points that lie inside that square only
    - return the closest point

Here I implemented a 2D points cloud search for aligning the hyperspectral images with the LiDAR data[1] in the open source software DASOS [2], but it can easily be extended to work for 3D.

At first we need to find the limits of the points cloud. So we need to loop through all the points m_points, and find the minimum and maximum values.

 // Loop through all the points and find the minimum and max
if(i_pointsCloud.size() < 1)
{
   std::cout << "Warning: Points cloud too small\n";
   m_min.m_x = 0.0f; m_min.m_y = 0.0f;
   m_max.m_y = 0.1f; m_max.m_y = 0.1f;
   return;
}
m_min.m_x = m_points[0].m_x;
m_min.m_y = m_points[0].m_y;
m_max.m_x = m_points[0].m_x;
m_max.m_y = m_points[0].m_y;
for(unsigned int i=1; i < m_points.size(); ++i)
{
   if (m_min.m_x > m_points[i].m_x)
   {
      m_min.m_x=m_points[i].m_x;
   } else if (m_max.m_x < m_points[i].m_x)
   {
      m_max.m_x=m_points[i].m_x;
   }
   if (m_min.m_y > m_points[i].m_y)
   {
      m_min.m_y=m_points[i].m_y;
   } else if (m_max.m_y < m_points[i].m_y)
   {
      m_max.m_x=m_points[i].m_y;
   }
}

We also need to calculate how many squares our grid will have in each dimension. In the example below i_samplingRate is the average number of expected points per square and m_nX, m_nY is the number of squares in the x,y axes respectively.

float rate = (m_max.m_x-m_min.m_x)/(m_max.m_y-m_min.m_y);
float numOfSquares = m_points.size() / i_samplingRate;
m_nY = sqrt(numOfSquares/rate);
m_nX = ceil(numOfSquares/m_nY);

So, after we calculate the number of square per axis, how do we facilitate unordered_multimap for quick points cloud search?
Each element of an unordered_multimap has a key associated with its value. The key value of each square (x,y) on the grid will be equal to x + y*m_nX. All the points of the points cloud are saved into a 1D array (m_points), so the value of the element is the index of the point of our interest.

for (unsigned int i=0; i < m_points.size(); ++i)
{
   unsigned int sqX = double((m_points[i].m_x-m_min.m_x)/
                             (m_max.m_x-m_min.m_x))*m_nX;
   unsigned int sqY = double((m_points[i].m_y-m_min.m_y)/
                             (m_max.m_y-m_min.m_y))*m_nY;
   m_map.emplace(getKeyOfSquare(sqX,sqY),i);
}

So far, we have a grid with all the points associated to its squares. So, the next step is to be able to find the square (x,y) that a given point(i_point) lies inside.

const unsigned int x = float((i_point.m_x-m_min.m_x)/
                             (m_max.m_x-m_min.m_x)*(float)m_nX);
const unsigned int y = float((i_point.m_y-m_min.m_y)/
                             (m_max.m_y-m_min.m_y)*(float)m_nY);

Then we can get the key of that square as explained above (key = x+y*m_nX) and using that key we can query the unordered_multimap and get all the indices of the points that lie inside that square. Then by looping through all the those point, the closest point to i_point can be found.

unsigned int closestPointIndex = 0;
float minDis = sqrt(pow(m_points[0].m_x-i_point.m_x,2.0)+
                    pow(m_points[0].m_y-i_point.m_y,2.0));
auto itsElements = m_map.equal_range(i_point.m_x + i_point.m_y * m_nX);
for (auto it = itsElements.first; it != itsElements.second; ++it)
{
   const float distance = sqrt(pow(m_points[it->second].m_x-i_point.m_x,2.0)+
                                pow(m_points[it->second].m_y-i_point.m_y,2.0));
   if(distancesecond;
      minDis = distance;
   }
}
closestpoint = m_points[closestPointIndex];

You may download the full version of the program here:
https://github.com/Art-n-MathS/QuickPointsCloudSearch

Please note that Unordered_multimap is a part of C++2011. So, to compile and run the program you need to add the tag -std=c++11 as follow:

g++ -std=c++11 main.cpp GridSearch.cpp -o myProgram
./myProgram

The output of the program is the following:

All the points of the point cloud are:

(0,0) (0,1) (0,2) (0,3) (0,4) (0,5) (0,6) (0,7) (0,8) (0,9) (1,0) (1,1) (1,2) 
(1,3) (1,4) (1,5) (1,6) (1,7) (1,8) (1,9) (2,0) (2,1) (2,2) (2,3) (2,4) (2,5) 
(2,6) (2,7) (2,8) (2,9) (3,0) (3,1) (3,2) (3,3) (3,4) (3,5) (3,6) (3,7) (3,8) 
(3,9) (4,0) (4,1) (4,2) (4,3) (4,4) (4,5) (4,6) (4,7) (4,8) (4,9) (5,0) (5,1) 
(5,2) (5,3) (5,4) (5,5) (5,6) (5,7) (5,8) (5,9) (6,0) (6,1) (6,2) (6,3) (6,4) 
(6,5) (6,6) (6,7) (6,8) (6,9) (7,0) (7,1) (7,2) (7,3) (7,4) (7,5) (7,6) (7,7) 
(7,8) (7,9) (8,0) (8,1) (8,2) (8,3) (8,4) (8,5) (8,6) (8,7) (8,8) (8,9) (9,0) 
(9,1) (9,2) (9,3) (9,4) (9,5) (9,6) (9,7) (9,8) (9,9)

The closest point to (0,0.2) is (0,0)

The closest point to (5.2,5.9) is (5,6)

*** Note ***
There is a chance to miss the closest point and that may happen when the point of interest is very close to the edges of the cell that its contained. This can be resolved by checking whether the distance between the point of interest and its closest point that within the same cell is smaller than the distance between the point of interest and the edges of its cell.

PS: if you are interested on how unordered_multimap works in general, please have a look at one of my previous posts, which focuses on that: http://miltomiltiadou.blogspot.co.uk/2014/03/unorderedmultimap-for-quick-search-in.html

Reference:

[1] Miltiadou, M., Warren, M. A., Grant, M., & Brown, M. (2015). Alignment of hyperspectral imagery and full-waveform LiDAR data for visualisation and classification purposes. International Archives of the Photogrammetry, Remote Sensing and Spatial Information Sciences-ISPRS Archives.

DOI: 10.5194/isprsarchives-XL-7-W3-1257-2015

[2] Miltiadou, M., Grant, M. G., Campbell, N. D., Warren, M., Clewley, D., & Hadjimitsis, D. G. (2019, June). Open source software DASOS: Efficient accumulation, analysis, and visualisation of full-waveform lidar. In Seventh International Conference on Remote Sensing and Geoinformation of the Environment (RSCy2019) (Vol. 11174, p. 111741M). International Society for Optics and Photonics.

DOI: 10.1117/12.2537915

Pass an array pointer as an input to a function C++.

That's a really short post, but that's the second time I find it useful so I decided to keep it on my blog for future reference.

So let's assume that you have pointer to an array and you would like to use this array into a function. Here is an example of how to correctly send the pointers to avoid memory leaks:

#include < iostream >

void modifyData(unsigned int **i_array,unsigned int short i_len)
{
    (*i_array)[0] = 1;
    (*i_array)[2] = 2;
}

int main(int /*argc*/, char **/*argv*/)
{
  unsigned int short len = 5;
  unsigned int *mydata = new unsigned int[len];
  // initialise the values of the array
  for(unsigned int i=0; i < len; ++i)
  {
      mydata[i] = 0;
  }
  // print the original values of the array
  for(unsigned int i = 0; i < len; ++i)
  {
      std::cout << mydata[i] << " ";
  }
  std::cout << "\n";
  // call function that will modify the data
  modifyData(&mydata,len);
  // print the new values of the array
  for(unsigned int i = 0; i < len; ++i)
  {
      std::cout << mydata[i] << " ";
  }
  std::cout << "\n";
  delete []mydata;

  return 0;
}

Unordered_multimap

Hash tables are structures that allow you to search elements using a key. In my recent publication, I am using the data structure that was named "Voxel Hashing" to test the efficiency of hash table while voxelizing/interpreting full-waveform LiDAR data for 3D polygonal mesh creation [1]. This approach is the one selected to be used on the open source software DASOS [2] since it does not store empty voxels and even though it was not the fastest in the processes of 3D polygonal mesh creation, it is able to find the value of a voxel at constant time O(n) using the Hash function. The C++ implementation of the unordered_multimap is used and this blog post explains how to use it. 

Unordered_multimap:

As mentioned before, it is used for quick search and it allows you to save more than one values with  the same key, while unordered_map doesn't. How does it work? Simply, for every key there is a bucket where all the values associated with that key are saved. 

So, unordered_multimap is in the same header file with unordered_map, so the header file that need to be included is:

#include < unordered_map >

To create a map you need to define the type of the two components of the map. For example:

std::unordered_multimap < unsigned int , std::string > myMap;

if you would like to give initial values in the map you can do it as follow:

// this gives the initial values to the constructor
std::unordered_multimap < unsigned int , std::string > myMap ({{100,"a"},{120,"b"}});

// while the following on initialises the map and then adds the values
std::unordered_multimap < unsigned int , std::string > myMap ={{100,"a"},{120,"b"}};

If you would like to add items you can do it using the command "emplace":

// add more elements
myMap.emplace(100,"c");
myMap.emplace(100,"d");
myMap.emplace(200,"e");

You may like to loop through all its elements as follow:

//loop through all its elements
std::cout << "These are all the elements saved into the map:\n";
for (auto& x: myMap)
{
   std::cout << "(" << x.first << " , " << x.second << ")\n";
}

And in my opinion the most important feature is to be able to loop through all the elements with the same key. Here is an example of how you may do it:

// print all the elements with Key = 100
 unsigned int key = 100;
 std::cout << "These are all the elements with key value: " << key << "\n";
 auto itsElements = myMap.equal_range(key);
 for (auto it = itsElements.first; it != itsElements.second; ++it)
 {
    std::cout << "(" << it->first << " , " << it->second << ")\n";
 }

By the end, another small useful feature is the ability to query the unordered_multimap and get the number of elements that exists with the same key. This is achieved using the command count() as follow:

// counts how many entries exist with the same key
std::cout << "There are " << myMap.count(key) << " entries with key equal to " << key << "\n";

To sum up here is the entire program, which defines an unordered_multimap with some initial values, adds 3 more elements, prints all the elements inside the map and finally prints all the elements with key 100.

#include < iostream >
#include < unordered_map >

int main(int /*argc*/, char **/*argv*/)
{
   // define and initialise map
   std::unordered_multimap < unsigned int , std::string > myMap ({{100,"a"},{120,"b"}});

   // add more elements
   myMap.emplace(100,"c");
   myMap.emplace(100,"d");
   myMap.emplace(200,"e");

   //loop through all its elements
   std::cout << "These are all the elements saved into the map:\n";
   for (auto& x: myMap)
   {
      std::cout << "(" << x.first << " , " << x.second << ")\n";
   }

   // print all the elements with Key = 100
   unsigned int key = 100;
   std::cout << "These are all the elements with key value: " << key << "\n";
   auto itsElements = myMap.equal_range(key);
   for (auto it = itsElements.first; it != itsElements.second; ++it)
   {
       std::cout << "(" << it->first << " , " << it->second << ")\n";
   }

   // counts how many entries exist with the same key
   std::cout << "There are " << myMap.count(key) << " entries with key equal to " << key << "\n";


   return 0;
}

And the output of the program is the following:

These are all the elements saved into the map:
(200 , e)
(100 , d)
(100 , c)
(100 , a)
(120 , b)
These are all the elements with key value: 100
(100 , d)
(100 , c)
(100 , a)
There are 3 entries with key equal to 100

References

[1] Miltiadou, M.; Campbell, N.D.F.; Cosker, D.; Grant, M.G. A Comparative Study about Data Structures Used for Efficient Management of Voxelised Full-Waveform Airborne LiDAR Data during 3D Polygonal Model Creation. Remote Sens. 2021, 13, 559. https://doi.org/10.3390/rs13040559

[2] Miltiadou, M., Grant, M. G., Campbell, N. D., Warren, M., Clewley, D., & Hadjimitsis, D. G. (2019, June). Open source software DASOS: Efficient accumulation, analysis, and visualisation of full-waveform lidar. In Seventh International Conference on Remote Sensing and Geoinformation of the Environment (RSCy2019) (Vol. 11174, p. 111741M). International Society for Optics and Photonics. 

Link: https://www.researchgate.net/publication/334069759_Open_source_software_DASOS_efficient_accumulation_analysis_and_visualisation_of_full-waveform_lidar

For each in C++, std::vector

Here it is an example code of using for each in c++ in three different ways. The code first initialises an array with values (1 2 4 5) and then it adds 1 to each one of its elements 3 times.
The output is : 4 5 7 8

//function that adds one to a given reference value
void addOne(int &n){n++;}

// function that prints all the elements of a given vector
void print(const std::vector &i_vec)
{
    for(const int &n: i_vec){std::cout << n << " ";}
    std::cout << "\n";
}

int main(void)
{
  //initialisation of an std::vector
  std::vector myvec{1,2,4,5};

  // first method of adding one to each of its elements
  for (int &n : myvec){n++;}
  // 2nd method
  std::for_each(myvec.begin(), myvec.end(),[](int &n){n++;});
  // 3rd method by calling the addOne function
  std::for_each(myvec.begin(), myvec.end(),addOne);

  // print the values of the array
  print(myvec);

  return 0;
}

Summed Area Tables (Integral Images), Explanation and Implementation in C++

In 1984, Crow proposed an image representation where each pixel value is replaced by the sum of all the pixels that belong to the rectangle defined by the lower left corner of the image and the pixel of our interest [1].

Even though more storage space may be required to save the image, the sum of every rectangle in the image can be calculated in constant time once the table is constructed. The summed area table can be constructed in linear time O(n), where n is the number of pixels in the image, since one iteration through the entire image is enough to replace the pixel values inside the area table.

Figure 1. This figure depicts the parameters of the following equation [2].

 

So, let’s assume the we have the above image and we would like to find the sum of the blue area defined by the pixels (x,y) and (x+lenX, y+lenY) included. Then the sum is given by: 

sum =  T(x+lenX, y+lenY) - T(x+lenX, y-1) - T(x-1,y+lenY) +  T(x-1, y-1)

      

Figure 2. Once the Integral Image is constructed, the sum of any rectangular area is calculated in constant time [2].

Where T(x,y) is the value in the table with coordinates (x,y).

Code Available here:
https://www.dropbox.com/s/83oynkf11rrjd21/SumTables.zip

The images are taken from the following article, which explains how Sum Area Tables, in other words Integral Images, are used in 3D (named Integral Volumes) to optimise reconstruction of polygon representations from voxelised data[2]: https://www.mdpi.com/2072-4292/13/4/559

Work Cited

[1] Crow, F.C. (1984, July). Summed-Area Tables for Texture Mapping. ACM, Computer Graphics, Volume 18, Number 3

[2] Miltiadou M, Campbell NDF, Cosker D, Grant MG. A Comparative Study about Data Structures Used for Efficient Management of Voxelised Full-Waveform Airborne LiDAR Data during 3D Polygonal Model Creation. Remote Sensing. 2021; 13(4):559. https://doi.org/10.3390/rs13040559

std::unordered_map example

Unordered map is a structure that allows you to save data in an easy to access format.

For example, let assume that we would like to create a telephone catalogue. If we use an std::vector then looping though all the contact to find a number is time expensive. Instead we can use an std::unordered_map and have a much quicker search of data.

So this is an example code using the unordered_map:

#include 
#include 


int main(void)
{
   // initialise the map which takes as input a string and an integer
   std::unordered_map < std::string,unsigned int > mymap;

   // create and insert a new contact
   std::pair < std::string, unsigned int > pair("Maria",300);
   mymap.insert(pair);

   // search for a contract
   std::string input = "Maria";

   std::unordered_map < std::string,unsigned int > ::const_iterator got = mymap.find(input);
   if(got == mymap.end())
   {
       std::cout << "Contact does not exist\n";
   }
   else
   {
       std::cout << got -> first << "'s number is " << got->second <<"\n";
   }

  return 0;
}

So if you run the above example, this is what you get:

Maria's number is 300

Please note that unsigned int is not a prober type to save telephone numbers, because telephone numbers if are treated as numbers then you can easily end up with an overflow.
In this example I just wanted to show that unordered map allows you to save two different types of variables. A better approach will have been to have two string instead of string and an unsigned int.

Merge Sort without Recursion.

Well, I needed a sorting algorithm in C++, so I decided to write my own. Merge sort is quick, O(nlogn), and stable. Recursion is also slow so I decided to write it without recursion:

  // array to be sorted
  int array[] = {12,4,10,2,3,2,8,7,-1,-4,14,8,9,2,11};
  unsigned int len = 15;
  // allocate memory for temporarly saved values
  std::vector tempValues;
  tempValues.resize(len);
  // start sorting 2 elements each time, then merge them with the two next to them etc
  for(int step=2; step/2 < len; step*=2)
  {
     for(unsigned int i=0; i < len; i+=step)
     {
        int endOfT2 = i+step;
        if(i+ step/2 >= len)
        {
           continue;
        }
        else if (i+step >= len)
        {
           endOfT2 = len;
        }
        // both sets have step/2 items.
        // t1 points to the first set of values
        int t1 = i;
        // t2 points to the second set of values
        int t2 = i+step/2;
        // here we save all the values that have been overridden from the first set
        unsigned int tempIndex=0;
        while(t1 < i+step/2 && t2 < endOfT2)
        {
           if(array[t1]>array[t2])
           {
              tempValues[tempIndex]=array[t1];
              t1++;
           }
           else
           {
              tempValues[tempIndex]=array[t2];
              t2++;
           }
           tempIndex++;
        }
        while(t1 < i+step/2)
        {
           tempValues[tempIndex]=array[t1];
           t1++;
           tempIndex++;
        }
        // write values back to the array
        for(unsigned int t=0; t < tempIndex; ++t)
        {
            array[i+t]=tempValues[t];
        }
     }
  }

  // print the sorted array
  for(unsigned int i=0; i < len; ++i)
  {
      std::cout << " " << array[i];
  }
  std::cout << "\n";