// get__contour_depth_first: apply after boundary is clean to get the ordered boundary
// the parameter "once" specifies whether to look for only outer contour or for all the
// connected boundary segments (in the case of a multiply connected region
void Image::get__contour_depth_first(bool once)
{
int i;
point pt, aux, first, last = start-1;
bool is_on_top;
pt = first = get_next_boundary_point(last);
while ( pt != 0) {
last = first = pt;
stack<point> S;
*pt = bound;
S.push(pt);
while ( !S.empty()) {
pt=S.top();
is_on_top = true;
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
is_on_top = false;
*aux=bound;
S.push(aux);
break;
}
}
if (is_on_top) {
assert( are_4neighbors(pt, first) );
stack_to_queue(S, boundary_queue);
}
}
if (once) break;
pt = get_next_boundary_point(last);
}
}
// get__contour_breadth_first: apply after boundary is clean to get the ordered boundary
// the parameter "once" specifies whether to look for only outer contour or for all the
// connected boundary segments (in the case of a multiply connected region
void Image::get__contour_breadth_first(bool once)
{
int i;
int front, back;
point pt, aux, last = start-1;
while ( (pt = get_next_boundary_point(last)) ) {
vector<point> V;
last = pt;
*pt = bound;
front = back = 0;
V.push_back(pt);
++back;
while ( front != back ) {
pt = V[front];
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
*aux=bound;
V.push_back(aux);
++back;
}
}
++front;
}
unscramble(V, boundary_queue);
if (once) break;
}
}
// get_contour_breadth_first: gets ordered contour using BFS
void Image::get_contour_breadth_first(bool once)
{
int i;
int front, back;
point pt, aux, last = start-1;
filter_by_level(white);
while ( (pt = get_next_boundary_point(last)) ) {
last = pt;
vector<struct node> V;
*pt = bound;
front = back = 0;
V.push_back( make_node(pt, -1) );
++back;
while ( front != back ) {
pt = V[front].pt;
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
*aux=bound;
V.push_back( make_node(aux, front) );
++back;
}
}
++front;
}
make_ordered(V, boundary_queue, back-1);
if (once) break;
}
erase_points_with_level(bound);
erase_small_whites();
color_queue(boundary_queue, bound);
}
// get_boundary_depth_first: gets unordered boundary using DFS
void Image::get_boundary_depth_first()
{
int i;
bool is_on_top;
stack<point> S;
point pt, aux, last = start-1;
filter_by_level(white);
while ( (pt=get_next_boundary_point(last)) ) {
last = pt;
*pt = bound;
S.push(pt);
while ( !S.empty()) {
pt=S.top();
is_on_top = true;
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
*aux=bound;
S.push(aux);
is_on_top = false;
break;
}
}
if (is_on_top) {
S.pop();
boundary_queue.push(pt);
}
}
}
}
// get_boundary_breadth_first: gets unordered boundary using BFS
void Image::get_boundary_breadth_first()
{
int i;
queue<point> Q;
point pt, aux, last = start-1;
filter_by_level(white);
while ( (pt=get_next_boundary_point(last)) ) {
last = pt;
*pt = bound;
Q.push(pt);
while ( !Q.empty()) {
pt=Q.front();
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
*aux=bound;
Q.push(aux);
}
}
boundary_queue.push(pt);
Q.pop();
}
}
}
// get_contour_depth_first: gets ordered boundary using DFS
void Image::get_contour_depth_first(bool once)
{
int i;
point pt, aux, last = start-1;
bool is_on_top;
filter_by_level(white);
while ( (pt=get_next_boundary_point(last)) ) {
last = pt;
stack<point> S, Max;
*pt = bound;
S.push(pt);
while ( !S.empty()) {
pt=S.top();
is_on_top = true;
for (i=0; i<4; ++i) {
aux= pt + offset_4neighbor[i];
if (in_range(aux) &&
is_8boundary_point(aux)) {
is_on_top = false;
*aux=bound;
S.push(aux);
break;
}
}
if (is_on_top) {
if (S.size() > Max.size()) {
Max = S;
}
S.pop();
}
}
stack_to_queue(Max, boundary_queue);
if (once) break;
}
erase_small_whites();
erase_points_with_level(bound);
color_queue(boundary_queue, bound);
}
// create fence boundary points
void AP_Beacon::update_boundary_points()
{
// we need three beacons at least to create boundary fence.
// update boundary fence if number of beacons changes
if (!device_ready() || num_beacons < AP_BEACON_MINIMUM_FENCE_BEACONS || boundary_num_beacons == num_beacons) {
return;
}
// record number of beacons so we do not repeat calculations
boundary_num_beacons = num_beacons;
// accumulate beacon points
Vector2f beacon_points[AP_BEACON_MAX_BEACONS];
for (uint8_t index = 0; index < num_beacons; index++) {
const Vector3f& point_3d = beacon_position(index);
beacon_points[index].x = point_3d.x;
beacon_points[index].y = point_3d.y;
}
// create polygon around boundary points using the following algorithm
// set the "current point" as the first boundary point
// loop through all the boundary points looking for the point which creates a vector (from the current point to this new point) with the lowest angle
// check if point is already in boundary
// - no: add to boundary, move current point to this new point and repeat the above
// - yes: we've completed the bounding box, delete any boundary points found earlier than the duplicate
Vector2f boundary_points[AP_BEACON_MAX_BEACONS+1]; // array of boundary points
uint8_t curr_boundary_idx = 0; // index into boundary_sorted index. always points to the highest filled in element of the array
uint8_t curr_beacon_idx = 0; // index into beacon_point array. point indexed is same point as curr_boundary_idx's
// initialise first point of boundary_sorted with first beacon's position (this point may be removed later if it is found to not be on the outer boundary)
boundary_points[curr_boundary_idx] = beacon_points[curr_beacon_idx];
bool boundary_success = false; // true once the boundary has been successfully found
bool boundary_failure = false; // true if we fail to build the boundary
float start_angle = 0.0f; // starting angle used when searching for next boundary point, on each iteration this climbs but never climbs past PI * 2
while (!boundary_success && !boundary_failure) {
// look for next outer point
uint8_t next_idx;
float next_angle;
if (get_next_boundary_point(beacon_points, num_beacons, curr_beacon_idx, start_angle, next_idx, next_angle)) {
// add boundary point to boundary_sorted array
curr_boundary_idx++;
boundary_points[curr_boundary_idx] = beacon_points[next_idx];
curr_beacon_idx = next_idx;
start_angle = next_angle;
// check if we have a complete boundary by looking for duplicate points within the boundary_sorted
uint8_t dup_idx = 0;
bool dup_found = false;
while (dup_idx < curr_boundary_idx && !dup_found) {
dup_found = (boundary_points[dup_idx] == boundary_points[curr_boundary_idx]);
if (!dup_found) {
dup_idx++;
}
}
// if duplicate is found, remove all boundary points before the duplicate because they are inner points
if (dup_found) {
uint8_t num_pts = curr_boundary_idx - dup_idx + 1;
if (num_pts > AP_BEACON_MINIMUM_FENCE_BEACONS) {
// success, copy boundary points to boundary array and convert meters to cm
for (uint8_t j = 0; j < num_pts; j++) {
boundary[j] = boundary_points[j+dup_idx] * 100.0f;
}
boundary_num_points = num_pts;
boundary_success = true;
} else {
// boundary has too few points
boundary_failure = true;
}
}
} else {
// failed to create boundary - give up!
boundary_failure = true;
}
}
// clear boundary on failure
if (boundary_failure) {
boundary_num_points = 0;
}
}
