wo shi yi ge python/c++ fan yi ji

Classes/CameraArguments.cpp

@@ -1,8 +1,13 @@
 #include "CameraArguments.h"
 
+CameraArguments* CameraArguments::instance = nullptr;
+
+/**
+ * \brief 
+ */
 CameraArguments::CameraArguments()
 {
-	r12 = cv::Mat::zeros(cv::Size(3, 3), CV_8UC1);
+	//r12 = cv::Mat::zeros(cv::Size(3, 3), CV_8UC1);
 }
 
 CameraArguments::CameraArguments(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp)
@@ -19,10 +24,20 @@ CameraArguments::CameraArguments(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp)
 	hp2 = kp2 * tmp;
 }
 
-CameraArguments::~CameraArguments()
+CameraArguments* CameraArguments::getInstance(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp)
 {
+	if (instance == nullptr) instance = new CameraArguments(r, t, kc, kp);
+	return instance;
 }
 
+CameraArguments* CameraArguments::getInstance()
+{
+	if (instance) return instance;
+}
+
+CameraArguments::~CameraArguments()
+= default;
+
 cv::Mat CameraArguments::getR() const
 {
 	return r12;

Classes/CameraArguments.h

@@ -11,10 +11,15 @@ private:
 	cv::Mat hc1;
 	cv::Mat hp2;
 
-public:
 	CameraArguments();
 	CameraArguments(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp);
 	~CameraArguments();
+
+public:
+
+	static CameraArguments* getInstance(cv::Mat r, cv::Mat t, cv::Mat kc, cv::Mat kp);
+	static CameraArguments* getInstance();
+	static CameraArguments* instance ;
 	cv::Mat getR() const;
 	cv::Mat getT() const;
 	cv::Mat getKc() const;

Classes/CoreAlgorithm.cpp

@@ -1,15 +1,17 @@
 #include "CoreAlgorithm.h"
+#include <unordered_map>
 
 CoreAlgorithm::CoreAlgorithm(std::string path = "../Data/image/reconstruction/tq.png")
 {
 	image = cv::imread(path, cv::IMREAD_COLOR);
 	tmp = image.clone(); // 深拷贝
 	split(image, channel); //b,g,r
+	cvtColor(image, hsv, COLOR_BGR2HSV);
+	cArg = CameraArguments::getInstance();
 }
 
 CoreAlgorithm::~CoreAlgorithm()
-{
-}
+= default;
 
 Mat CoreAlgorithm::OSTU(Mat src)
 {
@@ -68,44 +70,140 @@ Mat CoreAlgorithm::OSTU(Mat src)
 	return dst;
 }
 
-void CoreAlgorithm::Rgb2Hsv(float r, float g, float b, float& h, float& s, float& v)
+vector<int> CoreAlgorithm::Debruijn(int k, int n)
 {
-	// r,g,b values are from 0 to 1
-	// h = [0,360], s = [0,1], v = [0,1]
-	// if s == 0, then h = -1 (undefined)
-	float min, max, delta, temp;
-	temp = r > g ? g : r;
-	min = temp > b ? b : temp;
-	temp = r > g ? r : g;
-	max = temp > b ? temp : b;
-	v = max; // v
-	delta = max - min;
-	if (max != 0)
-		s = delta / max; // s
-	else
+	std::vector<byte> a(k * n, 0);
+	std::vector<byte> seq;
+
+	std::function<void(int, int)> db;
+	db = [&](int t, int p)
 	{
-		// r = g = b = 0 // s = 0, v is undefined
-		s = 0;
-		h = 0;
-		return;
-	}
-	if (delta == 0)
-	{
-		h = 0;
-		return;
-	}
-	else if (r == max)
-	{
-		if (g >= b)
-			h = (g - b) / delta; // between yellow & magenta
+		if (t > n)
+		{
+			if (n % p == 0)
+			{
+				for (int i = 1; i < p + 1; i++)
+				{
+					seq.push_back(a[i]);
+				}
+			}
+		}
 		else
-			h = (g - b) / delta + 6.0;
+		{
+			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);
 	}
-	else if (g == max)
-		h = (b - r) / delta + 2.0; // between cyan & yellow
-	else if (b == max)
-		h = (r - g) / delta + 4.0; // between magenta & cyan
-	h *= 60.0; // degrees
+
+	std::vector<int> res;
+	std::string tmp = buf + buf.substr(0, n - 1);
+	for (char i : tmp)
+	{
+		res.push_back(i - '0');
+	}
+	return res;
+}
+
+vector<int> CoreAlgorithm::Hsv(int r, int g, int b)
+{
+	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++)
+	{
+		Mat rowPosition = Mat::zeros(cv::Size(700, 1), CV_32FC1);
+		auto counter = 0;
+		for (auto row = minY; row < maxY; row++)
+		{
+			if (featurePoint.at<uchar>(row, column) >= 0)
+			{
+				rowPosition.at<uchar>(0, counter) = row;
+				counter++;
+			}
+
+			if (counter < 4)
+			{
+				continue;
+			}
+
+			for (auto c = 0; c < counter - 3; c++)
+			{
+				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);
+
+				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)
+				{
+					coordinate.row(numOfCoord) = tang.t();
+					numOfCoord++;
+				}
+			}
+		}
+	}
+	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()
@@ -116,32 +214,30 @@ void CoreAlgorithm::run()
 	{
 		for (auto j = 0; j < 1280; j++)
 		{
-			if (!min)
-			{
-				if (tmp.at<Vec3b>(i, j)[0] == 255)
-				{
-					minX = i;
-					minY = j;
-					min = true;
-				}
-			}
-
 			if (tmp.at<Vec3b>(i, j)[0] == 255)
 			{
+				if (!min)
+				{
+					minX = i;
+					min = true;
+				}
+
+				if (j < minY) minY = j;
 				if (i > maxX) maxX = i;
 				if (j > maxY) maxY = j;
 			}
 		}
 	}
 
