Merge pull request #1570 from szk1509:apriltag
Apriltag (#1570) * doCornerRefinement to CornerRefinementMethod :: detected contours points are used to detect the corners * some little corrections * samples edited * documented :) * tabs corrected * Docu corrections * refinement for all candidates * refinement for all candidates :: copy paste error corrected * comment * apriltag * whitespace corrected * corr ini * correction :: warnings, c4244, preprocDirect, ... * try to ignore 4244, fix 2131, add test, and ... * try to ignore 4244 * test duplicate deleted * corrected test, warnings * test :: correction * warnings * warnings and test * warnings * perspective test, warning corrections * warning a_q_t * warning * warning * 3 clause BSD license * stacksz and typo * eliminate build warnings - cv::fastAtan2() - cvFloor() * small code refactoring * fix isfinite() * get rid of manual calloc/free calls * update file headerspull/1582/head
Lizeth Huertas
7 years ago
committed by
Alexander Alekhin
parent
d99ee92d76
commit
a817a19751
9 changed files with 2426 additions and 8 deletions
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,125 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
//
|
||||
// Copyright (C) 2013-2016, The Regents of The University of Michigan.
|
||||
//
|
||||
// This software was developed in the APRIL Robotics Lab under the
|
||||
// direction of Edwin Olson, ebolson@umich.edu. This software may be
|
||||
// available under alternative licensing terms; contact the address above.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the Regents of The University of Michigan.
|
||||
|
||||
// limitation: image size must be <32768 in width and height. This is
|
||||
// because we use a fixed-point 16 bit integer representation with one
|
||||
// fractional bit.
|
||||
|
||||
#ifndef _OPENCV_APRIL_QUAD_THRESH_HPP_ |
||||
#define _OPENCV_APRIL_QUAD_THRESH_HPP_ |
||||
|
||||
#include "opencv2/aruco.hpp" |
||||
#include "unionfind.hpp" |
||||
#include "zmaxheap.hpp" |
||||
#include "zarray.hpp" |
||||
|
||||
namespace cv { |
||||
namespace aruco { |
||||
|
||||
static inline uint32_t u64hash_2(uint64_t x) { |
||||
return uint32_t((2654435761UL * x) >> 32); |
||||
} |
||||
|
||||
struct uint64_zarray_entry{ |
||||
uint64_t id; |
||||
zarray_t *cluster; |
||||
|
||||
struct uint64_zarray_entry *next; |
||||
}; |
||||
|
||||
struct pt{ |
||||
// Note: these represent 2*actual value.
|
||||
uint16_t x, y; |
||||
float theta; |
||||
int16_t gx, gy; |
||||
}; |
||||
|
||||
struct remove_vertex{ |
||||
int i; // which vertex to remove?
|
||||
int left, right; // left vertex, right vertex
|
||||
|
||||
double err; |
||||
}; |
||||
|
||||
struct segment{ |
||||
int is_vertex; |
||||
|
||||
// always greater than zero, but right can be > size, which denotes
|
||||
// a wrap around back to the beginning of the points. and left < right.
|
||||
int left, right; |
||||
}; |
||||
|
||||
struct line_fit_pt{ |
||||
double Mx, My; |
||||
double Mxx, Myy, Mxy; |
||||
double W; // total weight
|
||||
}; |
||||
|
||||
/**
|
||||
* lfps contains *cumulative* moments for N points, with |
||||
* index j reflecting points [0,j] (inclusive). |
||||
* fit a line to the points [i0, i1] (inclusive). i0, i1 are both (0, sz) |
||||
* if i1 < i0, we treat this as a wrap around. |
||||
*/ |
||||
void fit_line(struct line_fit_pt *lfps, int sz, int i0, int i1, double *lineparm, double *err, double *mse); |
||||
|
||||
int err_compare_descending(const void *_a, const void *_b); |
||||
|
||||
/**
|
||||
1. Identify A) white points near a black point and B) black points near a white point. |
||||
|
||||
2. Find the connected components within each of the classes above, |
||||
yielding clusters of "white-near-black" and |
||||
"black-near-white". (These two classes are kept separate). Each |
||||
segment has a unique id. |
||||
|
||||
3. For every pair of "white-near-black" and "black-near-white" |
||||
clusters, find the set of points that are in one and adjacent to the |
||||
other. In other words, a "boundary" layer between the two |
||||
clusters. (This is actually performed by iterating over the pixels, |
||||
rather than pairs of clusters.) Critically, this helps keep nearby |
||||
edges from becoming connected. |
||||
**/ |
||||
int quad_segment_maxima(const Ptr<DetectorParameters> &td, int sz, struct line_fit_pt *lfps, int indices[4]); |
||||
|
||||
/**
|
||||
* returns 0 if the cluster looks bad. |
||||
*/ |
||||
int quad_segment_agg(int sz, struct line_fit_pt *lfps, int indices[4]); |
||||
|
||||
/**
|
||||
* return 1 if the quad looks okay, 0 if it should be discarded |
||||
* quad |
||||
**/ |
||||
int fit_quad(const Ptr<DetectorParameters> &_params, const Mat im, zarray_t *cluster, struct sQuad *quad); |
||||
|
||||
/**
|
||||
* |
||||
* @param mIm |
||||
* @param parameters |
||||
* @param mThresh |
||||
*/ |
||||
void threshold(const Mat mIm, const Ptr<DetectorParameters> ¶meters, Mat& mThresh); |
||||
|
||||
/**
|
||||
* |
||||
* @param parameters |
||||
* @param mImg |
||||
* @param contours |
||||
* @return |
||||
*/ |
||||
zarray_t *apriltag_quad_thresh(const Ptr<DetectorParameters> ¶meters, const Mat & mImg, std::vector< std::vector< Point > > &contours); |
||||
|
||||
}} |
||||
#endif |
||||
@ -0,0 +1,133 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
//
|
||||
// Copyright (C) 2013-2016, The Regents of The University of Michigan.
|
||||
//
|
||||
// This software was developed in the APRIL Robotics Lab under the
|
||||
// direction of Edwin Olson, ebolson@umich.edu. This software may be
|
||||
// available under alternative licensing terms; contact the address above.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the Regents of The University of Michigan.
|
||||
#ifndef _OPENCV_UNIONFIND_HPP_ |
||||
#define _OPENCV_UNIONFIND_HPP_ |
||||
|
||||
#include <stdint.h> |
||||
#include <stdlib.h> |
||||
namespace cv { |
||||
namespace aruco { |
||||
|
||||
typedef struct unionfind unionfind_t; |
||||
struct unionfind{ |
||||
uint32_t maxid; |
||||
struct ufrec *data; |
||||
}; |
||||
|
||||
struct ufrec{ |
||||
// the parent of this node. If a node's parent is its own index,
|
||||
// then it is a root.
|
||||
uint32_t parent; |
||||
|
||||
// for the root of a connected component, the number of components
|
||||
// connected to it. For intermediate values, it's not meaningful.
|
||||
uint32_t size; |
||||
}; |
||||
|
||||
static inline unionfind_t *unionfind_create(uint32_t maxid){ |
||||
unionfind_t *uf = (unionfind_t*) calloc(1, sizeof(unionfind_t)); |
||||
uf->maxid = maxid; |
||||
uf->data = (struct ufrec*) malloc((maxid+1) * sizeof(struct ufrec)); |
||||
for (unsigned int i = 0; i <= maxid; i++) { |
||||
uf->data[i].size = 1; |
||||
uf->data[i].parent = i; |
||||
} |
||||
return uf; |
||||
} |
||||
|
||||
static inline void unionfind_destroy(unionfind_t *uf){ |
||||
free(uf->data); |
||||
free(uf); |
||||
} |
||||
|
||||
/*
|
||||
static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id) |
||||
{ |
||||
// base case: a node is its own parent
|
||||
if (uf->data[id].parent == id) |
||||
return id; |
||||
|
||||
// otherwise, recurse
|
||||
uint32_t root = unionfind_get_representative(uf, uf->data[id].parent); |
||||
|
||||
// short circuit the path. [XXX This write prevents tail recursion]
|
||||
uf->data[id].parent = root; |
||||
|
||||
return root; |
||||
} |
||||
*/ |
||||
|
||||
// this one seems to be every-so-slightly faster than the recursive
|
||||
// version above.
|
||||
static inline uint32_t unionfind_get_representative(unionfind_t *uf, uint32_t id){ |
||||
uint32_t root = id; |
||||
|
||||
// chase down the root
|
||||
while (uf->data[root].parent != root) { |
||||
root = uf->data[root].parent; |
||||
} |
||||
|
||||
// go back and collapse the tree.
|
||||
//
|
||||
// XXX: on some of our workloads that have very shallow trees
|
||||
// (e.g. image segmentation), we are actually faster not doing
|
||||
// this...
|
||||
while (uf->data[id].parent != root) { |
||||
uint32_t tmp = uf->data[id].parent; |
||||
uf->data[id].parent = root; |
||||
id = tmp; |
||||
} |
||||
|
||||
return root; |
||||
} |
||||
|
||||
static inline uint32_t unionfind_get_set_size(unionfind_t *uf, uint32_t id){ |
||||
uint32_t repid = unionfind_get_representative(uf, id); |
||||
return uf->data[repid].size; |
||||
} |
||||
|
||||
static inline uint32_t unionfind_connect(unionfind_t *uf, uint32_t aid, uint32_t bid){ |
||||
uint32_t aroot = unionfind_get_representative(uf, aid); |
||||
uint32_t broot = unionfind_get_representative(uf, bid); |
||||
|
||||
if (aroot == broot) |
||||
return aroot; |
||||
|
||||
// we don't perform "union by rank", but we perform a similar
|
||||
// operation (but probably without the same asymptotic guarantee):
|
||||
// We join trees based on the number of *elements* (as opposed to
|
||||
// rank) contained within each tree. I.e., we use size as a proxy
|
||||
// for rank. In my testing, it's often *faster* to use size than
|
||||
// rank, perhaps because the rank of the tree isn't that critical
|
||||
// if there are very few nodes in it.
|
||||
uint32_t asize = uf->data[aroot].size; |
||||
uint32_t bsize = uf->data[broot].size; |
||||
|
||||
// optimization idea: We could shortcut some or all of the tree
|
||||
// that is grafted onto the other tree. Pro: those nodes were just
|
||||
// read and so are probably in cache. Con: it might end up being
|
||||
// wasted effort -- the tree might be grafted onto another tree in
|
||||
// a moment!
|
||||
if (asize > bsize) { |
||||
uf->data[broot].parent = aroot; |
||||
uf->data[aroot].size += bsize; |
||||
return aroot; |
||||
} else { |
||||
uf->data[aroot].parent = broot; |
||||
uf->data[broot].size += asize; |
||||
return broot; |
||||
} |
||||
} |
||||
}} |
||||
#endif |
||||
@ -0,0 +1,150 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
//
|
||||
// Copyright (C) 2013-2016, The Regents of The University of Michigan.
|
||||
//
|
||||
// This software was developed in the APRIL Robotics Lab under the
|
||||
// direction of Edwin Olson, ebolson@umich.edu. This software may be
|
||||
// available under alternative licensing terms; contact the address above.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the Regents of The University of Michigan.
|
||||
#ifndef _OPENCV_ZARRAY_HPP_ |
||||
#define _OPENCV_ZARRAY_HPP_ |
||||
|
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
|
||||
namespace cv { |
||||
namespace aruco { |
||||
|
||||
|
||||
struct sQuad{ |
||||
float p[4][2]; // corners
|
||||
}; |
||||
|
||||
/**
|
||||
* Defines a structure which acts as a resize-able array ala Java's ArrayList. |
||||
*/ |
||||
typedef struct zarray zarray_t; |
||||
struct zarray{ |
||||
size_t el_sz; // size of each element
|
||||
|
||||
int size; // how many elements?
|
||||
int alloc; // we've allocated storage for how many elements?
|
||||
char *data; |
||||
}; |
||||
|
||||
/**
|
||||
* Creates and returns a variable array structure capable of holding elements of |
||||
* the specified size. It is the caller's responsibility to call zarray_destroy() |
||||
* on the returned array when it is no longer needed. |
||||
*/ |
||||
inline static zarray_t *_zarray_create(size_t el_sz){ |
||||
zarray_t *za = (zarray_t*) calloc(1, sizeof(zarray_t)); |
||||
za->el_sz = el_sz; |
||||
return za; |
||||
} |
||||
|
||||
/**
|
||||
* Frees all resources associated with the variable array structure which was |
||||
* created by zarray_create(). After calling, 'za' will no longer be valid for storage. |
||||
*/ |
||||
inline static void _zarray_destroy(zarray_t *za){ |
||||
if (za == NULL) |
||||
return; |
||||
|
||||
if (za->data != NULL) |
||||
free(za->data); |
||||
memset(za, 0, sizeof(zarray_t)); |
||||
free(za); |
||||
} |
||||
|
||||
/**
|
||||
* Retrieves the number of elements currently being contained by the passed |
||||
* array, which may be different from its capacity. The index of the last element |
||||
* in the array will be one less than the returned value. |
||||
*/ |
||||
inline static int _zarray_size(const zarray_t *za){ |
||||
return za->size; |
||||
} |
||||
|
||||
/**
|
||||
* Allocates enough internal storage in the supplied variable array structure to |
||||
* guarantee that the supplied number of elements (capacity) can be safely stored. |
||||
*/ |
||||
inline static void _zarray_ensure_capacity(zarray_t *za, int capacity){ |
||||
if (capacity <= za->alloc) |
||||
return; |
||||
|
||||
while (za->alloc < capacity) { |
||||
za->alloc *= 2; |
||||
if (za->alloc < 8) |
||||
za->alloc = 8; |
||||
} |
||||
|
||||
za->data = (char*) realloc(za->data, za->alloc * za->el_sz); |
||||
} |
||||
|
||||
/**
|
||||
* Adds a new element to the end of the supplied array, and sets its value |
||||
* (by copying) from the data pointed to by the supplied pointer 'p'. |
||||
* Automatically ensures that enough storage space is available for the new element. |
||||
*/ |
||||
inline static void _zarray_add(zarray_t *za, const void *p){ |
||||
_zarray_ensure_capacity(za, za->size + 1); |
||||
|
||||
memcpy(&za->data[za->size*za->el_sz], p, za->el_sz); |
||||
za->size++; |
||||
} |
||||
|
||||
/**
|
||||
* Retrieves the element from the supplied array located at the zero-based |
||||
* index of 'idx' and copies its value into the variable pointed to by the pointer |
||||
* 'p'. |
||||
*/ |
||||
inline static void _zarray_get(const zarray_t *za, int idx, void *p){ |
||||
CV_DbgAssert(idx >= 0); |
||||
CV_DbgAssert(idx < za->size); |
||||
|
||||
memcpy(p, &za->data[idx*za->el_sz], za->el_sz); |
||||
} |
||||
|
||||
/**
|
||||
* Similar to zarray_get(), but returns a "live" pointer to the internal |
||||
* storage, avoiding a memcpy. This pointer is not valid across |
||||
* operations which might move memory around (i.e. zarray_remove_value(), |
||||
* zarray_remove_index(), zarray_insert(), zarray_sort(), zarray_clear()). |
||||
* 'p' should be a pointer to the pointer which will be set to the internal address. |
||||
*/ |
||||
inline static void _zarray_get_volatile(const zarray_t *za, int idx, void *p){ |
||||
CV_DbgAssert(idx >= 0); |
||||
CV_DbgAssert(idx < za->size); |
||||
|
||||
*((void**) p) = &za->data[idx*za->el_sz]; |
||||
} |
||||
|
||||
inline static void _zarray_truncate(zarray_t *za, int sz){ |
||||
za->size = sz; |
||||
} |
||||
|
||||
/**
|
||||
* Sets the value of the current element at index 'idx' by copying its value from |
||||
* the data pointed to by 'p'. The previous value of the changed element will be |
||||
* copied into the data pointed to by 'outp' if it is not null. |
||||
*/ |
||||
static inline void _zarray_set(zarray_t *za, int idx, const void *p, void *outp){ |
||||
CV_DbgAssert(idx >= 0); |
||||
CV_DbgAssert(idx < za->size); |
||||
|
||||
if (outp != NULL) |
||||
memcpy(outp, &za->data[idx*za->el_sz], za->el_sz); |
||||
|
||||
memcpy(&za->data[idx*za->el_sz], p, za->el_sz); |
||||
} |
||||
|
||||
} |
||||
} |
||||
#endif |
||||
@ -0,0 +1,207 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
//
|
||||
// Copyright (C) 2013-2016, The Regents of The University of Michigan.
|
||||
//
|
||||
// This software was developed in the APRIL Robotics Lab under the
|
||||
// direction of Edwin Olson, ebolson@umich.edu. This software may be
|
||||
// available under alternative licensing terms; contact the address above.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the Regents of The University of Michigan.
|
||||
|
||||
#include "precomp.hpp" |
||||
#include "zmaxheap.hpp" |
||||
|
||||
|
||||
// 0
|
||||
// 1 2
|
||||
// 3 4 5 6
|
||||
// 7 8 9 10 11 12 13 14
|
||||
//
|
||||
// Children of node i: 2*i+1, 2*i+2
|
||||
// Parent of node i: (i-1) / 2
|
||||
//
|
||||
// Heap property: a parent is greater than (or equal to) its children.
|
||||
|
||||
#define MIN_CAPACITY 16 |
||||
namespace cv { |
||||
namespace aruco { |
||||
struct zmaxheap |
||||
{ |
||||
size_t el_sz; |
||||
|
||||
int size; |
||||
int alloc; |
||||
|
||||
float *values; |
||||
char *data; |
||||
|
||||
void (*swap)(zmaxheap_t *heap, int a, int b); |
||||
}; |
||||
|
||||
static inline void _swap_default(zmaxheap_t *heap, int a, int b) |
||||
{ |
||||
float t = heap->values[a]; |
||||
heap->values[a] = heap->values[b]; |
||||
heap->values[b] = t; |
||||
|
||||
cv::AutoBuffer<char> tmp(heap->el_sz); |
||||
memcpy(tmp, &heap->data[a*heap->el_sz], heap->el_sz); |
||||
memcpy(&heap->data[a*heap->el_sz], &heap->data[b*heap->el_sz], heap->el_sz); |
||||
memcpy(&heap->data[b*heap->el_sz], tmp, heap->el_sz); |
||||
} |
||||
|
||||
static inline void _swap_pointer(zmaxheap_t *heap, int a, int b) |
||||
{ |
||||
float t = heap->values[a]; |
||||
heap->values[a] = heap->values[b]; |
||||
heap->values[b] = t; |
||||
|
||||
void **pp = (void**) heap->data; |
||||
void *tmp = pp[a]; |
||||
pp[a] = pp[b]; |
||||
pp[b] = tmp; |
||||
} |
||||
|
||||
|
||||
zmaxheap_t *zmaxheap_create(size_t el_sz) |
||||
{ |
||||
zmaxheap_t *heap = (zmaxheap_t*)calloc(1, sizeof(zmaxheap_t)); |
||||
heap->el_sz = el_sz; |
||||
|
||||
heap->swap = _swap_default; |
||||
|
||||
if (el_sz == sizeof(void*)) |
||||
heap->swap = _swap_pointer; |
||||
|
||||
return heap; |
||||
} |
||||
|
||||
void zmaxheap_destroy(zmaxheap_t *heap) |
||||
{ |
||||
free(heap->values); |
||||
free(heap->data); |
||||
memset(heap, 0, sizeof(zmaxheap_t)); |
||||
free(heap); |
||||
} |
||||
|
||||
static void _zmaxheap_ensure_capacity(zmaxheap_t *heap, int capacity) |
||||
{ |
||||
if (heap->alloc >= capacity) |
||||
return; |
||||
|
||||
int newcap = heap->alloc; |
||||
|
||||
while (newcap < capacity) { |
||||
if (newcap < MIN_CAPACITY) { |
||||
newcap = MIN_CAPACITY; |
||||
continue; |
||||
} |
||||
|
||||
newcap *= 2; |
||||
} |
||||
|
||||
heap->values = (float*)realloc(heap->values, newcap * sizeof(float)); |
||||
heap->data = (char*)realloc(heap->data, newcap * heap->el_sz); |
||||
heap->alloc = newcap; |
||||
} |
||||
|
||||
void zmaxheap_add(zmaxheap_t *heap, void *p, float v) |
||||
{ |
||||
_zmaxheap_ensure_capacity(heap, heap->size + 1); |
||||
|
||||
int idx = heap->size; |
||||
|
||||
heap->values[idx] = v; |
||||
memcpy(&heap->data[idx*heap->el_sz], p, heap->el_sz); |
||||
|
||||
heap->size++; |
||||
|
||||
while (idx > 0) { |
||||
|
||||
int parent = (idx - 1) / 2; |
||||
|
||||
// we're done!
|
||||
if (heap->values[parent] >= v) |
||||
break; |
||||
|
||||
// else, swap and recurse upwards.
|
||||
heap->swap(heap, idx, parent); |
||||
idx = parent; |
||||
} |
||||
} |
||||
|
||||
// Removes the item in the heap at the given index. Returns 1 if the
|
||||
// item existed. 0 Indicates an invalid idx (heap is smaller than
|
||||
// idx). This is mostly intended to be used by zmaxheap_remove_max.
|
||||
static int zmaxheap_remove_index(zmaxheap_t *heap, int idx, void *p, float *v) |
||||
{ |
||||
if (idx >= heap->size) |
||||
return 0; |
||||
|
||||
// copy out the requested element from the heap.
|
||||
if (v != NULL) |
||||
*v = heap->values[idx]; |
||||
if (p != NULL) |
||||
memcpy(p, &heap->data[idx*heap->el_sz], heap->el_sz); |
||||
|
||||
heap->size--; |
||||
|
||||
// If this element is already the last one, then there's nothing
|
||||
// for us to do.
|
||||
if (idx == heap->size) |
||||
return 1; |
||||
|
||||
// copy last element to first element. (which probably upsets
|
||||
// the heap property).
|
||||
heap->values[idx] = heap->values[heap->size]; |
||||
memcpy(&heap->data[idx*heap->el_sz], &heap->data[heap->el_sz * heap->size], heap->el_sz); |
||||
|
||||
// now fix the heap. Note, as we descend, we're "pushing down"
|
||||
// the same node the entire time. Thus, while the index of the
|
||||
// parent might change, the parent_score doesn't.
|
||||
int parent = idx; |
||||
float parent_score = heap->values[idx]; |
||||
|
||||
// descend, fixing the heap.
|
||||
while (parent < heap->size) { |
||||
|
||||
int left = 2*parent + 1; |
||||
int right = left + 1; |
||||
|
||||
// assert(parent_score == heap->values[parent]);
|
||||
|
||||
float left_score = (left < heap->size) ? heap->values[left] : -INFINITY; |
||||
float right_score = (right < heap->size) ? heap->values[right] : -INFINITY; |
||||
|
||||
// put the biggest of (parent, left, right) as the parent.
|
||||
|
||||
// already okay?
|
||||
if (parent_score >= left_score && parent_score >= right_score) |
||||
break; |
||||
|
||||
// if we got here, then one of the children is bigger than the parent.
|
||||
if (left_score >= right_score) { |
||||
CV_Assert(left < heap->size); |
||||
heap->swap(heap, parent, left); |
||||
parent = left; |
||||
} else { |
||||
// right_score can't be less than left_score if right_score is -INFINITY.
|
||||
CV_Assert(right < heap->size); |
||||
heap->swap(heap, parent, right); |
||||
parent = right; |
||||
} |
||||
} |
||||
|
||||
return 1; |
||||
} |
||||
|
||||
int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v) |
||||
{ |
||||
return zmaxheap_remove_index(heap, 0, p, v); |
||||
} |
||||
|
||||
}} |
||||
@ -0,0 +1,42 @@ |
||||
// This file is part of OpenCV project.
|
||||
// It is subject to the license terms in the LICENSE file found in the top-level directory
|
||||
// of this distribution and at http://opencv.org/license.html.
|
||||
//
|
||||
// Copyright (C) 2013-2016, The Regents of The University of Michigan.
|
||||
//
|
||||
// This software was developed in the APRIL Robotics Lab under the
|
||||
// direction of Edwin Olson, ebolson@umich.edu. This software may be
|
||||
// available under alternative licensing terms; contact the address above.
|
||||
//
|
||||
// The views and conclusions contained in the software and documentation are those
|
||||
// of the authors and should not be interpreted as representing official policies,
|
||||
// either expressed or implied, of the Regents of The University of Michigan.
|
||||
#ifndef _OPENCV_ZMAXHEAP_HPP_ |
||||
#define _OPENCV_ZMAXHEAP_HPP_ |
||||
|
||||
#include <stdlib.h> |
||||
#include <string.h> |
||||
#include <math.h> |
||||
|
||||
namespace cv { |
||||
namespace aruco { |
||||
typedef struct zmaxheap zmaxheap_t; |
||||
|
||||
typedef struct zmaxheap_iterator zmaxheap_iterator_t; |
||||
struct zmaxheap_iterator { |
||||
zmaxheap_t *heap; |
||||
int in, out; |
||||
}; |
||||
|
||||
zmaxheap_t *zmaxheap_create(size_t el_sz); |
||||
|
||||
void zmaxheap_destroy(zmaxheap_t *heap); |
||||
|
||||
void zmaxheap_add(zmaxheap_t *heap, void *p, float v); |
||||
|
||||
// returns 0 if the heap is empty, so you can do
|
||||
// while (zmaxheap_remove_max(...)) { }
|
||||
int zmaxheap_remove_max(zmaxheap_t *heap, void *p, float *v); |
||||
|
||||
}} |
||||
#endif |
||||
Loading…
Reference in new issue