add core files
This commit is contained in:
Tang1705
@@ -1,76 +1,111 @@
|
||||
#include "CoreAlgorithm.h"
|
||||
#include <unordered_map>
|
||||
|
||||
CoreAlgorithm::CoreAlgorithm(std::string path = "../Data/image/reconstruction/tq.png")
|
||||
CoreAlgorithm::CoreAlgorithm(const std::string& path, CameraArguments* cArgs)
|
||||
{
|
||||
image = cv::imread(path, cv::IMREAD_COLOR);
|
||||
tmp = image.clone(); // 深拷贝
|
||||
split(image, channel); //b,g,r
|
||||
cvtColor(image, hsv, COLOR_BGR2HSV);
|
||||
cArg = CameraArguments::getInstance();
|
||||
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::OSTU(Mat src)
|
||||
Mat CoreAlgorithm::OtsuAlgThreshold(Mat& src)
|
||||
{
|
||||
const auto width = src.cols;
|
||||
const auto height = src.rows;
|
||||
int hisData[256] = {0};
|
||||
|
||||
for (auto j = 0; j < height; j++)
|
||||
if (src.channels() != 1)
|
||||
{
|
||||
auto* data = src.ptr<uchar>(j);
|
||||
for (auto i = 0; i < width; i++)
|
||||
hisData[data[i]]++;
|
||||
cout << "Please input Gray-src!" << endl;
|
||||
}
|
||||
|
||||
auto t0 = 0;
|
||||
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;
|
||||
|
||||
for (auto i = 0; i < 256; i++)
|
||||
double totalNum = src.rows * src.cols;
|
||||
|
||||
for (auto i = 0; i < src.rows; i++)
|
||||
{
|
||||
t0 += i * hisData[i];
|
||||
}
|
||||
|
||||
t0 /= width * height;
|
||||
|
||||
auto t1 = 0, t2 = 0;
|
||||
auto num1 = 0, num2 = 0;
|
||||
auto t = 0;
|
||||
|
||||
while (true)
|
||||
{
|
||||
for (auto i = 0; i < t0 + 1; i++)
|
||||
for (auto j = 0; j < src.cols; j++)
|
||||
{
|
||||
t1 += i * hisData[i];
|
||||
num1 += hisData[i];
|
||||
if (src.at<float>(i, j) != 0) Histogram[data[i * src.step + j]]++;
|
||||
}
|
||||
|
||||
if (num1 == 0) continue;
|
||||
|
||||
for (auto i = t0 + 1; i < 256; i++)
|
||||
{
|
||||
t2 += i * hisData[i];
|
||||
num2 += hisData[i];
|
||||
}
|
||||
|
||||
if (num2 == 0) continue;
|
||||
|
||||
t = (t1 / num1 + t2 / num2) / 2;
|
||||
|
||||
if (t != t0) t0 = t;
|
||||
else break;
|
||||
}
|
||||
|
||||
Mat dst;
|
||||
auto minpos = 0, maxpos = 0;
|
||||
for (auto i = 0; i < 255; i++)
|
||||
{
|
||||
if (Histogram[i] != 0)
|
||||
{
|
||||
minpos = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
threshold(src, dst, t, 255, 0);
|
||||
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)
|
||||
vector<int> CoreAlgorithm::DeBruijn(int k, int n)
|
||||
{
|
||||
std::vector<byte> a(k * n, 0);
|
||||
std::vector<byte> seq;
|
||||
@@ -118,107 +153,149 @@ vector<int> CoreAlgorithm::Debruijn(int k, int n)
|
||||
return res;
|
||||
}
|
||||
|
||||
vector<int> CoreAlgorithm::Hsv(int r, int g, int b)
|
||||
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)
|
||||
{
|
||||
vector<int> h = {0, 0, 0, 0};
|
||||
h[0] = (2 * r - g - b) / (2 * sqrt(pow((r - g), 2) + (r - b) * (g - b)));
|
||||
h[1] = (2 * g - r - b) / (2 * sqrt(pow((g - r), 2) + (g - b) * (r - b)));
|
||||
h[2] = (2 * b - g - r) / (2 * sqrt(pow((b - g), 2) + (b - r) * (g - r)));
|
||||
h[3] = sqrt(1 - ((r * g + g * b + r * b) / (pow(r, 2) + pow(g, 2) + pow(b, 2)))); // 色彩强度
|
||||
return h;
|
||||
}
|
||||
|
||||
Mat CoreAlgorithm::Reconstruction(Mat featurePoint, int num, Mat Hc1, Mat Hp2, Mat map)
|
||||
{
|
||||
auto numOfCoord = 0;
|
||||
Mat coordinate = Mat::zeros(cv::Size(num, 3), CV_32FC1);
|
||||
for (auto column = minX; column < maxX; column++)
|
||||
for (auto i = 0; i < maximas.size(); i++)
|
||||
{
|
||||
Mat rowPosition = Mat::zeros(cv::Size(700, 1), CV_32FC1);
|
||||
auto counter = 0;
|
||||
for (auto row = minY; row < maxY; row++)
|
||||
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++)
|
||||
{
|
||||
if (featurePoint.at<uchar>(row, column) >= 0)
|
||||
double position;
|
||||
if (j < maximas[i].size() - 3)
|
||||
{
|
||||
rowPosition.at<uchar>(0, counter) = row;
|
||||
counter++;
|
||||
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))];
|
||||
}
|
||||
|
||||
if (counter < 4)
|
||||
else
|
||||
{
|
||||
continue;
|
||||
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;
|
||||
}
|
||||
|
||||
for (auto c = 0; c < counter - 3; c++)
|
||||
|
||||
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)
|
||||
{
|
||||
Mat matrix = Mat::zeros(cv::Size(3, 3), CV_32FC1);
|
||||
matrix.row(0) = Hc1.row(2) * (rowPosition.at<uchar>(0, c) + 1) - Hc1.row(0);
|
||||
matrix.row(1) = Hc1.row(2) * (column + 1) - Hc1.row(1);
|
||||
coordinate.push_back(tang.t());
|
||||
|
||||
auto p1 = rowPosition.at<uchar>(0, c);
|
||||
auto p2 = rowPosition.at<uchar>(0, c + 1);
|
||||
auto p3 = rowPosition.at<uchar>(0, c + 2);
|
||||
auto p4 = rowPosition.at<uchar>(0, c + 3);
|
||||
|
||||
auto id = map.step[0] * featurePoint.at<uchar>(column, p1)
|
||||
+ map.step[1] * featurePoint.at<uchar>(column, p2)
|
||||
+ map.step[2] * featurePoint.at<uchar>(column, p3)
|
||||
+ map.step[3] * featurePoint.at<uchar>(column, p4);
|
||||
|
||||
matrix.row(2) = Hp2.row(2) * (*reinterpret_cast<int*>(map.data + id)
|
||||
- Hp2.row(0));
|
||||
Mat tang = Mat::zeros(cv::Size(3, 1), CV_32FC1);
|
||||
Mat tmpMat = Mat::zeros(cv::Size(3, 1), CV_32FC1);
|
||||
tmpMat.row(0) = Hc1.row(0) - Hc1.row(2) * (rowPosition.at<uchar>(0, c) + 1);
|
||||
tmpMat.row(1) = Hc1.row(1) - Hc1.row(2) * (column + 1);
|
||||
tmpMat.row(2) = Hp2.row(0) - Hp2.at<uchar>(2, 3) * (*reinterpret_cast<int*>(map.data + id));
|
||||
tmpMat = tmpMat.t();
|
||||
solve(matrix, tmpMat, tang);
|
||||
|
||||
if (tang.at<uchar>(2, 0) > 750 && tang.at<uchar>(2, 0) < 1500)
|
||||
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++)
|
||||
{
|
||||
coordinate.row(numOfCoord) = tang.t();
|
||||
numOfCoord++;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
return coordinate;
|
||||
}
|
||||
|
||||
template <class ForwardIterator>
|
||||
size_t CoreAlgorithm::argmin(ForwardIterator first, ForwardIterator last)
|
||||
{
|
||||
return std::distance(first, std::min_element(first, last));
|
||||
}
|
||||
|
||||
|
||||
template <class ForwardIterator>
|
||||
size_t CoreAlgorithm::argmax(ForwardIterator first, ForwardIterator last)
|
||||
{
|
||||
return std::distance(first, std::max_element(first, last));
|
||||
}
|
||||
|
||||
|
||||
template <typename Tp>
|
||||
vector<Tp> CoreAlgorithm::convertMat2Vector(const cv::Mat& mat)
|
||||
{
|
||||
return (vector<Tp>)(mat.reshape(1, 1)); //通道数不变，按行转为一行
|
||||
}
|
||||
|
||||
void CoreAlgorithm::run()
|
||||
{
|
||||
tmp = OSTU(image);
|
||||
auto min = false;
|
||||
for (auto i = 0; i < 1024; i++)
|
||||
Mat mask = Mat::zeros(Size(cols, rows), CV_32FC1);
|
||||
for (auto i = 0; i < rows; i++)
|
||||
{
|
||||
for (auto j = 0; j < 1280; j++)
|
||||
for (auto j = 0; j < cols; j++)
|
||||
{
|
||||
if (tmp.at<Vec3b>(i, j)[0] == 255)
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -229,117 +306,207 @@ void CoreAlgorithm::run()
|
||||
}
|
||||
}
|
||||
|
||||
minX -= 50;
|
||||
minY -= 50;
|
||||
maxX += 50;
|
||||
maxY += 50;
|
||||
|
||||
Mat grayImage = Mat::zeros(cv::Size(1024, 1280), CV_32FC1);
|
||||
for (auto i = 0; i < 1024; i++)
|
||||
{
|
||||
for (auto j = 0; j < 1280; j++)
|
||||
{
|
||||
grayImage.at<uchar>(i, j) = 0.2989 * channel.at(2).at<uchar>(i, j) +
|
||||
0.5907 * channel.at(1).at<uchar>(i, j) +
|
||||
0.1140 * channel.at(0).at<uchar>(i, j);
|
||||
}
|
||||
}
|
||||
// cout << minX << "," << minY;
|
||||
// cout << maxX << "," << maxY;
|
||||
// cout<<rows<<","<<cols<<endl;
|
||||
|
||||
Mat featurePoint = Mat::zeros(cv::Size(1024, 1280), CV_32FC1);
|
||||
// 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++)
|
||||
{
|
||||
auto temp = grayImage(Rect(i, j - neighborhood, 2 * neighborhood + 1, 1));
|
||||
auto v = convertMat2Vector<uchar>(temp);
|
||||
|
||||
const auto tmpPosition1 = argmax(v.begin(), v.end());
|
||||
const auto tmpPosition2 = argmin(v.begin(), v.end());
|
||||
const auto position1 = static_cast<int>(tmpPosition1);
|
||||
const auto position2 = static_cast<int>(tmpPosition2);
|
||||
|
||||
if (position1 == neighborhood)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = 1;
|
||||
}
|
||||
if (position2 == neighborhood)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = -1;
|
||||
}
|
||||
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++)
|
||||
{
|
||||
Mat temp = Mat::zeros(cv::Size(100, 1), CV_32FC1);
|
||||
auto num = 0;
|
||||
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++)
|
||||
{
|
||||
if (featurePoint.at<uchar>(i, j) == 1)
|
||||
// cout << i << endl;
|
||||
if (derivative1.at<float>(i, j) > 0 && derivative1.at<float>(i, j + 1) < 0)
|
||||
{
|
||||
temp.at<uchar>(num, 0) = j;
|
||||
int rh = channel.at(2).at<uchar>(i, j);
|
||||
int gh = channel.at(1).at<uchar>(i, j);
|
||||
int bh = channel.at(0).at<uchar>(i, j);
|
||||
|
||||
if (rh == gh || gh == bh || rh == bh)
|
||||
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])
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = -2;
|
||||
continue;
|
||||
}
|
||||
|
||||
auto h = Hsv(rh, gh, bh);
|
||||
auto result = max_element(begin(h), end(h));
|
||||
auto nPos = static_cast<int>(max_element(h.begin(), h.end()) - (h.begin()));
|
||||
|
||||
if (nPos == 0)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = 0;
|
||||
}
|
||||
else if (nPos == 1)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = 1;
|
||||
}
|
||||
else if (nPos == 2)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = 2;
|
||||
}
|
||||
num = num + 1;
|
||||
}
|
||||
else if (featurePoint.at<uchar>(i, j) != -1)
|
||||
{
|
||||
featurePoint.at<uchar>(i, j) = -2;
|
||||
minimas[i - minX].push_back(tmpMin[j]);
|
||||
pos++;
|
||||
if (pos >= maximas[i - minX].size())break;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
auto number = 0;
|
||||
|
||||
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++)
|
||||
{
|
||||
for (auto j = minY; j < maxY; j++)
|
||||
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++)
|
||||
{
|
||||
if (featurePoint.at<uchar>(i, j) != -2) number++;
|
||||
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对象
|
||||
}
|
||||
}
|
||||
|
||||
auto db = Debruijn(3, 4);
|
||||
|
||||
int p, q, t, u;
|
||||
p = q = t = u = 3;
|
||||
int sizes[] = {p, q, t, u};
|
||||
auto all = p * q * t * u;
|
||||
auto d1 = new float[all];
|
||||
for (auto i = 0; i < all; i++)
|
||||
{
|
||||
d1[i] = i * 1;
|
||||
}
|
||||
|
||||
auto map = Mat(4, sizes, CV_32S, d1);
|
||||
|
||||
for (auto i = 0; i < 61; i++)
|
||||
{
|
||||
auto id = map.step[0] * db[i] + map.step[1] * db[i + 1]
|
||||
+ map.step[2] * db[i + 2] + map.step[3] * db[i + 3];
|
||||
auto* stub = reinterpret_cast<int*>(map.data + id);
|
||||
*stub = 7.5 + 14 * i;
|
||||
}
|
||||
|
||||
auto coordinate = Reconstruction(featurePoint, number, cArg->getHc(), cArg->getHp(), map);
|
||||
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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user