Q. and I also curious about result of matchPoints. After query function works, there are values in matchPoints. first one is point, and second one looks like some indexing number. I don't know what second one means.
Well, that's got to be a data member in your value type. What is in it depends solely on what you inserted into the rtree. it wouldn't surprise me if it was an ID that describes the geometry.
Since you do not even show the type of RT, we can only assume it is the same as nextRT. If so, we can assume that value is likely a pair like pair<box, unsigned> (because of what you insert). So, look at what got inserted for the unsigned value of the pair in RT...
Q.
if (bg::distance(p, matchPoints.back().first) > 3) matchPoints.pop_back();
else {
pointList.push_back(make_pair(keypoints[j + 1][i].pt.x, keypoints[j + 1][i].pt.y));
rtree.remove(matchPoints.back());
}
Simplify your code! Distilling the requirements:
It looks to me that for 4 sets of "key points", you want to create 4 rtrees containing all those key points with sequentially increasing ids.
Also for those 4 sets of "key points", you want to create a list of key points for which a geometry can be found with a radius of 3.
As a side-effect, remove those closely-matching geometries from the original rtree RT.
DECISION: Because these tasks are independent, let's do them separate:
// making up types that match the usage in your code:
struct keypoint_t { point pt; };
std::array<std::vector<keypoint_t>, 4> keypoints;
Now, let's do the tasks:
Note how RT is not used here:
for (auto const& current_key_set : keypoints) {
bgi::rtree< value, bgi::quadratic<16> > nextRT; // use a better name...
int i = 0;
for (auto const& kpd : current_key_set)
nextRT.insert(std::make_pair(kpd.pt, i++));
}
Creating the vector containing matched key-points (those with near geometries in RT):
for (auto const& current_key_set : keypoints) {
std::vector<point> matched_key_points;
for (auto const& kpd : current_key_set) {
point p = kpd.pt;
value match;
if (!RT.query(bgi::nearest(p, 1), &match))
continue;
if (bg::distance(p, match.first) <= 3) {
matched_key_points.push_back(p);
RT.remove(match);
}
}
}
Ironically, removing the matching geometries from RT became a bit of a minor issue in this: you can either delete by iterator or by a value. In this case, we use the overload that takes a value.
Summary
It was hard to understand the code enough to see what it did. I have shown how to clean up the code, and make it work. Maybe these aren't the things you need, but hopefully using the better separated code, you should be able to get further.
Note that the algorithms have side effects. This makes it hard to understand what really will happen. E.g.:
- removing points from the original RT affects what the subsequent key points (even from subsequent sets (next
j)) can match with
- if you have the same key point multiple times, they may match more than 1 source RT point (because after removal of the first match, there might be a second match within radius 3)
- key points are checked strictly sequentially. This means that if the first keypoint roughly matches a point X, this might cause a later keypoint to fail to match, even though the point X might be closer to that keypoint...
I'd suggest you THINK about the requirements really hard before implementing things with these side-effects. **Study the sample cases in the live demo below. If all these side-effects are exactly what you wanted, be sure to use much better naming and proper comments to describe what the code is doing.
Live Demo
Live On Coliru
#include <boost/geometry.hpp>
#include <boost/geometry/io/io.hpp>
#include <boost/geometry/index/rtree.hpp>
#include <iostream>
namespace bg = boost::geometry;
namespace bgi = bg::index;
typedef bg::model::point<float, 2, bg::cs::cartesian> point;
typedef std::pair<point, unsigned> pvalue;
typedef pvalue value;
int main() {
bgi::rtree< value, bgi::quadratic<16> > RT;
{
int i = 0;
for (auto p : { point(2.0f, 2.0f), point(2.5f, 2.5f) })
RT.insert(std::make_pair(p, i++));
}
struct keypoint_t { point pt; };
using keypoints_t = std::vector<keypoint_t>;
keypoints_t const keypoints[] = {
keypoints_t{ keypoint_t { point(-2, 2) } }, // should not match anything
keypoints_t{ keypoint_t { point(-1, 2) } }, // should match (2,2)
keypoints_t{ keypoint_t { point(2.0, 2.0) }, // matches (2.5,2.5)
{ point(2.5, 2.5) }, // nothing anymore...
},
};
for (auto const& current_key_set : keypoints) {
bgi::rtree< pvalue, bgi::quadratic<16> > nextRT; // use a better name...
int i = 0;
for (auto const& kpd : current_key_set)
nextRT.insert(std::make_pair(kpd.pt, i++));
}
for (auto const& current_key_set : keypoints) {
std::cout << "-----------\n";
std::vector<point> matched_key_points;
for (auto const& kpd : current_key_set) {
point p = kpd.pt;
std::cout << "Key: " << bg::wkt(p) << "\n";
value match;
if (!RT.query(bgi::nearest(p, 1), &match))
continue;
if (bg::distance(p, match.first) <= 3) {
matched_key_points.push_back(p);
std::cout << "\tRemoving close point: " << bg::wkt(match.first) << "\n";
RT.remove(match);
}
}
std::cout << "\nMatched keys: ";
for (auto& p : matched_key_points)
std::cout << bg::wkt(p) << " ";
std::cout << "\n\tElements remaining: " << RT.size() << "\n";
}
}
Prints
-----------
Key: POINT(-2 2)
Matched keys:
Elements remaining: 2
-----------
Key: POINT(-1 2)
Removing close point: POINT(2 2)
Matched keys: POINT(-1 2)
Elements remaining: 1
-----------
Key: POINT(2 2)
Removing close point: POINT(2.5 2.5)
Key: POINT(2.5 2.5)
Matched keys: POINT(2 2)
Elements remaining: 0
