Skip to content

Commit

Permalink
move april module
Browse files Browse the repository at this point in the history
  • Loading branch information
AleksandrPanov committed Apr 28, 2022
1 parent f6a39c5 commit e0791b3
Show file tree
Hide file tree
Showing 9 changed files with 132 additions and 146 deletions.
119 changes: 118 additions & 1 deletion modules/aruco/src/apriltag_quad_thresh.cpp → ...uco/src/apriltag/apriltag_quad_thresh.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@
// because we use a fixed-point 16 bit integer representation with one
// fractional bit.

#include "precomp.hpp"
#include "../precomp.hpp"
#include "apriltag_quad_thresh.hpp"

//#define APRIL_DEBUG
Expand Down Expand Up @@ -1567,4 +1567,121 @@ imwrite("2.5 debug_lines.pnm", out);
return quads;
}

void _apriltag(Mat im_orig, const Ptr<DetectorParameters> & _params, std::vector< std::vector< Point2f > > &candidates,
std::vector< std::vector< Point > > &contours){

///////////////////////////////////////////////////////////
/// Step 1. Detect quads according to requested image decimation
/// and blurring parameters.
Mat quad_im;
im_orig.copyTo(quad_im);

if (_params->aprilTagQuadDecimate > 1){
resize(im_orig, quad_im, Size(), 1/_params->aprilTagQuadDecimate, 1/_params->aprilTagQuadDecimate, INTER_AREA);
}

// Apply a Blur
if (_params->aprilTagQuadSigma != 0) {
// compute a reasonable kernel width by figuring that the
// kernel should go out 2 std devs.
//
// max sigma ksz
// 0.499 1 (disabled)
// 0.999 3
// 1.499 5
// 1.999 7

float sigma = fabsf((float) _params->aprilTagQuadSigma);

int ksz = cvFloor(4 * sigma); // 2 std devs in each direction
ksz |= 1; // make odd number

if (ksz > 1) {
if (_params->aprilTagQuadSigma > 0)
GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);
else {
Mat orig;
quad_im.copyTo(orig);
GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);

// SHARPEN the image by subtracting the low frequency components.
for (int y = 0; y < orig.rows; y++) {
for (int x = 0; x < orig.cols; x++) {
int vorig = orig.data[y*orig.step + x];
int vblur = quad_im.data[y*quad_im.step + x];

int v = 2*vorig - vblur;
if (v < 0)
v = 0;
if (v > 255)
v = 255;

quad_im.data[y*quad_im.step + x] = (uint8_t) v;
}
}
}
}
}

#ifdef APRIL_DEBUG
imwrite("1.1 debug_preprocess.pnm", quad_im);
#endif

///////////////////////////////////////////////////////////
/// Step 2. do the Threshold :: get the set of candidate quads
zarray_t *quads = apriltag_quad_thresh(_params, quad_im, contours);

CV_Assert(quads != NULL);

// adjust centers of pixels so that they correspond to the
// original full-resolution image.
if (_params->aprilTagQuadDecimate > 1) {
for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *q;
_zarray_get_volatile(quads, i, &q);
for (int j = 0; j < 4; j++) {
q->p[j][0] *= _params->aprilTagQuadDecimate;
q->p[j][1] *= _params->aprilTagQuadDecimate;
}
}
}

#ifdef APRIL_DEBUG
Mat im_quads = im_orig.clone();
im_quads = im_quads*0.5;
srandom(0);

for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *quad;
_zarray_get_volatile(quads, i, &quad);

const int bias = 100;
int color = bias + (random() % (255-bias));

line(im_quads, Point(quad->p[0][0], quad->p[0][1]), Point(quad->p[1][0], quad->p[1][1]), color, 1);
line(im_quads, Point(quad->p[1][0], quad->p[1][1]), Point(quad->p[2][0], quad->p[2][1]), color, 1);
line(im_quads, Point(quad->p[2][0], quad->p[2][1]), Point(quad->p[3][0], quad->p[3][1]), color, 1);
line(im_quads, Point(quad->p[3][0], quad->p[3][1]), Point(quad->p[0][0], quad->p[0][1]), color, 1);
}
imwrite("1.2 debug_quads_raw.pnm", im_quads);
#endif

