|
|
@@ -0,0 +1,147 @@
|
|
|
+#include "CommonFunction.h"
|
|
|
+#include "PCLInWorkSpace.h"
|
|
|
+#include "../../Common/logger/RusLog.h"
|
|
|
+
|
|
|
+// 设置Log目录和文件
|
|
|
+bool logDirectorySetWorkSpace = false;
|
|
|
+void SetLogDirectoryAndFileWorkSpace()
|
|
|
+{
|
|
|
+ if (!logDirectorySetWorkSpace)
|
|
|
+ {
|
|
|
+ SetLogFileDirectory("F:/RUSLog/WorkSpaceLog");
|
|
|
+ SetLogFileName("WorkSpace");
|
|
|
+ SetLogLevel(RUS_LOG_WARNING);
|
|
|
+ EnableLogFile(true);
|
|
|
+ logDirectorySetWorkSpace = true;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/**
|
|
|
+ * \brief 判断点在凸多面体的每个面的内侧还是外侧
|
|
|
+ */
|
|
|
+int IsInsidePlane(Eigen::VectorXf testPoint, Eigen::VectorXf vertex0, Eigen::VectorXf vertex1, Eigen::VectorXf vertex2, bool& InsidePlaneElement)
|
|
|
+{
|
|
|
+ Eigen::Vector3f v = vertex1 - vertex0;
|
|
|
+ Eigen::Vector3f u = vertex2 - vertex0;
|
|
|
+ Eigen::Vector3f n = v.cross(u).normalized();
|
|
|
+ float dist = n.dot(testPoint) - n.dot(vertex0);
|
|
|
+
|
|
|
+ if (dist >= 0)
|
|
|
+ InsidePlaneElement = true;
|
|
|
+ else
|
|
|
+ InsidePlaneElement = false;
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+/**
|
|
|
+ * \brief 判断点云中的点是否在灵活工作空间内，灵活工作空间由12个凸多面体构成
|
|
|
+ */
|
|
|
+int IsInWorkSpace(const pcl::PointXYZ &point, StructPoint3f transformedVertexPoints[13][3], bool& InWorkSpace)
|
|
|
+{
|
|
|
+
|
|
|
+ // 待测试的点转换成向量
|
|
|
+ Eigen::VectorXf testPoint(3);
|
|
|
+ testPoint << point.x, point.y, point.z;
|
|
|
+
|
|
|
+ // 待测试的点是否在凸多面体内
|
|
|
+ bool InsidePolyhedron[12] = { false };
|
|
|
+
|
|
|
+ for (size_t i = 0; i < 12; i++) {
|
|
|
+
|
|
|
+ bool InsidePlane[5] = { false };
|
|
|
+
|
|
|
+ Eigen::VectorXf vertex00(3);
|
|
|
+ vertex00 << transformedVertexPoints[i][0].X, transformedVertexPoints[i][0].Y, transformedVertexPoints[i][0].Z;
|
|
|
+ Eigen::VectorXf vertex01(3);
|
|
|
+ vertex01 << transformedVertexPoints[i][1].X, transformedVertexPoints[i][1].Y, transformedVertexPoints[i][1].Z;
|
|
|
+ Eigen::VectorXf vertex02(3);
|
|
|
+ vertex02 << transformedVertexPoints[i][2].X, transformedVertexPoints[i][2].Y, transformedVertexPoints[i][2].Z;
|
|
|
+ Eigen::VectorXf vertex10(3);
|
|
|
+ vertex10 << transformedVertexPoints[i + 1][0].X, transformedVertexPoints[i + 1][0].Y, transformedVertexPoints[i + 1][0].Z;
|
|
|
+ Eigen::VectorXf vertex11(3);
|
|
|
+ vertex11 << transformedVertexPoints[i + 1][1].X, transformedVertexPoints[i + 1][1].Y, transformedVertexPoints[i + 1][1].Z;
|
|
|
+ Eigen::VectorXf vertex12(3);
|
|
|
+ vertex12 << transformedVertexPoints[i + 1][2].X, transformedVertexPoints[i + 1][2].Y, transformedVertexPoints[i + 1][2].Z;
|
|
|
+
|
|
|
+ IsInsidePlane(testPoint, vertex00, vertex01, vertex02, InsidePlane[0]);
|
|
|
+ IsInsidePlane(testPoint, vertex00, vertex10, vertex01, InsidePlane[1]);
|
|
|
+ IsInsidePlane(testPoint, vertex00, vertex02, vertex12, InsidePlane[2]);
|
|
|
+ IsInsidePlane(testPoint, vertex02, vertex01, vertex11, InsidePlane[3]);
|
|
|
+ IsInsidePlane(testPoint, vertex10, vertex12, vertex11, InsidePlane[4]);
|
|
|
+
|
|
|
+ InsidePolyhedron[i] = std::all_of(std::begin(InsidePlane), std::end(InsidePlane), [](bool val) { return val; });
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ InWorkSpace = std::any_of(std::begin(InsidePolyhedron), std::end(InsidePolyhedron), [](bool val) { return val; });
|
|
|
+
|
|
|
+ //// 假设灵活工作空间的边界是 -10 到 10
|
|
|
+ //if (point.x >= -10.f && point.x <= 10.f &&
|
|
|
+ // point.y >= -10.f && point.y <= 10.f &&
|
|
|
+ // point.z >= -10.f && point.z <= 10.f) {
|
|
|
+ // InWorkSpace = true;
|
|
|
+ //}
|
|
|
+ //else {
|
|
|
+ // InWorkSpace = false;
|
|
|
+ //}
|
|
|
+
|
|
|
+ return 0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+/**
|
|
|
+ * \brief 根据灵活工作空间，提取灵活工作空间内的点云
|
|
|
+ * \param cloudData 点云数据
|
|
|
+ * \param return 0
|
|
|
+ */
|
|
|
+RUS_EXECUTION_STATUS __stdcall PCLInWorkSpace(StructPCLDataInfo cloudData, StructPoint3f transformedVertexPoints[13][3], StructPCLDataInfo& cloudInWorkSpace, double& cloudRatio)
|
|
|
+{
|
|
|
+ SetLogDirectoryAndFileWorkSpace();
|
|
|
+
|
|
|
+ if (cloudData.pData == nullptr)
|
|
|
+ {
|
|
|
+ LogError("the input point cloud data is empty in the PCLInWorkSpace!\n");
|
|
|
+ return RUS_POINT_DATA_NULL;
|
|
|
+ }
|
|
|
+
|
|
|
+ try
|
|
|
+ {
|
|
|
+ // 将点云数据转换成pcl格式
|
|
|
+ PointPtr cloud = Convert2PCLCloud(cloudData);
|
|
|
+
|
|
|
+ /*
|
|
|
+ // 提取灵活工作空间内的点云workSpacePoints
|
|
|
+ */
|
|
|
+ PointPtr workSpacePoints(new PointCloud);
|
|
|
+ for (const auto& point : cloud->points)
|
|
|
+ {
|
|
|
+ bool InWorkSpace = false;
|
|
|
+
|
|
|
+ IsInWorkSpace(point, transformedVertexPoints, InWorkSpace);
|
|
|
+
|
|
|
+ if (InWorkSpace == true)
|
|
|
+ {
|
|
|
+ workSpacePoints->points.push_back(point);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ workSpacePoints->width = workSpacePoints->points.size();
|
|
|
+ workSpacePoints->height = 1;
|
|
|
+ workSpacePoints->is_dense = false;
|
|
|
+
|
|
|
+ cloudRatio = workSpacePoints->width / cloud->width;
|
|
|
+
|
|
|
+ PCLCloudConvert2CloudData(workSpacePoints, cloudInWorkSpace);
|
|
|
+ }
|
|
|
+ catch (...)
|
|
|
+ {
|
|
|
+ LogError("exception in PCLInWorkSpace while processing the point cloud!\n");
|
|
|
+ return RUS_FAILED_UNKNOWN;
|
|
|
+ }
|
|
|
+
|
|
|
+ return RUS_SUCCESSED;
|
|
|
+}
|
