合并提交

camCalib/camCalib.cpp

@@ -129,6 +129,33 @@ typedef struct
 #define CALIB_CIRCLE_GRID   2
 #define CALIB_CHARUCO       3
 
+void initForwardRectMap(const cv::Mat& K, const cv::Mat& D, const cv::Mat& R,
+    const cv::Mat& newK, const cv::Size& size, cv::Mat& mapX, cv::Mat& mapY) {
+
+    std::vector<cv::Point2f> srcPts;
+    for (int r = 0; r < size.height; r++) {
+        for (int c = 0; c < size.width; c++) {
+            srcPts.push_back(cv::Point2f(c, r));
+        }
+    }
+
+    std::vector<cv::Point2f> dstPts;
+    cv::undistortPoints(srcPts, dstPts, K, D, R, newK);
+
+    mapX = cv::Mat::zeros(size.height, size.width, CV_32FC1);
+    mapY = cv::Mat::zeros(size.height, size.width, CV_32FC1);
+    int idx = 0;
+    for (int r = 0; r < size.height; r++) {
+        for (int c = 0; c < size.width; c++) {
+            mapX.ptr<float>(r)[c] = dstPts[idx].x;
+            mapY.ptr<float>(r)[c] = dstPts[idx].y;
+            idx++;
+        }
+    }
+
+    return;
+}
+
 #define CALIB_TEST_GROUP 4
 int main()
 {
@@ -251,7 +278,8 @@ int main()
 #else
         double alpha = 0.4; // 0.4;
         newCamMatrix = cv::getOptimalNewCameraMatrix(K, D, imageSize, alpha, imageSize, 0);
-        cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map_x, map_y);
+        //cv::initUndistortRectifyMap(K, D, cv::Mat(), newCamMatrix, imageSize, CV_32FC1, map_x, map_y);
+        initForwardRectMap(K, D, cv::Mat(), newCamMatrix, imageSize, map_x, map_y);
 #endif
 
         // 生成系数表
@@ -315,7 +343,7 @@ int main()
         sg_readCalibKD(calibKDName, K, D);
 #endif
 
- #if ENABLE_GEN_IMAGE
+#if ENABLE_GEN_IMAGE
         cv::Vec4f laserPE;
         generateLaserLine(10.f, 5.f, laserPE);
         std::cout << "generateLaserLine pe: " << laserPE << std::endl;
@@ -339,7 +367,7 @@ int main()
 
             cv::Mat image = generateVirtualLaserLineImage(laserPE, pe, K, D, imageSize);
 
-    #if ENABLE_DEBUG
+#if ENABLE_DEBUG
             cv::Mat color = image.clone();
             cv::resize(color, color, cv::Size(), 0.5, 0.5);
             cv::imshow("image", color);

camCalib/camCalib.vcxproj

@@ -147,11 +147,13 @@
   <ItemGroup>
     <ClCompile Include="camCalib.cpp" />
     <ClCompile Include="lineDetection_steger.cpp" />
+    <ClCompile Include="onnxDetector.cpp" />
     <ClCompile Include="sourceCode\FitMapParam.cpp" />
     <ClCompile Include="sourceCode\MonoLaserCalibrate.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="lineDetection_steger.h" />
+    <ClInclude Include="onnxDetector.h" />
     <ClInclude Include="sourceCode\FitMapParam.h" />
     <ClInclude Include="sourceCode\MonoLaserCalibrate.h" />
   </ItemGroup>

camCalib/camCalib.vcxproj.filters

@@ -27,6 +27,9 @@
     <ClCompile Include="lineDetection_steger.cpp">
       <Filter>源文件</Filter>
     </ClCompile>
+    <ClCompile Include="onnxDetector.cpp">
+      <Filter>源文件</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="sourceCode\MonoLaserCalibrate.h">
@@ -38,5 +41,8 @@
     <ClInclude Include="lineDetection_steger.h">
       <Filter>头文件</Filter>
     </ClInclude>
+    <ClInclude Include="onnxDetector.h">
+      <Filter>头文件</Filter>
+    </ClInclude>
   </ItemGroup>
 </Project>

camCalib/onnxDetector.cpp

