// The image is seperated into two parts, each part's length maximum loc+1;
// x is the seperation line, both two part have x;
float ReidDescriptor::sym_div(int x, cv::Mat img, cv::Mat MSK, int loc, float alpha)
{
int H = img.rows;
int W = img.cols;
int chs = img.channels();
cv::Mat imgL = img.colRange(0, x + 1); // [0,x]
cv::Mat imgR = img.colRange(x, img.cols);
cv::Mat MSK_L = MSK.colRange(0, x + 1);
cv::Mat MSK_R = MSK.colRange(x, MSK.cols);
int dimLoc = min(min(x + 1, MSK_R.cols), loc + 1);
if (dimLoc != 0)
{
cv::Mat imgLloc = img.colRange(x - dimLoc + 1, x + 1);//[x-dimLoc+1,x]
cv::Mat imgRloc;
cv::flip(imgR.colRange(0, dimLoc), imgRloc, 1);
cv::Mat temp;
cv::pow(imgLloc - imgRloc, 2, temp);
float ans = alpha * sqrt(cv::sum(temp.reshape(1))[0]) / dimLoc +
(1 - alpha) * (abs(sum(MSK_R)[0] - sum(MSK_L)[0])) / max(MSK_R.rows * MSK_R.cols, MSK_L.rows * MSK_L.cols);
return ans;
}
else
{
return 1;
}
}
void RandomFerns::get_linepoint(cv::Mat img,cv::Mat init_pose,std::vector<l1l2f1> line_pids, cv::Mat& feats){
int cs = init_pose.cols;
int w = img.cols;
int h = img.rows;
assert(cs==posdim);
assert(line_pids.size()==num_fea_locs);
feats = cv::Mat(num_fea_locs,1,CV_32FC1);
cv::Mat xs = init_pose.colRange(0,cs/2).clone();
cv::Mat ys = init_pose.colRange(cs/2,cs).clone();
float mx = cv::mean(xs).val[0];
float my = cv::mean(ys).val[0];
xs -= mx;
ys -= my;
for (int k =0; k < line_pids.size(); k++){
float p1x = xs.at<float>(0,line_pids[k].id1-1);
float p1y = ys.at<float>(0,line_pids[k].id1-1);
float p2x = xs.at<float>(0,line_pids[k].id2-1);
float p2y = ys.at<float>(0,line_pids[k].id2-1);
float a = (p2y-p1y)/(p2x-p1x);
float b = p1y - a*p1x;
float distx = (p2x-p1x)/2;
float ctrx = p1x + distx;
float cs1 = ctrx+line_pids[k].t1*distx;
float rs1 = a*cs1+b;
int cs = (int)(cs1+mx);
int rs = (int)(rs1+my);
// std::cout<< rs << " " << cs << std::endl;
feats.at<float>(k,0) = img.at<float>(max(0,min(h-1,rs)),max(0,min(w-1,cs)));
// std::cout << feats.at<float>(k,0) << std::endl;
}
}
void data_transformer_util::rotate_band(
cv::Mat image,
int shift_x_to_left,
int shift_y_to_top)
{
int actual_shift_x = (shift_x_to_left % image.cols);
if (actual_shift_x < 0)
actual_shift_x += image.cols;
int actual_shift_y = (shift_y_to_top % image.rows);
if (actual_shift_y < 0)
actual_shift_y += image.rows;
if ((actual_shift_x == 0) && (actual_shift_y == 0))
return;
cv::Mat cloned_image = image.clone();
if (actual_shift_y == 0)
{
cloned_image.colRange(actual_shift_x, image.cols).copyTo(image.colRange(0, image.cols - actual_shift_x));
cloned_image.colRange(0, actual_shift_x).copyTo(image.colRange(image.cols - actual_shift_x, image.cols));
}
else if (actual_shift_x == 0)
{
cloned_image.rowRange(actual_shift_y, image.rows).copyTo(image.rowRange(0, image.rows - actual_shift_y));
cloned_image.rowRange(0, actual_shift_y).copyTo(image.rowRange(image.rows - actual_shift_y, image.rows));
}
else
{
cloned_image.colRange(actual_shift_x, image.cols).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(0, image.rows - actual_shift_y));
cloned_image.colRange(0, actual_shift_x).rowRange(actual_shift_y, image.rows).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(0, image.rows - actual_shift_y));
cloned_image.colRange(actual_shift_x, image.cols).rowRange(0, actual_shift_y).copyTo(image.colRange(0, image.cols - actual_shift_x).rowRange(image.rows - actual_shift_y, image.rows));
cloned_image.colRange(0, actual_shift_x).rowRange(0, actual_shift_y).copyTo(image.colRange(image.cols - actual_shift_x, image.cols).rowRange(image.rows - actual_shift_y, image.rows));
}
}
cv::Rect_<T> get_enclosing_bbox(cv::Mat landmarks)
{
auto num_landmarks = landmarks.cols / 2;
double min_x_val, max_x_val, min_y_val, max_y_val;
cv::minMaxLoc(landmarks.colRange(0, num_landmarks), &min_x_val, &max_x_val);
cv::minMaxLoc(landmarks.colRange(num_landmarks, landmarks.cols), &min_y_val, &max_y_val);
return cv::Rect_<T>(min_x_val, min_y_val, max_x_val - min_x_val, max_y_val - min_y_val);
};
void projectpose(cv::Mat pose1, cv::Mat& pose2, cv::Rect bbox){
float sx = bbox.width/2.0;
float sy = bbox.height/2.0;
float ctrx = bbox.x + sx;
float ctry = bbox.y + sy;
pose2 = pose1.clone();
int nc = pose1.cols;
pose2.colRange(0,nc/2) -= ctrx;
pose2.colRange(0,nc/2) /= sx;
pose2.colRange(nc/2,nc) -= ctry;
pose2.colRange(nc/2,nc) /= sy;
}
void circshift(cv::Mat &A, int shitf_row, int shift_col) {
int row = A.rows, col = A.cols;
shitf_row = (row + (shitf_row % row)) % row;
shift_col = (col + (shift_col % col)) % col;
cv::Mat temp = A.clone();
if (shitf_row){
temp.rowRange(row - shitf_row, row).copyTo(A.rowRange(0, shitf_row));
temp.rowRange(0, row - shitf_row).copyTo(A.rowRange(shitf_row, row));
}
if (shift_col){
temp.colRange(col - shift_col, col).copyTo(A.colRange(0, shift_col));
temp.colRange(0, col - shift_col).copyTo(A.colRange(shift_col, col));
}
return;
}
void mergePoints(const std::vector<cv::Mat> &in_descriptors, const std::vector<cv::Mat> &in_points,
cv::Mat &out_descriptors, cv::Mat &out_points) {
// Figure out the number of points
size_t n_points = 0, n_images = in_descriptors.size();
for (size_t image_id = 0; image_id < n_images; ++image_id)
n_points += in_descriptors[image_id].rows;
if (n_points == 0)
return;
// Fill the descriptors and 3d points
out_descriptors = cv::Mat(n_points, in_descriptors[0].cols, in_descriptors[0].depth());
out_points = cv::Mat(1, n_points, CV_32FC3);
size_t row_index = 0;
for (size_t image_id = 0; image_id < n_images; ++image_id) {
// Copy the descriptors
const cv::Mat & descriptors = in_descriptors[image_id];
int n_points = descriptors.rows;
cv::Mat sub_descriptors = out_descriptors.rowRange(row_index, row_index + n_points);
descriptors.copyTo(sub_descriptors);
// Copy the 3d points
const cv::Mat & points = in_points[image_id];
cv::Mat sub_points = out_points.colRange(row_index, row_index + n_points);
points.copyTo(sub_points);
row_index += n_points;
}
}
void AkazeSymmetryMatcher::apply(cv::Mat &srcGray, cv::Mat &dst) {
// clear points
leftKeyPoints.clear();
rightKeyPoints.clear();
try {
detector_descriptor->detectAndCompute(srcGray.colRange(0, srcGray.cols / 2), cv::Mat(),
leftKeyPoints, leftDescriptors, false);
detector_descriptor->detectAndCompute(srcGray.colRange(srcGray.cols / 2, srcGray.cols),
cv::Mat(), rightKeyPoints, rightDescriptors, false);
if(leftKeyPoints.size() < 1 || rightKeyPoints.size() < 1 ) return;
matches.clear();
descriptorMatcher->match(leftDescriptors, rightDescriptors, matches, cv::Mat());
//Select the best matching points and draw them
float min_dist = 100;
for( int i = 0; i < leftDescriptors.rows; i++ )
{
float dist = matches[i].distance;
if( dist < min_dist ) min_dist = dist;
}
bestMatches.clear();
for( int i = 0; i < leftDescriptors.rows; i++ )
{
if( matches[i].distance <= 3 * min_dist )
{
bestMatches.push_back( matches[i]);
}
}
cv::drawMatches(srcGray.colRange(0, srcGray.cols / 2), leftKeyPoints,
srcGray.colRange(srcGray.cols / 2, srcGray.cols), rightKeyPoints,
bestMatches, dst);
} catch (cv::Exception& e){
// LOGD(LOG_TAG, e.msg.c_str());
}
}
bool checkCheirality (cv::Mat P, cv::Mat X) // cheirality constraint
// input: Project Matrix P 4x3, P = [R | t], Homogeneous 3D point X 4x1
// output: true - if X is in front of camera
{
if(X.cols * X.rows < 4) {
X = (cv::Mat_<double>(4,1)<<X.at<double>(0),X.at<double>(1),X.at<double>(2),1);
}
cv::Mat x = P*X;
return cv::determinant(P.colRange(0,3))*x.at<double>(2)*X.at<double>(3)>0;
}
NearestNeighbor(std::vector<cv::Mat> vm) :
confusion_Mat(3,3,CV_64F), NN_Mat(vm.size(),vm[0].cols,CV_64F)
{
std::cout << "cols are: " << vm[0].cols << std::endl;
for(size_t i = 0; i < vm.size();i++)
{
cv::Mat dest(NN_Mat.colRange(0,vm[0].cols));
vm[i].copyTo(dest);
}
}
void ofxIcp::compute_transformation(const vector<cv::Point3d> & input, const vector<cv::Point3d> & target, cv::Mat & transformation){
vector<cv::Point3d> transformed;
cv::Mat rotation, translation;
double error;
vector<cv::Point3d> closest_current;
vector<cv::Point3d> closest_target;
int increment = input.size()/5;
for(int i = 0; i < input.size(); i += increment){
closest_current.push_back(input[i]);
closest_target.push_back(target[i]);
}
cv::Mat centroid_current;
cv::Mat centroid_target;
cv::reduce(cv::Mat(closest_current, false), centroid_current, 0, CV_REDUCE_AVG);
cv::reduce(cv::Mat(closest_target, false), centroid_target, 0, CV_REDUCE_AVG);
centroid_current = centroid_current.reshape(1);
centroid_target = centroid_target.reshape(1);
compute_rigid_transformation(closest_current, centroid_current, closest_target, centroid_target, rotation, translation);
vector<cv::Point3d> transformed_closest_current;
vector<cv::Point3d> transformed_current;
transform(closest_current, rotation, translation, transformed_closest_current);
compute_error(transformed_closest_current, closest_target, error);
transformation = (cv::Mat_<double>(4,4) << 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
cv::Mat rotation_tmp = transformation.colRange(0,3).rowRange(0,3);
cv::Mat translation_tmp = transformation.colRange(3,4).rowRange(0,3);
cv::Mat translation_t = translation.reshape(1).t();
rotation.copyTo(rotation_tmp);
translation_t.copyTo(translation_tmp);
}
void convolveDFT(const cv::Mat& imgOriginal, const cv::Mat& kernel, cv::Mat& out, const bool& corr, const bool& full)
{ //this method is also valid for complex image and kernel, set DFT_COMPLEX_OUTPUT then
//convolution in fourier space, keep code for future use
//CONVOLUTION_FULL: Return the full convolution, including border
//to completeley emulate filter2D operation, image should be first double sized and then cut back to origianl size
cv::Mat source, kernelPadded;
const int marginSrcTop = corr ? std::ceil((kernel.rows-1)/2.0) : std::floor((kernel.rows-1)/2.0);
const int marginSrcBottom = corr ? std::floor((kernel.rows-1)/2.0) : std::ceil((kernel.rows-1)/2.0);
const int marginSrcLeft = corr ? std::ceil((kernel.cols-1)/2.0) : std::floor((kernel.cols-1)/2.0);
const int marginSrcRight = corr ? std::floor((kernel.cols-1)/2.0) : std::ceil((kernel.cols-1)/2.0);
cv::copyMakeBorder(imgOriginal, source, marginSrcTop, marginSrcBottom, marginSrcLeft, marginSrcRight, cv::BORDER_CONSTANT);
const int marginKernelTop = std::ceil((source.rows-kernel.rows)/2.0);
const int marginKernelBottom = std::floor((source.rows-kernel.rows)/2.0);
const int marginKernelLeft = std::ceil((source.cols-kernel.cols)/2.0);
const int marginKernelRight = std::floor((source.cols-kernel.cols)/2.0);
cv::copyMakeBorder(kernel, kernelPadded, marginKernelTop, marginKernelBottom, marginKernelLeft, marginKernelRight, cv::BORDER_CONSTANT);
//Divided by 2.0 instead of 2 to consider the result as double instead of as an int
//The +1 in the shift changes slightly the finest plane in the wavelet,
shift(kernelPadded, kernelPadded, std::ceil(kernelPadded.cols/2.0), std::ceil(kernelPadded.rows/2.0));
cv::Mat kernelPadded_ft, input_ft, output_ft;
cv::dft(kernelPadded, kernelPadded_ft, cv::DFT_COMPLEX_OUTPUT + cv::DFT_SCALE);
cv::dft(source, input_ft, cv::DFT_COMPLEX_OUTPUT + cv::DFT_SCALE);
cv::mulSpectrums(input_ft, kernelPadded_ft.mul(kernelPadded.total()), output_ft, cv::DFT_COMPLEX_OUTPUT, corr);
if(imgOriginal.channels() == 1 && kernel.channels() == 1) cv::idft(output_ft, out, cv::DFT_REAL_OUTPUT);
else cv::idft(output_ft, out, cv::DFT_COMPLEX_OUTPUT);
if(!full)
{
//colRange and rowRange are semi-open intervals. first included, last is not
//this frist option is what i think should be the correct one, but the next is what filter2D function gives for this inputs
// out = out.colRange(marginSrcLeft, out.cols - marginSrcRight).
// rowRange(marginSrcTop, out.rows - marginSrcBottom);
out = out.colRange(marginSrcRight, out.cols - marginSrcLeft).
rowRange(marginSrcBottom, out.rows - marginSrcTop);
}
}
void conv2D(const cv::Mat &img, cv::Mat& dest, const cv::Mat& kernel, ConvolutionType ctype, int btype) {
cv::Mat source = img;
cv::Mat flip_kernel(kernel.rows,kernel.cols,kernel.type());
cv::flip(kernel,flip_kernel,-1);
if(ctype == CONVOLUTION_FULL) {
source = cv::Mat();
const int additionalRows = kernel.rows-1, additionalCols = kernel.cols-1;
cv::copyMakeBorder(img, source, (additionalRows+1)/2, additionalRows/2, (additionalCols+1)/2, additionalCols/2, btype, cv::Scalar(0));
}
cv::Point anchor(kernel.cols - kernel.cols/2 - 1, kernel.rows - kernel.rows/2 - 1);
int borderMode = btype;
cv::filter2D(source, dest, img.depth(), flip_kernel, anchor, 0, borderMode);
if(ctype == CONVOLUTION_VALID) {
dest = dest.colRange((kernel.cols-1)/2, dest.cols - kernel.cols/2)
.rowRange((kernel.rows-1)/2, dest.rows - kernel.rows/2);
}
}
std::vector<Entry> HierClassifier::extractEntries( cv::Mat imageBGR,
cv::Mat terrain,
cv::Mat regionsOnImage,
cv::Mat maskIgnore,
int entryWeightThreshold)
{
using namespace std::chrono;
high_resolution_clock::time_point start = high_resolution_clock::now();
if(maskIgnore.empty()){
maskIgnore = Mat(imageBGR.rows, imageBGR.cols, CV_32SC1, Scalar(0));
}
//namedWindow("imageBGR");
Mat imageHSV;
cvtColor(imageBGR, imageHSV, CV_BGR2HSV);
vector<Pixel> pixels;
//pixels.resize(imageHSV.rows * imageHSV.cols);
for(int r = 0; r < imageHSV.rows; r++){
for(int c = 0; c < imageHSV.cols; c++){
if(maskIgnore.at<int>(r, c) == 0){
pixels.push_back(Pixel(r, c, regionsOnImage.at<int>(r, c)));
}
}
}
sort(pixels.begin(), pixels.end());
vector<pair<int, int> > terrainRegion;
if(!terrain.empty()){
Mat terrainPointsImage(terrain.cols, 2, CV_32FC1);
Mat tmpTerrain = terrain.rowRange(0, 3).t();
Mat tvec(1, 3, CV_32FC1, Scalar(0));
Mat rvec(1, 3, CV_32FC1, Scalar(0));
Mat distCoeffs(1, 5, CV_32FC1, Scalar(0));
projectPoints(tmpTerrain, tvec, rvec, cameraMatrix, Mat(), terrainPointsImage);
//terrainPointsImage = terrainPointsImage.t();
terrainPointsImage = terrainPointsImage.reshape(1).t();
//cout << tmpTerrain.rowRange(1, 10) << endl;
//cout << terrainPointsImage.rowRange(1, 10) << endl;
//cout << cameraMatrix << endl;
for(int p = 0; p < terrain.cols; p++){
int imageRow = round(terrainPointsImage.at<float>(1, p));
int imageCol = round(terrainPointsImage.at<float>(0, p));
if(imageRow >= 0 && imageRow < imageBGR.rows &&
imageCol >= 0 && imageCol < imageBGR.cols &&
maskIgnore.at<int>(imageRow, imageCol) == 0)
{
int region = regionsOnImage.at<int>(imageRow, imageCol);
terrainRegion.push_back(pair<int, int>(region, p));
}
}
sort(terrainRegion.begin(), terrainRegion.end());
}
high_resolution_clock::time_point endSorting = high_resolution_clock::now();
if(debugLevel >= 1){
cout << "End sorting terrain, terrainRegion.size() = " << terrainRegion.size() << endl;
}
vector<Entry> ret;
int endIm = 0;
int endTer = 0;
while(endIm < pixels.size()){
Mat values, valuesTer, histogramHS, histogramV, statisticsHSV, stisticsLaser;
int begIm = endIm;
while(pixels[begIm].imageId == pixels[endIm].imageId){
endIm++;
if(endIm == pixels.size()){
break;
}
}
if(debugLevel >= 1){
cout << "segment id = " << pixels[begIm].imageId << ", begIm = " << begIm << ", endIm = " << endIm << endl;
}
values = Mat(1, endIm - begIm, CV_8UC3);
for(int p = begIm; p < endIm; p++){
values.at<Vec3b>(p - begIm) = imageHSV.at<Vec3b>(pixels[p].r, pixels[p].c);
}
int begTer = endTer;
if(begTer < terrainRegion.size()){
while(terrainRegion[begTer].first != pixels[begIm].imageId){
begTer++;
if(begTer >= terrainRegion.size()){
break;
}
}
}
endTer = begTer;
if(endTer < terrainRegion.size()){
while(terrainRegion[begTer].first == terrainRegion[endTer].first){
endTer++;
if(endTer >= terrainRegion.size()){
break;
}
}
}
if(endTer - begTer > 0){
valuesTer = Mat(terrain.rows, endTer - begTer, CV_32FC1);
Mat tmpImageBGR(imageBGR);
for(int p = begTer; p < endTer; p++){
//cout << terrainRegion[p].second << endl;
terrain.colRange(terrainRegion[p].second, terrainRegion[p].second + 1).copyTo(valuesTer.colRange(p - begTer, p - begTer + 1));
//cout << "terrainRegion[p].second = " << terrainRegion[p].second << endl;
//int imageRow = round(terrainPointsImage.at<float>(1 ,terrainRegion[p].second));
//int imageCol = round(terrainPointsImage.at<float>(0, terrainRegion[p].second));
//cout << "Point: " << imageRow << ", " << imageCol << endl;
//tmpImageBGR.at<Vec3b>(imageRow, imageCol) = Vec3b(0x00, 0x00, 0xff);
}
//cout << "ImageId = " << pixels[begIm].imageId << endl;
//imshow("imageBGR", imageBGR);
//waitKey();
}
else{
//cout << "Warning - no terrain values for imageId " << pixels[begIm].imageId << endl;
valuesTer = Mat(6, 1, CV_32FC1, Scalar(0));
}
if(endIm - begIm > 0){
int channelsHS[] = {0, 1};
float rangeH[] = {0, 60};
float rangeS[] = {0, 256};
const float* rangesHS[] = {rangeH, rangeS};
int sizeHS[] = {histHLen, histSLen};
int channelsV[] = {2};
float rangeV[] = {0, 256};
const float* rangesV[] = {rangeV};
int sizeV[] = {histVLen};
calcHist(&values, 1, channelsHS, Mat(), histogramHS, 2, sizeHS, rangesHS);
calcHist(&values, 1, channelsV, Mat(), histogramV, 1, sizeV, rangesV);
histogramHS = histogramHS.reshape(0, 1);
histogramV = histogramV.reshape(0, 1);
normalize(histogramHS, histogramHS);
normalize(histogramV, histogramV);
if(debugLevel >= 1){
cout << "V size = " << histogramV.size() << ", HS size = " << histogramHS.size() << endl;
}
values = values.reshape(1, 3);
//cout << "values size = " << values.size() << endl;
Mat covarHSV;
Mat meanHSV;
calcCovarMatrix(values,
covarHSV,
meanHSV,
CV_COVAR_NORMAL | CV_COVAR_SCALE | CV_COVAR_COLS,
CV_32F);
if(debugLevel >= 1){
cout << "Calculated covar matrix" << endl;
}
covarHSV = covarHSV.reshape(0, 1);
meanHSV = meanHSV.reshape(0, 1);
//normalize(covarHSV, covarHSV);
//normalize(meanHSV, meanHSV);
Mat covarLaser, meanLaser;
calcCovarMatrix(valuesTer.rowRange(4, 6),
covarLaser,
meanLaser,
CV_COVAR_NORMAL | CV_COVAR_SCALE | CV_COVAR_COLS,
CV_32F);
covarLaser = covarLaser.reshape(0, 1);
meanLaser = meanLaser.reshape(0, 1);
//normalize(covarLaser, covarLaser);
//normalize(meanLaser, meanLaser);
//cout << "covarLaser = " << covarLaser << endl;
//cout << "meanLaser = " << meanLaser << endl;
//cout << "Entry " << ret.size() << endl;
//cout << "histHS = " << histogramHS << endl;
//cout << "histV = " << histogramV << endl;
//cout << "covarHSV = " << covarHSV << endl;
//cout << "meanHSV = " << meanHSV << endl;
Mat kurtLaser(1, 2, CV_32FC1);
Mat tmpVal;
valuesTer.rowRange(4, 6).copyTo(tmpVal);
//cout << "tmpVal = " << tmpVal << endl;
//cout << "mean(0) = " << meanLaser.at<float>(0) << ", mean(1) = " << meanLaser.at<float>(1) << endl;
//cout << "stdDev^4(0) = " << pow(covarLaser.at<float>(0), 2) << ", stdDev^4(3) = " << pow(covarLaser.at<float>(3), 2) << endl;
tmpVal.rowRange(0, 1) -= meanLaser.at<float>(0);
tmpVal.rowRange(1, 2) -= meanLaser.at<float>(1);
pow(tmpVal, 4, tmpVal);
kurtLaser.at<float>(0) = sum(tmpVal.rowRange(0, 1))(0);
if(tmpVal.cols * pow(covarLaser.at<float>(0), 2) != 0){
kurtLaser.at<float>(0) = kurtLaser.at<float>(0) / (tmpVal.cols * pow(covarLaser.at<float>(0), 2)) - 3;
}
kurtLaser.at<float>(1) = sum(tmpVal.rowRange(1, 2))(0);
if(tmpVal.cols * pow(covarLaser.at<float>(3), 2) != 0){
kurtLaser.at<float>(1) = kurtLaser.at<float>(1) / (tmpVal.cols * pow(covarLaser.at<float>(3), 2)) - 3;
}
Mat histogramDI;
int channelsDI[] = {0, 1};
float rangeD[] = {1500, 2500};
float rangeI[] = {1800, 4000};
const float* rangesDI[] = {rangeD, rangeI};
int sizeDI[] = {histDLen, histILen};
Mat valHistD = valuesTer.rowRange(4, 5);
Mat valHistI = valuesTer.rowRange(5, 6);
Mat valuesHistDI[] = {valHistD, valHistI};
calcHist(valuesHistDI, 2, channelsDI, Mat(), histogramDI, 2, sizeDI, rangesDI);
histogramDI = histogramDI.reshape(0, 1);
normalize(histogramDI, histogramDI);
//cout << "histogramDI = " << histogramDI << endl;
Entry tmp;
tmp.imageId = pixels[begIm].imageId;
tmp.weight = (endIm - begIm)/* + (endTer - begTer)*/;
tmp.descriptor = Mat(1, histHLen*histSLen +
histVLen +
meanHSVLen +
covarHSVLen +
histDLen*histILen +
meanLaserLen +
covarLaserLen,
CV_32FC1);
int begCol = 0;
histogramHS.copyTo(tmp.descriptor.colRange(begCol, begCol + histHLen*histSLen));
begCol += histHLen*histSLen;
histogramV.copyTo(tmp.descriptor.colRange(begCol, begCol + histVLen));
begCol += histVLen;
meanHSV.copyTo(tmp.descriptor.colRange(begCol, begCol + meanHSVLen));
begCol += meanHSVLen;
covarHSV.copyTo(tmp.descriptor.colRange(begCol, begCol + covarHSVLen));
begCol += covarHSVLen;
histogramDI.copyTo(tmp.descriptor.colRange(begCol, begCol + histDLen*histILen));
begCol += histDLen*histILen;
meanLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + meanLaserLen));
begCol += meanLaserLen;
covarLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + covarLaserLen));
begCol += covarLaserLen;
//kurtLaser.copyTo(tmp.descriptor.colRange(begCol, begCol + kurtLaserLen));
//begCol += kurtLaserLen;
//cout << "descriptor = " << tmp.descriptor << endl;
if(endIm - begIm > entryWeightThreshold){
ret.push_back(tmp);
}
}
}
static duration<double> sortingTime = duration<double>::zero();
static duration<double> compTime = duration<double>::zero();
static duration<double> wholeTime = duration<double>::zero();
static int times = 0;
high_resolution_clock::time_point endComp = high_resolution_clock::now();
sortingTime += duration_cast<duration<double> >(endSorting - start);
compTime += duration_cast<duration<double> >(endComp - endSorting);
wholeTime += duration_cast<duration<double> >(endComp - start);
times++;
if(debugLevel >= 1){
cout << "Times: " << times << endl;
cout << "Extract Average sorting time: " << sortingTime.count()/times << endl;
cout << "Extract Average computing time: " << compTime.count()/times << endl;
cout << "Extract Average whole time: " << wholeTime.count()/times << endl;
}
return ret;
}
void ConstraintsHelpers::processPointCloud(cv::Mat hokuyoData,
cv::Mat& pointCloudOrigMapCenter,
std::queue<PointsPacket>& pointsInfo,
std::chrono::high_resolution_clock::time_point hokuyoTimestamp,
std::chrono::high_resolution_clock::time_point curTimestamp,
cv::Mat curPosOrigMapCenter,
std::mutex& mtxPointCloud,
cv::Mat cameraOrigLaser,
cv::Mat cameraOrigImu,
ros::NodeHandle &nh)
{
Mat hokuyoCurPoints(6, hokuyoData.cols, CV_32FC1);
int minLaserDist, pointCloudTimeout;
nh.getParam("minLaserDist", minLaserDist);
nh.getParam("pointCloudTimeout", pointCloudTimeout);
//cout << "Copying hokuyo data" << endl;
int countPoints = 0;
for(int c = 0; c < hokuyoData.cols; c++){
//cout << hokuyoData.at<int>(2, c) << endl;
if(hokuyoData.at<int>(2, c) > minLaserDist){
hokuyoCurPoints.at<float>(0, countPoints) = -hokuyoData.at<int>(1, c);
hokuyoCurPoints.at<float>(1, countPoints) = 0.0;
hokuyoCurPoints.at<float>(2, countPoints) = hokuyoData.at<int>(0, c);
hokuyoCurPoints.at<float>(3, countPoints) = 1.0;
hokuyoCurPoints.at<float>(4, countPoints) = hokuyoData.at<int>(2, c);
hokuyoCurPoints.at<float>(5, countPoints) = hokuyoData.at<int>(3, c);
countPoints++;
}
}
hokuyoCurPoints = hokuyoCurPoints.colRange(0, countPoints);
// cout << "hokuyoCurPoints.size() = " << hokuyoCurPoints.size() << endl;
// cout << "hokuyoData.size() = " << hokuyoData.size() << endl;
//cout << "Removing old points" << endl;
//remove all points older than pointCloudTimeout ms
int pointsSkipped = 0;
if(pointsInfo.size() > 0){
//cout << "dt = " << std::chrono::duration_cast<std::chrono::milliseconds>(curTimestamp - pointsQueue.front().timestamp).count() << endl;
while(std::chrono::duration_cast<std::chrono::milliseconds>(curTimestamp - pointsInfo.front().timestamp).count() > pointCloudTimeout){
pointsSkipped += pointsInfo.front().numPoints;
pointsInfo.pop();
if(pointsInfo.size() == 0){
break;
}
}
}
std::unique_lock<std::mutex> lck(mtxPointCloud);
//cout << "Moving pointCloudOrigMapCenter, pointsSkipped = " << pointsSkipped << endl;
Mat tmpAllPoints(hokuyoCurPoints.rows, pointCloudOrigMapCenter.cols + hokuyoCurPoints.cols - pointsSkipped, CV_32FC1);
if(!pointCloudOrigMapCenter.empty()){
//cout << "copyTo" << endl;
if(pointsSkipped < pointCloudOrigMapCenter.cols){
pointCloudOrigMapCenter.colRange(pointsSkipped,
pointCloudOrigMapCenter.cols).
copyTo(tmpAllPoints.colRange(0, pointCloudOrigMapCenter.cols - pointsSkipped));
}
}
// if(debugLevel >= 1){
// cout << "countPoints = " << countPoints << ", hokuyoCurPoints.size = " << hokuyoCurPoints.size() << endl;
// }
if(countPoints > 0){
//cout << "Moving to camera orig" << endl;
hokuyoCurPoints.rowRange(0, 4) = cameraOrigLaser.inv()*hokuyoCurPoints.rowRange(0, 4);
//cout << "Addding hokuyoCurPoints" << endl;
// if(debugLevel >= 1){
// cout << "Addding hokuyoCurPoints" << endl;
// }
Mat curPointCloudOrigMapCenter(hokuyoCurPoints.rows, hokuyoCurPoints.cols, CV_32FC1);
Mat tmpCurPoints = curPosOrigMapCenter*cameraOrigImu*hokuyoCurPoints.rowRange(0, 4);
tmpCurPoints.copyTo(curPointCloudOrigMapCenter.rowRange(0, 4));
hokuyoCurPoints.rowRange(4, 6).copyTo(curPointCloudOrigMapCenter.rowRange(4, 6));
//cout << hokuyoCurPointsGlobal.channels() << ", " << hokuyoAllPointsGlobal.channels() << endl;
// cout << pointCloudOrigMapCenter.size() << " " << curPointCloudCameraMapCenter.size() << " " << tmpAllPoints.size() << endl;
// if(debugLevel >= 1){
// cout << "pointsSkipped = " << pointsSkipped << endl;
// }
curPointCloudOrigMapCenter.copyTo(tmpAllPoints.colRange(pointCloudOrigMapCenter.cols - pointsSkipped,
pointCloudOrigMapCenter.cols + hokuyoCurPoints.cols - pointsSkipped));
// if(debugLevel >= 1){
// cout << "curPointCloudCameraMapCenter copied" << endl;
// }
pointsInfo.push(PointsPacket(hokuyoTimestamp, hokuyoCurPoints.cols));
pointCloudOrigMapCenter = tmpAllPoints;
}
lck.unlock();
}
void FSolver::normalizePoints(const cv::Mat &P, cv::Mat &Q) const
{
// turn P into homogeneous coords first
if(P.rows == 3) // P is 3xN
{
if(P.type() == CV_64F)
Q = P.clone();
else
P.convertTo(Q, CV_64F);
cv::Mat aux = Q.row(0);
cv::divide(aux, Q.row(2), aux);
aux = Q.row(1);
cv::divide(aux, Q.row(2), aux);
}
else if(P.rows == 2) // P is 2xN
{
const int N = P.cols;
Q.create(3, N, CV_64F);
Q.row(2) = 1;
if(P.type() == CV_64F)
{
Q.rowRange(0,2) = P * 1;
}
else
{
cv::Mat aux = Q.rowRange(0,2);
P.convertTo(aux, CV_64F);
}
}
else if(P.cols == 3) // P is Nx3
{
if(P.type() == CV_64F)
Q = P.t();
else
{
P.convertTo(Q, CV_64F);
Q = Q.t();
}
cv::Mat aux = Q.row(0);
cv::divide(aux, Q.row(2), aux);
aux = Q.row(1);
cv::divide(aux, Q.row(2), aux);
}
else if(P.cols == 2) // P is Nx2
{
const int N = P.rows;
Q.create(N, 3, CV_64F);
Q.col(2) = 1;
cv::Mat aux;
if(P.type() == CV_64F)
{
aux = Q.rowRange(0,2);
P.rowRange(0,2).copyTo(aux);
}
else
{
aux = Q.colRange(0,2);
P.convertTo(aux, CV_64F);
}
Q = Q.t();
}
}
std::shared_ptr<Detector2DResult> Detector2D::detect(Detector2DProperties& props, cv::Mat& image, cv::Mat& color_image, cv::Mat& debug_image, cv::Rect& roi, bool visualize, bool use_debug_image, bool pause_video = false)
{
std::shared_ptr<Detector2DResult> result = std::make_shared<Detector2DResult>();
result->current_roi = roi;
result->image_width = image.size().width;
result->image_height = image.size().height;
const int image_width = image.size().width;
const int image_height = image.size().height;
const cv::Mat pupil_image = cv::Mat(image, roi).clone(); // image with roi, copy the image, since we alter it
const int w = pupil_image.size().width / 2;
const float coarse_pupil_width = w / 2.0f;
const int padding = int(coarse_pupil_width / 4.0f);
const int offset = props.intensity_range;
const int spectral_offset = 5;
cv::Mat histogram;
int histSize;
histSize = 256; //from 0 to 255
/// Set the ranges
float range[] = { 0, 256 } ; //the upper boundary is exclusive
const float* histRange = { range };
cv::calcHist(&pupil_image, 1 , 0, cv::Mat(), histogram , 1 , &histSize, &histRange, true, false);
int lowest_spike_index = 255;
int highest_spike_index = 0;
float max_intensity = 0;
singleeyefitter::detector::calculate_spike_indices_and_max_intenesity(histogram, 40, lowest_spike_index, highest_spike_index, max_intensity);
if (visualize) {
const int scale_x = 100;
const int scale_y = 1 ;
// display the histogram and the spikes
for (int i = 0; i < histogram.rows; i++) {
const float norm_i = histogram.ptr<float>(i)[0] / max_intensity ; // normalized intensity
cv::line(color_image, {image_width, i * scale_y}, { image_width - int(norm_i * scale_x), i * scale_y}, mBlue_color);
}
cv::line(color_image, {image_width, lowest_spike_index * scale_y}, { int(image_width - 0.5f * scale_x), lowest_spike_index * scale_y }, mRed_color);
cv::line(color_image, {image_width, (lowest_spike_index + offset)* scale_y}, { int(image_width - 0.5f * scale_x), (lowest_spike_index + offset)* scale_y }, mYellow_color);
cv::line(color_image, {image_width, (highest_spike_index)* scale_y}, { int(image_width - 0.5f * scale_x), highest_spike_index * scale_y }, mRed_color);
cv::line(color_image, {image_width, (highest_spike_index - spectral_offset)* scale_y}, { int(image_width - 0.5f * scale_x), (highest_spike_index - spectral_offset)* scale_y }, mWhite_color);
}
//create dark and spectral glint masks
cv::Mat binary_img,spec_mask,kernel;
cv::inRange(pupil_image, cv::Scalar(0) , cv::Scalar(lowest_spike_index + props.intensity_range), binary_img); // binary threshold
kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, {7, 7});
cv::dilate(binary_img, binary_img, kernel, { -1, -1}, 2);
cv::inRange(pupil_image, cv::Scalar(0) , cv::Scalar(highest_spike_index - spectral_offset), spec_mask); // binary threshold
cv::erode(spec_mask, spec_mask, kernel);
kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, {9, 9});
//open operation to remove eye lashes
cv::morphologyEx(pupil_image, pupil_image, cv::MORPH_OPEN, kernel);
if (props.blur_size > 1)
cv::medianBlur(pupil_image, pupil_image, props.blur_size);
cv::Mat edges;
cv::Canny(pupil_image, edges, props.canny_treshold, props.canny_treshold * props.canny_ration, props.canny_aperture);
//remove edges in areas not dark enough and where the glint is (spectral refelction from IR leds)
cv::min(edges, spec_mask, edges);
cv::min(edges, binary_img, edges);
if (visualize) {
// get sub matrix
cv::Mat overlay = cv::Mat(color_image, roi);
cv::Mat g_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat b_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat r_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat out[] = {b_channel, g_channel, r_channel};
cv::split(overlay, out);
cv::max(g_channel, edges, g_channel);
cv::max(b_channel, binary_img, b_channel);
cv::min(b_channel, spec_mask, b_channel);
cv::merge(out, 3, overlay);
//draw a frame around the automatic pupil ROI in overlay.
auto rect = cv::Rect(0, 0, overlay.size().width, overlay.size().height);
cvx::draw_dotted_rect(overlay, rect, mWhite_color);
//draw a frame around the area we require the pupil center to be.
rect = cv::Rect(padding, padding, roi.width - padding, roi.height - padding);
cvx::draw_dotted_rect(overlay, rect, mWhite_color);
//draw size ellipses
cv::Point center(100, image_height - 100);
cv::circle(color_image, center, props.pupil_size_min / 2.0, mRed_color);
// real pupil size of this frame is calculated further down, so this size is from the last frame
cv::circle(color_image, center, mPupil_Size / 2.0, mGreen_color);
cv::circle(color_image, center, props.pupil_size_max / 2.0, mRed_color);
auto text_string = std::to_string(mPupil_Size);
cv::Size text_size = cv::getTextSize(text_string, cv::FONT_HERSHEY_SIMPLEX, 0.4 , 1, 0);
cv::Point text_pos = { center.x - text_size.width / 2 , center.y + text_size.height / 2};
cv::putText(color_image, text_string, text_pos, cv::FONT_HERSHEY_SIMPLEX, 0.4, mRoyalBlue_color);
}
//GuoHallThinner thinner;
//thinner.thin(edges, true);
//get raw edge pixel for later
std::vector<cv::Point> raw_edges;
// find zero crashes if it doesn't find one. replace with cv implementation if opencv version is 3.0 or above
singleeyefitter::cvx::findNonZero(edges, raw_edges);
///////////////////////////////
/// Strong Prior Part Begin ///
///////////////////////////////
//if we had a good ellipse before ,let see if it is still a good first guess:
if( mUse_strong_prior ){
mUse_strong_prior = false;
//recalculate center in coords system of new ROI views! roi changes every framesub
Ellipse ellipse = mPrior_ellipse;
ellipse.center[0] -= roi.x ;
ellipse.center[1] -= roi.y ;
if( !raw_edges.empty() ){
std::vector<cv::Point> support_pixels;
double ellipse_circumference = ellipse.circumference();
support_pixels = ellipse_true_support(props,ellipse, ellipse_circumference, raw_edges);
double support_ratio = support_pixels.size() / ellipse_circumference;
if(support_ratio >= props.strong_perimeter_ratio_range_min){
cv::RotatedRect refit_ellipse = cv::fitEllipse(support_pixels);
if(use_debug_image){
cv::ellipse(debug_image, toRotatedRect(ellipse), mRoyalBlue_color, 4);
cv::ellipse(debug_image, refit_ellipse, mRed_color, 1);
}
ellipse = toEllipse<double>(refit_ellipse);
ellipse_circumference = ellipse.circumference();
support_pixels = ellipse_true_support(props,ellipse, ellipse_circumference, raw_edges);
support_ratio = support_pixels.size() / ellipse_circumference;
ellipse.center[0] += roi.x;
ellipse.center[1] += roi.y;
mPrior_ellipse = ellipse;
mUse_strong_prior = true;
double goodness = std::min(1.0, support_ratio);
mPupil_Size = ellipse.major_radius * 2.0;
result->confidence = goodness;
result->ellipse = ellipse;
//result->final_contours = std::move(best_contours); // no contours when strong prior
//result->contours = std::move(split_contours);
result->raw_edges = std::move(raw_edges); // do we need it when strong prior ?
result->final_edges = std::move(support_pixels); // need for optimisation
return result;
}
}
}
///////////////////////////////
/// Strong Prior Part End ///
///////////////////////////////
//from edges to contours
Contours_2D contours ;
cv::findContours(edges, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);
//first we want to filter out the bad stuff, to short ones
const auto contour_size_min_pred = [&props](const Contour_2D & contour) {
return contour.size() > props.contour_size_min;
};
contours = singleeyefitter::fun::filter(contour_size_min_pred , contours);
//now we learn things about each contour through looking at the curvature.
//For this we need to simplyfy the contour so that pt to pt angles become more meaningfull
Contours_2D approx_contours;
std::for_each(contours.begin(), contours.end(), [&](const Contour_2D & contour) {
std::vector<cv::Point> approx_c;
cv::approxPolyDP(contour, approx_c, 1.5, false);
approx_contours.push_back(std::move(approx_c));
});
// split contours looking at curvature and angle
double split_angle = 80;
int split_contour_size_min = 3; //removing stubs makes combinatorial search feasable
//split_contours = singleeyefitter::detector::split_rough_contours(approx_contours, split_angle );
//removing stubs makes combinatorial search feasable
// MOVED TO split_contours_optimized
//split_contours = singleeyefitter::fun::filter( [](std::vector<cv::Point>& v){ return v.size() <= 3;} , split_contours);
Contours_2D split_contours = singleeyefitter::detector::split_rough_contours_optimized(approx_contours, split_angle , split_contour_size_min);
if (split_contours.empty()) {
result->confidence = 0.0;
// Does it make seens to return anything ?
//result->ellipse = toEllipse<double>(refit_ellipse);
//result->final_contours = std::move(best_contours);
//result->contours = std::move(split_contours);
//result->raw_edges = std::move(raw_edges);
return result;
}
if (use_debug_image) {
// debug segments
int colorIndex = 0;
for (const auto& segment : split_contours) {
const cv::Scalar_<int> colors[] = {mRed_color, mBlue_color, mRoyalBlue_color, mYellow_color, mWhite_color, mGreen_color};
cv::polylines(debug_image, segment, false, colors[colorIndex], 1, 4);
colorIndex++;
colorIndex %= 6;
}
}
std::sort(split_contours.begin(), split_contours.end(), [](const Contour_2D & a,const Contour_2D & b) { return a.size() > b.size(); });
const cv::Rect ellipse_center_varianz = cv::Rect(padding, padding, pupil_image.size().width - 2.0 * padding, pupil_image.size().height - 2.0 * padding);
const EllipseEvaluation2D is_Ellipse(ellipse_center_varianz, props.ellipse_roundness_ratio, props.pupil_size_min, props.pupil_size_max);
//finding potential candidates for ellipse seeds that describe the pupil.
auto seed_contours = detector::divide_strong_and_weak_contours(
split_contours, is_Ellipse, props.initial_ellipse_fit_treshhold,
props.strong_perimeter_ratio_range_min, props.strong_perimeter_ratio_range_max,
props.strong_area_ratio_range_min, props.strong_area_ratio_range_max
);
std::vector<int> seed_indices = seed_contours.first; // strong contours
if (seed_indices.empty() && !seed_contours.second.empty()) {
seed_indices = seed_contours.second; // weak contours
}
// still empty ? --> exits
if (seed_indices.empty()) {
result->confidence = 0.0;
// Does it make seens to return anything ?
//result->ellipse = toEllipse<double>(refit_ellipse);
//result->final_contours = std::move(best_contours);
//result->contours = std::move(split_contours);
result->raw_edges = std::move(raw_edges);
return result;
}
auto pruning_quick_combine = [&](const std::vector<std::vector<cv::Point>>& contours, std::set<int>& seed_indices, int max_evals = 1e20, int max_depth = 5) {
// describes different combinations of contours
typedef std::set<int> Path;
// combinations we wanna test
std::vector<Path> unknown(seed_indices.size());
// init with paths of size 1 == seed indices
int n = 0;
std::generate(unknown.begin(), unknown.end(), [&]() { return Path{n++}; }); // fill with increasing values, starting from 0
std::vector<int> mapping; // contains all indices, starting with seed_indices
mapping.reserve(contours.size());
mapping.insert(mapping.begin(), seed_indices.begin(), seed_indices.end());
// add indices which are not used to the end of mapping
for (int i = 0; i < contours.size(); i++) {
if (seed_indices.find(i) == seed_indices.end()) { mapping.push_back(i); }
}
// contains all the indices for the contours, which altogther fit best
std::vector<Path> results;
// contains bad paths, we won't test again
// even a superset is not tested again, because if a subset is bad, we can't make it better if more contours are added
std::vector<Path> prune;
int eval_count = 0;
while (!unknown.empty() && eval_count <= max_evals) {
eval_count++;
//take a path and combine it with others to see if the fit gets better
Path current_path = unknown.back();
unknown.pop_back();
if (current_path.size() <= max_depth) {
bool includes_bad_paths = fun::isSubset(current_path, prune);
if (!includes_bad_paths) {
int size = 0;
for (int j : current_path) { size += contours.at(mapping.at(j)).size(); };
std::vector<cv::Point> test_contour;
test_contour.reserve(size);
std::set<int> test_contour_indices;
//concatenate contours to one contour
for (int k : current_path) {
const std::vector<cv::Point>& c = contours.at(mapping.at(k));
test_contour.insert(test_contour.end(), c.begin(), c.end());
test_contour_indices.insert(mapping.at(k));
}
//we have not tested this and a subset of this was sucessfull before
double fit_variance = detector::contour_ellipse_deviation_variance(test_contour);
if (fit_variance < props.initial_ellipse_fit_treshhold) {
//yes this was good, keep as solution
results.push_back(test_contour_indices);
//lets explore more by creating paths to each remaining node
for (int l = (*current_path.rbegin()) + 1 ; l < mapping.size(); l++) {
unknown.push_back(current_path);
unknown.back().insert(l); // add a new path
}
} else {
prune.push_back(current_path);
}
}
}
}
return results;
};
std::set<int> seed_indices_set = std::set<int>(seed_indices.begin(), seed_indices.end());
std::vector<std::set<int>> solutions = pruning_quick_combine(split_contours, seed_indices_set, 1000, 5);
//find largest sets which contains all previous ones
auto filter_subset = [](std::vector<std::set<int>>& sets) {
std::vector<std::set<int>> filtered_set;
int i = 0;
for (auto& current_set : sets) {
//check if this current_set is a subset of set
bool isSubset = false;
for (int j = 0; j < sets.size(); j++) {
if (j == i) continue;// don't compare to itself
auto& set = sets.at(j);
// for std::include both containers need to be ordered. std::set guarantees this
isSubset |= std::includes(set.begin(), set.end(), current_set.begin(), current_set.end());
}
if (!isSubset) {
filtered_set.push_back(current_set);
}
i++;
}
return filtered_set;
};
solutions = filter_subset(solutions);
int index_best_Solution = -1;
int enum_index = 0;
for (auto& s : solutions) {
std::vector<cv::Point> test_contour;
//concatenate contours to one contour
for (int i : s) {
std::vector<cv::Point>& c = split_contours.at(i);
test_contour.insert(test_contour.end(), c.begin(), c.end());
}
auto cv_ellipse = cv::fitEllipse(test_contour);
if (use_debug_image) {
cv::ellipse(debug_image, cv_ellipse , mRed_color);
}
Ellipse ellipse = toEllipse<double>(cv_ellipse);
double ellipse_circumference = ellipse.circumference();
std::vector<cv::Point> support_pixels = ellipse_true_support(props, ellipse, ellipse_circumference, raw_edges);
double support_ratio = support_pixels.size() / ellipse_circumference;
//TODO: refine the selection of final candidate
if (support_ratio >= props.final_perimeter_ratio_range_min && is_Ellipse(cv_ellipse)) {
index_best_Solution = enum_index;
if (support_ratio >= props.strong_perimeter_ratio_range_min) {
ellipse.center[0] += roi.x;
ellipse.center[1] += roi.y;
mPrior_ellipse = ellipse;
mUse_strong_prior = true;
if (use_debug_image) {
cv::ellipse(debug_image, cv_ellipse , mGreen_color);
}
}
}
enum_index++;
}
// select ellipse
if (index_best_Solution == -1) {
// no good final ellipse found
result->confidence = 0.0;
// Does it make seens to return anything ?
//result->ellipse = toEllipse<double>(refit_ellipse);
//result->final_contours = std::move(best_contours);
//result->contours = std::move(split_contours);
result->raw_edges = std::move(raw_edges);
return result;
}
auto& best_solution = solutions.at(index_best_Solution);
std::vector<std::vector<cv::Point>> best_contours;
std::vector<cv::Point>best_contour;
//concatenate contours to one contour
for (int i : best_solution) {
std::vector<cv::Point>& c = split_contours.at(i);
best_contours.push_back(c);
best_contour.insert(best_contour.end(), c.begin(), c.end());
}
auto cv_ellipse = cv::fitEllipse(best_contour);
// final calculation of goodness of fit
auto ellipse = toEllipse<double>(cv_ellipse);
double ellipse_circumference = ellipse.circumference();
std::vector<cv::Point> support_pixels = ellipse_true_support(props, ellipse, ellipse_circumference, raw_edges);
double support_ratio = support_pixels.size() / ellipse_circumference;
double goodness = std::min(double(0.99), support_ratio);
//final fitting and return of result
auto final_fitting = [&](std::vector<std::vector<cv::Point>>& contours, cv::Mat & edges) -> std::vector<cv::Point> {
//use the real edge pixels to fit, not the aproximated contours
cv::Mat support_mask(edges.rows, edges.cols, edges.type(), {0, 0, 0});
cv::polylines(support_mask, contours, false, {255, 255, 255}, 2);
//draw into the suport mask with thickness 2
cv::Mat new_edges;
std::vector<cv::Point> new_contours;
cv::min(edges, support_mask, new_edges);
// can't do this here, because final result gets much distorted.
// see if it even can crash !!!
//new_edges.at<int>(0,0) = 1; // find zero crashes if it doesn't find one. remove if opencv version is 3.0 or above
cv::findNonZero(new_edges, new_contours);
if (visualize)
{
cv::Mat overlay = color_image.colRange(roi.x, roi.x + roi.width).rowRange(roi.y, roi.y + roi.height);
cv::Mat g_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat b_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat r_channel(overlay.rows, overlay.cols, CV_8UC1);
cv::Mat out[] = {b_channel, g_channel, r_channel};
cv::split(overlay, out);
cv::threshold(new_edges, new_edges, 0, 255, cv::THRESH_BINARY);
cv::max(r_channel, new_edges, r_channel);
cv::merge(out, 3, overlay);
}
return new_contours;
};
std::vector<cv::Point> final_edges = final_fitting(best_contours, edges);
auto cv_new_Ellipse = cv::fitEllipse(final_edges);
double size_difference = std::abs(1.0 - cv_ellipse.size.height / cv_new_Ellipse.size.height);
auto& cv_final_Ellipse = cv_ellipse;
if (is_Ellipse(cv_new_Ellipse) && size_difference < 0.3) {
if (use_debug_image) {
cv::ellipse(debug_image, cv_new_Ellipse, mGreen_color);
}
cv_final_Ellipse = cv_new_Ellipse;
}
//cv::imshow("debug_image", debug_image);
mPupil_Size = cv_final_Ellipse.size.height;
result->confidence = goodness;
result->ellipse = toEllipse<double>(cv_final_Ellipse);
result->ellipse.center[0] += roi.x;
result->ellipse.center[1] += roi.y;
//result->final_contours = std::move(best_contours);
// TODO optimize
// just do this if we really need it
// std::for_each(contours.begin(), contours.end(), [&](const Contour_2D & contour) {
// std::vector<cv::Point> approx_c;
// cv::approxPolyDP(contour, approx_c, 1.0, false);
// approx_contours.push_back(std::move(approx_c));
// });
// split_contours = singleeyefitter::detector::split_rough_contours_optimized(approx_contours, 150.0 , split_contour_size_min);
// result->contours = std::move(split_contours);
result->final_edges = std::move(final_edges);// need for optimisation
result->raw_edges = std::move(raw_edges);
return result;
}