////////////////////////////////////////////////////////////////
/// Step 3. Save the output :: candidate corners
for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *quad;
_zarray_get_volatile(quads, i, &quad);

std::vector< Point2f > corners;
corners.push_back(Point2f(quad->p[3][0], quad->p[3][1])); //pA
corners.push_back(Point2f(quad->p[0][0], quad->p[0][1])); //pB
corners.push_back(Point2f(quad->p[1][0], quad->p[1][1])); //pC
corners.push_back(Point2f(quad->p[2][0], quad->p[2][1])); //pD

candidates.push_back(corners);
}

_zarray_destroy(quads);
}

}}
11 changes: 11 additions & 0 deletions modules/aruco/src/apriltag_quad_thresh.hpp → ...uco/src/apriltag/apriltag_quad_thresh.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#ifndef _OPENCV_APRIL_QUAD_THRESH_HPP_
#define _OPENCV_APRIL_QUAD_THRESH_HPP_

#include <opencv2/imgproc.hpp>
#include "opencv2/aruco.hpp"
#include "unionfind.hpp"
#include "zmaxheap.hpp"
Expand Down Expand Up @@ -121,5 +122,15 @@ void threshold(const Mat mIm, const Ptr<DetectorParameters> &parameters, Mat& mT
*/
zarray_t *apriltag_quad_thresh(const Ptr<DetectorParameters> &parameters, const Mat & mImg, std::vector< std::vector< Point > > &contours);

/**
*
* @param im_orig
* @param _params
* @param candidates
* @param contours
*/
void _apriltag(Mat im_orig, const Ptr<DetectorParameters> & _params, std::vector< std::vector< Point2f > > &candidates,
std::vector< std::vector< Point > > &contours);

}}
#endif
0 .../src/predefined_dictionaries_apriltag.hpp → ...ltag/predefined_dictionaries_apriltag.hpp
View file
File renamed without changes.
0 modules/aruco/src/unionfind.hpp → modules/aruco/src/apriltag/unionfind.hpp
View file
File renamed without changes.
0 modules/aruco/src/zarray.hpp → modules/aruco/src/apriltag/zarray.hpp
View file
File renamed without changes.
2 changes: 1 addition & 1 deletion modules/aruco/src/zmaxheap.cpp → modules/aruco/src/apriltag/zmaxheap.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
// 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 "../precomp.hpp"
#include "zmaxheap.hpp"


Expand Down
0 modules/aruco/src/zmaxheap.hpp → modules/aruco/src/apriltag/zmaxheap.hpp
View file
File renamed without changes.
144 changes: 1 addition & 143 deletions modules/aruco/src/aruco.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,19 +38,11 @@ the use of this software, even if advised of the possibility of such damage.

#include "precomp.hpp"
#include "opencv2/aruco.hpp"
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>

#include "apriltag_quad_thresh.hpp"
#include "zarray.hpp"
#include "apriltag/apriltag_quad_thresh.hpp"

#include <cmath>

//#define APRIL_DEBUG
#ifdef APRIL_DEBUG
#include "opencv2/imgcodecs.hpp"
#endif

