- 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.
[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
No comments:
Post a Comment