518 lines
14 KiB
C++
518 lines
14 KiB
C++
#include "CoreAlgorithm.h"
|
|
|
|
CoreAlgorithm::CoreAlgorithm(const std::string& path, CameraArguments* cArgs)
|
|
{
|
|
image = imread(path, IMREAD_COLOR);
|
|
rows = image.rows;
|
|
cols = image.cols;
|
|
split(image, rgbChannel); //b,g,r
|
|
hsv = image.clone();
|
|
cvtColor(image, hsv, COLOR_BGR2HSV, 3);
|
|
split(hsv, hsvChannel);
|
|
cvtColor(image, lab, COLOR_BGR2Lab);
|
|
cArg = cArgs;
|
|
}
|
|
|
|
CoreAlgorithm::~CoreAlgorithm()
|
|
= default;
|
|
|
|
Mat CoreAlgorithm::OtsuAlgThreshold(Mat& src)
|
|
{
|
|
if (src.channels() != 1)
|
|
{
|
|
cout << "Please input Gray-src!" << endl;
|
|
}
|
|
|
|
auto T = 0;
|
|
double varValue = 0;
|
|
double w0 = 0;
|
|
double w1 = 0;
|
|
double u0 = 0;
|
|
double u1 = 0;
|
|
double Histogram[256] = {0};
|
|
uchar* data = src.data;
|
|
|
|
double totalNum = src.rows * src.cols;
|
|
|
|
for (auto i = 0; i < src.rows; i++)
|
|
{
|
|
for (auto j = 0; j < src.cols; j++)
|
|
{
|
|
if (src.at<float>(i, j) != 0) Histogram[data[i * src.step + j]]++;
|
|
}
|
|
}
|
|
|
|
auto minpos = 0, maxpos = 0;
|
|
for (auto i = 0; i < 255; i++)
|
|
{
|
|
if (Histogram[i] != 0)
|
|
{
|
|
minpos = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (auto i = 255; i > 0; i--)
|
|
{
|
|
if (Histogram[i] != 0)
|
|
{
|
|
maxpos = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (auto i = minpos; i <= maxpos; i++)
|
|
{
|
|
w1 = 0;
|
|
u1 = 0;
|
|
w0 = 0;
|
|
u0 = 0;
|
|
for (auto j = 0; j <= i; j++)
|
|
{
|
|
w1 += Histogram[j];
|
|
u1 += j * Histogram[j];
|
|
}
|
|
if (w1 == 0)
|
|
{
|
|
break;
|
|
}
|
|
u1 = u1 / w1;
|
|
w1 = w1 / totalNum;
|
|
for (auto k = i + 1; k < 255; k++)
|
|
{
|
|
w0 += Histogram[k];
|
|
u0 += k * Histogram[k];
|
|
}
|
|
if (w0 == 0)
|
|
{
|
|
break;
|
|
}
|
|
u0 = u0 / w0;
|
|
w0 = w0 / totalNum;
|
|
|
|
auto varValueI = w0 * w1 * (u1 - u0) * (u1 - u0);
|
|
if (varValue < varValueI)
|
|
{
|
|
varValue = varValueI;
|
|
T = i;
|
|
}
|
|
}
|
|
// cout << T << endl;
|
|
Mat dst = src.clone();
|
|
for (auto i = 0; i < src.rows; i++)
|
|
for (auto j = 0; j < src.cols; j++)
|
|
dst.at<float>(i, j) = src.at<float>(i, j) > T ? 255 : 0;
|
|
return dst;
|
|
}
|
|
|
|
vector<int> CoreAlgorithm::DeBruijn(int k, int n)
|
|
{
|
|
std::vector<byte> a(k * n, 0);
|
|
std::vector<byte> seq;
|
|
|
|
std::function<void(int, int)> db;
|
|
db = [&](int t, int p)
|
|
{
|
|
if (t > n)
|
|
{
|
|
if (n % p == 0)
|
|
{
|
|
for (int i = 1; i < p + 1; i++)
|
|
{
|
|
seq.push_back(a[i]);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
a[t] = a[t - p];
|
|
db(t + 1, p);
|
|
auto j = a[t - p] + 1;
|
|
while (j < k)
|
|
{
|
|
a[t] = j & 0xFF;
|
|
db(t + 1, t);
|
|
j++;
|
|
}
|
|
}
|
|
};
|
|
|
|
db(1, 1);
|
|
std::string buf;
|
|
for (auto i : seq)
|
|
{
|
|
buf.push_back('0' + i);
|
|
}
|
|
|
|
std::vector<int> res;
|
|
std::string tmp = buf + buf.substr(0, n - 1);
|
|
for (char i : tmp)
|
|
{
|
|
res.push_back(i - '0');
|
|
}
|
|
return res;
|
|
}
|
|
|
|
void CoreAlgorithm::Reconstruction(vector<vector<float>> maximas, vector<vector<float>> minimas,
|
|
vector<vector<float>> colorLabel, vector<vector<float>> phases, const Mat& Hc1,
|
|
Mat Hp2, const double* map)
|
|
{
|
|
for (auto i = 0; i < maximas.size(); i++)
|
|
{
|
|
if (maximas[i].empty())continue;
|
|
if (maximas[i].size() < 4)continue;
|
|
auto mark = 0;
|
|
// double pc = 0;
|
|
for (auto j = 0; j < maximas[i].size(); j++)
|
|
{
|
|
double position;
|
|
if (j < maximas[i].size() - 3)
|
|
{
|
|
position = map[int(pow(3, 3) * colorLabel[i].at(j) + pow(3, 2) * colorLabel[i].at(j + 1) +
|
|
3 * colorLabel[i].at(j + 2) + colorLabel[i].at(j + 3))];
|
|
}
|
|
else
|
|
{
|
|
auto fix = maximas[i].size() - 4;
|
|
auto index = j - maximas[i].size() + 4;
|
|
position = map[int(pow(3, 3) * colorLabel[i].at(fix) + pow(3, 2) * colorLabel[i].at(fix + 1) +
|
|
3 * colorLabel[i].at(fix + 2) + colorLabel[i].at(fix + 3))] + 14.0 * index;
|
|
}
|
|
|
|
Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1);
|
|
matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (maximas[i][j]) - Hc1(Rect(0, 0, 3, 1));
|
|
matrix.row(1) = Hc1(Rect(0, 2, 3, 1)) * (float(i + minX)) - Hc1(Rect(0, 1, 3, 1));
|
|
matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * position - Hp2(Rect(0, 0, 3, 1));
|
|
Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1);
|
|
Mat b = Mat::zeros(cv::Size(1, 3), CV_32FC1);
|
|
b.row(0) = Hc1.at<float>(0, 3) - Hc1.at<float>(2, 3) * (maximas[i][j]);
|
|
b.row(1) = Hc1.at<float>(1, 3) - Hc1.at<float>(2, 3) * (float(i + minX));
|
|
b.row(2) = Hp2.at<float>(0, 3) - Hp2.at<float>(2, 3) * position;
|
|
solve(matrix, b, tang);
|
|
|
|
if (tang.at<float>(2, 0) > 750 && tang.at<float>(2, 0) < 1500)
|
|
{
|
|
coordinate.push_back(tang.t());
|
|
|
|
int r = (int)rgbChannel[2].at<uchar>(i + minX, maximas[i][j]),
|
|
g = rgbChannel[1].at<uchar>(i + minX, maximas[i][j]),
|
|
b = rgbChannel[0].at<uchar>(i + minX, maximas[i][j]);
|
|
int rgb = ((int)r << 16 | (int)g << 8 | (int)b);
|
|
float frgb = *reinterpret_cast<float*>(&rgb);
|
|
color.push_back(frgb);
|
|
}
|
|
// if (i == 200)cout << maximas[i][j] << "," << 0 << "," << position << endl;
|
|
if (phases[i].empty())continue;
|
|
auto pi = false;
|
|
auto start = minimas[i][0];
|
|
if (start > maximas[i][j]) continue;
|
|
if (j == 0)
|
|
{
|
|
for (auto k = mark; k + start < maximas[i][j]; k++)
|
|
{
|
|
if ((start + k) < maximas[i][j] && phases[i][k] < 0)continue;
|
|
if ((start + k) < maximas[i][j] && phases[i][k] > 0)
|
|
{
|
|
if (maximas[i][j] - (start + k) < 1)
|
|
{
|
|
continue;
|
|
}
|
|
mark = k + 1;
|
|
}
|
|
else if ((start + k) > maximas[i][j]) break;
|
|
}
|
|
}
|
|
|
|
for (auto k = mark; k < phases[i].size()-1; k++)
|
|
{
|
|
mark++;
|
|
double newPosition;
|
|
if ((start + k) < maximas[i][j] && phases[i][k] < 0) newPosition = position + phases[i][k];
|
|
else if ((maximas[i][j] - (start + k)) > 1 && phases[i][k] > 0)
|
|
newPosition = position + phases[i][k] - 7;
|
|
else if ((start + k) > maximas[i][j] && phases[i][k] > 0)newPosition = position + phases[i][k];
|
|
else if (((start + k) - maximas[i][j]) > 1 && phases[i][k] < 0)
|
|
newPosition = position + phases[i][k] + 7;
|
|
else continue;
|
|
|
|
matrix.row(0) = Hc1(Rect(0, 2, 3, 1)) * (start + k) - Hc1(Rect(0, 0, 3, 1));
|
|
matrix.row(2) = Hp2(Rect(0, 2, 3, 1)) * newPosition - Hp2(Rect(0, 0, 3, 1));
|
|
b.row(0) = Hc1.at<float>(0, 3) - Hc1.at<float>(2, 3) * (start + k);
|
|
b.row(2) = Hp2.at<float>(0, 3) - Hp2.at<float>(2, 3) * newPosition;
|
|
solve(matrix, b, tang);
|
|
if (tang.at<float>(2, 0) > 750 && tang.at<float>(2, 0) < 1500)
|
|
{
|
|
coordinate.push_back(tang.t());
|
|
int r = (int)rgbChannel[2].at<uchar>(i + minX, (start + k)),
|
|
g = rgbChannel[1].at<uchar>(i + minX, (start + k)),
|
|
b = rgbChannel[0].at<uchar>(i + minX, (start + k));
|
|
int rgb = ((int)r << 16 | (int)g << 8 | (int)b);
|
|
float frgb = *reinterpret_cast<float*>(&rgb);
|
|
color.push_back(frgb);
|
|
}
|
|
|
|
if ((start + k) > maximas[i][j] && !pi && phases[i][k] > 0) pi = true;
|
|
|
|
if ((start + k) > maximas[i][j] && phases[i][k] < 0 && phases[i][k + 1] > 0 && pi)break;
|
|
|
|
}
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void CoreAlgorithm::run()
|
|
{
|
|
Mat mask = Mat::zeros(Size(cols, rows), CV_32FC1);
|
|
for (auto i = 0; i < rows; i++)
|
|
{
|
|
for (auto j = 0; j < cols; j++)
|
|
{
|
|
mask.at<float>(i, j) = (int)rgbChannel[0].at<uchar>(i, j) > (int)rgbChannel[1].at<uchar>(i, j)
|
|
? (
|
|
(int)rgbChannel[0].at<uchar>(i, j) > (int)rgbChannel[2].at<uchar>(i, j)
|
|
? (int)rgbChannel[0].at<uchar>(i, j)
|
|
: (int)rgbChannel[2].at<uchar>(i, j))
|
|
: (
|
|
(int)rgbChannel[1].at<uchar>(i, j) > (int)rgbChannel[2].at<uchar>(i, j)
|
|
? (int)rgbChannel[1].at<uchar>(i, j)
|
|
: (int)rgbChannel[2].at<uchar>(i, j));
|
|
}
|
|
}
|
|
|
|
tmp = OtsuAlgThreshold(mask);
|
|
|
|
auto kernel = getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
|
|
morphologyEx(tmp, tmp, MORPH_OPEN, kernel);
|
|
auto min = false;
|
|
for (auto i = 0; i < rows; i++)
|
|
{
|
|
for (auto j = 0; j < cols; j++)
|
|
{
|
|
if (tmp.at<float>(i, j) == 255)
|
|
{
|
|
if (!min)
|
|
{
|
|
minX = i;
|
|
minY = j;
|
|
min = true;
|
|
}
|
|
|
|
if (j < minY) minY = j;
|
|
if (i > maxX) maxX = i;
|
|
if (j > maxY) maxY = j;
|
|
}
|
|
}
|
|
}
|
|
|
|
minX -= 50;
|
|
minY -= 50;
|
|
maxX += 50;
|
|
maxY += 50;
|
|
|
|
// cout << minX << "," << minY;
|
|
// cout << maxX << "," << maxY;
|
|
// cout<<rows<<","<<cols<<endl;
|
|
|
|
// auto rec = image(Rect(minY, minX, maxY - minY, maxX - minX));
|
|
|
|
Mat img = Mat::zeros(Size(cols, rows), CV_32FC1);
|
|
for (auto i = minX; i < maxX; i++)
|
|
{
|
|
for (auto j = minY; j < maxY; j++)
|
|
{
|
|
img.at<float>(i, j) = 0.2989 * (int)rgbChannel.at(2).at<uchar>(i, j) +
|
|
0.5907 * (int)rgbChannel.at(1).at<uchar>(i, j) +
|
|
0.1140 * (int)rgbChannel.at(0).at<uchar>(i, j);
|
|
}
|
|
}
|
|
|
|
kernel = getStructuringElement(MORPH_RECT, cv::Size(3, 3));
|
|
morphologyEx(img, img, MORPH_CLOSE, kernel);
|
|
|
|
GaussianBlur(img, img, Size(5, 5), 0, 0);
|
|
|
|
Mat derivative1 = Mat::zeros(Size(cols, rows), CV_32FC1);
|
|
Mat derivative2 = Mat::zeros(Size(cols, rows), CV_32FC1);
|
|
|
|
for (auto i = 0; i < rows; i++)
|
|
{
|
|
for (auto j = 1; j < cols - 1; j++)
|
|
{
|
|
derivative1.at<float>(i, j) = img.at<float>(i, j + 1) - img.at<float>(i, j);
|
|
derivative2.at<float>(i, j) = img.at<float>(i, j + 1) + img.at<float>(i, j - 1) - 2 * img.at<float>(i, j);
|
|
}
|
|
}
|
|
|
|
vector<vector<float>> maximas(0, vector<float>(0, 0));
|
|
vector<vector<float>> minimas(0, vector<float>(0, 0));
|
|
vector<vector<float>> colorLabel(0, vector<float>(0, 0));
|
|
for (auto i = minX; i < maxX; i++)
|
|
{
|
|
maximas.resize(i - minX + 1);
|
|
minimas.resize(i - minX + 1);
|
|
colorLabel.resize(i - minX + 1);
|
|
vector<double> tmpMin;
|
|
for (auto j = minY; j < maxY; j++)
|
|
{
|
|
// cout << i << endl;
|
|
if (derivative1.at<float>(i, j) > 0 && derivative1.at<float>(i, j + 1) < 0)
|
|
{
|
|
double k = derivative1.at<float>(i, j + 1) - derivative1.at<float>(i, j);
|
|
double b = derivative1.at<float>(i, j) - k * j;
|
|
double zero = -b / k;
|
|
double k2 = derivative2.at<float>(i, j + 1) - derivative2.at<float>(i, j);
|
|
double b2 = derivative2.at<float>(i, j) - k2 * j;
|
|
double value = k2 * zero + b2;
|
|
if (value < 0 && lab.at<Vec3b>(i, zero)[0] > 5)
|
|
{
|
|
maximas[i - minX].push_back(zero);
|
|
if (lab.at<Vec3b>(i, zero)[2] < 126)
|
|
{
|
|
colorLabel[i - minX].push_back(2); //blue
|
|
}
|
|
else
|
|
{
|
|
if (lab.at<Vec3b>(i, zero)[1] >= 128)
|
|
{
|
|
colorLabel[i - minX].push_back(0); //red
|
|
}
|
|
else
|
|
{
|
|
colorLabel[i - minX].push_back(1); //green
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (derivative1.at<float>(i, j) < 0 && derivative1.at<float>(i, j + 1) > 0)
|
|
{
|
|
double k = derivative1.at<float>(i, j + 1) - derivative1.at<float>(i, j);
|
|
double b = derivative1.at<float>(i, j) - k * j;
|
|
double zero = -b / k;
|
|
double k2 = derivative2.at<float>(i, j + 1) - derivative2.at<float>(i, j);
|
|
double b2 = derivative2.at<float>(i, j) - k2 * j;
|
|
double value = k2 * zero + b2;
|
|
if (value > 0)
|
|
{
|
|
tmpMin.push_back(zero);
|
|
}
|
|
}
|
|
}
|
|
if (!tmpMin.empty() && !maximas[i - minX].empty())
|
|
{
|
|
auto pos = 0;
|
|
for (auto j = 0; j < tmpMin.size()-1; j++)
|
|
{
|
|
|
|
if (tmpMin[j + 1] < maximas[i - minX][pos])
|
|
{
|
|
continue;
|
|
}
|
|
minimas[i - minX].push_back(tmpMin[j]);
|
|
pos++;
|
|
if (pos >= maximas[i - minX].size())break;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
vector<vector<float>> phases(0, vector<float>(0, 0));
|
|
emxArray_real_T* phase;
|
|
double x_data[1280] = {0};
|
|
int x_size[2] = {0};
|
|
emxInitArray_real_T(&phase, 2);
|
|
x_size[0] = 1;
|
|
for (auto i = minX; i < maxX; i++)
|
|
{
|
|
phases.resize(i - minX + 1);
|
|
if (minimas[i - minX].empty())continue;
|
|
int start = minimas[i - minX][0];
|
|
int end = minimas[i - minX][minimas[i - minX].size() - 1];
|
|
x_size[1] = end - start;
|
|
for (auto j = start; j < end; j++)
|
|
{
|
|
x_data[j - start] = (float)lab.at<Vec3b>(i, j)[0];
|
|
// if (i -minX== 300)
|
|
// cout<<x_data[j - start]<<",";
|
|
}
|
|
cwt(x_data, x_size, phase);
|
|
|
|
for (auto j = 0; j < x_size[1]; j++)
|
|
{
|
|
phases[i - minX].push_back(*(phase->data + j) / PI * 7);
|
|
// if (i - minX == 300)
|
|
// cout << j + start << "," << *(phase->data + j) << endl;
|
|
}
|
|
// if (i == 300) {
|
|
// for (auto j = 0; j < maximas[i - minX].size(); j++) {
|
|
// cout <<j+start<<","<< *(phase->data + int(maximas[i - minX][j] - start)) << endl;
|
|
//
|
|
// }
|
|
// }
|
|
}
|
|
auto db = DeBruijn(3, 4);
|
|
double map[76]{0};
|
|
for (auto i = 0; i < 61; i++)
|
|
{
|
|
int index = int(pow(3, 3) * db.at(i) + pow(3, 2) * db.at(i + 1) + 3 * db.at(i + 2) + db.at(i + 3));
|
|
map[index] = 7.5 + 14 * i;
|
|
}
|
|
|
|
Reconstruction(maximas, minimas, colorLabel, phases, cArg->getHc(), cArg->getHp(), map);
|
|
ofstream destFile("./Data/result/result.txt", ios::out); //以文本模式打开out.txt备写
|
|
for (auto i = 0; i < coordinate.size(); i++)
|
|
{
|
|
if (i == coordinate.size() - 1)
|
|
{
|
|
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
|
|
<< coordinate[i].at<float>(0, 2);
|
|
}
|
|
else
|
|
{
|
|
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
|
|
<< coordinate[i].at<float>(0, 2) << endl; //可以像用cout那样用ofstream对象
|
|
}
|
|
}
|
|
|
|
destFile.close();
|
|
saveCoordinate();
|
|
}
|
|
|
|
void CoreAlgorithm::saveCoordinate()
|
|
{
|
|
ofstream destFile("./Data/result/result.pcd", ios::out); //以文本模式打开out.txt备写
|
|
destFile << "# .PCD v0.7 - Point Cloud Data file format" << endl;
|
|
destFile << "VERSION 0.7" << endl;
|
|
destFile << "FIELDS x y z rgb" << endl;
|
|
destFile << "SIZE 4 4 4 4" << endl;
|
|
destFile << "TYPE F F F F" << endl;
|
|
destFile << "COUNT 1 1 1 1" << endl;
|
|
destFile << "WIDTH " << coordinate.size() << endl;
|
|
destFile << "HEIGHT 1" << endl;
|
|
destFile << "VIEWPOINT 0 0 0 1 0 0 0" << endl;
|
|
destFile << "POINTS " << coordinate.size() << endl;
|
|
destFile << "DATA ascii" << endl;
|
|
for (auto i = 0; i < coordinate.size(); i++)
|
|
{
|
|
// cout << i << endl;
|
|
if (i == coordinate.size() - 1)
|
|
{
|
|
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
|
|
<< coordinate[i].at<float>(0, 2) << " " << color[i];
|
|
}
|
|
else
|
|
{
|
|
destFile << coordinate[i].at<float>(0, 0) << " " << coordinate[i].at<float>(0, 1) << " "
|
|
<< coordinate[i].at<float>(0, 2) << " " << color[i] << endl; //可以像用cout那样用ofstream对象
|
|
}
|
|
}
|
|
destFile.close();
|
|
}
|
|
|
|
vector<Mat> CoreAlgorithm::getCoordinates()
|
|
{
|
|
return coordinate;
|
|
}
|