namespace cv {
namespace aruco {

Expand Down Expand Up @@ -919,140 +911,6 @@ static void _refineCandidateLines(std::vector<Point>& nContours, std::vector<Poi
}
}

#ifdef APRIL_DEBUG
static void _darken(const Mat &im){
for (int y = 0; y < im.rows; y++) {
for (int x = 0; x < im.cols; x++) {
im.data[im.cols*y+x] /= 2;
}
}
}
#endif

/**
*
* @param im_orig
* @param _params
* @param candidates
* @param contours
*/
static void _apriltag(Mat im_orig, const Ptr<DetectorParameters> & _params, std::vector< std::vector< Point2f > > &candidates,
std::vector< std::vector< Point > > &contours){

///////////////////////////////////////////////////////////
/// Step 1. Detect quads according to requested image decimation
/// and blurring parameters.
Mat quad_im;
im_orig.copyTo(quad_im);

if (_params->aprilTagQuadDecimate > 1){
resize(im_orig, quad_im, Size(), 1/_params->aprilTagQuadDecimate, 1/_params->aprilTagQuadDecimate, INTER_AREA );
}

// Apply a Blur
if (_params->aprilTagQuadSigma != 0) {
// compute a reasonable kernel width by figuring that the
// kernel should go out 2 std devs.
//
// max sigma ksz
// 0.499 1 (disabled)
// 0.999 3
// 1.499 5
// 1.999 7

float sigma = fabsf((float) _params->aprilTagQuadSigma);

int ksz = cvFloor(4 * sigma); // 2 std devs in each direction
ksz |= 1; // make odd number

if (ksz > 1) {
if (_params->aprilTagQuadSigma > 0)
GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);
else {
Mat orig;
quad_im.copyTo(orig);
GaussianBlur(quad_im, quad_im, Size(ksz, ksz), sigma, sigma, BORDER_REPLICATE);

// SHARPEN the image by subtracting the low frequency components.
for (int y = 0; y < orig.rows; y++) {
for (int x = 0; x < orig.cols; x++) {
int vorig = orig.data[y*orig.step + x];
int vblur = quad_im.data[y*quad_im.step + x];

int v = 2*vorig - vblur;
if (v < 0)
v = 0;
if (v > 255)
v = 255;

quad_im.data[y*quad_im.step + x] = (uint8_t) v;
}
}
}
}
}

#ifdef APRIL_DEBUG
imwrite("1.1 debug_preprocess.pnm", quad_im);
#endif

///////////////////////////////////////////////////////////
/// Step 2. do the Threshold :: get the set of candidate quads
zarray_t *quads = apriltag_quad_thresh(_params, quad_im, contours);

CV_Assert(quads != NULL);

// adjust centers of pixels so that they correspond to the
// original full-resolution image.
if (_params->aprilTagQuadDecimate > 1) {
for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *q;
_zarray_get_volatile(quads, i, &q);
for (int j = 0; j < 4; j++) {
q->p[j][0] *= _params->aprilTagQuadDecimate;
q->p[j][1] *= _params->aprilTagQuadDecimate;
}
}
}

#ifdef APRIL_DEBUG
Mat im_quads = im_orig.clone();
im_quads = im_quads*0.5;
srandom(0);

for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *quad;
_zarray_get_volatile(quads, i, &quad);

const int bias = 100;
int color = bias + (random() % (255-bias));

line(im_quads, Point(quad->p[0][0], quad->p[0][1]), Point(quad->p[1][0], quad->p[1][1]), color, 1);
line(im_quads, Point(quad->p[1][0], quad->p[1][1]), Point(quad->p[2][0], quad->p[2][1]), color, 1);
line(im_quads, Point(quad->p[2][0], quad->p[2][1]), Point(quad->p[3][0], quad->p[3][1]), color, 1);
line(im_quads, Point(quad->p[3][0], quad->p[3][1]), Point(quad->p[0][0], quad->p[0][1]), color, 1);
}
imwrite("1.2 debug_quads_raw.pnm", im_quads);
#endif

////////////////////////////////////////////////////////////////
/// Step 3. Save the output :: candidate corners
for (int i = 0; i < _zarray_size(quads); i++) {
struct sQuad *quad;
_zarray_get_volatile(quads, i, &quad);

std::vector< Point2f > corners;
corners.push_back(Point2f(quad->p[3][0], quad->p[3][1])); //pA
corners.push_back(Point2f(quad->p[0][0], quad->p[0][1])); //pB
corners.push_back(Point2f(quad->p[1][0], quad->p[1][1])); //pC
corners.push_back(Point2f(quad->p[2][0], quad->p[2][1])); //pD

candidates.push_back(corners);
}

_zarray_destroy(quads);
}

static inline void findCornerInPyrImage(const float scale_init, const int closest_pyr_image_idx,
const std::vector<cv::Mat>& grey_pyramid, Mat corners,
const Ptr<DetectorParameters>& params) {
Expand Down
2 changes: 1 addition & 1 deletion modules/aruco/src/dictionary.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,7 +41,7 @@ the use of this software, even if advised of the possibility of such damage.
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include "predefined_dictionaries.hpp"
#include "predefined_dictionaries_apriltag.hpp"
#include "apriltag/predefined_dictionaries_apriltag.hpp"
#include "opencv2/core/hal/hal.hpp"

namespace cv {
Expand Down

0 comments on commit e0791b3

Please sign in to comment.