18 #ifndef PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP 19 #define PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP 21 #include "../filtering/filter_luminance.hpp" 22 #include "../filtering/filter_bilateral_2ds.hpp" 24 #include "../computer_vision/iterative_closest_point_2D.hpp" 25 #include "../computer_vision/nelder_mead_opt_ICP_2D.hpp" 27 #include "../features_matching/harris_corner_detector.hpp" 28 #include "../features_matching/orb_descriptor.hpp" 30 #include "../util/rasterizer.hpp" 31 #include "../util/eigen_util.hpp" 33 #ifndef PIC_DISABLE_EIGEN 35 #ifndef PIC_EIGEN_NOT_BUNDLED 36 #include "../externals/Eigen/Dense" 37 #include "../externals/Eigen/SVD" 38 #include "../externals/Eigen/Geometry" 40 #include <Eigen/Dense> 42 #include <Eigen/Geometry> 54 #ifndef PIC_DISABLE_EIGEN 59 for(
unsigned int i = 0; i < points.size(); i++) {
63 for(
unsigned int j = 0; j < points.size(); j++) {
68 auto delta_ij = p_i - points[j];
69 float dist = delta_ij.norm();
87 if(points.size() < 2) {
93 int n = int(points.size());
95 std::vector<float> m_d;
96 for(
int i = 0; i < n; i++) {
99 float closest = FLT_MAX;
101 for(
int j = 0; j < n; j++) {
106 auto delta_ij = p_i - points[j];
108 float dist = delta_ij.norm();
115 if(closest < FLT_MAX) {
116 m_d.push_back(closest);
121 std::sort(m_d.begin(), m_d.end());
123 ret = m_d[m_d.size() / 2];
136 if(points.size() < 2) {
142 int n = int(points.size());
144 std::vector<float> m_d;
145 for(
int i = 0; i < n; i++) {
146 auto p_i = points[i];
148 float c[] = {FLT_MAX, FLT_MAX, FLT_MAX};
149 int ci[] = {-1, -1, -1};
151 for(
int j = 0; j < n; j++) {
156 auto delta_ij = p_i - points[j];
158 float dist = delta_ij.norm();
176 Eigen::Vector2f v0 = (points[ci[0]] - p_i) / c[0];
177 Eigen::Vector2f v1 = (points[ci[1]] - p_i) / c[1];
178 Eigen::Vector2f v2 = (points[ci[2]] - p_i) / c[2];
180 float v01 = fabsf(v0.dot(v1));
181 float v02 = fabsf(v0.dot(v2));
182 float v12 = fabsf(v1.dot(v2));
184 if((v01 < 0.1f) || (v02 < 0.1f) || (v12 < 0.1f))
191 std::sort(m_d.begin(), m_d.end());
193 ret = m_d[m_d.size() / 2];
209 Image *ret =
new Image(1, (checkers_x + 1) * checkers_size, (checkers_y + 1) * checkers_size, 1);
212 for(
int i = 1; i <= checkers_y; i++) {
213 Eigen::Vector2f point;
215 int y = i * checkers_size;
218 for(
int j = 1; j <= checkers_x; j++) {
220 int x = j * checkers_size;
225 if(((j % 2) == 0) && ((i % 2) == 0)) {
231 if(((j % 2) == 1) && ((i % 2) == 1)) {
237 for(
int yy = y; yy < (y + checkers_size); yy++) {
238 for(
int xx = x; xx < (x + checkers_size); xx++) {
239 float *pixel_value = (*ret)(xx, yy);
240 pixel_value[0] = 0.0f;
245 out.push_back(point);
261 corners_model.clear();
265 printf(
"Extracting corners...\n");
270 std::vector< Eigen::Vector2f > corners_from_img;
271 hcd.
execute(img, &corners_from_img);
281 float red[] = {1.0f, 0.0f, 0.0f};
282 float green[] = {0.0f, 1.0f, 0.0f};
283 float blue[] = {1.0f, 0.0f, 1.0f};
284 float yellow[] = {1.0f, 1.0f, 0.0f};
289 std::vector< Eigen::Vector2f > cfi_out;
303 std::vector< Eigen::Vector2f > cfi_valid2;
304 auto n = cfi_out.size();
305 for(
unsigned int i = 0; i < n; i++) {
306 auto p_i = cfi_out[i];
310 for(
unsigned int j = 0; j < n; j++) {
312 auto delta_ij = p_i - cfi_out[j];
313 float dist = delta_ij.norm();
315 if(dist < (checker_size)) {
323 cfi_valid2.push_back(p_i);
331 std::vector< Eigen::Vector2f > cfi_valid;
332 n = cfi_valid2.size();
333 for(
unsigned int i = 0; i < n; i++) {
334 auto p_i = cfi_valid2[i];
337 for(
unsigned int j = 0; j < n; j++) {
339 auto delta_ij = p_i - cfi_valid2[j];
340 float t_dist = delta_ij.norm();
348 if(dist < (checker_size * 3)) {
349 cfi_valid.push_back(p_i);
354 printf(
"Checker size: %f\n", checker_size);
361 printf(
"Re-fit Checker size: %f\n", checker_size);
365 int checkers_size = 32;
372 std::vector< unsigned int *> descs_model, descs_cfi_valid;
373 b_desc.
getAll(img_pattern, corners_model, descs_model);
374 b_desc.
getAll(img, cfi_valid, descs_cfi_valid);
378 float scaling_factor = checker_size / min_dist;
381 t_init.
scale = scaling_factor;
382 t_init.
applyC(corners_model);
395 float prev_err = FLT_MAX;
396 float *x =
new float[3];
399 float *tmp =
new float[4];
400 for(
float i = 0; i < nSample; i++) {
401 float angle = float(i) *
C_PI_2 / float(nSample);
402 float start[] = {0.0f, 0.0f, angle};
403 opt.
run(start, 3, 1e-9f, 100, tmp);
406 memcpy(x, tmp,
sizeof(
float) * 3);
412 for(
int i = 0; i < 4; i++) {
413 printf(
"%f\n", x[i]);
417 float start[] = {x[0], x[1], x[2], 1.0f};
418 opt.
run(start, 4, 1e-12f, 100, tmp);
422 for(
int i = 0; i < 4; i++) {
423 printf(
"%f\n", tmp[i]);
431 img_wb->
Write(
"../data/output/img_wb.bmp");
448 if(corners_model.size() < 8) {
453 auto p_0 = corners_model[selected];
457 for(
auto j = 0; j < corners_model.size(); j++) {
459 auto delta_ij = p_0 - corners_model[j];
460 float dist = delta_ij.norm();
470 p1 = corners_model[closest];
485 Eigen::Vector2f ret(-1.0f, -1.0f);
487 if(img == NULL || corners_model.empty()) {
493 for(
int i = 0; i < (checkerBoardSizeY -1) ; i++) {
494 for(
int j = 0; j < (checkerBoardSizeX - 1); j++) {
496 int ind0 = (i * checkerBoardSizeX) + j;
497 int ind1 = (i + 1) * checkerBoardSizeX + j + 1;
499 auto p0 = corners_model[ind0];
500 auto p1 = corners_model[ind1];
502 auto pMid = (p0 + p1) / 2.0f;
504 int x = int(pMid[0]);
505 int y = int(pMid[1]);
506 float *color = (*img)(x, y);
508 float meanColor = 0.0f;
509 for(
auto c = 0; c < img->
channels; c++) {
510 meanColor += color[c];
513 if(meanColor > maxVal) {
535 if(point[0] >= 0.0f && point[1] >= 0.0f) {
537 float *ret =
new float[img->
channels];
538 float *color = (*img)(int(point[0]), int(point[1]));
539 memcpy(ret, color, img->
channels *
sizeof(
float));
551 #endif // PIC_COMPUTER_VISION_FIND_CHECKER_BOARD_HPP Scalar output_error
Definition: nelder_mead_opt_base.hpp:296
int channels
Definition: image.hpp:80
static void removeClosestCorners(std::vector< Eigen::Vector2f > *corners, std::vector< Eigen::Vector2f > *out, float threshold, int max_limit)
removeClosestCorners
Definition: general_corner_detector.hpp:195
float * getMeanVal(BBox *box, float *ret)
getMeanVal computes the mean for the current Image.
PIC_INLINE float estimateCheckerBoardSizeCross(std::vector< Eigen::Vector2f > &points)
estimateCheckerBoardSizeCross
Definition: find_checker_board.hpp:134
void execute(Image *img, std::vector< Eigen::Vector2f > *corners)
execute
Definition: harris_corner_detector.hpp:137
PIC_INLINE float estimateCheckerBoardSize(std::vector< Eigen::Vector2f > &points)
estimateCheckerBoardSize
Definition: find_checker_board.hpp:85
virtual Image * Process(ImageVec imgIn, Image *imgOut)
Process.
Definition: filter.hpp:390
const float C_PI_2
Definition: math.hpp:52
PIC_INLINE float * estimateWhitePointFromCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX=4, int checkerBoardSizeY=6)
estimateWhitePointFromCheckerBoard
Definition: find_checker_board.hpp:531
PIC_INLINE Eigen::Vector2f estimateCoordinatesWhitePointFromCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX=4, int checkerBoardSizeY=6)
estimateCoordinatesWhitePointFromCheckerBoard
Definition: find_checker_board.hpp:483
PIC_INLINE float getMinDistance(std::vector< Eigen::Vector2f > &points)
getMinDistance
Definition: find_checker_board.hpp:56
static float * getScalingFactors(float *white, int nWhite)
getScalingFactors
Definition: filter_white_balance.hpp:108
The HarrisCornerDetector class.
Definition: harris_corner_detector.hpp:54
PIC_INLINE void drawPoints(Image *img, std::vector< Eigen::Vector2f > &points, float *color)
drawPoints
Definition: rasterizer.hpp:162
The FilterWhiteBalance class.
Definition: filter_white_balance.hpp:31
Definition: nelder_mead_opt_ICP_2D.hpp:40
virtual Scalar * run(Scalar *x_start, unsigned int n, Scalar epsilon=1e-4f, int max_iterations=1000, Scalar *x=NULL)
Definition: nelder_mead_opt_base.hpp:333
#define PIC_INLINE
Definition: base.hpp:33
PIC_INLINE Image * getCheckerBoardModel(int checkers_x, int checkers_y, int checkers_size, std::vector< Eigen::Vector2f > &out)
getCheckerBoardModel
Definition: find_checker_board.hpp:207
The Image class stores an image as buffer of float.
Definition: image.hpp:60
void getAll(Image *img, std::vector< Eigen::Vector2f > &corners, std::vector< uint * > &descs)
getAll
Definition: brief_descriptor.hpp:226
The ORBDescriptor class.
Definition: orb_descriptor.hpp:34
PIC_INLINE ImageVec Single(Image *img)
Single creates an std::vector which contains img; this is for filters input.
Definition: image_vec.hpp:36
Definition: bilateral_separation.hpp:25
int getDescriptorSize()
getDescriptorSize returns the descriptor size.
Definition: brief_descriptor.hpp:244
PIC_INLINE float estimateLengthOfCheckers(std::vector< Eigen::Vector2f > &corners_model, Eigen::Vector2f &p0, Eigen::Vector2f &p1)
estimateLengthInPixelOfCheckers
Definition: find_checker_board.hpp:446
PIC_INLINE void iterativeClosestPoints2D(std::vector< Eigen::Vector2f > &points_pattern, std::vector< Eigen::Vector2f > &points, std::vector< unsigned int *> &points_pattern_descs, std::vector< unsigned int *> &points_descs, int size_descs, int maxIterations=1000)
iterativeClosestPoints2D
Definition: iterative_closest_point_2D.hpp:339
bool Write(std::string nameFile, LDR_type typeWrite, int writerCounter)
Write saves an Image into a file on the disk.
PIC_INLINE void findCheckerBoard(Image *img, std::vector< Eigen::Vector2f > &corners_model, int checkerBoardSizeX=4, int checkerBoardSizeY=7)
findCheckerBoard
Definition: find_checker_board.hpp:259