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

因偏移一定程度后圆形目标在相机下呈现椭圆形状，所以利用(1)中的方法对图像进行hough圆变换，对目标进行粗略定位后，需要对目标进行精确定位。首先定义一个以检测到的圆心为中心点，以radius+5为半径的mask，然后选取原图像中mask大小的图像区域，目的是将目标区域及其周围的背景选取出来，对选取出来的背景进行统计，生成一幅背景比较单一的并带有目标的图，对背景简化后的图像进行阈值分割，区分出背景和图像后，对目标进行canny边缘检测，然后进行hough椭圆变换。
（hough椭圆变换参考：
http://blog.csdn.net/yanxiaopan/article/details/56841249）

主程序如下：

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

using namespace cv;
using namespace std;
int otsu(const Mat &img);//最大类间方差阈值分割

int main(int argc, char**argv)
{
    Mat src, gray, edge, erzhitu, bifilter;
    //读入图像
    src = imread("D:\\yanxiaopan\\my project\\2017.2.22\\Y\\-170\\f.jpg");
    if (!src.data)
        return -1;
    //复制原图像
    Mat src_copy = src.clone();
    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;
    int radius;
    for (size_t i = 0; i < circles.size(); i++)
    {
        Point center(cvRound(circles[i][0]), cvRound(circles[i][1]));
        final_center = center;
        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);
    }
    //对图像进行hough圆变换，对目标进行粗略定位后
    Mat mask = Mat::zeros(src.size(), CV_8UC1);
    //定义