@@ -0,0 +1,302 @@
+#include "onnxDetector.h"
+#include <opencv2/opencv.hpp>
+
+using namespace std;
+using namespace cv;
+using namespace cv::dnn;
+
+class OnnxDetectorImpl :
+	public OnnxDetector
+{
+public:
+	OnnxDetectorImpl() :
+        classNum_(1), imgStride_(32), confidence_(0.25f),
+        scoreThreshold_(0.45f), nmsThreshold_(0.3f){};
+	~OnnxDetectorImpl() {};
+
+public:
+	virtual int loadOnnxModel(const char* filename, cv::Size2f inferSize) override;
+	virtual std::vector<Detection2d> detect(cv::Mat image) override;
+	
+private:
+    vector<Mat> preProcess(Mat& input_image);
+    vector<Detection2d> postProcess(Mat& input_image, vector<Mat>& outputs);
+
+private:
+    int classNum_;
+    int imgStride_;
+    float confidence_;
+    float scoreThreshold_;
+    float nmsThreshold_;
+    cv::Size2f size_;
+	cv::dnn::Net network_;
+};
+
+void letterbox(const cv::Mat& image, cv::Mat& outImage,
+    const cv::Size& newShape = cv::Size(640, 640),
+    const cv::Scalar& color = cv::Scalar(114, 114, 114),
+    bool auto_ = true,
+    bool scaleFill = false,
+    bool scaleUp = true,
+    int stride = 32)
+{
+    cv::Size shape = image.size();
+    float r = std::min((float)newShape.height / (float)shape.height,
+        (float)newShape.width / (float)shape.width);
+    if (!scaleUp)
+        r = std::min(r, 1.0f);
+
+    float ratio[2]{ r, r };
+    int newUnpad[2]{ (int)std::round((float)shape.width * r),
+                     (int)std::round((float)shape.height * r) };
+
+    auto dw = (float)(newShape.width - newUnpad[0]);
+    auto dh = (float)(newShape.height - newUnpad[1]);
+
+    if (auto_)
+    {
+        dw = (float)((int)dw % stride);
+        dh = (float)((int)dh % stride);
+    }
+    else if (scaleFill)
+    {
+        dw = 0.0f;
+        dh = 0.0f;
+        newUnpad[0] = newShape.width;
+        newUnpad[1] = newShape.height;
+        ratio[0] = (float)newShape.width / (float)shape.width;
+        ratio[1] = (float)newShape.height / (float)shape.height;
+    }
+
+    dw /= 2.0f;
+    dh /= 2.0f;
+
+    if (shape.width != newUnpad[0] && shape.height != newUnpad[1])
+    {
+        cv::resize(image, outImage, cv::Size(newUnpad[0], newUnpad[1]));
+    }
+
+    int top = int(std::round(dh - 0.1f));
+    int bottom = int(std::round(dh + 0.1f));
+    int left = int(std::round(dw - 0.1f));
+    int right = int(std::round(dw + 0.1f));
+    cv::copyMakeBorder(outImage, outImage, top, bottom, left, right, cv::BORDER_CONSTANT, color);
+}
+
+// Ô¤´¦Àí
+vector<Mat> OnnxDetectorImpl::preProcess(Mat& input_image)
+{
+    Mat blob;
+    blobFromImage(input_image, blob, 1. / 255., Size(size_), Scalar(), false, false);
+
+    network_.setInput(blob);
+
+    vector<std::string> names = network_.getUnconnectedOutLayersNames();
+
+    vector<Mat> outputs;
+    network_.forward(outputs, names);
+
+    return outputs;
+}
+
+// ºó´¦Àí
+#if 1
+vector<Detection2d> OnnxDetectorImpl::postProcess(Mat& input_image, vector<Mat>& outputs)
+{
+    vector<int> class_ids;
+    vector<float> confidences;
+    vector<Rect> boxes;
+
+    float x_factor = input_image.cols / size_.width;
+    float y_factor = input_image.rows / size_.height;
+
+    float* data = (float*)outputs[0].data;
+
+    const int dimensions = 5 + classNum_;
+    //const int rows = 25200;
+    const int rows =(1 + 4 + 16) * (size_.width / imgStride_) * (size_.height / imgStride_);
+    for (int i = 0; i < rows; ++i)
+    {
+#if 1
+        float confidence = data[rows * 4 + i];
+        if (confidence > confidence_)
+        {
+            int centerX = (int)(data[rows * 0 + i] * x_factor);
+            int centerY = (int)(data[rows * 1 + i] * y_factor);
+            int width = (int)(data[rows * 2 + i] * x_factor);
+            int height = (int)(data[rows * 3 + i] * y_factor);
+            int left = centerX - width / 2;
+            int top = centerY - height / 2;
+
+            boxes.emplace_back(left, top, width, height);
+            confidences.emplace_back(confidence);
+            class_ids.emplace_back(0);
+        }
+#else
+        float confidence = data[4];
+        if (confidence >= confidence_)
+        {
+            float* classes_scores = data + 5;
+            Mat scores(1, classNum_, CV_32FC1, classes_scores);
+            Point class_id;
+            double max_class_score;
+            minMaxLoc(scores, 0, &max_class_score, 0, &class_id);
+            if (max_class_score > scoreThreshold_)
+            {
+                confidences.push_back(confidence);
+                class_ids.push_back(class_id.x);
+
+                float cx = data[0];
+                float cy = data[1];
+                float w = data[2];
+                float h = data[3];
+                int left = int((cx - 0.5 * w) * x_factor);
+                int top = int((cy - 0.5 * h) * y_factor);
+                int width = int(w * x_factor);
+                int height = int(h * y_factor);
+                boxes.push_back(Rect(left, top, width, height));
+            }
+        }
+#endif
+        //data += dimensions;
+    }
+
+    vector<Detection2d> res2d;
+    vector<int> indices;
+    NMSBoxes(boxes, confidences, scoreThreshold_, nmsThreshold_, indices);
+    for (int i = 0; i < indices.size(); i++)
+    {
+        int idx = indices[i];
+        Rect box = boxes[idx];
+
+        int left = box.x;
+        int top = box.y;
+        int width = box.width;
+        int height = box.height;
+        //rectangle(output_image, Point(left, top), Point(left + width, top + height), BLUE, 3 * THICKNESS);
+
+        //string label = format("%.2f", confidences[idx]);
+        //label = class_name[class_ids[idx]] + ":" + label;
+        //draw_label(output_image, label, left, top);
+
+        Detection2d d2d;
+        d2d.bbox = box;
+        d2d.classIdx = idx;
+        d2d.confidence = confidences[idx];
+
+        res2d.push_back(d2d);
+    }
+    return res2d;
+}
+#else
+inline float sigmoid(float x)
+{
+    return 1.f / (1.f + exp(-x));
+}
+
+vector<Detection2d> OnnxDetectorImpl::postProcess(Mat& cv_src, vector<Mat>& outs)
+{
+    std::vector<int> classIds;
+    std::vector<float> confidences;
+    std::vector<cv::Rect> boxes;
+
+    int strides[] = { 8, 16, 32 };
+    std::vector<std::vector<int> > anchors = {
+        { 10,13, 16,30, 33,23 },
+        { 30,61, 62,45, 59,119 },
+        { 116,90, 156,198, 373,326 }
+    };
+
+    for (size_t k = 0; k < outs.size(); k++) {
+
+        float* data = outs[k].ptr<float>();
+        int stride = strides[k];
+        int num_classes = outs[k].size[4] - 5;
+        for (int i = 0; i < outs[k].size[2]; i++) {
+            for (int j = 0; j < outs[k].size[3]; j++) {
+                for (int a = 0; a < outs[k].size[1]; ++a) {
+
+                    float* record = data + a * outs[k].size[2] * outs[k].size[3] * outs[k].size[4] +
+                        i * outs[k].size[3] * outs[k].size[4] + j * outs[k].size[4];
+                    float* cls_ptr = record + 5;
+                    for (int cls = 0; cls < num_classes; cls++) {
+
+                        float score = sigmoid(cls_ptr[cls]) * sigmoid(record[4]);
+                        if (score > scoreThreshold_) {
+
+                            float cx = (sigmoid(record[0]) * 2.f - 0.5f + (float)j) * (float)stride;
+                            float cy = (sigmoid(record[1]) * 2.f - 0.5f + (float)i) * (float)stride;
+                            float w = pow(sigmoid(record[2]) * 2.f, 2) * anchors[k][2 * a];
+                            float h = pow(sigmoid(record[3]) * 2.f, 2) * anchors[k][2 * a + 1];
+                            float x1 = std::max(0, std::min(cv_src.cols, int((cx - w / 2.f) * (float)cv_src.cols / (float)size_.width)));
+                            float y1 = std::max(0, std::min(cv_src.rows, int((cy - h / 2.f) * (float)cv_src.rows / (float)size_.width)));
+                            float x2 = std::max(0, std::min(cv_src.cols, int((cx + w / 2.f) * (float)cv_src.cols / (float)size_.width)));
+                            float y2 = std::max(0, std::min(cv_src.rows, int((cy + h / 2.f) * (float)cv_src.rows / (float)size_.width)));
+                            classIds.push_back(cls);
+                            confidences.push_back(score);
+                            boxes.push_back(cv::Rect(cv::Point(x1, y1), cv::Point(x2, y2)));
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    vector<Detection2d> res2d;
+    std::vector<int> indices;
+    cv::dnn::NMSBoxes(boxes, confidences, scoreThreshold_, nmsThreshold_, indices);
+    for (size_t i = 0; i < indices.size(); i++){
+
+        int idx = indices[i];
+        cv::Rect box = boxes[idx];
+        //drawPred(classIds[idx], confidences[idx], box.x, box.y,
+        //    box.x + box.width, box.y + box.height, cv_src, classes);
+
+        Detection2d d2d;
+        d2d.classIdx = classIds[idx];
+        d2d.bbox = box;
+        d2d.confidence = confidences[idx];
+        res2d.push_back(d2d);
+    }
+
+    return res2d;
+}
+#endif
+
+int OnnxDetectorImpl::loadOnnxModel(const char* filename, cv::Size2f inferSize) {
+
+    std::vector< std::pair<cv::dnn::Backend, cv::dnn::Target> > backends = cv::dnn::getAvailableBackends();
+    for (std::pair<cv::dnn::Backend, cv::dnn::Target>& det : backends) {
+        std::cout << "Detected Valid Backends: " << det.first << ", " << det.second << std::endl;
+    }
+
+    size_ = inferSize;
+	network_ = cv::dnn::readNet(filename);
+
+    //std::vector< std::pair<cv::dnn::Backend, cv::dnn::Target> > backends = cv::dnn::getAvailableBackends();
+    //for (std::pair<cv::dnn::Backend, cv::dnn::Target>& det : backends) {
+    //    std::cout << "Detected Valid Backends: " << det.first << ", " << det.second << std::endl;
+    //}
+    //network_.setPreferableBackend(cv::dnn::DNN_BACKEND_CUDA);
+    //network_.setPreferableTarget(cv::dnn::DNN_TARGET_CUDA);
+
+	return 0;
+}
+
+std::vector<Detection2d> OnnxDetectorImpl::detect(cv::Mat image) {
+
+    std::vector<Detection2d> res2d;
+    if (image.empty() || network_.empty())
+        return res2d;
+
+    const int width = std::max(image.rows, image.cols);
+    cv::Mat img = cv::Mat::zeros(image.rows, image.cols, image.type());
+    image.copyTo(img(cv::Rect(0, 0, image.cols, image.rows)));
+
+    vector<Mat> detections = preProcess(img);
+    return postProcess(img, detections);
+}
+
+OnnxDetector* OnnxDetector::CreateInstance() {
+    return new OnnxDetectorImpl();
+}

camCalib/onnxDetector.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+typedef struct Detection2d {
+	int classIdx;
+	float confidence;
+	cv::Rect bbox;
+}_detection2d;
+
+class OnnxDetector
+{
+public:
+	static OnnxDetector* CreateInstance();
+
+public:
+	virtual int loadOnnxModel(const char* filename, cv::Size2f inferSize) = 0;
+	virtual std::vector<Detection2d> detect(cv::Mat image) = 0;
+
+};
+

camCalib/sourceCode/MonoLaserCalibrate.cpp

@@ -7,6 +7,9 @@
 #include "MonoLaserCalibrate.h"
 #include "../lineDetection_steger.h"
 
+#include "../onnxDetector.h"
+OnnxDetector* cbDetector = nullptr;
+
 /*Breif：角点检测函数*/
 void detectCorners(const cv::Mat& img,
     const cv::Size& patternSize,
@@ -42,11 +45,116 @@ void detectCirclePoints(const cv::Mat& img,
 
     //需要将圆点外的背景去除，否则复杂的背景会导致检测失败
     //使用连通域检测
-    cv::Mat grayBin;
+    cv::Mat roiGray;
+    {
+#if 1
+        if (nullptr == cbDetector) {
+            cbDetector = OnnxDetector::CreateInstance();
+            cbDetector->loadOnnxModel("detectCB.onnx", cv::Size(640, 640));
+        }
 
+        std::vector<Detection2d> result2d = cbDetector->detect(img);
+        for (size_t i = 0; i < result2d.size(); i++) {
+            cv::Rect roi = result2d[i].bbox;
+            if (roi.x < 0)
+                roi.x = 0;
+            if (roi.y < 0)
+                roi.y = 0;
+            if ((roi.x + roi.width) > gray.cols)
+                roi.width = gray.cols - roi.x;
+            if ((roi.y + roi.height) > gray.rows)
+                roi.height = gray.rows - roi.y;
+
+            if (roiGray.empty()) {
+                roiGray = cv::Mat::zeros(gray.rows, gray.cols, CV_8UC1);
+                gray(roi).copyTo(roiGray(roi));
+            }
+            else {
+                gray(roi).copyTo(roiGray(roi));
+            }
+        }
+
+#ifdef _DEBUG
+        cv::Mat result = img.clone();
+        for (size_t i = 0; i < result2d.size(); i++) {
+            cv::rectangle(img, result2d[i].bbox, cv::Scalar(0, 255, 0), 2);
+        }
+        cv::imwrite("_cbResult.png", result);
+#endif
+
+#else
+        cv::Mat grayBin;
+        cv::adaptiveThreshold(gray, grayBin, 255,
+            cv::ADAPTIVE_THRESH_GAUSSIAN_C, cv::THRESH_BINARY_INV, 9, 2);
+
+        cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, { 3,3 });
+        cv::morphologyEx(grayBin, grayBin, cv::MORPH_OPEN, kernel);
+        cv::morphologyEx(grayBin, grayBin, cv::MORPH_CLOSE, kernel);
+
+        //cv::connectedComponents();
+        std::vector<std::vector<cv::Point>> contours;
+        std::vector<cv::Vec4i> hierarchy;
+        cv::findContours(grayBin, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);
+
+#ifdef _DEBUG
+        cv::Mat result = grayBin.clone();
+#endif
+        
+        cv::Rect roi;
+        const int N = patternSize.area();
+        for (int i = 0; i < contours.size(); ++i) {
+            // 检查当前轮廓是否为父轮廓且子轮廓数量为N
+            int childCount = 0;
+            int childIdx = hierarchy[i][2]; // 第一个子轮廓索引
+            while (childIdx != -1) {
+                childCount++;
+                childIdx = hierarchy[childIdx][0]; // 下一个同级子轮廓
+            }
+
+            if (childCount >= N) { // 子数量匹配
+#ifdef _DEBUG
+                cv::drawContours(result, contours, i, cv::Scalar(0, 255, 0), 2);
+#endif
+
+                cv::Point lt(10000, 10000), rb(0, 0);
+                for (const cv::Point pt : contours[i]) {
+                    lt.x = std::min(lt.x, pt.x);
+                    rb.x = std::max(rb.x, pt.x);
+                    lt.y = std::min(lt.y, pt.y);
+                    rb.y = std::max(rb.y, pt.y);
+                }
+                roi = cv::Rect(lt, rb);
+                if (roiGray.empty()) {
+                    roiGray = cv::Mat::zeros(gray.rows, gray.cols, CV_8UC1);
+                    gray(roi).copyTo(roiGray(roi));
+                }
+                else {
+                    gray(roi).copyTo(roiGray(roi));
+                }
+
+                // 标记子轮廓（可选）
+                childIdx = hierarchy[i][2];
+                while (childIdx != -1) {
+#ifdef _DEBUG
+                    cv::drawContours(result, contours, childIdx, cv::Scalar(255, 0, 0), 1);
+#endif
+                    childIdx = hierarchy[childIdx][0];
+                }
+            }
+        }
+
+#ifdef _DEBUG
+        cv::imwrite("_cbContour.png", result);
+#endif
+#endif
+#ifdef _DEBUG
+        if (false == roiGray.empty())
+            cv::imwrite("_roiGray.png", roiGray);
+#endif
+    }
 
     // 检测圆形网格
-    bool found = cv::findCirclesGrid(gray, patternSize, corners, cv::CALIB_CB_SYMMETRIC_GRID);
+    bool found = cv::findCirclesGrid(roiGray.empty() ? gray : roiGray, patternSize, corners, cv::CALIB_CB_SYMMETRIC_GRID);
     return;
 }