圆形目标中心点在tof相机坐标系下的三维坐标的计算(1)

大家好，又见面了，我是你们的朋友全栈君。

将tof相机得到的深度图像转换为灰度图像，然后对灰度图像进行双边滤波，去除噪声的同时使边缘得到较好的保持，然后对滤波后的灰度图像进行hough圆变换，得到圆心在图像中的像素坐标，然后利用tof相机的点云数据，求得圆心在tof相机坐标系下的三维坐标。
程序如下：

#include "opencv2/core/core.hpp" 
#include "opencv2/highgui/highgui.hpp" 
#include "opencv2/imgproc/imgproc.hpp" 
#include "opencv2/features2d/features2d.hpp" //需要添加该头文件 
#include <iostream> 
#include "Math.h"

using namespace cv;
using namespace std;


int main(int argc, char**argv)
{
    Mat src, gray, edge, erzhitu, bifilter;
    //读入图像
    src = imread("D:\\yanxiaopan\\my project\17.2.22\\X\0\\f.jpg");
    if (!src.data)
        return -1;
    //灰度化
    cvtColor(src, gray, CV_BGR2GRAY);
    //双边滤波，双边滤波既可以滤除噪声，也能较好的保持图像中的边缘
    bilateralFilter(gray, bifilter, 25, 25 * 2, 25 / 2);
    //huogh圆变换
    vector<Vec3f> circles;
    HoughCircles(bifilter, circles, HOUGH_GRADIENT, 1.5, 3, 50, 25, 0, 25);
    cout << "circles.size()=" << circles.size() << endl;
    Point final_center;
    for (size_t i = 0; i < circles.size(); i++)
    {
        Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
        final_center = center;
        int radius = cvRound(circles[i][2]);
        cout << "center(" << i << ")=(" << cvRound(circles[i][0]) << "," << cvRound(circles[i][1]) << ")" << endl;
        cout << "radius=" << radius << endl;
        circle(src, center, 1, Scalar(0, 255, 0), -1, 8, 0);
        circle(src, center, radius, Scalar(155, 50, 255), 1, 8, 0);
    }
    imshow("src", src);
    imshow("gray", gray);
    imshow("bifilter", bifilter);
    //读入点云数据
    FILE *fp = fopen("D:\\yanxiaopan\\my project\17.2.22\\X\0\\clo31.txt", "r");
    if (fp == NULL)
    {
        cout << "file open error\n" << endl;
        return -1;
    }
    //将所有像素点的三维坐标分别保存在以下数组中
    double xx[25344];//所有像素点的x坐标
    double yy[25344];
    double zz[25344];

    for (int i = 0; i <25344; i++)
    {
        fscanf(fp, "%lf,%lf,%lf", &xx[i], &yy[i], &zz[i]);
    }
    fclose(fp);

    double center_x = xx[(final_center.x - 1) * 176 + final_center.y];
    double center_y = yy[(final_center.x - 1) * 176 + final_center.y];
    double center_z = zz[(final_center.x - 1) * 176 + final_center.y];
    cout << "center_x=" << center_x << endl;
    cout << "center_y=" << center_y << endl;
    cout << "center_z=" << center_z << endl;

    waitKey(0);
    return 0;
}