上传文件至 'v1'

v1/ArithPmd.h

@@ -0,0 +1,195 @@
+#ifndef ARITHPMD_H
+#define ARITHPMD_H
+
+#include "inc.h"
+#include "nlohmann/json.hpp"
+#include <opencv2/core/eigen.hpp>
+#include <pcl/point_types.h>
+#include <pcl/common/transforms.h>
+#include "pcl/common/centroid.h" 
+// #include <opencv2/aruco.hpp>
+
+struct Screen {
+    float width;    // 屏幕宽度(m)
+    float height;  // 屏幕高度(m)
+    uint16_t cols;  // 屏幕分辨率列数
+    uint16_t rows;  // 屏幕分辨率行数
+};
+
+struct ROI {
+    uint32_t startRow;
+    uint32_t startCol;
+    uint32_t blockRows;
+    uint32_t blockCols;
+};
+
+/*
+ * 重要变量用 Matrix，中间变量要是需要时传 Array
+ * 能用 row，col，width，height，就不用x，y
+ * 能用Qt的功能就用Qt的，如目录
+ * 图像用img，用拍摄图像而非照片表示
+ * R旋转，TL平移向量，T变换矩阵，CVT将V坐标系的变量变换到C坐标系的变换矩阵
+ */
+
+bool save_matrix_as_img(const MatrixXf& m, std::string path);
+
+/*
+ * 矩阵数据输出到文件
+ * add_index: 是否将第一行或第一列变为索引下标
+ * orientation：0，第一行变为索引，否则，第一列变为索引
+*/
+bool save_matrix_as_txt(const MatrixXf& m, std::string path, bool add_index = false, int orientation = 0);
+
+/*
+ * 矩阵保存为 ply 格式的点云文件
+ * params: 矩阵, file path
+ */
+bool save_matrix_as_ply(const MatrixXf& m, std::string path);
+
+/*
+ * 将矩阵变为齐次
+ */
+MatrixXf matrix_to_home(const MatrixXf& origin);
+
+/*
+ * Rays from camera, for every pixel, a line cross this pixel and origin of camera coordinate
+ * 每个像素拍摄的路径设为一个向量，本函数产生一个矩形平面，为每个像素生成这样一个向量，每个向量用两点定义，第一点为相机原点，
+ * 第二点(x,y,z)为相机坐标系下给定z所求得的三维坐标，忽略镜头畸变
+ * 参数：相机内参，相机分辨率，给定Z，camera_rays(相机坐标系下z=给定值的 rays from camera)
+ * camrea_rays 格式: 3 x (img.h x img.w), 每列格式为(x,y,z)，像素保存顺序为从左往右，从上往下
+ */
+bool configCameraRay(const Matrix3f& cameraMatrix, const cv::Size& imageSize, float Z, MatrixXf& camera_rays);
+
+/*
+ * 计算相机光线与参考平面的交点，参考平面用点法式表达
+ * 参数：参考平面点，参考平面法线，rays from camera，保存交点矩阵
+ * refPlane 格式：3 x (img.h x img.w), 每列格式为(x,y,z)，像素保存顺序为从左往右，从上往下
+ */
+bool configRefPlane(const Eigen::Vector3f& plane_point, const Eigen::Vector3f& plane_normal, const MatrixXf& camera_rays, MatrixXf& refPlane);
+
+/*
+ * for every screen pixel, compute their position in world coordinate system
+ * params: Screen information, rotation of screen relative to world, translation of screen relative to world, matrix save screen pixels postion,
+ * screen_pix_pos 格式: 3 x (screen.h x screen.w), 每列格式为(x,y,z)，像素保存顺序为从左往右，从上往下
+ */
+bool configScreenPixelPos(const Screen& screen, const Matrix4f& WST, MatrixXf& screen_pix_pos);
+
+/*
+ * 生成四步相移法条纹图案
+ * 参数：图案尺寸，x，y方向周期，保存结果(size = 16，顺序为高频x,y，低频x,y)
+ */
+// bool pattern_gen(std::vector<uint8_t> pat_size, std::vector<uint8_t> period, std::vector<MatrixXf>& pats);
+
+/* four step phase shifting
+ * params: images(size=16), result wraped phase map(size=4)
+ */
+bool phase_shifting(const std::vector<MatrixXf>& imgs, std::vector<MatrixXf>& wraped_ps_maps);
+
+/*
+ * two frequency phase unwrapping
+ * params: 包裹相位(高频竖条纹，高频横条纹，低频竖条纹，低频横条纹),高频竖条纹周期数，高频横条纹周期数，保存相位结果
+ */
+bool phase_unwrapping(const vector<MatrixXf>& wraped_ps_maps, uint16_t period_width, uint16_t period_height, vector<MatrixXf>& unwrap_ps_maps);
+
+/*
+ * 求屏幕上相位与相机img对应的像素的三维坐标
+ * params：相位图，屏幕信息，条纹图宽度方向上周期数，高度方向周期数，屏幕像素位置，
+ * 与相机img相位匹配的屏幕像素坐标（格式: 3 x (cam_img.w x cam_img.h), 每列格式为(x,y,z)，像素保存顺序为从左往右，从上往下）
+ */
+bool screen_camera_phase_match(const vector<MatrixXf>& unwrap_ps_maps, const Screen& screen, uint16_t period_width, uint16_t period_height,
+    const MatrixXf& screen_pix_pos, MatrixXf& screen_camera_phase_match_pos);
+
+/*
+ * 斜率计算
+ * params: 相机光心坐标，参考平面，屏幕相机相位匹配，保存斜率结果X和Y方向,尺寸为1 x img.size
+ */
+bool slope_calculate(const Vector3f& camera_world, const MatrixXf& refPlane, const MatrixXf& screen_camera_phase_match_pos,
+    std::vector<MatrixXfR>& slope);
+
+/*
+ * 从斜率恢复相位，modal 法，使用 Zernike polynomials
+ * params: x方向斜率(M x N)，y方向斜率，保存高度结果，x和y方向的物理尺寸范围(mm), Zernike polynomials 项数
+ */
+bool modal_reconstruction(const MatrixXf& sx, const MatrixXf& sy, MatrixXfR& Z, const std::vector<float>& range, uint32_t terms);
+
+double computeReprojectionErrors(
+    const vector<vector<cv::Point3f>>& objectPoints,
+    const vector<vector<cv::Point2f>>& imagePoints,
+    const vector<Mat>& rvecs, const vector<Mat>& tvecs,
+    const Mat& cameraMatrix, const Mat& distCoeffs,
+    vector<float>& perViewErrors);
+
+/*
+// aruco 标定板参数
+struct ArucoBoard {
+    cv::Size markers_size;
+    float markerLength;    // unit: m
+    float markerSeparation;
+    cv::Ptr<cv::aruco::Dictionary> dictionary;
+    float screenOffset_X;
+    float screenOffset_Y;
+};
+
+
+bool aruco_analyze(const vector<Mat>& imgs_board, const ArucoBoard& board, const Mat& cameraMatrix, const Mat& distCoeffs, vector<Mat>& Rmats,
+    vector<cv::Vec3f>& Tvecs, vector<vector<int>>& Ids, vector<vector<vector<cv::Point2f>>>& corners,
+    vector<vector<vector<cv::Point2f>>>& rejectedCandidates, std::string output_dir);
+*/
+/*
+ * system geometry calibration
+ * implement of paper "Flexible geometrical calibration for fringe-reflection
+ measurement(2012)"
+ */
+bool system_calib(vector<Matrix3f>& CVR, vector<Vector3f>& CVTL, Matrix3f& CSR, VectorXf& CSTL_D, vector<Vector3f>& n);
+
+class ArithPmd
+{
+public:
+    ArithPmd();
+    ~ArithPmd();
+
+public:
+    cv::Size img_size;      // 相机图像分辨率
+    uint16_t period_width;  // 宽度方向条纹周期数
+    uint16_t period_height; // 高度方向条纹周期数
+
+    cv::Mat mask0, mask1, mask2, mask3;
+    
+private:
+    Screen screen;          // 屏幕信息
+    ROI roi0,roi1,roi2,roi3;                // 拍摄图片上的重建区域
+
+    Matrix4f CTC0,CTC1, CTC2, CTC3;
+
+    MatrixXf refPlane0, refPlane1, refPlane2, refPlane3;
+    Vector3f camera_origin_world0, camera_origin_world1, camera_origin_world2, camera_origin_world3;
+    MatrixXf screen_pix_pos0, screen_pix_pos1, screen_pix_pos2, screen_pix_pos3;
+    MatrixXf roi_m0, roi_m1, roi_m2, roi_m3; // mask 区域的矩阵
+
+    // std::vector<Matrix4f*> CTC{}; // 旋转平移矩阵
+
+    // 计算出来的结果
+    // std::vector<MatrixXf*> refPlane{};      // 相机光线与参考平面的交点，世界坐标系下，3 x (img.h x img.w)
+    // std::vector <Vector3f*> camera_origin_world{ };   // 相机坐标系原点在世界坐标系下的表示
+    // std::vector<MatrixXf*> screen_pix_pos{};// 屏幕像素在世界坐标系下的位置，3 x (screen.h x screen.w)
+
+    
+    // std::vector<Mat> patterns;   // 投影的条纹图案
+    // std::vector<Eigen::MatrixXf> img_pats;  // 拍摄的条纹图像
+    // uint16_t screen_delay;  // 投影条纹时等待的时间(毫秒)
+
+
+public:
+
+    int Initlization(const nlohmann::json& param);
+
+    int Preprocess(const std::vector<cv::Mat>& images, int area_threshold, int32_t camera_index);
+
+    int Predict(const std::vector<Eigen::MatrixXf>& img_pats, MatrixXfR& pcl, int32_t camera_index);
+
+    int Stitch(const pcl::PointCloud<pcl::PointXYZ>& cloud1, const pcl::PointCloud<pcl::PointXYZ>& cloud2, pcl::PointCloud<pcl::PointXYZ>& cloud_output, int32_t camera_index);
+};
+
+
+
+#endif // ARITHPMD_H

