bagPosition version 1.1.0

（1）版本号（2）输出模式，支持按序输出和按大输出

bagPositioning_test/bagPositioning_test.cpp

@@ -2707,9 +2707,13 @@ int main()
 	for (int i = 0; i < 9; i++)
 		poseCalibPara.invRMatrix[i] = poseCalibPara.planeCalib[i];
 
+	const char* ver = sg_bagPositioningVersion();
+	printf("ver:%s\n", ver);
+
 	char _scan_file[256];
 
 	SG_bagPositionParam algoParam;
+	algoParam.outputMode = 0; //0-按序输出（左上），1-按大小输出（最大）
 	int endGroup =  TEST_GROUP - 1;
 	for (int grp = 0; grp <= 19; grp++)
 	{

sourceCode/SG_bagPositioning.cpp

@@ -5,6 +5,13 @@
 #include <opencv2/opencv.hpp>
 #include <limits>
 
+//version 1.1.0 : 基准版本基础上添加了（1）版本号（2）输出模式，支持按序输出和按大输出
+std::string	m_strVersion = "1.1.0";
+const char* sg_bagPositioningVersion(void)
+{
+	return m_strVersion.c_str();
+}
+
 void sg_lineDataR(SVzNL3DLaserLine* a_line,
 	const double* camPoseR,
 	double groundH)
@@ -61,14 +68,20 @@ cv::Point2f _rotate2D(cv::Point2f pt, double sinTheta, double cosTheta)
 	return (cv::Point2f((float)(pt.x*cosTheta-pt.y*sinTheta), (float)(pt.x*sinTheta+pt.y*cosTheta)));
 }
 
+//从大到小
 bool compareByHeight(const SSG_2DValueI& a, const SSG_2DValueI& b) {
 	return a.valueD > b.valueD;
 }
 
+//从左到右
 bool compareByXValue(const SSG_peakRgnInfo& a, const SSG_peakRgnInfo& b) {
 	return a.centerPos.x < b.centerPos.x;
 }
 
+bool compareBySize(const SSG_peakRgnInfo& a, const SSG_peakRgnInfo& b)
+{
+	return ((a.objSize.dWidth * a.objSize.dHeight) > (b.objSize.dWidth * b.objSize.dHeight));
+}
 //检查是否是孤立突起
 bool _LRChkAbnormal(SVzNL3DLaserLine* laser3DPoints, int lineNum, int col, int y, int abnormalChkWin)
 {
@@ -2212,6 +2225,7 @@ SSG_2DValueI _backIndexingPeakPos(SSG_2DValueI dt_pk, cv::Mat& distTranformIndex
 	}
 	return { 0 , 0 };
 }
+
 ///数据输入必须是grid格式，以进行水平方向和垂直方向的处理
 ///（1）寻找边界点
 ///（2）从最高点开始进行区域生长
@@ -3817,50 +3831,58 @@ if (i == 3)
 		peakRgns.clear();
 		peakRgns.insert(peakRgns.end(), level0_objs.begin(), level0_objs.end());
 		int level0_size = (int)peakRgns.size();
-		if (level0_size > 1)  //进一步排序，分行
+		if (algoParam.outputMode == 0) //按序输出
 		{
-			//取Y最小的目标
+			if (level0_size > 1)  //进一步排序，分行
-			double minY = 0;
-			double minY_idx = -1;
-			for (int i = 0; i < level0_size; i++)
 			{
-				if (minY_idx < 0)
+				//取Y最小的目标
+				double minY = 0;
+				double minY_idx = -1;
+				for (int i = 0; i < level0_size; i++)
 				{
-					minY = peakRgns[i].centerPos.y;
+					if (minY_idx < 0)
-					minY_idx = i;
-				}
-				else
-				{
-					if (minY > peakRgns[i].centerPos.y)
 					{
 						minY = peakRgns[i].centerPos.y;
 						minY_idx = i;
 					}
+					else
+					{
+						if (minY > peakRgns[i].centerPos.y)
+						{
+							minY = peakRgns[i].centerPos.y;
+							minY_idx = i;
+						}
+					}
 				}
-			}
+				std::vector<int> row_0_outlier;
-			std::vector<int> row_0_outlier;
+				for (int i = 0; i < level0_size; i++)
-			for (int i = 0; i < level0_size; i++)
+				{
-			{
+					double y_diff = peakRgns[i].centerPos.y - minY;
-				double y_diff = peakRgns[i].centerPos.y - minY;
+					if (y_diff < algoParam.bagParam.bagW / 2) //第一行
-				if (y_diff < algoParam.bagParam.bagW / 2) //第一行
+						objOps.push_back(peakRgns[i]);
-					objOps.push_back(peakRgns[i]);
+					else
-				else
+						row_0_outlier.push_back(i);  //将其它行的序号记录下来
-					row_0_outlier.push_back(i);  //将其它行的序号记录下来
+				}
-			}
+				//对第一行的目标按从左到右排序
-			//对第一行的目标按从左到右排序
+				if (objOps.size() > 1)
-			if (objOps.size() > 1)
+				{
-			{
+					std::sort(objOps.begin(), objOps.end(), compareByXValue);
-				std::sort(objOps.begin(), objOps.end(), compareByXValue);
+				}
-			}
+				for (int i = 0; i < row_0_outlier.size(); i++)
-			for (int i = 0; i < row_0_outlier.size(); i++)
+					objOps.push_back(peakRgns[row_0_outlier[i]]);
-				objOps.push_back(peakRgns[row_0_outlier[i]]);
 #if 0
-			for (int i = level0_end + 1; i < peakRgns.size(); i++)
+				for (int i = level0_end + 1; i < peakRgns.size(); i++)
-				objOps.push_back(peakRgns[i]);
+					objOps.push_back(peakRgns[i]);
 #endif
+			}
+			else
+				objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end());
 		}
