mirror of
https://github.com/opencv/opencv.git
synced 2026-07-29 15:23:05 +04:00
Merge pull request #22368 from AleksandrPanov:move_contrib_aruco_to_main_objdetect
Megre together with https://github.com/opencv/opencv_contrib/pull/3325 1. Move aruco_detector, aruco_board, aruco_dictionary, aruco_utils to objdetect 1.1 add virtual Board::draw(), virtual ~Board() 1.2 move `testCharucoCornersCollinear` to Board classes (and rename to `checkCharucoCornersCollinear`) 1.3 add wrappers to keep the old api working 3. Reduce inludes 4. Fix java tests (add objdetect import) 5. Refactoring ### Pull Request Readiness Checklist See details at https://github.com/opencv/opencv/wiki/How_to_contribute#making-a-good-pull-request - [x] I agree to contribute to the project under Apache 2 License. - [x] To the best of my knowledge, the proposed patch is not based on a code under GPL or another license that is incompatible with OpenCV - [x] The PR is proposed to the proper branch - [x] There is a reference to the original bug report and related work - [x] There is accuracy test, performance test and test data in opencv_extra repository, if applicable Patch to opencv_extra has the same branch name. - [x] The feature is well documented and sample code can be built with the project CMake ``` **WIP** force_builders=linux,win64,docs,Linux x64 Debug,Custom Xbuild_contrib:Docs=OFF build_image:Custom=ubuntu:22.04 build_worker:Custom=linux-1 ```
This commit is contained in:
Alexander Panov
GitHub
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,117 @@
|
||||
// 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 "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 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 DetectorParameters &_params, const Mat im, zarray_t *cluster, struct sQuad *quad);
|
||||
|
||||
|
||||
void threshold(const Mat mIm, const DetectorParameters ¶meters, Mat& mThresh);
|
||||
|
||||
|
||||
zarray_t *apriltag_quad_thresh(const DetectorParameters ¶meters, const Mat & mImg,
|
||||
std::vector<std::vector<Point> > &contours);
|
||||
|
||||
void _apriltag(Mat im_orig, const DetectorParameters &_params, std::vector<std::vector<Point2f> > &candidates,
|
||||
std::vector<std::vector<Point> > &contours);
|
||||
|
||||
}}
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,131 @@
|
||||
// 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_
|
||||
|
||||
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,148 @@
|
||||
// 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_
|
||||
|
||||
|
||||
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.data(), &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.data(), 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,38 @@
|
||||
// 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_
|
||||
|
||||
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
|
||||
@@ -0,0 +1,507 @@
|
||||
// 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
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include <opencv2/objdetect/aruco_dictionary.hpp>
|
||||
#include <numeric>
|
||||
|
||||
namespace cv {
|
||||
namespace aruco {
|
||||
using namespace std;
|
||||
|
||||
struct Board::BoardImpl {
|
||||
std::vector<std::vector<Point3f> > objPoints;
|
||||
Dictionary dictionary;
|
||||
Point3f rightBottomBorder;
|
||||
std::vector<int> ids;
|
||||
|
||||
BoardImpl() {
|
||||
dictionary = Dictionary(getPredefinedDictionary(PredefinedDictionaryType::DICT_4X4_50));
|
||||
}
|
||||
};
|
||||
|
||||
Board::Board(): boardImpl(makePtr<BoardImpl>()) {}
|
||||
|
||||
Board::~Board() {}
|
||||
|
||||
Ptr<Board> Board::create(InputArrayOfArrays objPoints, const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(objPoints.total() == ids.total());
|
||||
CV_Assert(objPoints.type() == CV_32FC3 || objPoints.type() == CV_32FC1);
|
||||
|
||||
vector<vector<Point3f> > obj_points_vector;
|
||||
Point3f rightBottomBorder = Point3f(0.f, 0.f, 0.f);
|
||||
for (unsigned int i = 0; i < objPoints.total(); i++) {
|
||||
vector<Point3f> corners;
|
||||
Mat corners_mat = objPoints.getMat(i);
|
||||
|
||||
if (corners_mat.type() == CV_32FC1)
|
||||
corners_mat = corners_mat.reshape(3);
|
||||
CV_Assert(corners_mat.total() == 4);
|
||||
|
||||
for (int j = 0; j < 4; j++) {
|
||||
const Point3f &corner = corners_mat.at<Point3f>(j);
|
||||
corners.push_back(corner);
|
||||
rightBottomBorder.x = std::max(rightBottomBorder.x, corner.x);
|
||||
rightBottomBorder.y = std::max(rightBottomBorder.y, corner.y);
|
||||
rightBottomBorder.z = std::max(rightBottomBorder.z, corner.z);
|
||||
}
|
||||
obj_points_vector.push_back(corners);
|
||||
}
|
||||
Board board;
|
||||
Ptr<Board> res = makePtr<Board>(board);
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
res->boardImpl->objPoints = obj_points_vector;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
res->boardImpl->rightBottomBorder = rightBottomBorder;
|
||||
return res;
|
||||
}
|
||||
|
||||
const Dictionary& Board::getDictionary() const {
|
||||
return this->boardImpl->dictionary;
|
||||
}
|
||||
|
||||
const vector<vector<Point3f> >& Board::getObjPoints() const {
|
||||
return this->boardImpl->objPoints;
|
||||
}
|
||||
|
||||
const Point3f& Board::getRightBottomCorner() const {
|
||||
return this->boardImpl->rightBottomBorder;
|
||||
}
|
||||
|
||||
const vector<int>& Board::getIds() const {
|
||||
return this->boardImpl->ids;
|
||||
}
|
||||
|
||||
/** @brief Implementation of draw planar board that accepts a raw Board pointer.
|
||||
*/
|
||||
void Board::generateImage(Size outSize, OutputArray img, int marginSize, int borderBits) const {
|
||||
CV_Assert(!outSize.empty());
|
||||
CV_Assert(marginSize >= 0);
|
||||
|
||||
img.create(outSize, CV_8UC1);
|
||||
Mat out = img.getMat();
|
||||
out.setTo(Scalar::all(255));
|
||||
out.adjustROI(-marginSize, -marginSize, -marginSize, -marginSize);
|
||||
|
||||
// calculate max and min values in XY plane
|
||||
CV_Assert(this->getObjPoints().size() > 0);
|
||||
float minX, maxX, minY, maxY;
|
||||
minX = maxX = this->getObjPoints()[0][0].x;
|
||||
minY = maxY = this->getObjPoints()[0][0].y;
|
||||
|
||||
for(unsigned int i = 0; i < this->getObjPoints().size(); i++) {
|
||||
for(int j = 0; j < 4; j++) {
|
||||
minX = min(minX, this->getObjPoints()[i][j].x);
|
||||
maxX = max(maxX, this->getObjPoints()[i][j].x);
|
||||
minY = min(minY, this->getObjPoints()[i][j].y);
|
||||
maxY = max(maxY, this->getObjPoints()[i][j].y);
|
||||
}
|
||||
}
|
||||
|
||||
float sizeX = maxX - minX;
|
||||
float sizeY = maxY - minY;
|
||||
|
||||
// proportion transformations
|
||||
float xReduction = sizeX / float(out.cols);
|
||||
float yReduction = sizeY / float(out.rows);
|
||||
|
||||
// determine the zone where the markers are placed
|
||||
if(xReduction > yReduction) {
|
||||
int nRows = int(sizeY / xReduction);
|
||||
int rowsMargins = (out.rows - nRows) / 2;
|
||||
out.adjustROI(-rowsMargins, -rowsMargins, 0, 0);
|
||||
} else {
|
||||
int nCols = int(sizeX / yReduction);
|
||||
int colsMargins = (out.cols - nCols) / 2;
|
||||
out.adjustROI(0, 0, -colsMargins, -colsMargins);
|
||||
}
|
||||
|
||||
// now paint each marker
|
||||
Mat marker;
|
||||
Point2f outCorners[3];
|
||||
Point2f inCorners[3];
|
||||
for(unsigned int m = 0; m < this->getObjPoints().size(); m++) {
|
||||
// transform corners to markerZone coordinates
|
||||
for(int j = 0; j < 3; j++) {
|
||||
Point2f pf = Point2f(this->getObjPoints()[m][j].x, this->getObjPoints()[m][j].y);
|
||||
// move top left to 0, 0
|
||||
pf -= Point2f(minX, minY);
|
||||
pf.x = pf.x / sizeX * float(out.cols);
|
||||
pf.y = pf.y / sizeY * float(out.rows);
|
||||
outCorners[j] = pf;
|
||||
}
|
||||
|
||||
// get marker
|
||||
Size dst_sz(outCorners[2] - outCorners[0]); // assuming CCW order
|
||||
dst_sz.width = dst_sz.height = std::min(dst_sz.width, dst_sz.height); //marker should be square
|
||||
getDictionary().generateImageMarker(this->getIds()[m], dst_sz.width, marker, borderBits);
|
||||
|
||||
if((outCorners[0].y == outCorners[1].y) && (outCorners[1].x == outCorners[2].x)) {
|
||||
// marker is aligned to image axes
|
||||
marker.copyTo(out(Rect(outCorners[0], dst_sz)));
|
||||
continue;
|
||||
}
|
||||
|
||||
// interpolate tiny marker to marker position in markerZone
|
||||
inCorners[0] = Point2f(-0.5f, -0.5f);
|
||||
inCorners[1] = Point2f(marker.cols - 0.5f, -0.5f);
|
||||
inCorners[2] = Point2f(marker.cols - 0.5f, marker.rows - 0.5f);
|
||||
|
||||
// remove perspective
|
||||
Mat transformation = getAffineTransform(inCorners, outCorners);
|
||||
warpAffine(marker, out, transformation, out.size(), INTER_LINEAR,
|
||||
BORDER_TRANSPARENT);
|
||||
}
|
||||
}
|
||||
|
||||
void Board::matchImagePoints(InputArray detectedCorners, InputArray detectedIds,
|
||||
OutputArray _objPoints, OutputArray imgPoints) const {
|
||||
CV_Assert(getIds().size() == getObjPoints().size());
|
||||
CV_Assert(detectedIds.total() == detectedCorners.total());
|
||||
|
||||
size_t nDetectedMarkers = detectedIds.total();
|
||||
|
||||
vector<Point3f> objPnts;
|
||||
objPnts.reserve(nDetectedMarkers);
|
||||
|
||||
vector<Point2f> imgPnts;
|
||||
imgPnts.reserve(nDetectedMarkers);
|
||||
|
||||
// look for detected markers that belong to the board and get their information
|
||||
for(unsigned int i = 0; i < nDetectedMarkers; i++) {
|
||||
int currentId = detectedIds.getMat().ptr< int >(0)[i];
|
||||
for(unsigned int j = 0; j < getIds().size(); j++) {
|
||||
if(currentId == getIds()[j]) {
|
||||
for(int p = 0; p < 4; p++) {
|
||||
objPnts.push_back(getObjPoints()[j][p]);
|
||||
imgPnts.push_back(detectedCorners.getMat(i).ptr<Point2f>(0)[p]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// create output
|
||||
Mat(objPnts).copyTo(_objPoints);
|
||||
Mat(imgPnts).copyTo(imgPoints);
|
||||
}
|
||||
|
||||
struct GridBoard::GridImpl {
|
||||
GridImpl(){};
|
||||
// number of markers in X and Y directions
|
||||
int sizeX = 3, sizeY = 3;
|
||||
|
||||
// marker side length (normally in meters)
|
||||
float markerLength = 1.f;
|
||||
|
||||
// separation between markers in the grid
|
||||
float markerSeparation = .5f;
|
||||
};
|
||||
|
||||
GridBoard::GridBoard(): gridImpl(makePtr<GridImpl>()) {}
|
||||
|
||||
Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation,
|
||||
const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(markersX > 0 && markersY > 0 && markerLength > 0 && markerSeparation > 0);
|
||||
GridBoard board;
|
||||
Ptr<GridBoard> res = makePtr<GridBoard>(board);
|
||||
res->gridImpl->sizeX = markersX;
|
||||
res->gridImpl->sizeY = markersY;
|
||||
res->gridImpl->markerLength = markerLength;
|
||||
res->gridImpl->markerSeparation = markerSeparation;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
|
||||
size_t totalMarkers = (size_t) markersX * markersY;
|
||||
CV_Assert(totalMarkers == ids.total());
|
||||
vector<vector<Point3f> > objPoints;
|
||||
objPoints.reserve(totalMarkers);
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
// calculate Board objPoints
|
||||
for (int y = 0; y < markersY; y++) {
|
||||
for (int x = 0; x < markersX; x++) {
|
||||
vector <Point3f> corners(4);
|
||||
corners[0] = Point3f(x * (markerLength + markerSeparation),
|
||||
y * (markerLength + markerSeparation), 0);
|
||||
corners[1] = corners[0] + Point3f(markerLength, 0, 0);
|
||||
corners[2] = corners[0] + Point3f(markerLength, markerLength, 0);
|
||||
corners[3] = corners[0] + Point3f(0, markerLength, 0);
|
||||
objPoints.push_back(corners);
|
||||
}
|
||||
}
|
||||
res->boardImpl->objPoints = objPoints;
|
||||
res->boardImpl->rightBottomBorder = Point3f(markersX * markerLength + markerSeparation * (markersX - 1),
|
||||
markersY * markerLength + markerSeparation * (markersY - 1), 0.f);
|
||||
return res;
|
||||
}
|
||||
|
||||
Ptr<GridBoard> GridBoard::create(int markersX, int markersY, float markerLength, float markerSeparation,
|
||||
const Dictionary &dictionary, int firstMarker) {
|
||||
vector<int> ids(markersX*markersY);
|
||||
std::iota(ids.begin(), ids.end(), firstMarker);
|
||||
return GridBoard::create(markersX, markersY, markerLength, markerSeparation, dictionary, ids);
|
||||
}
|
||||
|
||||
void GridBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const {
|
||||
Board::generateImage(outSize, _img, marginSize, borderBits);
|
||||
}
|
||||
|
||||
Size GridBoard::getGridSize() const {
|
||||
return Size(gridImpl->sizeX, gridImpl->sizeY);
|
||||
}
|
||||
|
||||
float GridBoard::getMarkerLength() const {
|
||||
return gridImpl->markerLength;
|
||||
}
|
||||
|
||||
float GridBoard::getMarkerSeparation() const {
|
||||
return gridImpl->markerSeparation;
|
||||
}
|
||||
|
||||
struct CharucoBoard::CharucoImpl : GridBoard::GridImpl {
|
||||
// size of chessboard squares side (normally in meters)
|
||||
float squareLength;
|
||||
|
||||
// marker side length (normally in meters)
|
||||
float markerLength;
|
||||
|
||||
static void _getNearestMarkerCorners(CharucoBoard &board, float squareLength);
|
||||
|
||||
// vector of chessboard 3D corners precalculated
|
||||
std::vector<Point3f> chessboardCorners;
|
||||
|
||||
// for each charuco corner, nearest marker id and nearest marker corner id of each marker
|
||||
std::vector<std::vector<int> > nearestMarkerIdx;
|
||||
std::vector<std::vector<int> > nearestMarkerCorners;
|
||||
};
|
||||
|
||||
CharucoBoard::CharucoBoard(): charucoImpl(makePtr<CharucoImpl>()) {}
|
||||
|
||||
void CharucoBoard::generateImage(Size outSize, OutputArray _img, int marginSize, int borderBits) const {
|
||||
CV_Assert(!outSize.empty());
|
||||
CV_Assert(marginSize >= 0);
|
||||
|
||||
_img.create(outSize, CV_8UC1);
|
||||
_img.setTo(255);
|
||||
Mat out = _img.getMat();
|
||||
Mat noMarginsImg =
|
||||
out.colRange(marginSize, out.cols - marginSize).rowRange(marginSize, out.rows - marginSize);
|
||||
|
||||
double totalLengthX, totalLengthY;
|
||||
totalLengthX = charucoImpl->squareLength * charucoImpl->sizeX;
|
||||
totalLengthY = charucoImpl->squareLength * charucoImpl->sizeY;
|
||||
|
||||
// proportional transformation
|
||||
double xReduction = totalLengthX / double(noMarginsImg.cols);
|
||||
double yReduction = totalLengthY / double(noMarginsImg.rows);
|
||||
|
||||
// determine the zone where the chessboard is placed
|
||||
Mat chessboardZoneImg;
|
||||
if(xReduction > yReduction) {
|
||||
int nRows = int(totalLengthY / xReduction);
|
||||
int rowsMargins = (noMarginsImg.rows - nRows) / 2;
|
||||
chessboardZoneImg = noMarginsImg.rowRange(rowsMargins, noMarginsImg.rows - rowsMargins);
|
||||
} else {
|
||||
int nCols = int(totalLengthX / yReduction);
|
||||
int colsMargins = (noMarginsImg.cols - nCols) / 2;
|
||||
chessboardZoneImg = noMarginsImg.colRange(colsMargins, noMarginsImg.cols - colsMargins);
|
||||
}
|
||||
|
||||
// determine the margins to draw only the markers
|
||||
// take the minimum just to be sure
|
||||
double squareSizePixels = min(double(chessboardZoneImg.cols) / double(charucoImpl->sizeX),
|
||||
double(chessboardZoneImg.rows) / double(charucoImpl->sizeY));
|
||||
|
||||
double diffSquareMarkerLength = (charucoImpl->squareLength - charucoImpl->markerLength) / 2;
|
||||
int diffSquareMarkerLengthPixels =
|
||||
int(diffSquareMarkerLength * squareSizePixels / charucoImpl->squareLength);
|
||||
|
||||
// draw markers
|
||||
Mat markersImg;
|
||||
Board::generateImage(chessboardZoneImg.size(), markersImg, diffSquareMarkerLengthPixels, borderBits);
|
||||
markersImg.copyTo(chessboardZoneImg);
|
||||
|
||||
// now draw black squares
|
||||
for(int y = 0; y < charucoImpl->sizeY; y++) {
|
||||
for(int x = 0; x < charucoImpl->sizeX; x++) {
|
||||
|
||||
if(y % 2 != x % 2) continue; // white corner, dont do anything
|
||||
|
||||
double startX, startY;
|
||||
startX = squareSizePixels * double(x);
|
||||
startY = squareSizePixels * double(y);
|
||||
|
||||
Mat squareZone = chessboardZoneImg.rowRange(int(startY), int(startY + squareSizePixels))
|
||||
.colRange(int(startX), int(startX + squareSizePixels));
|
||||
|
||||
squareZone.setTo(0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Fill nearestMarkerIdx and nearestMarkerCorners arrays
|
||||
*/
|
||||
void CharucoBoard::CharucoImpl::_getNearestMarkerCorners(CharucoBoard &board, float squareLength) {
|
||||
board.charucoImpl->nearestMarkerIdx.resize(board.charucoImpl->chessboardCorners.size());
|
||||
board.charucoImpl->nearestMarkerCorners.resize(board.charucoImpl->chessboardCorners.size());
|
||||
|
||||
unsigned int nMarkers = (unsigned int)board.getIds().size();
|
||||
unsigned int nCharucoCorners = (unsigned int)board.charucoImpl->chessboardCorners.size();
|
||||
for(unsigned int i = 0; i < nCharucoCorners; i++) {
|
||||
double minDist = -1; // distance of closest markers
|
||||
Point3f charucoCorner = board.charucoImpl->chessboardCorners[i];
|
||||
for(unsigned int j = 0; j < nMarkers; j++) {
|
||||
// calculate distance from marker center to charuco corner
|
||||
Point3f center = Point3f(0, 0, 0);
|
||||
for(unsigned int k = 0; k < 4; k++)
|
||||
center += board.getObjPoints()[j][k];
|
||||
center /= 4.;
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - center;
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(j == 0 || fabs(sqDistance - minDist) < cv::pow(0.01 * squareLength, 2)) {
|
||||
// if same minimum distance (or first iteration), add to nearestMarkerIdx vector
|
||||
board.charucoImpl->nearestMarkerIdx[i].push_back(j);
|
||||
minDist = sqDistance;
|
||||
} else if(sqDistance < minDist) {
|
||||
// if finding a closest marker to the charuco corner
|
||||
board.charucoImpl->nearestMarkerIdx[i].clear(); // remove any previous added marker
|
||||
board.charucoImpl->nearestMarkerIdx[i].push_back(j); // add the new closest marker index
|
||||
minDist = sqDistance;
|
||||
}
|
||||
}
|
||||
// for each of the closest markers, search the marker corner index closer
|
||||
// to the charuco corner
|
||||
for(unsigned int j = 0; j < board.charucoImpl->nearestMarkerIdx[i].size(); j++) {
|
||||
board.charucoImpl->nearestMarkerCorners[i].resize(board.charucoImpl->nearestMarkerIdx[i].size());
|
||||
double minDistCorner = -1;
|
||||
for(unsigned int k = 0; k < 4; k++) {
|
||||
double sqDistance;
|
||||
Point3f distVector = charucoCorner - board.getObjPoints()[board.charucoImpl->nearestMarkerIdx[i][j]][k];
|
||||
sqDistance = distVector.x * distVector.x + distVector.y * distVector.y;
|
||||
if(k == 0 || sqDistance < minDistCorner) {
|
||||
// if this corner is closer to the charuco corner, assing its index
|
||||
// to nearestMarkerCorners
|
||||
minDistCorner = sqDistance;
|
||||
board.charucoImpl->nearestMarkerCorners[i][j] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Ptr<CharucoBoard> CharucoBoard::create(int squaresX, int squaresY, float squareLength, float markerLength,
|
||||
const Dictionary &dictionary, InputArray ids) {
|
||||
CV_Assert(squaresX > 1 && squaresY > 1 && markerLength > 0 && squareLength > markerLength);
|
||||
CharucoBoard board;
|
||||
Ptr<CharucoBoard> res = makePtr<CharucoBoard>(board);
|
||||
|
||||
res->charucoImpl->sizeX = squaresX;
|
||||
res->charucoImpl->sizeY = squaresY;
|
||||
res->charucoImpl->squareLength = squareLength;
|
||||
res->charucoImpl->markerLength = markerLength;
|
||||
res->boardImpl->dictionary = dictionary;
|
||||
vector<vector<Point3f> > objPoints;
|
||||
|
||||
float diffSquareMarkerLength = (squareLength - markerLength) / 2;
|
||||
int totalMarkers = (int)(ids.total());
|
||||
ids.copyTo(res->boardImpl->ids);
|
||||
// calculate Board objPoints
|
||||
int nextId = 0;
|
||||
for(int y = 0; y < squaresY; y++) {
|
||||
for(int x = 0; x < squaresX; x++) {
|
||||
|
||||
if(y % 2 == x % 2) continue; // black corner, no marker here
|
||||
|
||||
vector<Point3f> corners(4);
|
||||
corners[0] = Point3f(x * squareLength + diffSquareMarkerLength,
|
||||
y * squareLength + diffSquareMarkerLength, 0);
|
||||
corners[1] = corners[0] + Point3f(markerLength, 0, 0);
|
||||
corners[2] = corners[0] + Point3f(markerLength, markerLength, 0);
|
||||
corners[3] = corners[0] + Point3f(0, markerLength, 0);
|
||||
objPoints.push_back(corners);
|
||||
// first ids in dictionary
|
||||
if (totalMarkers == 0)
|
||||
res->boardImpl->ids.push_back(nextId);
|
||||
nextId++;
|
||||
}
|
||||
}
|
||||
if (totalMarkers > 0 && nextId != totalMarkers)
|
||||
CV_Error(cv::Error::StsBadSize, "Size of ids must be equal to the number of markers: "+std::to_string(nextId));
|
||||
res->boardImpl->objPoints = objPoints;
|
||||
|
||||
// now fill chessboardCorners
|
||||
for(int y = 0; y < squaresY - 1; y++) {
|
||||
for(int x = 0; x < squaresX - 1; x++) {
|
||||
Point3f corner;
|
||||
corner.x = (x + 1) * squareLength;
|
||||
corner.y = (y + 1) * squareLength;
|
||||
corner.z = 0;
|
||||
res->charucoImpl->chessboardCorners.push_back(corner);
|
||||
}
|
||||
}
|
||||
res->boardImpl->rightBottomBorder = Point3f(squaresX * squareLength, squaresY * squareLength, 0.f);
|
||||
CharucoBoard::CharucoImpl::_getNearestMarkerCorners(*res, res->charucoImpl->squareLength);
|
||||
return res;
|
||||
}
|
||||
|
||||
Size CharucoBoard::getChessboardSize() const { return Size(charucoImpl->sizeX, charucoImpl->sizeY); }
|
||||
|
||||
float CharucoBoard::getSquareLength() const { return charucoImpl->squareLength; }
|
||||
|
||||
float CharucoBoard::getMarkerLength() const { return charucoImpl->markerLength; }
|
||||
|
||||
bool CharucoBoard::checkCharucoCornersCollinear(InputArray charucoIds) const {
|
||||
unsigned int nCharucoCorners = (unsigned int)charucoIds.getMat().total();
|
||||
if (nCharucoCorners <= 2)
|
||||
return true;
|
||||
|
||||
// only test if there are 3 or more corners
|
||||
CV_Assert(charucoImpl->chessboardCorners.size() >= charucoIds.getMat().total());
|
||||
|
||||
Vec<double, 3> point0(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].x,
|
||||
charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(0)].y, 1);
|
||||
|
||||
Vec<double, 3> point1(charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].x,
|
||||
charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(1)].y, 1);
|
||||
|
||||
// create a line from the first two points.
|
||||
Vec<double, 3> testLine = point0.cross(point1);
|
||||
Vec<double, 3> testPoint(0, 0, 1);
|
||||
|
||||
double divisor = sqrt(testLine[0]*testLine[0] + testLine[1]*testLine[1]);
|
||||
CV_Assert(divisor != 0.0);
|
||||
|
||||
// normalize the line with normal
|
||||
testLine /= divisor;
|
||||
|
||||
double dotProduct;
|
||||
for (unsigned int i = 2; i < nCharucoCorners; i++){
|
||||
testPoint(0) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].x;
|
||||
testPoint(1) = charucoImpl->chessboardCorners[charucoIds.getMat().at<int>(i)].y;
|
||||
|
||||
// if testPoint is on testLine, dotProduct will be zero (or very, very close)
|
||||
dotProduct = testPoint.dot(testLine);
|
||||
|
||||
if (std::abs(dotProduct) > 1e-6){
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// no points found that were off of testLine, return true that all points collinear.
|
||||
return true;
|
||||
}
|
||||
|
||||
std::vector<Point3f> CharucoBoard::getChessboardCorners() const {
|
||||
return charucoImpl->chessboardCorners;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerIdx() const {
|
||||
return charucoImpl->nearestMarkerIdx;
|
||||
}
|
||||
|
||||
std::vector<std::vector<int> > CharucoBoard::getNearestMarkerCorners() const {
|
||||
return charucoImpl->nearestMarkerCorners;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,441 @@
|
||||
// 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
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "opencv2/core/hal/hal.hpp"
|
||||
|
||||
#include "aruco_utils.hpp"
|
||||
#include "predefined_dictionaries.hpp"
|
||||
#include "apriltag/predefined_dictionaries_apriltag.hpp"
|
||||
#include <opencv2/objdetect/aruco_dictionary.hpp>
|
||||
|
||||
namespace cv {
|
||||
namespace aruco {
|
||||
|
||||
using namespace std;
|
||||
|
||||
Dictionary::Dictionary(): markerSize(0), maxCorrectionBits(0) {}
|
||||
|
||||
|
||||
Dictionary::Dictionary(const Mat &_bytesList, int _markerSize, int _maxcorr) {
|
||||
markerSize = _markerSize;
|
||||
maxCorrectionBits = _maxcorr;
|
||||
bytesList = _bytesList;
|
||||
}
|
||||
|
||||
|
||||
bool Dictionary::readDictionary(const cv::FileNode& fn) {
|
||||
int nMarkers = 0, _markerSize = 0;
|
||||
if (fn.empty() || !readParameter("nmarkers", nMarkers, fn) || !readParameter("markersize", _markerSize, fn))
|
||||
return false;
|
||||
Mat bytes(0, 0, CV_8UC1), marker(_markerSize, _markerSize, CV_8UC1);
|
||||
std::string markerString;
|
||||
for (int i = 0; i < nMarkers; i++) {
|
||||
std::ostringstream ostr;
|
||||
ostr << i;
|
||||
if (!readParameter("marker_" + ostr.str(), markerString, fn))
|
||||
return false;
|
||||
for (int j = 0; j < (int) markerString.size(); j++)
|
||||
marker.at<unsigned char>(j) = (markerString[j] == '0') ? 0 : 1;
|
||||
bytes.push_back(Dictionary::getByteListFromBits(marker));
|
||||
}
|
||||
int _maxCorrectionBits = 0;
|
||||
readParameter("maxCorrectionBits", _maxCorrectionBits, fn);
|
||||
*this = Dictionary(bytes, _markerSize, _maxCorrectionBits);
|
||||
return true;
|
||||
}
|
||||
|
||||
void Dictionary::writeDictionary(FileStorage &fs) {
|
||||
fs << "nmarkers" << bytesList.rows;
|
||||
fs << "markersize" << markerSize;
|
||||
fs << "maxCorrectionBits" << maxCorrectionBits;
|
||||
for (int i = 0; i < bytesList.rows; i++) {
|
||||
Mat row = bytesList.row(i);;
|
||||
Mat bitMarker = getBitsFromByteList(row, markerSize);
|
||||
std::ostringstream ostr;
|
||||
ostr << i;
|
||||
string markerName = "marker_" + ostr.str();
|
||||
string marker;
|
||||
for (int j = 0; j < markerSize * markerSize; j++)
|
||||
marker.push_back(bitMarker.at<uint8_t>(j) + '0');
|
||||
fs << markerName << marker;
|
||||
}
|
||||
}
|
||||
|
||||
void Dictionary::writeDictionary(Ptr<FileStorage>& fs, const String &name) {
|
||||
if(name.empty())
|
||||
return writeDictionary(*fs);
|
||||
*fs << name << "{";
|
||||
writeDictionary(*fs);
|
||||
*fs << "}";
|
||||
}
|
||||
|
||||
|
||||
bool Dictionary::identify(const Mat &onlyBits, int &idx, int &rotation, double maxCorrectionRate) const {
|
||||
CV_Assert(onlyBits.rows == markerSize && onlyBits.cols == markerSize);
|
||||
|
||||
int maxCorrectionRecalculed = int(double(maxCorrectionBits) * maxCorrectionRate);
|
||||
|
||||
// get as a byte list
|
||||
Mat candidateBytes = getByteListFromBits(onlyBits);
|
||||
|
||||
idx = -1; // by default, not found
|
||||
|
||||
// search closest marker in dict
|
||||
for(int m = 0; m < bytesList.rows; m++) {
|
||||
int currentMinDistance = markerSize * markerSize + 1;
|
||||
int currentRotation = -1;
|
||||
for(unsigned int r = 0; r < 4; r++) {
|
||||
int currentHamming = cv::hal::normHamming(
|
||||
bytesList.ptr(m)+r*candidateBytes.cols,
|
||||
candidateBytes.ptr(),
|
||||
candidateBytes.cols);
|
||||
|
||||
if(currentHamming < currentMinDistance) {
|
||||
currentMinDistance = currentHamming;
|
||||
currentRotation = r;
|
||||
}
|
||||
}
|
||||
|
||||
// if maxCorrection is fulfilled, return this one
|
||||
if(currentMinDistance <= maxCorrectionRecalculed) {
|
||||
idx = m;
|
||||
rotation = currentRotation;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return idx != -1;
|
||||
}
|
||||
|
||||
|
||||
int Dictionary::getDistanceToId(InputArray bits, int id, bool allRotations) const {
|
||||
|
||||
CV_Assert(id >= 0 && id < bytesList.rows);
|
||||
|
||||
unsigned int nRotations = 4;
|
||||
if(!allRotations) nRotations = 1;
|
||||
|
||||
Mat candidateBytes = getByteListFromBits(bits.getMat());
|
||||
int currentMinDistance = int(bits.total() * bits.total());
|
||||
for(unsigned int r = 0; r < nRotations; r++) {
|
||||
int currentHamming = cv::hal::normHamming(
|
||||
bytesList.ptr(id) + r*candidateBytes.cols,
|
||||
candidateBytes.ptr(),
|
||||
candidateBytes.cols);
|
||||
|
||||
if(currentHamming < currentMinDistance) {
|
||||
currentMinDistance = currentHamming;
|
||||
}
|
||||
}
|
||||
return currentMinDistance;
|
||||
}
|
||||
|
||||
|
||||
void Dictionary::generateImageMarker(int id, int sidePixels, OutputArray _img, int borderBits) const {
|
||||
CV_Assert(sidePixels >= (markerSize + 2*borderBits));
|
||||
CV_Assert(id < bytesList.rows);
|
||||
CV_Assert(borderBits > 0);
|
||||
|
||||
_img.create(sidePixels, sidePixels, CV_8UC1);
|
||||
|
||||
// create small marker with 1 pixel per bin
|
||||
Mat tinyMarker(markerSize + 2 * borderBits, markerSize + 2 * borderBits, CV_8UC1,
|
||||
Scalar::all(0));
|
||||
Mat innerRegion = tinyMarker.rowRange(borderBits, tinyMarker.rows - borderBits)
|
||||
.colRange(borderBits, tinyMarker.cols - borderBits);
|
||||
// put inner bits
|
||||
Mat bits = 255 * getBitsFromByteList(bytesList.rowRange(id, id + 1), markerSize);
|
||||
CV_Assert(innerRegion.total() == bits.total());
|
||||
bits.copyTo(innerRegion);
|
||||
|
||||
// resize tiny marker to output size
|
||||
cv::resize(tinyMarker, _img.getMat(), _img.getMat().size(), 0, 0, INTER_NEAREST);
|
||||
}
|
||||
|
||||
|
||||
Mat Dictionary::getByteListFromBits(const Mat &bits) {
|
||||
// integer ceil
|
||||
int nbytes = (bits.cols * bits.rows + 8 - 1) / 8;
|
||||
|
||||
Mat candidateByteList(1, nbytes, CV_8UC4, Scalar::all(0));
|
||||
unsigned char currentBit = 0;
|
||||
int currentByte = 0;
|
||||
|
||||
// the 4 rotations
|
||||
uchar* rot0 = candidateByteList.ptr();
|
||||
uchar* rot1 = candidateByteList.ptr() + 1*nbytes;
|
||||
uchar* rot2 = candidateByteList.ptr() + 2*nbytes;
|
||||
uchar* rot3 = candidateByteList.ptr() + 3*nbytes;
|
||||
|
||||
for(int row = 0; row < bits.rows; row++) {
|
||||
for(int col = 0; col < bits.cols; col++) {
|
||||
// circular shift
|
||||
rot0[currentByte] <<= 1;
|
||||
rot1[currentByte] <<= 1;
|
||||
rot2[currentByte] <<= 1;
|
||||
rot3[currentByte] <<= 1;
|
||||
// set bit
|
||||
rot0[currentByte] |= bits.at<uchar>(row, col);
|
||||
rot1[currentByte] |= bits.at<uchar>(col, bits.cols - 1 - row);
|
||||
rot2[currentByte] |= bits.at<uchar>(bits.rows - 1 - row, bits.cols - 1 - col);
|
||||
rot3[currentByte] |= bits.at<uchar>(bits.rows - 1 - col, row);
|
||||
currentBit++;
|
||||
if(currentBit == 8) {
|
||||
// next byte
|
||||
currentBit = 0;
|
||||
currentByte++;
|
||||
}
|
||||
}
|
||||
}
|
||||
return candidateByteList;
|
||||
}
|
||||
|
||||
|
||||
Mat Dictionary::getBitsFromByteList(const Mat &byteList, int markerSize) {
|
||||
CV_Assert(byteList.total() > 0 &&
|
||||
byteList.total() >= (unsigned int)markerSize * markerSize / 8 &&
|
||||
byteList.total() <= (unsigned int)markerSize * markerSize / 8 + 1);
|
||||
Mat bits(markerSize, markerSize, CV_8UC1, Scalar::all(0));
|
||||
|
||||
unsigned char base2List[] = { 128, 64, 32, 16, 8, 4, 2, 1 };
|
||||
int currentByteIdx = 0;
|
||||
// we only need the bytes in normal rotation
|
||||
unsigned char currentByte = byteList.ptr()[0];
|
||||
int currentBit = 0;
|
||||
for(int row = 0; row < bits.rows; row++) {
|
||||
for(int col = 0; col < bits.cols; col++) {
|
||||
if(currentByte >= base2List[currentBit]) {
|
||||
bits.at<unsigned char>(row, col) = 1;
|
||||
currentByte -= base2List[currentBit];
|
||||
}
|
||||
currentBit++;
|
||||
if(currentBit == 8) {
|
||||
currentByteIdx++;
|
||||
currentByte = byteList.ptr()[currentByteIdx];
|
||||
// if not enough bits for one more byte, we are in the end
|
||||
// update bit position accordingly
|
||||
if(8 * (currentByteIdx + 1) > (int)bits.total())
|
||||
currentBit = 8 * (currentByteIdx + 1) - (int)bits.total();
|
||||
else
|
||||
currentBit = 0; // ok, bits enough for next byte
|
||||
}
|
||||
}
|
||||
}
|
||||
return bits;
|
||||
}
|
||||
|
||||
|
||||
Dictionary getPredefinedDictionary(PredefinedDictionaryType name) {
|
||||
// DictionaryData constructors calls
|
||||
// moved out of globals so construted on first use, which allows lazy-loading of opencv dll
|
||||
static const Dictionary DICT_ARUCO_DATA = Dictionary(Mat(1024, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_ARUCO_BYTES), 5, 0);
|
||||
|
||||
static const Dictionary DICT_4X4_50_DATA = Dictionary(Mat(50, (4 * 4 + 7) / 8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
|
||||
static const Dictionary DICT_4X4_100_DATA = Dictionary(Mat(100, (4 * 4 + 7) / 8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
|
||||
static const Dictionary DICT_4X4_250_DATA = Dictionary(Mat(250, (4 * 4 + 7) / 8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 1);
|
||||
static const Dictionary DICT_4X4_1000_DATA = Dictionary(Mat(1000, (4 * 4 + 7) / 8, CV_8UC4, (uchar*)DICT_4X4_1000_BYTES), 4, 0);
|
||||
|
||||
static const Dictionary DICT_5X5_50_DATA = Dictionary(Mat(50, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 3);
|
||||
static const Dictionary DICT_5X5_100_DATA = Dictionary(Mat(100, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 3);
|
||||
static const Dictionary DICT_5X5_250_DATA = Dictionary(Mat(250, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 2);
|
||||
static const Dictionary DICT_5X5_1000_DATA = Dictionary(Mat(1000, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_5X5_1000_BYTES), 5, 2);
|
||||
|
||||
static const Dictionary DICT_6X6_50_DATA = Dictionary(Mat(50, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 6);
|
||||
static const Dictionary DICT_6X6_100_DATA = Dictionary(Mat(100, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 5);
|
||||
static const Dictionary DICT_6X6_250_DATA = Dictionary(Mat(250, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 5);
|
||||
static const Dictionary DICT_6X6_1000_DATA = Dictionary(Mat(1000, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_6X6_1000_BYTES), 6, 4);
|
||||
|
||||
static const Dictionary DICT_7X7_50_DATA = Dictionary(Mat(50, (7 * 7 + 7) / 8, CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 9);
|
||||
static const Dictionary DICT_7X7_100_DATA = Dictionary(Mat(100, (7 * 7 + 7) / 8, CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 8);
|
||||
static const Dictionary DICT_7X7_250_DATA = Dictionary(Mat(250, (7 * 7 + 7) / 8, CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 8);
|
||||
static const Dictionary DICT_7X7_1000_DATA = Dictionary(Mat(1000, (7 * 7 + 7) / 8, CV_8UC4, (uchar*)DICT_7X7_1000_BYTES), 7, 6);
|
||||
|
||||
static const Dictionary DICT_APRILTAG_16h5_DATA = Dictionary(Mat(30, (4 * 4 + 7) / 8, CV_8UC4, (uchar*)DICT_APRILTAG_16h5_BYTES), 4, 0);
|
||||
static const Dictionary DICT_APRILTAG_25h9_DATA = Dictionary(Mat(35, (5 * 5 + 7) / 8, CV_8UC4, (uchar*)DICT_APRILTAG_25h9_BYTES), 5, 0);
|
||||
static const Dictionary DICT_APRILTAG_36h10_DATA = Dictionary(Mat(2320, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_APRILTAG_36h10_BYTES), 6, 0);
|
||||
static const Dictionary DICT_APRILTAG_36h11_DATA = Dictionary(Mat(587, (6 * 6 + 7) / 8, CV_8UC4, (uchar*)DICT_APRILTAG_36h11_BYTES), 6, 0);
|
||||
|
||||
switch(name) {
|
||||
|
||||
case DICT_ARUCO_ORIGINAL:
|
||||
return Dictionary(DICT_ARUCO_DATA);
|
||||
|
||||
case DICT_4X4_50:
|
||||
return Dictionary(DICT_4X4_50_DATA);
|
||||
case DICT_4X4_100:
|
||||
return Dictionary(DICT_4X4_100_DATA);
|
||||
case DICT_4X4_250:
|
||||
return Dictionary(DICT_4X4_250_DATA);
|
||||
case DICT_4X4_1000:
|
||||
return Dictionary(DICT_4X4_1000_DATA);
|
||||
|
||||
case DICT_5X5_50:
|
||||
return Dictionary(DICT_5X5_50_DATA);
|
||||
case DICT_5X5_100:
|
||||
return Dictionary(DICT_5X5_100_DATA);
|
||||
case DICT_5X5_250:
|
||||
return Dictionary(DICT_5X5_250_DATA);
|
||||
case DICT_5X5_1000:
|
||||
return Dictionary(DICT_5X5_1000_DATA);
|
||||
|
||||
case DICT_6X6_50:
|
||||
return Dictionary(DICT_6X6_50_DATA);
|
||||
case DICT_6X6_100:
|
||||
return Dictionary(DICT_6X6_100_DATA);
|
||||
case DICT_6X6_250:
|
||||
return Dictionary(DICT_6X6_250_DATA);
|
||||
case DICT_6X6_1000:
|
||||
return Dictionary(DICT_6X6_1000_DATA);
|
||||
|
||||
case DICT_7X7_50:
|
||||
return Dictionary(DICT_7X7_50_DATA);
|
||||
case DICT_7X7_100:
|
||||
return Dictionary(DICT_7X7_100_DATA);
|
||||
case DICT_7X7_250:
|
||||
return Dictionary(DICT_7X7_250_DATA);
|
||||
case DICT_7X7_1000:
|
||||
return Dictionary(DICT_7X7_1000_DATA);
|
||||
|
||||
case DICT_APRILTAG_16h5:
|
||||
return Dictionary(DICT_APRILTAG_16h5_DATA);
|
||||
case DICT_APRILTAG_25h9:
|
||||
return Dictionary(DICT_APRILTAG_25h9_DATA);
|
||||
case DICT_APRILTAG_36h10:
|
||||
return Dictionary(DICT_APRILTAG_36h10_DATA);
|
||||
case DICT_APRILTAG_36h11:
|
||||
return Dictionary(DICT_APRILTAG_36h11_DATA);
|
||||
|
||||
}
|
||||
return Dictionary(DICT_4X4_50_DATA);
|
||||
}
|
||||
|
||||
|
||||
Dictionary getPredefinedDictionary(int dict) {
|
||||
return getPredefinedDictionary(PredefinedDictionaryType(dict));
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Generates a random marker Mat of size markerSize x markerSize
|
||||
*/
|
||||
static Mat _generateRandomMarker(int markerSize, RNG &rng) {
|
||||
Mat marker(markerSize, markerSize, CV_8UC1, Scalar::all(0));
|
||||
for(int i = 0; i < markerSize; i++) {
|
||||
for(int j = 0; j < markerSize; j++) {
|
||||
unsigned char bit = (unsigned char) (rng.uniform(0,2));
|
||||
marker.at<unsigned char>(i, j) = bit;
|
||||
}
|
||||
}
|
||||
return marker;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Calculate selfDistance of the codification of a marker Mat. Self distance is the Hamming
|
||||
* distance of the marker to itself in the other rotations.
|
||||
* See S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
|
||||
* "Automatic generation and detection of highly reliable fiducial markers under occlusion".
|
||||
* Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
|
||||
*/
|
||||
static int _getSelfDistance(const Mat &marker) {
|
||||
Mat bytes = Dictionary::getByteListFromBits(marker);
|
||||
int minHamming = (int)marker.total() + 1;
|
||||
for(int r = 1; r < 4; r++) {
|
||||
int currentHamming = cv::hal::normHamming(bytes.ptr(), bytes.ptr() + bytes.cols*r, bytes.cols);
|
||||
if(currentHamming < minHamming) minHamming = currentHamming;
|
||||
}
|
||||
return minHamming;
|
||||
}
|
||||
|
||||
|
||||
Dictionary extendDictionary(int nMarkers, int markerSize, const Dictionary &baseDictionary, int randomSeed) {
|
||||
RNG rng((uint64)(randomSeed));
|
||||
|
||||
Dictionary out = Dictionary(Mat(), markerSize);
|
||||
out.markerSize = markerSize;
|
||||
|
||||
// theoretical maximum intermarker distance
|
||||
// See S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 2014.
|
||||
// "Automatic generation and detection of highly reliable fiducial markers under occlusion".
|
||||
// Pattern Recogn. 47, 6 (June 2014), 2280-2292. DOI=10.1016/j.patcog.2014.01.005
|
||||
int C = (int)std::floor(float(markerSize * markerSize) / 4.f);
|
||||
int tau = 2 * (int)std::floor(float(C) * 4.f / 3.f);
|
||||
|
||||
// if baseDictionary is provided, calculate its intermarker distance
|
||||
if(baseDictionary.bytesList.rows > 0) {
|
||||
CV_Assert(baseDictionary.markerSize == markerSize);
|
||||
out.bytesList = baseDictionary.bytesList.clone();
|
||||
|
||||
int minDistance = markerSize * markerSize + 1;
|
||||
for(int i = 0; i < out.bytesList.rows; i++) {
|
||||
Mat markerBytes = out.bytesList.rowRange(i, i + 1);
|
||||
Mat markerBits = Dictionary::getBitsFromByteList(markerBytes, markerSize);
|
||||
minDistance = min(minDistance, _getSelfDistance(markerBits));
|
||||
for(int j = i + 1; j < out.bytesList.rows; j++) {
|
||||
minDistance = min(minDistance, out.getDistanceToId(markerBits, j));
|
||||
}
|
||||
}
|
||||
tau = minDistance;
|
||||
}
|
||||
|
||||
// current best option
|
||||
int bestTau = 0;
|
||||
Mat bestMarker;
|
||||
|
||||
// after these number of unproductive iterations, the best option is accepted
|
||||
const int maxUnproductiveIterations = 5000;
|
||||
int unproductiveIterations = 0;
|
||||
|
||||
while(out.bytesList.rows < nMarkers) {
|
||||
Mat currentMarker = _generateRandomMarker(markerSize, rng);
|
||||
|
||||
int selfDistance = _getSelfDistance(currentMarker);
|
||||
int minDistance = selfDistance;
|
||||
|
||||
// if self distance is better or equal than current best option, calculate distance
|
||||
// to previous accepted markers
|
||||
if(selfDistance >= bestTau) {
|
||||
for(int i = 0; i < out.bytesList.rows; i++) {
|
||||
int currentDistance = out.getDistanceToId(currentMarker, i);
|
||||
minDistance = min(currentDistance, minDistance);
|
||||
if(minDistance <= bestTau) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// if distance is high enough, accept the marker
|
||||
if(minDistance >= tau) {
|
||||
unproductiveIterations = 0;
|
||||
bestTau = 0;
|
||||
Mat bytes = Dictionary::getByteListFromBits(currentMarker);
|
||||
out.bytesList.push_back(bytes);
|
||||
} else {
|
||||
unproductiveIterations++;
|
||||
|
||||
// if distance is not enough, but is better than the current best option
|
||||
if(minDistance > bestTau) {
|
||||
bestTau = minDistance;
|
||||
bestMarker = currentMarker;
|
||||
}
|
||||
|
||||
// if number of unproductive iterarions has been reached, accept the current best option
|
||||
if(unproductiveIterations == maxUnproductiveIterations) {
|
||||
unproductiveIterations = 0;
|
||||
tau = bestTau;
|
||||
bestTau = 0;
|
||||
Mat bytes = Dictionary::getByteListFromBits(bestMarker);
|
||||
out.bytesList.push_back(bytes);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update the maximum number of correction bits for the generated dictionary
|
||||
out.maxCorrectionBits = (tau - 1) / 2;
|
||||
|
||||
return out;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
// 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
|
||||
|
||||
#include "../precomp.hpp"
|
||||
#include "aruco_utils.hpp"
|
||||
|
||||
namespace cv {
|
||||
namespace aruco {
|
||||
using namespace std;
|
||||
|
||||
void _copyVector2Output(vector<vector<Point2f> > &vec, OutputArrayOfArrays out, const float scale) {
|
||||
out.create((int)vec.size(), 1, CV_32FC2);
|
||||
if(out.isMatVector()) {
|
||||
for (unsigned int i = 0; i < vec.size(); i++) {
|
||||
out.create(4, 1, CV_32FC2, i);
|
||||
Mat &m = out.getMatRef(i);
|
||||
Mat(Mat(vec[i]).t()*scale).copyTo(m);
|
||||
}
|
||||
}
|
||||
else if(out.isUMatVector()) {
|
||||
for (unsigned int i = 0; i < vec.size(); i++) {
|
||||
out.create(4, 1, CV_32FC2, i);
|
||||
UMat &m = out.getUMatRef(i);
|
||||
Mat(Mat(vec[i]).t()*scale).copyTo(m);
|
||||
}
|
||||
}
|
||||
else if(out.kind() == _OutputArray::STD_VECTOR_VECTOR){
|
||||
for (unsigned int i = 0; i < vec.size(); i++) {
|
||||
out.create(4, 1, CV_32FC2, i);
|
||||
Mat m = out.getMat(i);
|
||||
Mat(Mat(vec[i]).t()*scale).copyTo(m);
|
||||
}
|
||||
}
|
||||
else {
|
||||
CV_Error(cv::Error::StsNotImplemented,
|
||||
"Only Mat vector, UMat vector, and vector<vector> OutputArrays are currently supported.");
|
||||
}
|
||||
}
|
||||
|
||||
void _convertToGrey(InputArray _in, OutputArray _out) {
|
||||
CV_Assert(_in.type() == CV_8UC1 || _in.type() == CV_8UC3);
|
||||
if(_in.type() == CV_8UC3)
|
||||
cvtColor(_in, _out, COLOR_BGR2GRAY);
|
||||
else
|
||||
_in.copyTo(_out);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
// 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
|
||||
#ifndef __OPENCV_OBJDETECT_ARUCO_UTILS_HPP__
|
||||
#define __OPENCV_OBJDETECT_ARUCO_UTILS_HPP__
|
||||
|
||||
#include <opencv2/core.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace cv {
|
||||
namespace aruco {
|
||||
|
||||
/**
|
||||
* @brief Copy the contents of a corners vector to an OutputArray, settings its size.
|
||||
*/
|
||||
void _copyVector2Output(std::vector<std::vector<Point2f> > &vec, OutputArrayOfArrays out, const float scale = 1.f);
|
||||
|
||||
/**
|
||||
* @brief Convert input image to gray if it is a 3-channels image
|
||||
*/
|
||||
void _convertToGrey(InputArray _in, OutputArray _out);
|
||||
|
||||
template<typename T>
|
||||
inline bool readParameter(const std::string& name, T& parameter, const FileNode& node)
|
||||
{
|
||||
if (!node.empty() && !node[name].empty()) {
|
||||
node[name] >> parameter;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
inline bool readWriteParameter(const std::string& name, T& parameter, const FileNode& readNode, FileStorage& writeStorage) {
|
||||
if (!readNode.empty())
|
||||
return readParameter(name, parameter, readNode);
|
||||
writeStorage << name << parameter;
|
||||
return true;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user