v1/WaferBuild.cpp

@@ -0,0 +1,292 @@
+#ifndef _EXPORTING 
+#define _EXPORTING 
+#endif 
+
+#include "WaferRebuild.h"
+#include "ArithPmd.h"
+
+WaferRebuild::WaferRebuild()
+{
+    arith_pmd = new ArithPmd();
+}
+
+WaferRebuild::~WaferRebuild()
+{
+    if (arith_pmd != nullptr)
+    {
+        delete arith_pmd;
+        arith_pmd = nullptr;
+    }
+}
+
+std::tuple<int, std::string> WaferRebuild::initParam(const nlohmann::json& param)
+{
+    try
+    {
+        int retcode = arith_pmd->Initlization(param);
+        return std::make_tuple(0, "sucess");
+    }
+    catch (const std::exception& e)
+    {
+        return std::make_tuple(-1, e.what());
+    }
+	
+}
+
+
+std::tuple<int, std::string> WaferRebuild::calcCloud(const std::vector<std::shared_ptr<infra::Image>>& image_list, std::shared_ptr<infra::CloudPoint<double>>& cloud_points, int32_t camera_index)
+{
+    try
+    {
+        if (!cloud_points) {
+            throw std::runtime_error("Error: cloud_points is nullptr!");
+        }
+        std::vector<Eigen::MatrixXf> img_pats;
+        std::vector<cv::Mat> gray_images;
+        img_pats.resize(image_list.size());
+        for (int i = 0; i < image_list.size(); ++i) {
+            int type = (image_list[i]->type() == -1) ? CV_8UC1 : CV_8UC3; // 假设 -1 是灰度图，其他都是三通道图像
+            cv::Mat mat;
+            cv::Mat grayMat;
+            if (type == CV_8UC1) {
+                grayMat = cv::Mat(arith_pmd->img_size.height, arith_pmd->img_size.width, type, image_list[i]->data());
+                gray_images.push_back(grayMat);
+            }
+            else {
+                mat = cv::Mat(arith_pmd->img_size.height, arith_pmd->img_size.width, type, image_list[i]->data());
+                cv::cvtColor(mat, grayMat, cv::COLOR_BGR2GRAY);
+                gray_images.push_back(grayMat);
+            }
+            // cv::cv2eigen(grayMat, img_pats[i]);
+        }
+        std::vector<cv::Mat> gray_temp{ gray_images[gray_images.size() - 4], gray_images[gray_images.size() - 3], gray_images[gray_images.size() - 2], gray_images[gray_images.size() - 1] };
+        for (int i = 0; i < gray_images.size(); ++i)
+        {
+            cv::Mat temp;
+            if (camera_index == 0)
+            {
+                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask0);
+            }
+            else if (camera_index == 1)
+            {
+                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask1);
+            }
+            else if (camera_index == 2)
+            {
+                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask2);
+            }
+            else if (camera_index == 3)
+            {
+                cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask3);
+            }
+            // cv::bitwise_and(gray_images[i], gray_images[i], temp, arith_pmd->mask);
+            cv::imwrite("temp.bmp", temp);
+            cv::cv2eigen(temp, img_pats[i]);
+        }
+        int retcode0 = arith_pmd->Preprocess(gray_temp, 500, camera_index);
+        MatrixXfR pcl;
+        int retcode = arith_pmd->Predict(img_pats, pcl, camera_index);
+
+        // std::vector<infra::Point3d<float>> points;
+
+        // 预留足够空间
+        int numPoints = pcl.cols();
+        cloud_points->cloud_point.resize(numPoints);
+        // cloud_points->cloud_point.reserve(numPoints);
+        // cloud_points.reserve(numPoints);
+        // 遍历每一个点
+        for (int i = 0; i < numPoints; ++i) {
+            // 创建一个新的 Point3d 对象并添加到 vector
+            infra::Point3d<double> point = infra::Point3d<double>(
+                pcl(0, i), // X 坐标
+                pcl(1, i), // Y 坐标 m  
+                pcl(2, i)  // Z 坐标
+            );
+            cloud_points->cloud_point[i] = point;
+        }
+        save_matrix_as_ply(pcl, "surface.ply");
+        return std::make_tuple(0, "sucess");
+    }
+    catch (const std::exception& e) {
+        return std::make_tuple(-1, e.what());
+    }
+
+}
+
+pcl::PointCloud<pcl::PointXYZ> convertToPCLPointCloud(const std::shared_ptr<infra::CloudPoint<double>>& cloud_points) {
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+
+    if (!cloud_points) {
+        std::cerr << "Error: Input cloud_points is nullptr." << std::endl;
+        return cloud;
+    }
+
+    // Resize the PCL PointCloud to the size of cloud_points
+    cloud.points.resize(cloud_points->size());
+
+    // Convert each point from infra::CloudPoint<double> to pcl::PointXYZ
+    for (size_t i = 0; i < cloud_points->size(); ++i) {
+        const infra::Point3d<double>& point = cloud_points->cloud_point[i];
+        cloud.points[i].x = static_cast<float>(point.x);
+        cloud.points[i].y = static_cast<float>(point.y);
+        cloud.points[i].z = static_cast<float>(point.z);
+    }
+
+    return cloud;
+}
+
+static std::shared_ptr<infra::CloudPoint<double>> convertToInfraCloudPoint(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
+    // Create a new infra::CloudPoint<double> object
+    auto cloud_points = std::make_shared<infra::CloudPoint<double>>();
+
+    // Resize the cloud_points->cloud_point vector to the size of cloud
+    cloud_points->cloud_point.resize(cloud.points.size());
+
+    // Convert each pcl::PointXYZ point to infra::Point3d<double> and store in cloud_points
+    for (size_t i = 0; i < cloud.points.size(); ++i) {
+        cloud_points->cloud_point[i].x = static_cast<double>(cloud.points[i].x);
+        cloud_points->cloud_point[i].y = static_cast<double>(cloud.points[i].y);
+        cloud_points->cloud_point[i].z = static_cast<double>(cloud.points[i].z);
+    }
+
+    return cloud_points;
+}
+
+std::tuple<int, std::string> WaferRebuild::mergeCloud(const std::map<int32_t, std::shared_ptr<infra::CloudPoint<double>>>& cloud_points_list,
+    std::shared_ptr<infra::CloudPoint<double>>& cloud_points)
+{
+    try
+    {
+        int error_code = 0;
+        std::string error_message = "Success";
+        pcl::PointCloud<pcl::PointXYZ> cloud_output;
+        auto it = cloud_points_list.begin();
+        if (it == cloud_points_list.end()) {
+            error_code = -1;
+            error_message = "Error: Input cloud_points_list is empty.";
+            return std::make_tuple(error_code, error_message);
+        }
+       cloud_output = convertToPCLPointCloud(it->second);
+        ++it;
+        // Merge each subsequent cloud in the list
+        while (it != cloud_points_list.end()) {
+            pcl::PointCloud<pcl::PointXYZ> cloud_to_merge = convertToPCLPointCloud(it->second);
+
+            // Call Stitch function to merge cloud_output and cloud_to_merge
+            int stitch_result = arith_pmd->Stitch(cloud_output, cloud_to_merge, cloud_output, it->first);
+            if (stitch_result != 0) {
+                error_code = stitch_result;
+                error_message = "Error: Stitching failed for camera index " + std::to_string(it->first);
+                break;
+            }
+            ++it;
+        }
+        cloud_points = convertToInfraCloudPoint(cloud_output);
+
+        return std::make_tuple(error_code, error_message);
+    }
+    catch (const std::exception& e) {
+        return std::make_tuple(-1, e.what());
+    }
+}
+
+
+static infra::Point3d<double> calculateCentroid(const pcl::PointCloud<pcl::PointXYZ>& pcl_cloud) {
+    infra::Point3d<double> centroid;
+
+    // Create an Eigen vector to store the centroid
+    Eigen::Vector4f pcl_centroid;
+
+    // Compute centroid using PCL function
+    pcl::compute3DCentroid(pcl_cloud, pcl_centroid);
+
+    // Convert the centroid from Eigen vector to infra::Point3d<double>
+    centroid.x = pcl_centroid[0];
+    centroid.y = pcl_centroid[1];
+    centroid.z = pcl_centroid[2];
+
+    return centroid;
+}
+
+
+std::tuple<int, std::string> WaferRebuild::warpage(const std::shared_ptr<infra::CloudPoint<double>>& cloud_points, int32_t filterMM, nlohmann::json& result)
+{
+    try {
+        if (!cloud_points) {
+            throw std::runtime_error("Input cloud_points is nullptr.");
+        }
+
+        // Convert infra::CloudPoint<double> to pcl::PointCloud<pcl::PointXYZ>
+        pcl::PointCloud<pcl::PointXYZ> pcl_cloud = convertToPCLPointCloud(cloud_points);
+
+        // Calculate centroid (assuming centroid calculation is implemented in math_tool.h)
+        infra::Point3d<double> centroid = calculateCentroid(pcl_cloud);  // Example function, replace with actual implementation
+
+        // Define x and y axes
+        infra::Point3d<double> axis_x(1.0, 0.0, 0.0);  // Assuming x-axis direction
+        infra::Point3d<double> axis_y(0.0, 1.0, 0.0);  // Assuming y-axis direction
+
+        // Calculate heights and populate result JSON
+        for (auto& axis : { "x", "y" }) {
+            std::vector<double> heights;
+            double bow = 0.0;
+            double warpage = 0.0;
+
+            infra::Point3d<double> axis_vector = (axis == "x") ? axis_x : axis_y;
+
+            // Calculate heights and populate heights vector
+            for (size_t i = 0; i < pcl_cloud.points.size(); ++i) {
+                infra::Point3d<double>& point = cloud_points->cloud_point[i];
+                double height = point.x * axis_vector.x + point.y * axis_vector.y + point.z * axis_vector.z;
+                heights.push_back(height);
+            }
+
+            // Calculate bow (average height)
+            if (!heights.empty()) {
+                bow = std::accumulate(heights.begin(), heights.end(), 0.0) / heights.size();
+            }
+
+            // Calculate warpage (standard deviation of heights)
+            if (heights.size() > 1) {
+                double sum_sq_diff = std::inner_product(heights.begin(), heights.end(), heights.begin(), 0.0);
+                double mean = bow;
+                double variance = sum_sq_diff / heights.size() - mean * mean;
+                warpage = std::sqrt(variance);
+            }
+
+            // Fill JSON result
+            result[axis]["height"] = heights;
+            result[axis]["bow"] = bow;
+            result[axis]["warpage"] = warpage;
+        }
+
+        /*
+        // Calculate max bow and max warpage
+        double max_bow = 0.0;
+        double max_warpage = 0.0;
+        double angle_max_bow = 0.0;
+        double angle_max_warpage = 0.0;
+
+        // Calculate max_bow and max_warpage values
+        // (example calculation, replace with actual implementation)
+        // Assume you have functions to find maximum bow and warpage and their angles
+        max_bow = calculateMaxBow(result);
+        max_warpage = calculateMaxWarpage(result);
+        angle_max_bow = calculateAngleMaxBow(result);
+        angle_max_warpage = calculateAngleMaxWarpage(result);
+
+        // Fill max bow and max warpage in JSON result
+        result["max_bow"]["angle"] = angle_max_bow;
+        result["max_bow"]["height"] = result[angle_max_bow]["height"];
+        result["max_bow"]["max_bow"] = max_bow;
+
+        result["max_warpage"]["angle"] = angle_max_warpage;
+        result["max_warpage"]["height"] = result[angle_max_warpage]["height"];
+        result["max_warpage"]["max_warpage"] = max_warpage;
+        */
+        return std::make_tuple(0, "Success");
+    }
+    catch (const std::exception& e) {
+        return std::make_tuple(-1, e.what());
+    }
+}

