diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fab7372 --- /dev/null +++ b/.gitignore @@ -0,0 +1,73 @@ +# This file is used to ignore files which are generated +# ---------------------------------------------------------------------------- + +*~ +*.autosave +*.a +*.core +*.moc +*.o +*.obj +*.orig +*.rej +*.so +*.so.* +*_pch.h.cpp +*_resource.rc +*.qm +.#* +*.*# +core +!core/ +tags +.DS_Store +.directory +*.debug +Makefile* +*.prl +*.app +moc_*.cpp +ui_*.h +qrc_*.cpp +Thumbs.db +*.res +*.rc +/.qmake.cache +/.qmake.stash + +# qtcreator generated files +*.pro.user* + +# xemacs temporary files +*.flc + +# Vim temporary files +.*.swp + +# Visual Studio generated files +*.ib_pdb_index +*.idb +*.ilk +*.pdb +*.sln +*.suo +*.vcproj +*vcproj.*.*.user +*.ncb +*.sdf +*.opensdf +*.vcxproj +*vcxproj.* + +# MinGW generated files +*.Debug +*.Release + +# Python byte code +*.pyc + +# Binaries +# -------- +*.dll +*.exe + diff --git a/02.png b/02.png new file mode 100644 index 0000000..91c54f3 Binary files /dev/null and b/02.png differ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..2083573 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 2.8) +#onlyliucat + +project(test_MultichessboardExtraction) +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fPIC -lm -Wl,--no-as-needed -DLINUX -g -Wall \ +-std=c++11 -Wunused-variable -Wwrite-strings -Wsign-compare -fpermissive ") +set(CMAKE_BUILD_TYPE "Release") + + +find_package( OpenCV REQUIRED ) +include_directories( ${OpenCV_INCLUDE_DIRS} ) +set(OpenCV_LIBS ${OpenCV_LIBRARIES}) +MESSAGE(STATUS "The Opencv's include directory is:" ${OpenCV_INCLUDE_DIRS}) +MESSAGE(STATUS "The Opencv's OpenCV_LIBS is:" ${OpenCV_LIBS}) + + +INCLUDE_DIRECTORIES(./) +INCLUDE_DIRECTORIES(./include) +AUX_SOURCE_DIRECTORY(./ gSOURCE_FILES_) + + +add_executable(${PROJECT_NAME} "main.cpp" + ${gSOURCE_FILES_} + ) + +target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES}) diff --git a/ChessboradStruct.cpp b/ChessboradStruct.cpp new file mode 100644 index 0000000..5535300 --- /dev/null +++ b/ChessboradStruct.cpp @@ -0,0 +1,785 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + +#include "ChessboradStruct.h" +#include +#include +#include + +#define BOLDBLUE "\033[1m\033[34m" /* Bold Blue */ + +ChessboradStruct::ChessboradStruct() +{ + +} + +ChessboradStruct::~ChessboradStruct() +{ + +} + +inline float distv(cv::Vec2f a, cv::Vec2f b) +{ + return std::sqrt((a[0] - b[0])*(a[0] - b[0]) + (a[1] - b[1])*(a[1] - b[1])); +} + +inline float mean_l(std::vector &resultSet) +{ + double sum = std::accumulate(std::begin(resultSet), std::end(resultSet), 0.0); + double mean = sum / resultSet.size(); //ŸùÖµ + return mean; +} + +inline float stdev_l(std::vector &resultSet, float &mean) +{ + double accum = 0.0; + mean = mean_l(resultSet); + std::for_each(std::begin(resultSet), std::end(resultSet), [&](const double d) { + accum += (d - mean)*(d - mean); + }); + double stdev = sqrt(accum / (resultSet.size() - 1)); //·œ²î + return stdev; +} + +inline float stdevmean(std::vector &resultSet) +{ + float stdvalue, meanvalue; + + stdvalue = stdev_l(resultSet, meanvalue); + + return stdvalue / meanvalue; +} + +int ChessboradStruct::directionalNeighbor(int idx, cv::Vec2f v, cv::Mat chessboard, Corners& corners, int& neighbor_idx, float& min_dist) +{ + +#if 1 + // list of neighboring elements, which are currently not in use + std::vector unused(corners.p.size()); + for (int i = 0; i < unused.size(); i++) + { + unused[i] = i; + } + for (int i = 0; i < chessboard.rows; i++) + for (int j = 0; j < chessboard.cols; j++) + { + int xy = chessboard.at(i, j); + if (xy >= 0) + { + unused[xy] = -1; + } + } + + int nsize = unused.size(); + + for (int i = 0; i < nsize;) + { + if (unused[i] < 0) + { + std::vector::iterator iter = unused.begin() + i; + unused.erase(iter); + i = 0; + nsize = unused.size(); + continue; + } + i++; + } + + std::vector dist_edge; + std::vector dist_point; + + cv::Vec2f idxp = cv::Vec2f(corners.p[idx].x, corners.p[idx].y); + // direction and distance to unused corners + for (int i = 0; i < unused.size(); i++) + { + int ind = unused[i]; + cv::Vec2f diri = cv::Vec2f(corners.p[ind].x, corners.p[ind].y) - idxp; + float disti = diri[0] * v[0] + diri[1] * v[1]; + + cv::Vec2f de = diri - disti*v; + dist_edge.push_back(distv(de, cv::Vec2f(0, 0))); + // distances + dist_point.push_back(disti); + } +#else + // list of neighboring elements, which are currently not in use + std::vector unused(corners.p.size()); + for (int i = 0; i < unused.size(); i++) + { + unused[i] = i; + } + for (int i = 0; i < chessboard.rows; i++) + for (int j = 0; j < chessboard.cols; j++) + { + int xy = chessboard.at(i, j); + if (xy >= 0) + { + unused[xy] = -1;//flag the used idx + } + } + + std::vector dist_edge; + std::vector dist_point; + + cv::Vec2f idxp = cv::Vec2f(corners.p[idx].x, corners.p[idx].y); + // direction and distance to unused corners + for (int i = 0; i < corners.p.size(); i++) + { + if (unused[i] == -1) + { + dist_point.push_back(std::numeric_limits::max()); + dist_edge.push_back(0); + continue; + } + cv::Vec2f diri = cv::Vec2f(corners.p[i].x, corners.p[i].y) - idxp; + float disti = diri[0] * v[0] + diri[1] * v[1]; + + cv::Vec2f de = diri - disti*v; + dist_edge.push_back(distv(de, cv::Vec2f(0, 0))); + // distances + dist_point.push_back(disti); + } + +#endif + + // find best neighbor + int min_idx = 0; + min_dist = std::numeric_limits::max(); + + //min_dist = dist_point[0] + 5 * dist_edge[0]; + for (int i = 0; i < dist_point.size(); i++) + { + if (dist_point[i] > 0) + { + float m = dist_point[i] + 5 * dist_edge[i]; + if (m < min_dist) + { + min_dist = m; + min_idx = i; + } + } + } + neighbor_idx = unused[min_idx]; + + return 1; +} + + +cv::Mat ChessboradStruct::initChessboard(Corners& corners, int idx) +{ + // return if not enough corners + if (corners.p.size() < 9) + { + logd("not enough corners!\n"); + chessboard.release();//return empty! + return chessboard; + } + // init chessboard hypothesis + chessboard = -1 * cv::Mat::ones(3, 3, CV_32S); + + // extract feature index and orientation(central element) + cv::Vec2f v1 = corners.v1[idx]; + cv::Vec2f v2 = corners.v2[idx]; + chessboard.at(1, 1) = idx; + std::vector dist1(2), dist2(6); + + // find left / right / top / bottom neighbors + directionalNeighbor(idx, +1 * v1, chessboard, corners, chessboard.at(1, 2), dist1[0]); + directionalNeighbor(idx, -1 * v1, chessboard, corners, chessboard.at(1, 0), dist1[1]); + directionalNeighbor(idx, +1 * v2, chessboard, corners, chessboard.at(2, 1), dist2[0]); + directionalNeighbor(idx, -1 * v2, chessboard, corners, chessboard.at(0, 1), dist2[1]); + + // find top - left / top - right / bottom - left / bottom - right neighbors + + directionalNeighbor(chessboard.at(1, 0), -1 * v2, chessboard, corners, chessboard.at(0, 0), dist2[2]); + directionalNeighbor(chessboard.at(1, 0), +1 * v2, chessboard, corners, chessboard.at(2, 0), dist2[3]); + directionalNeighbor(chessboard.at(1, 2), -1 * v2, chessboard, corners, chessboard.at(0, 2), dist2[4]); + directionalNeighbor(chessboard.at(1, 2), +1 * v2, chessboard, corners, chessboard.at(2, 2), dist2[5]); + + // initialization must be homogenously distributed + + + bool sigood = false; + sigood = sigood||(dist1[0]<0) || (dist1[1]<0); + sigood = sigood || (dist2[0]<0) || (dist2[1]<0) || (dist2[2]<0) || (dist2[3]<0) || (dist2[4]<0) || (dist2[5]<0); + + + sigood = sigood || (stdevmean(dist1) > 0.3) || (stdevmean(dist2) > 0.3); + + if (sigood == true) + { + chessboard.release(); + return chessboard; + } + return chessboard; +} + +float ChessboradStruct::chessboardEnergy(cv::Mat chessboard, Corners& corners) +{ + float lamda = m_lamda; + //energy: number of corners + float E_corners = -1 * chessboard.size().area(); + //energy: structur + float E_structure = 0; + //walk through rows + for (int i = 0; i < chessboard.rows; i++) + for (int j = 0; j < chessboard.cols-2; j++) + { + std::vector x; + float E_structure0 = 0; + for (int k = j; k <= j + 2; k++) + { + int n = chessboard.at(i, k); + x.push_back(corners.p[n]); + } + E_structure0 = distv(x[0] + x[2] - 2 * x[1], cv::Vec2f(0,0)); + float tv = distv(x[0] - x[2], cv::Vec2f(0, 0)); + E_structure0 = E_structure0 / tv; + if (E_structure < E_structure0) + E_structure = E_structure0; + } + + //walk through columns + for (int i = 0; i < chessboard.cols; i++) + for (int j = 0; j < chessboard.rows-2; j++) + { + std::vector x; + float E_structure0 = 0; + for (int k = j; k <= j + 2; k++) + { + int n = chessboard.at(k, i); + x.push_back(corners.p[n]); + } + E_structure0 = distv(x[0] + x[2] - 2 * x[1], cv::Vec2f(0, 0)); + float tv = distv(x[0] - x[2], cv::Vec2f(0, 0)); + E_structure0 = E_structure0 / tv; + if (E_structure < E_structure0) + E_structure = E_structure0; + } + + // final energy + float E = E_corners + lamda*chessboard.size().area()*E_structure; + + return E; +} + +// replica prediction(new) +void ChessboradStruct::predictCorners(std::vector& p1, std::vector& p2, + std::vector& p3, std::vector& pred) +{ + cv::Vec2f v1, v2; + float a1, a2, a3; + float s1, s2, s3; + pred.resize(p1.size()); + for (int i = 0; i < p1.size(); i++) + { + // compute vectors + v1 = p2[i] - p1[i]; + v2 = p3[i] - p2[i]; + // predict angles + a1 = atan2(v1[1], v1[0]); + a2 = atan2(v1[1], v1[0]); + a3 = 2.0 * a2 - a1; + + //predict scales + s1 = distv(v1, cv::Vec2f(0, 0)); + s2 = distv(v2, cv::Vec2f(0, 0)); + s3 = 2 * s2 - s1; + pred[i] = p3[i] + 0.75*s3*cv::Vec2f(cos(a3), sin(a3)); + } +} + +void ChessboradStruct::assignClosestCorners(std::vector&cand, std::vector&pred, std::vector &idx) +{ + //return error if not enough candidates are available + if (cand.size() < pred.size()) + { + idx.resize(1); + idx[0] = -1; + return; + } + idx.resize(pred.size()); + + //build distance matrix + cv::Mat D = cv::Mat::zeros(cand.size(), pred.size(), CV_32FC1); + float mind = FLT_MAX; + for (int i = 0; i < D.cols; i++)//ÁÐÓÅÏÈ + { + cv::Vec2f delta; + for (int j = 0; j < D.rows; j++) + { + delta = cand[j] - pred[i]; + float s = distv(delta, cv::Vec2f(0, 0)); + D.at(j, i) = s; + if (s < mind) + { + mind = s; + } + } + } + + // search greedily for closest corners + for (int k = 0; k < pred.size(); k++) + { + bool isbreak = false; + for (int i = 0; i < D.rows; i++) + { + for (int j = 0; j < D.cols; j++) + { + if (fabs(D.at(i, j) - mind) < 10e-10) + { + idx[j] = i; + for (int m = 0; m < D.cols; m++) + { + D.at(i, m) = FLT_MAX; + } + for (int m = 0; m < D.rows; m++) + { + D.at(m,j) = FLT_MAX; + } + isbreak = true; + break; + } + } + if (isbreak == true) + break; + } + mind = FLT_MAX; + for (int i = 0; i < D.rows; i++) + { + for (int j = 0; j < D.cols; j++) + { + if (D.at(i, j) < mind) + { + mind = D.at(i, j); + } + } + } + } +} + + + +cv::Mat ChessboradStruct::growChessboard(cv::Mat chessboard, Corners& corners, int border_type) +{ + if (chessboard.empty() == true) + { + return chessboard; + } + std::vector p = corners.p; + // list of unused feature elements + std::vector unused(p.size()); + for (int i = 0; i < unused.size(); i++) + { + unused[i] = i; + } + for (int i = 0; i < chessboard.rows; i++) + for (int j = 0; j < chessboard.cols; j++) + { + int xy = chessboard.at(i, j); + if (xy >= 0) + { + unused[xy] = -1; + } + } + + int nsize = unused.size(); + + for (int i = 0; i < nsize; ) + { + if (unused[i] < 0) + { + std::vector::iterator iter = unused.begin() + i; + unused.erase(iter); + i = 0; + nsize = unused.size(); + continue; + } + i++; + } + + // candidates from unused corners + std::vector cand; + for (int i = 0; i < unused.size(); i++) + { + cand.push_back(corners.p[unused[i]]); + } + // switch border type 1..4 + cv::Mat chesstemp; + + switch (border_type) + { + case 0: + { + std::vector p1, p2, p3,pred; + for (int row = 0; row < chessboard.rows; row++) + for (int col = 0; col < chessboard.cols; col++) + { + if (col == chessboard.cols - 3) + { + int ij = chessboard.at(row, col); + p1.push_back(cv::Vec2f(p[ij])); + } + if (col == chessboard.cols - 2) + { + int ij = chessboard.at(row, col); + p2.push_back(cv::Vec2f(p[ij])); + } + if (col == chessboard.cols - 1) + { + int ij = chessboard.at(row, col); + p3.push_back(cv::Vec2f(p[ij])); + } + } + std::vector idx; + predictCorners(p1, p2, p3, pred); + assignClosestCorners(cand, pred, idx); + if (idx[0] < 0) + { + return chessboard; + } + + cv::copyMakeBorder(chessboard, chesstemp, 0, 0, 0, 1, 0,0); + + for (int i = 0; i < chesstemp.rows; i++) + { + chesstemp.at(i, chesstemp.cols - 1) = unused[idx[i]];//ÓÒ + } + chessboard = chesstemp.clone(); + + break; + } + case 1: + { + std::vector p1, p2, p3, pred; + for (int row = 0; row < chessboard.rows; row++) + for (int col = 0; col < chessboard.cols; col++) + { + if (row == chessboard.rows - 3) + { + int ij = chessboard.at(row, col); + p1.push_back(cv::Vec2f(p[ij])); + } + if (row == chessboard.rows - 2) + { + int ij = chessboard.at(row, col); + p2.push_back(cv::Vec2f(p[ij])); + } + if (row == chessboard.rows - 1) + { + int ij = chessboard.at(row, col); + p3.push_back(cv::Vec2f(p[ij])); + } + } + std::vector idx; + predictCorners(p1, p2, p3, pred); + assignClosestCorners(cand, pred, idx); + if (idx[0] < 0) + { + return chessboard; + } + + cv::copyMakeBorder(chessboard, chesstemp, 0, 1, 0, 0, 0, 0); + for (int i = 0; i < chesstemp.cols; i++) + { + chesstemp.at(chesstemp.rows - 1, i) = unused[idx[i]];//Ï + } + chessboard = chesstemp.clone(); + + break; + } + case 2: + { + std::vector p1, p2, p3, pred; + for (int row = 0; row < chessboard.rows; row++) + for (int col = 0; col < chessboard.cols; col++) + { + if (col == 2) + { + int ij = chessboard.at(row, col); + p1.push_back(cv::Vec2f(p[ij])); + } + if (col == 1) + { + int ij = chessboard.at(row, col); + p2.push_back(cv::Vec2f(p[ij])); + } + if (col == 0) + { + int ij = chessboard.at(row, col); + p3.push_back(cv::Vec2f(p[ij])); + } + } + std::vector idx; + predictCorners(p1, p2, p3, pred); + assignClosestCorners(cand, pred, idx); + if (idx[0] < 0) + { + return chessboard; + } + + cv::copyMakeBorder(chessboard, chesstemp, 0, 0, 1, 0, 0, 0);//×ó + for (int i = 0; i < chesstemp.rows; i++) + { + chesstemp.at(i, 0) = unused[idx[i]]; + } + chessboard = chesstemp.clone(); + + break; + } + case 3: + { + std::vector p1, p2, p3, pred; + for (int row = 0; row < chessboard.rows; row++) + for (int col = 0; col < chessboard.cols; col++) + { + if (row == 2) + { + int ij = chessboard.at(row, col); + p1.push_back(cv::Vec2f(p[ij])); + } + if (row == 1) + { + int ij = chessboard.at(row, col); + p2.push_back(cv::Vec2f(p[ij])); + } + if (row == 0) + { + int ij = chessboard.at(row, col); + p3.push_back(cv::Vec2f(p[ij])); + } + } + std::vector idx; + predictCorners(p1, p2, p3, pred); + assignClosestCorners(cand, pred, idx); + if (idx[0] < 0) + { + return chessboard; + } + cv::copyMakeBorder(chessboard, chesstemp, 1, 0, 0, 0, 0, 0);//ÉÏ + for (int i = 0; i < chesstemp.cols; i++) + { + chesstemp.at(0, i) = unused[idx[i]]; + } + chessboard = chesstemp.clone(); + break; + } + default: + break; + } + return chessboard; +} + + + + +void ChessboradStruct::chessboardsFromCorners( Corners& corners, std::vector& chessboards, float lamda) +{ + logd("Structure recovery:\n"); + m_lamda = lamda; + for (int i = 0; i < corners.p.size(); i++) + { + if (i % 128 == 0) + printf("%d, %d\n", i, corners.p.size()); + + cv::Mat csbd = initChessboard(corners, i); + if (csbd.empty() == true) + { + continue; + } + float E =chessboardEnergy(csbd, corners); + if (E > 0){ continue; } + + int s = 0; + //try growing chessboard + while (true) + { + s++; + // compute current energy + float energy = chessboardEnergy(chessboard, corners); + + std::vector proposal(4); + std::vector p_energy(4); + //compute proposals and energies + for (int j = 0; j < 4; j++) + { + proposal[j] = growChessboard(chessboard, corners, j); + p_energy[j] = chessboardEnergy(proposal[j], corners); + } + // find best proposal + float min_value = p_energy[0]; + int min_idx = 0; + for (int i0 = 1; i0 < p_energy.size(); i0++) + { + if (min_value > p_energy[i0]) + { + min_value = p_energy[i0]; + min_idx = i0; + } + } + // accept best proposal, if energy is reduced + cv::Mat chessboardt; + if (p_energy[min_idx] < energy) + { + chessboardt = proposal[min_idx]; + chessboard = chessboardt.clone(); + } + else + { + break; + } + }//end while + + if (chessboardEnergy(chessboard, corners) < -10) + { + //check if new chessboard proposal overlaps with existing chessboards + cv::Mat overlap = cv::Mat::zeros(cv::Size(2,chessboards.size()), CV_32FC1); + for (int j = 0; j < chessboards.size(); j++) + { + bool isbreak = false; + for (int k = 0; k < chessboards[j].size().area(); k++) + { + int refv = chessboards[j].at(k / chessboards[j].cols, k%chessboards[j].cols); + for (int l = 0; l < chessboard.size().area(); l++) + { + int isv = chessboard.at(l/ chessboard.cols, l%chessboard.cols); + if (refv == isv) + { + overlap.at(j, 0) = 1.0; + float s = chessboardEnergy(chessboards[j], corners); + overlap.at(j, 1) = s; + isbreak = true; + break; + } + } + // if (isbreak == true) + // { + // break; + // } + } + //if (isbreak == true) + //{ + // break; + //} + }//endfor + + // add chessboard(and replace overlapping if neccessary) + bool isoverlap = false; + for (int i0 = 0; i0 < overlap.rows; i0++) + { + if (overlap.empty() == false) + { + if (fabs(overlap.at(i0, 0)) > 0.000001)// ==1 + { + isoverlap = true; + break; + } + } + } + if (isoverlap == false) + { + chessboards.push_back(chessboard); + } + else + { + bool flagpush = true; + std::vector flagerase(overlap.rows); + for (int m = 0; m < flagerase.size(); m++) + { + flagerase[m] = false; + } + float ce = chessboardEnergy(chessboard, corners); + for (int i1 = 0; i1 < overlap.rows; i1++) + { + if (fabs(overlap.at(i1, 0)) > 0.0001)// ==1//ÓÐÖصþ + { + bool isb1 = overlap.at(i1, 1) > ce; + + int a = int(overlap.at(i1, 1) * 1000); + int b = int(ce * 1000); + + bool isb2 = a > b; + if (isb1 != isb2) + printf("find bug!\n"); + + if (isb2) + { + flagerase[i1] = true; + } + else + { + flagpush = false; + // break; + + }//endif + }//endif + + }//end for + + if (flagpush == true) + { + for (int i1 = 0; i1 < chessboards.size();) + { + std::vector::iterator it = chessboards.begin() + i1; + std::vector::iterator it1 = flagerase.begin() + i1; + if (*it1 == true) + { + chessboards.erase(it); + flagerase.erase(it1); + i1 = 0; + } + i1++; + } + chessboards.push_back(chessboard); + } + + }//endif + + }//endif + }//end for + +} +#define BOLDGREEN "\033[1m\033[32m" /* Bold Green */ +void ChessboradStruct::drawchessboard(cv::Mat img, Corners& corners, std::vector& chessboards, char * title, int t_, cv::Rect rect) +{ + printf("end!\n"); + + cv::RNG rng(0xFFFFFFFF); + std::string s("If it's useful, please give a star ^-^."); + std::string s1("https://github.com/onlyliucat\n"); + std::cout<(i, j); + cv::circle(disp, cv::Point2f(corners.p[d].x + rect.x, corners.p[d].y + rect.y), n, s, n); + } + } + cv::Mat SmallMat; + cv::resize(disp, SmallMat, cv::Size(), scale, scale); + cv::namedWindow(title); + cv::imshow(title, SmallMat); + cv::waitKey(t_); +} + + + diff --git a/ChessboradStruct.h b/ChessboradStruct.h new file mode 100644 index 0000000..50fb094 --- /dev/null +++ b/ChessboradStruct.h @@ -0,0 +1,32 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + +#pragma once + +#include "opencv2/opencv.hpp" +#include +#include "HeaderCB.h" + +class ChessboradStruct +{ +public: + ChessboradStruct(); + ~ChessboradStruct(); + + cv::Mat initChessboard(Corners& corners, int idx); + void chessboardsFromCorners(Corners& corners, std::vector& chessboards, float lamda = 0.5); + int directionalNeighbor(int idx, cv::Vec2f v, cv::Mat chessboard, Corners& corners, int& neighbor_idx, float& min_dist); + float chessboardEnergy(cv::Mat chessboard, Corners& corners); + void predictCorners(std::vector& p1, std::vector& p2, std::vector& p3, std::vector& pred); + cv::Mat growChessboard(cv::Mat chessboard, Corners& corners, int border_type); + void assignClosestCorners(std::vector&cand, std::vector&pred, std::vector &idx); + void drawchessboard(cv::Mat img, Corners& corners, std::vector& chessboards, char * title = "chessboard", int t = 0, cv::Rect rect = cv::Rect(0,0,0,0)); + + cv::Mat chessboard; + float m_lamda; + +}; + diff --git a/CornerDetAC.cpp b/CornerDetAC.cpp new file mode 100644 index 0000000..2664713 --- /dev/null +++ b/CornerDetAC.cpp @@ -0,0 +1,826 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* part of source code come from https://github.com/qibao77/cornerDetect */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + + +#include "CornerDetAC.h" +#include "corealgmatlab.h" + + +using namespace cv; +using namespace std; + +//#define show_ + +CornerDetAC::CornerDetAC() +{ +} + + +CornerDetAC::~CornerDetAC() +{ +} +CornerDetAC::CornerDetAC(cv::Mat img) +{ + //3 scales + radius.push_back(4); + radius.push_back(8); + radius.push_back(12); + + templateProps.push_back(Point2f((dtype)0, (dtype)CV_PI / 2)); + templateProps.push_back(Point2f((dtype)CV_PI / 4, (dtype)-CV_PI / 4)); + templateProps.push_back(Point2f((dtype)0, (dtype)CV_PI / 2)); + templateProps.push_back(Point2f((dtype)CV_PI / 4, (dtype)-CV_PI / 4)); + templateProps.push_back(Point2f((dtype)0, (dtype)CV_PI / 2)); + templateProps.push_back(Point2f((dtype)CV_PI / 4, (dtype)-CV_PI / 4)); +} + +//Normal probability density function (pdf). +dtype CornerDetAC::normpdf(dtype dist, dtype mu, dtype sigma) +{ + dtype s = exp(-0.5*(dist - mu)*(dist - mu) / (sigma*sigma)); + s = s / (std::sqrt(2 * CV_PI)*sigma); + return s; +} + + +//**************************ɺ*****************************// +//angleͣ45Ⱥ˺90Ⱥ +//kernelSize˴Сɵĺ˵ĴСΪkernelSize*2+1 +//kernelA...kernelDɵĺ +//*************************************************************************// +void CornerDetAC::createkernel(float angle1, float angle2, int kernelSize, Mat &kernelA, Mat &kernelB, Mat &kernelC, Mat &kernelD) +{ + + int width = (int)kernelSize * 2 + 1; + int height = (int)kernelSize * 2 + 1; + kernelA = cv::Mat::zeros(height, width, mtype); + kernelB = cv::Mat::zeros(height, width, mtype); + kernelC = cv::Mat::zeros(height, width, mtype); + kernelD = cv::Mat::zeros(height, width, mtype); + + for (int u = 0; u < width; ++u){ + for (int v = 0; v < height; ++v){ + dtype vec[] = { u - kernelSize, v - kernelSize };//൱ڽԭƶ + dtype dis = std::sqrt(vec[0] * vec[0] + vec[1] * vec[1]);//൱ڼ㵽ĵľ + dtype side1 = vec[0] * (-sin(angle1)) + vec[1] * cos(angle1);//൱ڽԭƶĺ˽תԴ˲ֺ + dtype side2 = vec[0] * (-sin(angle2)) + vec[1] * cos(angle2);//X=X0*cos+Y0*sin;Y=Y0*cos-X0*sin + if (side1 <= -0.1&&side2 <= -0.1){ + kernelA.ptr(v)[u] = normpdf(dis, 0, kernelSize / 2); + } + if (side1 >= 0.1&&side2 >= 0.1){ + kernelB.ptr(v)[u] = normpdf(dis, 0, kernelSize / 2); + } + if (side1 <= -0.1&&side2 >= 0.1){ + kernelC.ptr(v)[u] = normpdf(dis, 0, kernelSize / 2); + } + if (side1 >= 0.1&&side2 <= -0.1){ + kernelD.ptr(v)[u] = normpdf(dis, 0, kernelSize / 2); + } + } + } + //std::cout << "kernelA:" << kernelA << endl << "kernelB:" << kernelB << endl + // << "kernelC:" << kernelC<< endl << "kernelD:" << kernelD << endl; + //һ + kernelA = kernelA / cv::sum(kernelA)[0]; + kernelB = kernelB / cv::sum(kernelB)[0]; + kernelC = kernelC / cv::sum(kernelC)[0]; + kernelD = kernelD / cv::sum(kernelD)[0]; + +} + +//**************************//ȡСֵ*****************************// +//*************************************************************************// +void CornerDetAC::getMin(Mat src1, Mat src2, Mat &dst){ + int rowsLeft = src1.rows; + int colsLeft = src1.cols; + int rowsRight = src2.rows; + int colsRight = src2.cols; + if (rowsLeft != rowsRight || colsLeft != colsRight)return; + + int channels = src1.channels(); + + int nr = rowsLeft; + int nc = colsLeft; + if (src1.isContinuous()){ + nc = nc*nr; + nr = 1; + //std::cout<<"continue"<(i); + const dtype* dataRight = src2.ptr(i); + dtype* dataResult = dst.ptr(i); + for (int j = 0; j < nc*channels; ++j){ + dataResult[j] = (dataLeft[j] < dataRight[j]) ? dataLeft[j] : dataRight[j]; + } + } +} +//**************************//ȡֵ*****************************// +//*************************************************************************// +void CornerDetAC::getMax(Mat src1, Mat src2, Mat &dst) +{ + int rowsLeft = src1.rows; + int colsLeft = src1.cols; + int rowsRight = src2.rows; + int colsRight = src2.cols; + if (rowsLeft != rowsRight || colsLeft != colsRight)return; + + int channels = src1.channels(); + + int nr = rowsLeft; + int nc = colsLeft; + if (src1.isContinuous()){ + nc = nc*nr; + nr = 1; + //std::cout<<"continue"<(i); + const dtype* dataRight = src2.ptr(i); + dtype* dataResult = dst.ptr(i); + for (int j = 0; j < nc*channels; ++j){ + dataResult[j] = (dataLeft[j] >= dataRight[j]) ? dataLeft[j] : dataRight[j]; + } + } +} +//ȡݶȽǶȺȨ +void CornerDetAC::getImageAngleAndWeight(Mat img, Mat &imgDu, Mat &imgDv, Mat &imgAngle, Mat &imgWeight) +{ + + Mat sobelKernel(3, 3, mtype); + Mat sobelKernelTrs(3, 3, mtype); + //soble˲Ӻ + sobelKernel.col(0).setTo(cv::Scalar(-1.0)); + sobelKernel.col(1).setTo(cv::Scalar(0.0)); + sobelKernel.col(2).setTo(cv::Scalar(1.0)); + + sobelKernelTrs = sobelKernel.t(); + + imgDu = corealgmatlab::conv2(img, sobelKernel, CONVOLUTION_SAME); + imgDv = corealgmatlab::conv2(img, sobelKernelTrs, CONVOLUTION_SAME); + + if (imgDu.size() != imgDv.size())return; + + cartToPolar(imgDu, imgDv, imgWeight, imgAngle, false); + for (int i = 0; i < imgDu.rows; i++) + { + for (int j = 0; j < imgDu.cols; j++) + { + dtype* dataAngle = imgAngle.ptr(i); + if (dataAngle[j] < 0) + dataAngle[j] = dataAngle[j] + CV_PI; + else if (dataAngle[j] > CV_PI) + dataAngle[j] = dataAngle[j] - CV_PI; + } + } + /* + for (int i = 0; i < imgDu.rows; i++) + { + dtype* dataDv = imgDv.ptr(i); + dtype* dataDu = imgDu.ptr(i); + dtype* dataAngle = imgAngle.ptr(i); + dtype* dataWeight = imgWeight.ptr(i); + for (int j = 0; j < imgDu.cols; j++) + { + if (dataDu[j] > 0.000001) + { + dataAngle[j] = atan2((dtype)dataDv[j], (dtype)dataDu[j]); + if (dataAngle[j] < 0)dataAngle[j] = dataAngle[j] + CV_PI; + else if (dataAngle[j] > CV_PI)dataAngle[j] = dataAngle[j] - CV_PI; + } + dataWeight[j] = std::sqrt((dtype)dataDv[j] * (dtype)dataDv[j] + (dtype)dataDu[j] * (dtype)dataDu[j]); + } + } + */ +} +//**************************Ǽֵ*****************************// +//inputCornersǵ㣬outputCornersǷǼֵƺĽǵ +//threshold趨ֵ +//marginǽзǼֵʱ鷽߽ľ룬patchSizeǸ÷ĴС +//*************************************************************************// +void CornerDetAC::nonMaximumSuppression(Mat& inputCorners, vector& outputCorners, int patchSize, dtype threshold, int margin) +{ + if (inputCorners.size <= 0) + { + cout << "The imput mat is empty!" << endl; return; + } + for (int i = margin + patchSize; i <= inputCorners.cols - (margin + patchSize+1); i = i + patchSize + 1)//ƶ鷽飬ÿƶһĴС + { + for (int j = margin + patchSize; j <= inputCorners.rows - (margin + patchSize+1); j = j + patchSize + 1) + { + dtype maxVal = inputCorners.ptr(j)[i]; + int maxX = i; int maxY = j; + for (int m = i; m <= i + patchSize ; m++)//ҳü鷽еľֲֵ + { + for (int n = j; n <= j + patchSize ; n++) + { + dtype temp = inputCorners.ptr(n)[m]; + if (temp > maxVal) + { + maxVal = temp; maxX = m; maxY = n; + } + } + } + if (maxVal < threshold)continue;//þֲֵСֵҪ + int flag = 0; + for (int m = maxX - patchSize; m <= min(maxX + patchSize, inputCorners.cols - margin-1); m++)//μ + { + for (int n = maxY - patchSize; n <= min(maxY + patchSize, inputCorners.rows - margin-1); n++) + { + if (inputCorners.ptr(n)[m]>maxVal && (mi + patchSize || nj + patchSize)) + { + flag = 1; break; + } + } + if (flag)break; + } + if (flag)continue; + outputCorners.push_back(Point(maxX, maxY)); + std::vector e1(2, 0.0); + std::vector e2(2, 0.0); + cornersEdge1.push_back(e1); + cornersEdge2.push_back(e2); + } + } +} + +int cmp(const pair &a, const pair &b) +{ + return a.first > b.first; +} + +//find modes of smoothed histogram +void CornerDetAC::findModesMeanShift(vector hist, vector &hist_smoothed, vector> &modes, dtype sigma){ + //efficient mean - shift approximation by histogram smoothing + //compute smoothed histogram + bool allZeros = true; + for (int i = 0; i < hist.size(); i++) + { + dtype sum = 0; + for (int j = -(int)round(2 * sigma); j <= (int)round(2 * sigma); j++) + { + int idx = 0; + idx = (i + j) % hist.size(); + sum = sum + hist[idx] * normpdf(j, 0, sigma); + } + hist_smoothed[i] = sum; + if (abs(hist_smoothed[i] - hist_smoothed[0]) > 0.0001)allZeros = false;// check if at least one entry is non - zero + //(otherwise mode finding may run infinitly) + } + if (allZeros)return; + + //mode finding + for (int i = 0; i < hist.size(); i++) + { + int j = i; + while (true) + { + float h0 = hist_smoothed[j]; + int j1 = (j + 1) % hist.size(); + int j2 = (j - 1) % hist.size(); + float h1 = hist_smoothed[j1]; + float h2 = hist_smoothed[j2]; + if (h1 >= h0 && h1 >= h2) + j = j1; + else if (h2 > h0 && h2 > h1) + j = j2; + else + break; + } + bool ys = true; + if (modes.size() == 0) + { + ys = true; + } + else + { + for (int k = 0; k < modes.size(); k++) + { + if (modes[k].second == j) + { + ys = false; + break; + } + } + } + if (ys == true) + { + modes.push_back(std::make_pair(hist_smoothed[j], j)); + } + } + std::sort(modes.begin(), modes.end(), cmp); +} + +//estimate edge orientations +void CornerDetAC::edgeOrientations(Mat imgAngle, Mat imgWeight, int index){ + //number of bins (histogram parameter) + int binNum = 32; + + //convert images to vectors + if (imgAngle.size() != imgWeight.size())return; + vector vec_angle, vec_weight; + for (int i = 0; i < imgAngle.cols; i++) + { + for (int j = 0; j < imgAngle.rows; j++) + { + // convert angles from .normals to directions + float angle = imgAngle.ptr(j)[i] + CV_PI / 2; + angle = angle > CV_PI ? (angle - CV_PI) : angle; + vec_angle.push_back(angle); + + vec_weight.push_back(imgWeight.ptr(j)[i]); + } + } + + //create histogram + dtype pin = (CV_PI / binNum); + vector angleHist(binNum, 0); + for (int i = 0; i < vec_angle.size(); i++) + { + int bin = max(min((int)floor(vec_angle[i] / pin), binNum - 1), 0); + angleHist[bin] = angleHist[bin] + vec_weight[i]; + } + + // find modes of smoothed histogram + vector hist_smoothed(angleHist); + vector > modes; + findModesMeanShift(angleHist, hist_smoothed, modes, 1); + + // if only one or no mode = > return invalid corner + if (modes.size() <= 1)return; + + //compute orientation at modes and sort by angle + float fo[2]; + fo[0] = modes[0].second*pin; + fo[1] = modes[1].second*pin; + dtype deltaAngle = 0; + if (fo[0] > fo[1]) + { + dtype t = fo[0]; + fo[0] = fo[1]; + fo[1] = t; + } + + deltaAngle = MIN(fo[1] - fo[0], fo[0] - fo[1] + (dtype)CV_PI); + // if angle too small => return invalid corner + if (deltaAngle <= 0.3)return; + + //set statistics: orientations + cornersEdge1[index][0] = cos(fo[0]); + cornersEdge1[index][1] = sin(fo[0]); + cornersEdge2[index][0] = cos(fo[1]); + cornersEdge2[index][1] = sin(fo[1]); +} + +float CornerDetAC::norm2d(cv::Point2f o) +{ + return sqrt(o.x*o.x + o.y*o.y); +} +//ؾҽǵ +void CornerDetAC::refineCorners(vector &cornors, Mat imgDu, Mat imgDv, Mat imgAngle, Mat imgWeight, float radius){ + // image dimensions + int width = imgDu.cols; + int height = imgDu.rows; + // for all corners do + for (int i = 0; i < cornors.size(); i++) + { + //extract current corner location + int cu = cornors[i].x; + int cv = cornors[i].y; + // estimate edge orientations + int startX, startY, ROIwidth, ROIheight; + startX = MAX(cu - radius, (dtype)0); + startY = MAX(cv - radius, (dtype)0); + ROIwidth = MIN(cu + radius + 1, (dtype)width - 1) - startX; + ROIheight = MIN(cv + radius + 1, (dtype)height - 1) - startY; + + Mat roiAngle, roiWeight; + roiAngle = imgAngle(Rect(startX, startY, ROIwidth, ROIheight)); + roiWeight = imgWeight(Rect(startX, startY, ROIwidth, ROIheight)); + edgeOrientations(roiAngle, roiWeight, i); + + // continue, if invalid edge orientations + if (cornersEdge1[i][0] == 0 && cornersEdge1[i][1] == 0 || cornersEdge2[i][0] == 0 && cornersEdge2[i][1] == 0) + continue; + // continue; + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + //% corner orientation refinement % + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + cv::Mat A1 = cv::Mat::zeros(cv::Size(2, 2), mtype); + cv::Mat A2 = cv::Mat::zeros(cv::Size(2, 2), mtype); + + for (int u = startX; u < startX + ROIwidth; u++) + for (int v = startY; v < startY + ROIheight; v++) + { + // pixel orientation vector + cv::Point2f o(imgDu.at(v, u), imgDv.at(v, u)); + float no = norm2d(o); + if (no < 0.1) + continue; + o = o / no; + // robust refinement of orientation 1 + dtype t0 = abs(o.x*cornersEdge1[i][0] + o.y*cornersEdge1[i][1]); + if (t0 < 0.25) // inlier ? + { + Mat addtion(1, 2, mtype); + addtion.col(0).setTo(imgDu.at(v, u)); + addtion.col(1).setTo(imgDv.at(v, u)); + Mat addtionu = imgDu.at(v, u)*addtion; + Mat addtionv = imgDv.at(v, u)*addtion; + for (int j = 0; j < A1.cols; j++) + { + A1.at(0, j) = A1.at(0, j) + addtionu.at(0, j); + A1.at(1, j) = A1.at(1, j) + addtionv.at(0, j); + } + } + // robust refinement of orientation 2 + dtype t1 = abs(o.x*cornersEdge2[i][0] + o.y*cornersEdge2[i][1]); + if (t1 < 0.25) // inlier ? + { + Mat addtion(1, 2, mtype); + addtion.col(0).setTo(imgDu.at(v, u)); + addtion.col(1).setTo(imgDv.at(v, u)); + Mat addtionu = imgDu.at(v, u)*addtion; + Mat addtionv = imgDv.at(v, u)*addtion; + for (int j = 0; j < A2.cols; j++) + { + A2.at(0, j) = A2.at(0, j) + addtionu.at(0, j); + A2.at(1, j) = A2.at(1, j) + addtionv.at(0, j); + } + } + }//end for + // set new corner orientation + cv::Mat v1, foo1; + cv::Mat v2, foo2; + cv::eigen(A1, v1, foo1); + cv::eigen(A2, v2, foo2); + cornersEdge1[i][0] = -foo1.at(1, 0); + cornersEdge1[i][1] = -foo1.at(1, 1); + cornersEdge2[i][0] = -foo2.at(1, 0); + cornersEdge2[i][1] = -foo2.at(1, 1); + + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + //% corner location refinement % + //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + cv::Mat G = cv::Mat::zeros(cv::Size(2, 2), mtype); + cv::Mat b = cv::Mat::zeros(cv::Size(1, 2), mtype); + for (int u = startX; u < startX + ROIwidth; u++) + for (int v = startY; v < startY + ROIheight; v++) + { + // pixel orientation vector + cv::Point2f o(imgDu.at(v, u), imgDv.at(v, u)); + float no = norm2d(o); + if (no < 0.1) + continue; + o = o / no; + //robust subpixel corner estimation + if (u != cu || v != cv)// % do not consider center pixel + { + //compute rel. position of pixel and distance to vectors + cv::Point2f w(u - cu, v - cv); + float wvv1 = w.x*cornersEdge1[i][0] + w.y*cornersEdge1[i][1]; + float wvv2 = w.x*cornersEdge2[i][0] + w.y*cornersEdge2[i][1]; + + cv::Point2f wv1(wvv1 * cornersEdge1[i][0], wvv1 * cornersEdge1[i][1]); + cv::Point2f wv2(wvv2 * cornersEdge2[i][0], wvv2 * cornersEdge2[i][1]); + cv::Point2f vd1(w.x - wv1.x, w.y - wv1.y); + cv::Point2f vd2(w.x - wv2.x, w.y - wv2.y); + dtype d1 = norm2d(vd1), d2 = norm2d(vd2); + //if pixel corresponds with either of the vectors / directions + if ((d1 < 3) && abs(o.x*cornersEdge1[i][0] + o.y*cornersEdge1[i][1]) < 0.25 \ + || (d2 < 3) && abs(o.x*cornersEdge2[i][0] + o.y*cornersEdge2[i][1]) < 0.25) + { + dtype du = imgDu.at(v, u), dv = imgDv.at(v, u); + cv::Mat uvt = (Mat_(2, 1) << u, v); + cv::Mat H = (Mat_(2, 2) << du*du, du*dv, dv*du, dv*dv); + G = G + H; + cv::Mat t = H*(uvt); + b = b + t; + } + } + }//endfor + //set new corner location if G has full rank + Mat s, u, v; + SVD::compute(G, s, u, v); + int rank = 0; + for (int k = 0; k < s.rows; k++) + { + if (s.at(k, 0) > 0.0001 || s.at(k, 0) < -0.0001)// not equal zero + { + rank++; + } + } + if (rank == 2) + { + cv::Mat mp = G.inv()*b; + cv::Point2f corner_pos_new(mp.at(0, 0), mp.at(1, 0)); + // % set corner to invalid, if position update is very large + if (norm2d(cv::Point2f(corner_pos_new.x - cu, corner_pos_new.y - cv)) >= 4) + { + cornersEdge1[i][0] = 0; + cornersEdge1[i][1] = 0; + cornersEdge2[i][0] = 0; + cornersEdge2[i][1] = 0; + } + else + { + cornors[i].x = mp.at(0, 0); + cornors[i].y = mp.at(1, 0); + } + } + else//otherwise: set corner to invalid + { + cornersEdge1[i][0] = 0; + cornersEdge1[i][1] = 0; + cornersEdge2[i][0] = 0; + cornersEdge2[i][1] = 0; + } + } +} + +//compute corner statistics +void CornerDetAC::cornerCorrelationScore(Mat img, Mat imgWeight, vector cornersEdge, float &score){ + //center + int c[] = { imgWeight.cols / 2, imgWeight.cols / 2 }; + + //compute gradient filter kernel(bandwith = 3 px) + Mat img_filter = Mat::ones(imgWeight.size(), imgWeight.type()); + img_filter = img_filter*-1; + for (int i = 0; i < imgWeight.cols; i++) + { + for (int j = 0; j < imgWeight.rows; j++) + { + Point2f p1 = Point2f(i - c[0], j - c[1]); + Point2f p2 = Point2f(p1.x*cornersEdge[0].x*cornersEdge[0].x + p1.y*cornersEdge[0].x*cornersEdge[0].y, + p1.x*cornersEdge[0].x*cornersEdge[0].y + p1.y*cornersEdge[0].y*cornersEdge[0].y); + Point2f p3 = Point2f(p1.x*cornersEdge[1].x*cornersEdge[1].x + p1.y*cornersEdge[1].x*cornersEdge[1].y, + p1.x*cornersEdge[1].x*cornersEdge[1].y + p1.y*cornersEdge[1].y*cornersEdge[1].y); + float norm1 = sqrt((p1.x - p2.x)*(p1.x - p2.x) + (p1.y - p2.y)*(p1.y - p2.y)); + float norm2 = sqrt((p1.x - p3.x)*(p1.x - p3.x) + (p1.y - p3.y)*(p1.y - p3.y)); + if (norm1 <= 1.5 || norm2 <= 1.5) + { + img_filter.ptr(j)[i] = 1; + } + } + } + + //normalize + Mat mean, std, mean1, std1; + meanStdDev(imgWeight, mean, std); + meanStdDev(img_filter, mean1, std1); + for (int i = 0; i < imgWeight.cols; i++) + { + for (int j = 0; j < imgWeight.rows; j++) + { + imgWeight.ptr(j)[i] = (dtype)(imgWeight.ptr(j)[i] - mean.ptr(0)[0]) / (dtype)std.ptr(0)[0]; + img_filter.ptr(j)[i] = (dtype)(img_filter.ptr(j)[i] - mean1.ptr(0)[0]) / (dtype)std1.ptr(0)[0]; + } + } + + //convert into vectors + vector vec_filter, vec_weight; + for (int i = 0; i < imgWeight.cols; i++) + { + for (int j = 0; j < imgWeight.rows; j++) + { + vec_filter.push_back(img_filter.ptr(j)[i]); + vec_weight.push_back(imgWeight.ptr(j)[i]); + } + } + + //compute gradient score + float sum = 0; + for (int i = 0; i < vec_weight.size(); i++) + { + sum += vec_weight[i] * vec_filter[i]; + } + sum = (dtype)sum / (dtype)(vec_weight.size() - 1); + dtype score_gradient = sum >= 0 ? sum : 0; + + //create intensity filter kernel + Mat kernelA, kernelB, kernelC, kernelD; + createkernel(atan2(cornersEdge[0].y, cornersEdge[0].x), atan2(cornersEdge[1].y, cornersEdge[1].x), c[0], kernelA, kernelB, kernelC, kernelD);//1.1 ֺ + + //checkerboard responses + float a1, a2, b1, b2; + a1 = kernelA.dot(img); + a2 = kernelB.dot(img); + b1 = kernelC.dot(img); + b2 = kernelD.dot(img); + + float mu = (a1 + a2 + b1 + b2) / 4; + + float score_a = (a1 - mu) >= (a2 - mu) ? (a2 - mu) : (a1 - mu); + float score_b = (mu - b1) >= (mu - b2) ? (mu - b2) : (mu - b1); + float score_1 = score_a >= score_b ? score_b : score_a; + + score_b = (b1 - mu) >= (b2 - mu) ? (b2 - mu) : (b1 - mu); + score_a = (mu - a1) >= (mu - a2) ? (mu - a2) : (mu - a1); + float score_2 = score_a >= score_b ? score_b : score_a; + + float score_intensity = score_1 >= score_2 ? score_1 : score_2; + score_intensity = score_intensity > 0.0 ? score_intensity : 0.0; + + score = score_gradient*score_intensity; +} +//score corners +void CornerDetAC::scoreCorners(Mat img, Mat imgAngle, Mat imgWeight, vector &cornors, vector radius, vector &score){ + //for all corners do + for (int i = 0; i < cornors.size(); i++) + { + //corner location + int u = cornors[i].x+0.5; + int v = cornors[i].y+0.5; + if (i == 278) + { + int aaa = 0; + } + //compute corner statistics @ radius 1 + vector scores; + for (int j = 0; j < radius.size(); j++) + { + scores.push_back(0); + int r = radius[j]; + if (u > r&&u <= (img.cols - r - 1) && v>r && v <= (img.rows - r - 1)) + { + int startX, startY, ROIwidth, ROIheight; + startX = u - r; + startY = v - r; + ROIwidth = 2 * r + 1; + ROIheight = 2 * r + 1; + + Mat sub_img = img(Rect(startX, startY, ROIwidth, ROIheight)).clone(); + Mat sub_imgWeight = imgWeight(Rect(startX, startY, ROIwidth, ROIheight)).clone(); + vector cornersEdge; + cornersEdge.push_back(Point2f((float)cornersEdge1[i][0], (float)cornersEdge1[i][1])); + cornersEdge.push_back(Point2f((float)cornersEdge2[i][0], (float)cornersEdge2[i][1])); + cornerCorrelationScore(sub_img, sub_imgWeight, cornersEdge, scores[j]); + } + } + //take highest score + score.push_back(*max_element(begin(scores), end(scores))); + } + +} + + +void CornerDetAC::detectCorners(Mat &Src, vector &resultCornors, Corners& mcorners, dtype scoreThreshold, bool isrefine) +{ + Mat gray, imageNorm; + gray = Mat(Src.size(), CV_8U); + + // convert to double grayscale image + if (Src.channels() == 3) + { + cvtColor(Src, gray, COLOR_BGR2GRAY); + } + else + { + gray = Src.clone(); + } + + cv::GaussianBlur(gray, gray, cv::Size(9,9), 1.5); + + + // scale input image + normalize(gray, imageNorm, 0, 1, cv::NORM_MINMAX, mtype); + //gray.convertTo(imageNorm, CV_32F, 1 / 255.0); + + // filter image + Mat imgCorners = Mat::zeros(imageNorm.size(), mtype); + + Mat imgCornerA1(imageNorm.size(), mtype); + Mat imgCornerB1(imageNorm.size(), mtype); + Mat imgCornerC1(imageNorm.size(), mtype); + Mat imgCornerD1(imageNorm.size(), mtype); + + Mat imgCornerA(imageNorm.size(), mtype); + Mat imgCornerB(imageNorm.size(), mtype); + Mat imgCorner1(imageNorm.size(), mtype); + Mat imgCorner2(imageNorm.size(), mtype); + Mat imgCornerMean(imageNorm.size(), mtype); + + std::cout << "begin filtering !" << std::endl; + double t = (double)getTickCount(); + + //#pragma omp parallel for num_threads(4) + for (int i = 0; i < 6; i++) + { + Mat kernelA1, kernelB1, kernelC1, kernelD1; + createkernel(templateProps[i].x, templateProps[i].y, radius[i / 2], kernelA1, kernelB1, kernelC1, kernelD1);//1.1 ֺ + + //std::cout << "kernelA:" << kernelA1 << endl << "kernelB:" << kernelB1 << endl + // << "kernelC:" << kernelC1 << endl << "kernelD:" << kernelD1 << endl; + // filter image according with current template +#if 1 + imgCornerA1 = corealgmatlab::conv2(imageNorm, kernelA1, CONVOLUTION_SAME); + imgCornerB1 = corealgmatlab::conv2(imageNorm, kernelB1, CONVOLUTION_SAME); + imgCornerC1 = corealgmatlab::conv2(imageNorm, kernelC1, CONVOLUTION_SAME); + imgCornerD1 = corealgmatlab::conv2(imageNorm, kernelD1, CONVOLUTION_SAME); +#else + filter2D(imageNorm, imgCornerA1, mtype, kernelA1);//a1 + filter2D(imageNorm, imgCornerB1, mtype, kernelB1);//a2 + filter2D(imageNorm, imgCornerC1, mtype, kernelC1);//b1 + filter2D(imageNorm, imgCornerD1, mtype, kernelD1);//b2 +#endif + //compute mean + imgCornerMean = (imgCornerA1 + imgCornerB1 + imgCornerC1 + imgCornerD1) / 4.0;//1.3 չʽм + // case 1: a = white, b = black + getMin(imgCornerA1 - imgCornerMean, imgCornerB1 - imgCornerMean, imgCornerA); + getMin(imgCornerMean - imgCornerC1, imgCornerMean - imgCornerD1, imgCornerB); + getMin(imgCornerA, imgCornerB, imgCorner1); + // case 2: b = white, a = black + getMin(imgCornerMean - imgCornerA1, imgCornerMean - imgCornerB1, imgCornerA); + getMin(imgCornerC1 - imgCornerMean, imgCornerD1 - imgCornerMean, imgCornerB); + getMin(imgCornerA, imgCornerB, imgCorner2); + + // update corner map + getMax(imgCorners, imgCorner1, imgCorners); + getMax(imgCorners, imgCorner2, imgCorners); + } +#ifdef show_ + namedWindow("ROI");//ڣʾԭʼͼ + imshow("ROI", imgCorners); waitKey(10); +#endif + + t = ((double)getTickCount() - t) / getTickFrequency(); + std::cout << "filtering time cost :" << t << std::endl; + + // extract corner candidates via non maximum suppression + nonMaximumSuppression(imgCorners, cornerPoints, 3, 0.025, 5);//1.5 Ǽֵ㷨йˣȡ̸ǵ + + //post processing + + Mat imageDu(gray.size(), mtype); + Mat imageDv(gray.size(), mtype); + + Mat img_angle = cv::Mat::zeros(gray.size(), mtype); + Mat img_weight = cv::Mat::zeros(gray.size(), mtype); + + getImageAngleAndWeight(imageNorm, imageDu, imageDv, img_angle, img_weight); + if (isrefine == true) + { + //subpixel refinement + refineCorners(cornerPoints, imageDu, imageDv, img_angle, img_weight, 10); + if (cornerPoints.size() > 0) + { + for (int i = 0; i < cornerPoints.size(); i++) + { + if (cornersEdge1[i][0] == 0 && cornersEdge1[i][0] == 0) + { + cornerPoints[i].x = 0; cornerPoints[i].y = 0; + } + } + } + } + + //remove corners without edges + + //score corners + vector score; + scoreCorners(imageNorm, img_angle, img_weight, cornerPoints, radius, score); + +#ifdef show_ + namedWindow("src");//ڣʾԭʼͼ + imshow("src", Src); + waitKey(0); +#endif + + // remove low scoring corners + int nlen = cornerPoints.size(); + if (nlen > 0) + { + for (int i = 0; i < nlen;i++) + { + if (score[i] > scoreThreshold) + { + mcorners.p.push_back(cornerPoints[i]); + mcorners.v1.push_back(cv::Vec2f(cornersEdge1[i][0], cornersEdge1[i][1])); + mcorners.v2.push_back(cv::Vec2f(cornersEdge2[i][0], cornersEdge2[i][1])); + mcorners.score.push_back(score[i]); + } + } + } + + std::vector corners_n1(mcorners.p.size()); + for (int i = 0; i < corners_n1.size(); i++) + { + if (mcorners.v1[i][0] + mcorners.v1[i][1] < 0.0) + { + mcorners.v1[i] = -mcorners.v1[i]; + } + corners_n1[i] = mcorners.v1[i]; + float flipflag = corners_n1[i][0] * mcorners.v2[i][0] + corners_n1[0][1] * mcorners.v2[i][1]; + if (flipflag > 0) + flipflag = -1; + else + flipflag = 1; + mcorners.v2[i] = flipflag * mcorners.v2[i]; + } +} + diff --git a/CornerDetAC.h b/CornerDetAC.h new file mode 100644 index 0000000..855e9f0 --- /dev/null +++ b/CornerDetAC.h @@ -0,0 +1,65 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* part of source code come from https://github.com/qibao77/cornerDetect */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + + +#pragma once + +#include "opencv2/opencv.hpp" +#include +#include + +#include "HeaderCB.h" + +#if 1 +#define mtype CV_32F +#define dtype float +#else +#define mtype CV_64F +#define dtype double +#endif + +class CornerDetAC +{ +public: + CornerDetAC(); + ~CornerDetAC(); + CornerDetAC(cv::Mat img); + void detectCorners(cv::Mat &Src, std::vector &resultCornors, Corners& mcorners, dtype scoreThreshold, bool isrefine = true); + void chessboardsFromCorners(std::vector>chessboards); +private: + + dtype normpdf(dtype dist, dtype mu, dtype sigma); + + void getMin(cv::Mat src1, cv::Mat src2, cv::Mat &dst); + + void getMax(cv::Mat src1, cv::Mat src2, cv::Mat &dst); + + void getImageAngleAndWeight(cv::Mat img, cv::Mat &imgDu, cv::Mat &imgDv, cv::Mat &imgAngle, cv::Mat &imgWeight); + + void edgeOrientations(cv::Mat imgAngle, cv::Mat imgWeight, int index); + + void findModesMeanShift(std::vector hist, std::vector &hist_smoothed, std::vector> &modes, dtype sigma); + + void scoreCorners(cv::Mat img, cv::Mat imgAngle, cv::Mat imgWeight, std::vector &cornors, std::vector radius, std::vector &score); + + void cornerCorrelationScore(cv::Mat img, cv::Mat imgWeight, std::vector cornersEdge, float &score); + + void refineCorners(std::vector &cornors, cv::Mat imgDu, cv::Mat imgDv, cv::Mat imgAngle, cv::Mat imgWeight, float radius); + + void createkernel(float angle1, float angle2, int kernelSize, cv::Mat &kernelA, cv::Mat &kernelB, cv::Mat &kernelC, cv::Mat &kernelD); + + void nonMaximumSuppression(cv::Mat& inputCorners, std::vector& outputCorners, int patchSize, dtype threshold, int margin); + float norm2d(cv::Point2f o); + std::vector templateProps; + std::vector radius; + std::vector cornerPoints; + std::vector> cornersEdge1; + std::vector > cornersEdge2; + std::vector cornerPointsRefined; + +}; + diff --git a/HeaderCB.h b/HeaderCB.h new file mode 100644 index 0000000..8c6208d --- /dev/null +++ b/HeaderCB.h @@ -0,0 +1,75 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* part of source code come from https://github.com/qibao77/cornerDetect */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ +#pragma once + +#include "opencv2/opencv.hpp" +#include +#include "stdio.h" + +#define logd std::printf; + +struct Corners +{ + std::vector p; + std::vector v1; + std::vector v2; + std::vector score; +} ; + + +struct ConerInfo +{ + cv::Point2f p; + int chessidx; + int row, col; + int idx; + float x, y; + bool vaild; + int neardiskidx; + float nearestdiskdistance; + + ConerInfo operator=(const ConerInfo& value) + { + p = value.p; + chessidx = value.chessidx; + row = value.row; + col = value.col; + idx = value.idx; + x = value.x; + y = value.y; + vaild = value.vaild; + neardiskidx = value.neardiskidx; + nearestdiskdistance = value.nearestdiskdistance; + + return *this; + } + ConerInfo() + { + vaild = true; + neardiskidx = -1; + nearestdiskdistance = -1.0; + } + +}; +struct ImageChessesStruct +{ + std::vector > flagpostion; + std::vector > > idxconersflags; + std::vectorchoosecorneri; + + cv::Rect rt; + int cbnum; + std::vector > chesscorners; + bool flagbeginfromzero; + ImageChessesStruct& operator=( ImageChessesStruct& value) + { + cbnum = value.cbnum; + chesscorners = value.chesscorners; + return *this; + } + +}; diff --git a/corealgmatlab.cpp b/corealgmatlab.cpp new file mode 100644 index 0000000..4d8c1a6 --- /dev/null +++ b/corealgmatlab.cpp @@ -0,0 +1,41 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* part of source code come from https://github.com/qibao77/cornerDetect */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + +#include "corealgmatlab.h" + +using namespace cv; + +corealgmatlab::corealgmatlab() +{ +} + + +corealgmatlab::~corealgmatlab() +{ +} +Mat corealgmatlab::conv2(const Mat &img, const Mat& ikernel, ConvolutionType type) +{ + Mat dest; + Mat kernel; + flip(ikernel, kernel, -1); + Mat source = img; + if (CONVOLUTION_FULL == type) + { + source = Mat(); + const int additionalRows = kernel.rows - 1, additionalCols = kernel.cols - 1; + copyMakeBorder(img, source, (additionalRows + 1) / 2, additionalRows / 2, (additionalCols + 1) / 2, additionalCols / 2, BORDER_CONSTANT, Scalar(0)); + } + Point anchor(kernel.cols - kernel.cols / 2 - 1, kernel.rows - kernel.rows / 2 - 1); + int borderMode = BORDER_CONSTANT; + filter2D(source, dest, img.depth(), kernel, anchor, 0, borderMode); + + if (CONVOLUTION_VALID == type) + { + dest = dest.colRange((kernel.cols - 1) / 2, dest.cols - kernel.cols / 2).rowRange((kernel.rows - 1) / 2, dest.rows - kernel.rows / 2); + } + return dest; +} diff --git a/corealgmatlab.h b/corealgmatlab.h new file mode 100644 index 0000000..e796b26 --- /dev/null +++ b/corealgmatlab.h @@ -0,0 +1,21 @@ +#pragma once + +#include "opencv2/opencv.hpp" +enum ConvolutionType { + /* Return the full convolution, including border */ + CONVOLUTION_FULL, + + /* Return only the part that corresponds to the original image */ + CONVOLUTION_SAME, + /* Return only the submatrix containing elements that were not influenced by the border */ + CONVOLUTION_VALID +}; + +class corealgmatlab +{ +public: + corealgmatlab(); + ~corealgmatlab(); + static cv::Mat conv2(const cv::Mat &img, const cv::Mat& ikernel, ConvolutionType type); +}; + diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..e41e614 --- /dev/null +++ b/main.cpp @@ -0,0 +1,88 @@ +/* Copyright 2017 onlyliu(997737609@qq.com). */ +/* */ +/* Automatic Camera and Range Sensor Calibration using a single Shot */ +/* this project realize the papar: Automatic Camera and Range Sensor */ +/* Calibration using a single Shot */ + + + +#include "opencv2/opencv.hpp" + +#include +#include "CornerDetAC.h" +#include "ChessboradStruct.h" + +#include +#include +#include + +using namespace cv; +using namespace std; +using std::vector; +vector points; + +int main(int argc, char* argv[]) +{ + Mat src1; + cv::Mat src; + printf("read file...\n"); + string sr; + if(argc < 2) + sr = string("02.png"); + else + sr = string(argv[1]); + string simage, stxt, ssave; + simage = sr ;//+ ".bmp"; + stxt = sr + ".txt"; + ssave = sr + ".png"; + ssave = "./t/"+ssave; + src1 = imread(simage.c_str(), -1);//载入测试图像 + if (src1.channels() == 1) + { + src = src1.clone(); + } + else + { + if (src1.channels() == 3) + { + cv::cvtColor(src1, src, CV_BGR2GRAY); + } + else + { + if (src1.channels() == 4) + { + cv::cvtColor(src1, src, CV_BGRA2GRAY); + } + } + } + + if (src.empty()) + { + printf("Cannot read image file: %s\n", simage.c_str()); + return -1; + } + else + { + printf("read image file ok\n"); + } + + vector corners_p;//存储找到的角点 + + double t = (double)getTickCount(); + std::vector chessboards; + CornerDetAC corner_detector(src); + ChessboradStruct chessboardstruct; + + Corners corners_s; + corner_detector.detectCorners(src, corners_p, corners_s, 0.01); + + t = ((double)getTickCount() - t) / getTickFrequency(); + std::cout << "time cost :" << t << std::endl; + + ImageChessesStruct ics; + chessboardstruct.chessboardsFromCorners(corners_s, chessboards, 0.6); + chessboardstruct.drawchessboard(src1, corners_s, chessboards, "cb", 0); + + return 0; +} +