|
@@ -3,3 +3,128 @@
|
|
|
//
|
|
|
|
|
|
#include "point3D_tool.h"
|
|
|
+
|
|
|
+bool Point3D_tool::WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
|
|
|
+ DLOG(INFO) << "begain push point data to " << file;
|
|
|
+ std::fstream fileTxtCloud;
|
|
|
+ fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
|
|
|
+ for (auto &point: *cloud) {
|
|
|
+ fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
|
|
|
+ }
|
|
|
+ fileTxtCloud.close();
|
|
|
+ DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool Point3D_tool::WriteTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud) {
|
|
|
+ DLOG(INFO) << "begain push point data to " << file;
|
|
|
+ std::fstream fileTxtCloud;
|
|
|
+ fileTxtCloud.open(file, std::ios_base::trunc | std::ios::in | std::ios::out);
|
|
|
+ for (auto &point: *cloud) {
|
|
|
+ fileTxtCloud << point.x << " " << point.y << " " << point.z << std::endl;
|
|
|
+ }
|
|
|
+ fileTxtCloud.close();
|
|
|
+ DLOG(INFO) << "Push point complete, push point number: " << cloud->size();
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+bool Point3D_tool::ReadTxtCloud(const std::string &file, pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {
|
|
|
+ DLOG(INFO) << "Load point data form file: " << file;
|
|
|
+ std::ifstream fin(file.c_str());
|
|
|
+ if (fin.bad()) {
|
|
|
+ LOG(WARNING) << "File " << file << " don't exist.";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ const int line_length = 64;
|
|
|
+ char str[line_length] = {0};
|
|
|
+ while (fin.getline(str, line_length)) {
|
|
|
+ pcl::PointXYZ point{};
|
|
|
+ if (string2PointXYZ(str, point)) {
|
|
|
+ cloud->push_back(point);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ DLOG(INFO) << "Load point complete, get point number: " << cloud->size();
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+template <typename PclPoint>
|
|
|
+int Point3D_tool::getMinRect(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds) {
|
|
|
+ if (clouds->points.size() < 4) {
|
|
|
+ DLOG(ERROR) << "points to little.";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ std::vector<cv::Point2f> cv_clouds;
|
|
|
+ for (auto &point: clouds->points) {
|
|
|
+ cv_clouds.emplace_back(point.x, point.y);
|
|
|
+ }
|
|
|
+ rect = cv::minAreaRect(cv_clouds);
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+int Point3D_tool::getRectCloud(cv::RotatedRect &rect, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value, int numbers) {
|
|
|
+ cv::Point2f cv_points[4];
|
|
|
+ rect.points(cv_points);
|
|
|
+ // PclPoint point;
|
|
|
+ pcl::PointXYZ point;
|
|
|
+ // 左侧点
|
|
|
+ double step_x = (cv_points[1].x - cv_points[0].x) / numbers;
|
|
|
+ double step_y = (cv_points[1].y - cv_points[0].y) / numbers;
|
|
|
+ for (int i = 0; i < numbers; i++) {
|
|
|
+ point.x = cv_points[0].x + i * step_x;
|
|
|
+ point.y = cv_points[0].y + i * step_y;
|
|
|
+ point.z = z_value;
|
|
|
+ points->points.emplace_back(point);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 上侧点
|
|
|
+ step_x = (cv_points[2].x - cv_points[1].x) / numbers;
|
|
|
+ step_y = (cv_points[2].y - cv_points[1].y) / numbers;
|
|
|
+ for (int i = 0; i < numbers; i++) {
|
|
|
+ point.x = cv_points[1].x + i * step_x;
|
|
|
+ point.y = cv_points[1].y + i * step_y;
|
|
|
+ point.z = z_value;
|
|
|
+ points->points.emplace_back(point);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 右侧点
|
|
|
+ step_x = (cv_points[3].x - cv_points[2].x) / numbers;
|
|
|
+ step_y = (cv_points[3].y - cv_points[2].y) / numbers;
|
|
|
+ for (int i = 0; i < numbers; i++) {
|
|
|
+ point.x = cv_points[2].x + i * step_x;
|
|
|
+ point.y = cv_points[2].y + i * step_y;
|
|
|
+ point.z = z_value;
|
|
|
+ points->points.emplace_back(point);
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ // 下侧点
|
|
|
+ step_x = (cv_points[0].x - cv_points[3].x) / numbers;
|
|
|
+ step_y = (cv_points[0].y - cv_points[3].y) / numbers;
|
|
|
+ for (int i = 0; i < numbers; i++) {
|
|
|
+ point.x = cv_points[3].x + i * step_x;
|
|
|
+ point.y = cv_points[3].y + i * step_y;
|
|
|
+ point.z = z_value;
|
|
|
+ points->points.emplace_back(point);
|
|
|
+ }
|
|
|
+
|
|
|
+ return points->size();
|
|
|
+}
|
|
|
+
|
|
|
+template <typename PclPoint>
|
|
|
+int Point3D_tool::getRectCloud(pcl::shared_ptr<pcl::PointCloud<PclPoint>> &clouds, pcl::shared_ptr<pcl::PointCloud<pcl::PointXYZ>> &points, float z_value, int numbers) {
|
|
|
+ cv::RotatedRect rect = getMinRect(clouds);
|
|
|
+ getRectCloud(rect, points, z_value, numbers);
|
|
|
+ return points->size();
|
|
|
+}
|
|
|
+
|
|
|
+void Point3D_tool::getRectWidthlength(cv::RotatedRect &rect, double &width, double &length) {
|
|
|
+ cv::Point2f points[4];
|
|
|
+ rect.points(points);
|
|
|
+
|
|
|
+ double a = sqrt((points[1].x - points[0].x) * (points[1].x - points[0].x) + (points[1].y - points[0].y) * (points[1].y - points[0].y));
|
|
|
+ double b = sqrt((points[2].x - points[1].x) * (points[2].x - points[1].x) + (points[2].y - points[1].y) * (points[2].y - points[1].y));
|
|
|
+
|
|
|
+ width = MIN(a, b);
|
|
|
+ length = MAX(a, b);
|
|
|
+}
|
|
|
+
|