v1/inc.h

@@ -0,0 +1,34 @@
+#ifndef PMDCFG_H
+#define PMDCFG_H
+
+/*
+ * ¸ÃÎļþ¶¨Òå³£ÓÃÀàÐÍ
+ */
+
+#include <opencv2/opencv.hpp>
+#include <Eigen/Dense>
+#include<Eigen/src/Core/util/Macros.h>
+
+using std::endl;
+using std::cout;
+using std::vector;
+
+
+typedef Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixXfR;
+using Eigen::MatrixXf;
+using Eigen::Matrix3f;
+using Eigen::Matrix4f;
+typedef Eigen::Matrix<uint32_t, Eigen::Dynamic, Eigen::Dynamic> MatrixXUI;
+
+using Eigen::VectorXf;
+using Eigen::Vector2f;
+using Eigen::Vector3f;
+using Eigen::Vector4f;
+typedef Eigen::Matrix<uint32_t, Eigen::Dynamic, 1> VectorXUI;
+
+using Eigen::ArrayXf;
+using Eigen::ArrayXXf;
+
+using cv::Mat;
+
+#endif // PMDCFG_H

v1/main.cpp

@@ -0,0 +1,29 @@
+#include <iostream>
+#include <thread>
+#include "sys_time.h"
+#include "log.h"
+#include "data_queue.h"
+#include "math_tool.h"
+
+int main()
+{
+	initLog("PMD", "./pmd.log", 0);
+
+	infra::DataQueue<std::string> queue;
+	queue.push("aaa");
+	queue.push("ddd");
+
+	LOG_WARN("size: %lu", queue.size());
+
+	int64_t s = infra::SysTime::current_ms();
+	std::this_thread::sleep_for(std::chrono::seconds(2));
+	int64_t e = infra::SysTime::current_ms();
+	std::cout << e - s << std::endl;
+	LOG_WARN("e-s: %lu", e - s);
+
+
+	infra::Point3d<float> ee(1.1, 3.2, 5.6);
+	std::cout << ee.x << ee.y << ee.z << std::endl;
+	getchar();
+	return 0;
+}