某边缘直线已知，另一边缘直线未知，但是知道在图像的左下角，于是
首先通过HSV提取蓝色箱子，然后做腐蚀操作，再遍历提取每行遇到的第一个蓝色值，即可提取边缘。
最后通过取平均值计算两个点的X、Y，然后作出一个向量，求与另一条已知直线（向量）的夹角、距离。
#include<iostream>
#include<opencv2/opencv.hpp>
#include<vector>
#include<algorithm>
#include<numeric>
#include<cmath>
#include<cassert>
using namespace cv;
using namespace std;
const float PI = 3.14159265358979323846; //圆周率
void ExtractLine(Mat& photo); //实现主要功能的子函数
Mat dstImage; //最终输出图像
float Need_Length; //爪子到箱子边缘的距离 ,单位为像素距离
float Need_Angle; //爪子的边与箱子的边所成的夹角，单位为度
// 弧度向角度的转换 最后的结果单位为度°
float toDegree(float val)
{
return val * 180 / PI;
}
//模长或者范数的计算
float norm(const vector<float>& x)
{
float val = 0.;
for (auto elem : x)
val += elem * elem;
return sqrt(val);
}
// *运算符重载，两个向量相乘
float operator*(const vector<float>& x, const vector<float>& y)
{
assert(x.size() == y.size());
float sum = 0.;
for (size_t i = 0; i < x.size(); ++i)
sum += x[i] * y[i];
return sum;
}
//计算向量之间的夹角，单位为度°
double angle(const vector<float>& x, const vector<float>& y)
{
return toDegree(acos(x * y / norm(x) / norm(y)));
// x*y, 计算二者的内积
}
// 向量减法- 运算符重载
vector<float> operator-(const vector<float>& x, const vector<float>& y)
{
assert(x.size() == y.size());
vector<float> tmp;
for (size_t i = 0; i < x.size(); ++i)
tmp.push_back(x[i] - y[i]);
return tmp; // 返回局部变量的拷贝
}
// 二维返回其模长
float twoDCrossProd(const vector<float>& x, const vector<float>& y)
{
return x[0] * y[1] - x[1] * y[0];
}
//点到线的距离
// x0, x1, x2 分别表示三角形的三个顶点的坐标
// 这里计算的是点x0到x1和x2构成的直线的距离
float distance(const vector<float>& x0, const vector<float>& x1, const vector<float>& x2)
{
return twoDCrossProd(x1 - x0, x2 - x0) / norm(x1 - x2);
}
int main()
{
cout << "Begin..." << endl;
Mat picture = imread("9.jpg");
ExtractLine(picture);
// unsigned char value = dstImage.at<uchar>(900, 620);//读出第 i 行第 j 列像素值
// int value1 = int(value);
// cout << value1 << endl;
namedWindow("HSV二值化后图像", WINDOW_NORMAL);
imshow("HSV二值化后图像", dstImage);
//imwrite("HSV二值化后腐蚀图像.jpg", dstImage);
cout << "距离：" << Need_Length << endl; //爪子到箱子边缘的距离
cout << "角度：" << Need_Angle << endl;
waitKey(0);
return 0;
}
//实现主要功能的子函数
void ExtractLine(Mat& photo)
{
vector<float> x; //用于保存最初测得的x
vector<float> y; //用于保存最初测得的y
vector<float> x_filtering; //滤波后的x
vector<float> y_filtering; //滤波后的y
//蓝色箱子的HSV范围
int iLowH = 100;
int iHighH = 124;
int iLowS = 43;
int iHighS = 255;
int iLowV = 46;
int iHighV = 255;
Mat HSVImage;
cvtColor(photo, HSVImage, COLOR_BGR2HSV);
inRange(HSVImage, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), dstImage); //HSV二值化
//开操作去噪点
cv::Mat elementRect;
//Mat dstImage_OPEN;
elementRect = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(11, 11), cv::Point(-1, -1));
cv::morphologyEx(dstImage, dstImage, cv::MORPH_OPEN, elementRect);
//遍历
for (int i = 0; i < dstImage.rows - 1; i++)
{
for (int j = dstImage.cols - 2; j > 1; j--)
{
if (dstImage.at<uchar>(i, j) == dstImage.at<uchar>(i, j - 1))
continue;
else //遇到了白色
{
/*
if (dstImage.at<uchar>(i, j - 5) =='0' && dstImage.at<uchar>(i, j - 13) == '0' && dstImage.at<uchar>(i, j - 21) == '0' && dstImage.at<uchar>(i, j - 32) == '0' && dstImage.at<uchar>(i, j - 40) == '0')
{
x.push_back(j-1);
y.push_back(i);
break; //已经找到间断点，跳出循环
}
else
continue;
*/
x.push_back(j);
y.push_back(i);
break; //已经找到间断点，跳出循环
}
}
}
// cout << "x.size():" << x.size() << endl;
// cout << "y.size():" << y.size() << endl;
int FilterCount = 150; // 滤波,过滤掉x，y数组的前后150个元素
for (vector<float>::iterator iter = (x.begin() + FilterCount); iter != (x.end() - FilterCount); iter++)
{
//cout << *iter << " ";
x_filtering.push_back(*iter);
}
for (vector<float>::iterator iter = (y.begin() + FilterCount); iter != (y.end() - FilterCount); iter++)
{
//cout << *iter << " ";
y_filtering.push_back(*iter);
}
// cout << "x_filtering.size():" << x_filtering.size() << endl;
// cout << "y_filtering.size():" << y_filtering.size() << endl;
//x_filtering分为两组，每组取中位数作为代表
// accumulate函数就是求vector和的函数；
float x_sumValue_qian = accumulate(begin(x_filtering), end(x_filtering) - x_filtering.size() / 2, 0.0); //x_filtering前半部分的和
float x_sumValue_hou = accumulate(end(x_filtering) - x_filtering.size() / 2, end(x_filtering), 0.0); //x_filtering后半部分的和
float y_sumValue_qian = accumulate(begin(y_filtering), end(y_filtering) - y_filtering.size() / 2, 0.0); //y_filtering前半部分的和
float y_sumValue_hou = accumulate(end(y_filtering) - y_filtering.size() / 2, end(y_filtering), 0.0); //y_filtering后半部分的和
float x_meanValue_qian = x_sumValue_qian / (x_filtering.size() / 2); // 求x数组前半部分均值
float x_meanValue_hou = x_sumValue_hou / (x_filtering.size() / 2); // 求x数组后半部分均值
float y_meanValue_qian = y_sumValue_qian / (y_filtering.size() / 2); // 求y数组前半部分均值
float y_meanValue_hou = y_sumValue_hou / (y_filtering.size() / 2); // 求y数组后半部分均值
cout << "x_qian:" << x_meanValue_qian << endl;
cout << "y_qian:" << y_meanValue_qian << endl;
cout << "x_hou:" << x_meanValue_hou << endl;
cout << "y_hou:" << y_meanValue_hou << endl;
// 在图像中
float Xd = 1276;
float Xc = 1281;
float Yd = 736;
float Yc = 638;
// 向量dc
vector<float> dc = {
Xc - Xd,Yc - Yd };
// 检测箱子边缘所得的向量 BoxVector
vector<float> BoxVector = {
x_meanValue_qian - x_meanValue_hou ,y_meanValue_qian - y_meanValue_hou };
// 计算箱子边缘与爪子的夹角
Need_Angle = angle(BoxVector, dc);
vector<float> d = {
Xd,Yd }; //爪子的d点坐标形式
vector<float> box_qian_MeanPoint = {
x_meanValue_qian ,y_meanValue_qian }; //箱子边缘上半部分点的平均
vector<float> box_hou_MeanPoint = {
x_meanValue_hou ,y_meanValue_hou }; //箱子边缘下半部分点的平均
//计算爪子上的d点到箱子边缘的距离
Need_Length = distance(d, box_qian_MeanPoint, box_hou_MeanPoint);
Need_Length = abs(Need_Length);
/*
float a = sqrt(2);
cout << "angle:" << acos( a/ 2) << endl;
vector<float> aa = { 1,1 };
cout << "length:" << norm(aa)<<endl;
vector<float> aa1 = { 1,0};
cout << "angle:" << angle(aa, aa1) << endl;
vector<float> a1 = { 3,1.5 };
vector<float> a2 = { 1,1 };
vector<float> a3 = { 2,1 };
cout << "distance:" << distance(a1, a2, a3) << endl;
*/
}