-		else
+		else //按大小输出,从在到小 
+		{
+			std::sort(peakRgns.begin(), peakRgns.end(), compareBySize);
 			objOps.insert(objOps.end(), peakRgns.begin(), peakRgns.end());
+		}
 	}
 	//碰撞检测
 	if ((objOps.size() > 0) && (smallObjPeaks.size() > 0)) 

sourceCode/SG_bagPositioning_Export.h

@@ -27,6 +27,7 @@ typedef struct
 	//SSG_objSortParam sortParam;
 	//SSG_polarScanParam polarScanParam;
 	int supportRotate;
+	int outputMode; //0 - 按序输出， 1 - 从大到小输出
 }SG_bagPositionParam;
 
 typedef struct
@@ -46,6 +47,9 @@ typedef struct
 	double baseHoleDist; //托盘两个孔洞的距离
 }SSG_stackBaseParam;
 
+//读版本号
+SG_APISHARED_EXPORT const char* sg_bagPositioningVersion(void);
+
 //数据调平
 SG_APISHARED_EXPORT void sg_lineDataR(
 	SVzNL3DLaserLine* a_line,

sourceCode/SG_baseAlgo_Export.h

@@ -343,12 +343,32 @@ SG_APISHARED_EXPORT void lineFitting(
     double* _k,
     double* _b);
 
+//拟合成通用直线方程，包括垂直
+SG_APISHARED_EXPORT void lineFitting_abc(
+    std::vector< SVzNL3DPoint>& inliers,
+    double* _a,
+    double* _b,
+    double* _c);
+
+SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs(
+    double a1, double b1, double c1,
+    double a2, double b2, double c2);
+
+//计算垂足点，直线方程：y = kx + b
 SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint(
     double x0,
     double y0,
     double k,
     double b);
 
+//计算垂足点，直线方程：ax+by+c = 0
+SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint_abc(
+    double x0,
+    double y0,
+    double A,
+    double B,
+    double C);
+
 //BresenhamËã·¨
 SG_APISHARED_EXPORT void drawLine(
     int x0, 

sourceCode/SG_baseFunc.cpp

@@ -234,6 +234,7 @@ void lineFitting(std::vector< SVzNL3DPoint>& inliers, double* _k, double* _b)
 	*_b = (-x_sum * xy_sum + xx_sum * y_sum) / (num * xx_sum - x_sum * x_sum);//根据公式求解b
 }
 
+//计算垂足点，直线方程：y = kx + b
 SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
 {
 	double A = k;
@@ -245,6 +246,14 @@ SVzNL2DPointD sx_getFootPoint(double x0, double y0, double k, double b)
 	return foot;
 }
 
+//计算垂足点，直线方程：ax+by+c = 0
+SVzNL2DPointD sx_getFootPoint_abc(double x0, double y0, double A, double B, double C)
+{
+	SVzNL2DPointD foot;
+	foot.x = (B * B * x0 - A * B * y0 - A * C) / (A * A + B * B);
+	foot.y = (-A * B * x0 + A * A * y0 - B * C) / (A * A + B * B);
+	return foot;
+}
 #if 0
 void icvprCcaByTwoPass(const cv::Mat& binImg, cv::Mat& lableImg)
 {