+
 	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<Vec3b>(i, j) = 0.2989 * channel.at(2).at<Vec3b>(i, j) +
-				0.5907 * channel.at(1).at<Vec3b>(i, j) +
-				0.1140 * channel.at(0).at<Vec3b>(i, 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);
 		}
 	}
 
@@ -151,6 +247,99 @@ void CoreAlgorithm::run()
 	{
 		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;
+			}
 		}
 	}
+
+	for (auto i = minX; i < maxX; i++)
+	{
+		Mat temp = Mat::zeros(cv::Size(100, 1), CV_32FC1);
+		auto num = 0;
+		for (auto j = minY; j < maxY; j++)
+		{
+			if (featurePoint.at<uchar>(i, j) == 1)
+			{
+				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)
+				{
+					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;
+			}
+		}
+	}
+
+	auto number = 0;
+	for (auto i = minX; i < maxX; i++)
+	{
+		for (auto j = minY; j < maxY; j++)
+		{
+			if (featurePoint.at<uchar>(i, j) != -2) number++;
+		}
+	}
+
+	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);
 }

Classes/CoreAlgorithm.h

@@ -1,22 +1,35 @@
 #pragma once
 #include <opencv2/opencv.hpp>
 #include <string>
-#include <math.h>
+#include <vector>
+#include "CameraArguments.h"
 
 using namespace std;
 using namespace cv;
 
+typedef unsigned char byte;
+
 class CoreAlgorithm
 {
 private:
 	Mat image, tmp;
-	vector<Mat> channel;
+	vector<Mat> channel,hsv;
 	const int neighborhood = 5;
-	int minX = 0, maxX = 0, minY = 0, maxY = 0;
-public:
+	int minX = 0, maxX = 0, minY = 1280, maxY = 0;
+	CameraArguments* cArg;
+private:
 	CoreAlgorithm(std::string path);
 	~CoreAlgorithm();
 	Mat OSTU(Mat src);
-	void Rgb2Hsv(float r, float g, float b, float& h, float& s, float& v);
+	vector<int> Debruijn(int k, int n);
+	vector<int> Hsv(int r,int g,int b);
+	Mat Reconstruction(Mat featurePoint, int num,Mat Hc1,Mat Hp2,Mat map);
+	template <class ForwardIterator>
+	size_t argmin(ForwardIterator first, ForwardIterator last);
+	template <class ForwardIterator>
+	size_t argmax(ForwardIterator first, ForwardIterator last);
+	template <typename Tp>
+	std::vector<Tp> convertMat2Vector(const cv::Mat& mat);
+public:
 	void run();
 };