VS生产dll把双目追踪四个圆点计算的物体位姿给unity，在unity中实时变化

unity

StereCamera.cs

using UnityEngine;
using System.Collections;
using System.Runtime.InteropServices;
using System.Threading;


// 双目定位追踪GameOjbect对应的脚本
public class SteroCamera : MonoBehaviour
{
    [DllImport("Stereo-Plugin.dll")]
    static extern bool Loadfile();

    [DllImport("Stereo-Plugin.dll")]
    static extern bool Open_mcam();

    [DllImport("Stereo-Plugin.dll")]
    static extern int Measure();

    [DllImport("Stereo-Plugin.dll")]
    static extern bool Close_cam();

    [DllImport("Stereo-Plugin.dll")]
    static extern void Trans_point(float[] point_arry);

    public bool StartTrack = false;              // 初始值应该为false
    int num = 0;

    public GameObject Target;

    // Use this for initialization
    void Start()
    {
        Loadfile();
        Open_mcam();
    }

    private void OnDestroy()
    {
        Close_cam();
    }

    // Update is called once per frame
    void Update()
    {
        if (StartTrack)
        {
            num = Measure();
            //Debug.Log(num);

            if (num != -1)
            {
                float[] k = new float[7];//点的坐标值是点的个数乘3
                Trans_point(k);

                //Debug.Log(" xr:" + k[1].ToString() + " yr:" + k[2].ToString() + " zr:" + k[3].ToString() + " wr:" + k[0].ToString());
                //Debug.Log(" x:" + k[5].ToString() + " y:" + k[5].ToString() + " z:" + k[6].ToString());
                Quaternion rr = new Quaternion(-k[1], k[2], -k[3], k[0]);

                Target.transform.rotation = rr;
                Target.transform.position = new Vector3(k[4] / 1000, -k[5] / 1000, k[6] / 1000);
            }
        }
      
    }
}

VS中追踪计算点的位置并生成dll传给unity

Stereo-Plugin.cpp

#include"imgpro.h"
#include"res_c_r_w.h"
#include"virtualcam.h"
#include<ctime>
//双目极线约束匹配，双目测量，精度验证

float size = 38;

res_c_r_w res;
Mat T, F;
Mat R;
Mat mRightRT;
Mat M1, M2, D1;

float fov;

Mat rvecs, tvecs;

vector<Point3f>word_point;

extern "C" _declspec(dllexport)//dll输出标准
bool Loadfile()
{
	bool ifdata = false;
	//判断是否已经标定，提示加文件查找
	//标定程序是D:\project\track\campcplus   获取图片
	//D:\project\track\calibrationtwo    双目标性

	ifdata = res.xml_read("extrinsics_1-3.yml", "R", R);
	ifdata = res.xml_read("extrinsics_1-3.yml", "T", T);
	ifdata = res.xml_read("extrinsics_1-3.yml", "F", F);
	cout << R << endl << T << endl << F << endl;




	ifdata = res.xml_read("intrinsics_1-3.yml", "M1", M1);
	ifdata = res.xml_read("intrinsics_1-3.yml", "D1", D1);
	ifdata = res.xml_read("intrinsics_1-3.yml", "M2", M2);
	//res.xml_read("intrinsics_1-3.yml", "D1", D1);
	//res.xml_read("intrinsics_1-3.yml", "D2", D2);
	cout << M1 << endl << M2 << endl;


	fov = atan2(320.0f , 8.83309279f)*2;
	fov = fov / PI * 180;
	cout << "fov:" << fov << endl;
	mRightRT = res.mergecols(R, T);
	return ifdata;
}

cam_monocular left_cam, right_cam;
imgpro mypro;
mycircle cir_l, cir_r;


float siyuanshu[4] = { 0, 0, 0, 0 };
float position[3] = { 0, 0, 0 };

extern "C" _declspec(dllexport)
bool Open_mcam()
{
	word_point.clear();

	Point3f p1 = { 0, 0, 0 };
	Point3f p2 = { size, 0, 0 };
	Point3f p3 = { size, size, 0 };
	Point3f p4 = { 0, size, 0 };

	word_point.push_back(p1);
	word_point.push_back(p2);
	word_point.push_back(p3);
	word_point.push_back(p4);

	bool res = false;
	res = left_cam.open_cam(0);
	res = right_cam.open_cam(1);
	return res;
}

vector<Point3f> vec_3d;

extern "C" _declspec(dllexport)
int Measure()
{
	int ress = -1;

	vec_3d.clear();
	left_cam.grab_frame();

	right_cam.grab_frame();

	if (!left_cam.frame.data || !right_cam.frame.data)return ress;

	//圆检测
	mypro.DecodeMark1(left_cam.frame, cir_l);
	mypro.DecodeMark1(right_cam.frame, cir_r);

	cir_l.show_circle(left_cam.frame);
	cir_r.show_circle(right_cam.frame);

	//imshow("left", left_cam.frame);
	//imshow("right", right_cam.frame);


	//极线匹配
	//mypro.matchEllipse(cir_l, cir_r, F);
	if (cir_l.cir_idle_ab_detect.size() == 4)
		//if (cir_r.cir_idle_ab_detect.size() == 4)
	{
		ress = 0;

		mypro.matchEllipse_convexHull(cir_l/*, cir_r*/);
		cir_l.show_match_order(left_cam.frame);
		//cir_r.show_match_order(right_cam.frame);

		solvePnP(word_point, cir_l.cir_match_vec, M1, D1, rvecs, tvecs);

		Mat r = Mat(3, 3, CV_32F);

		float *m_fExtrinsic;

		Rodrigues(rvecs, r);

		r.convertTo(r, CV_32F);
		tvecs.convertTo(tvecs, CV_32F);

		res.QuaternionFromMatrix(r, siyuanshu);

		cout << "myR:" << r << endl;
		cout << "myT:" << tvecs << endl;

		position[0] = tvecs.at<float>(0, 0);
		position[1] = tvecs.at<float>(1, 0);
		position[2] = tvecs.at<float>(2, 0);
		cout << "xr" << siyuanshu[0] << "yr" << siyuanshu[1] << "zr" << siyuanshu[2] << "wr" << siyuanshu[3] << endl;

		cout << "x" << position[0] << "y" << position[1] << "z" << position[2] << endl << endl;
		//waitKey(1000);
	}


	imshow("left", left_cam.frame);
	imshow("right", right_cam.frame);
	waitKey(1);
	return ress;
	//return res.cout_3d_point(vec_3d);
	//精度验证

	//思路，双目测量用于精度验证.png 用屏幕可视化、打印的方法严格控制标志点的空间位置

	//在用双目测量其距离与理论值相比较，验证单点测量精度，通过matlab可视化其精度，如误差分布直方图，等，非重点

	//改进，圆检测二值化或多线程来增加算法的实时性
}

extern "C" _declspec(dllexport)
void Trans_point(float *point_arry)
{
	point_arry[0] = siyuanshu[0];
	point_arry[1] = siyuanshu[1];
	point_arry[2] = siyuanshu[2];
	point_arry[3] = siyuanshu[3];
	point_arry[4] = position[0];
	point_arry[5] = position[1];
	point_arry[6] = position[2];
}
//void trans_point(float *point_arry)
//{
//	int size = vec_3d.size();
//	for (int i = 0; i < size; i++)
//	{
//		point_arry[3 * i] = vec_3d[i].x;
//		point_arry[3 * i + 1] = vec_3d[i].y;
//		point_arry[3 * i + 2] = vec_3d[i].z;
//	}
//}

extern "C" _declspec(dllexport)
void Close_cam()
{
	//mystero.close_cam();
	left_cam.close_cam();
	right_cam.close_cam();
	destroyWindow("left");
	destroyWindow("right");
}

imgpro.h

#pragma once
#include "mycircle.h"
//#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include <opencv2\opencv.hpp>
#include <iostream>
#include <stdio.h>
#include <cstdio>
#include<algorithm>
//#include<Windows.h>
#include<deque>
#ifndef PI
#define PI 3.141592653
using namespace cv;
using namespace std;
#endif



//根据椭圆第一定义, 用a表示椭圆长半轴的长, b表示椭圆短半轴的长, 且a>b>0. 椭圆周长公式：L = 2πb + 4(a - b)
//椭圆周长定理：椭圆的周长等于该椭圆短半轴长为半径的圆周长（2πb）加上四倍的该椭圆长半轴长（a）与短半轴长（b）的差.椭圆面积公式： S = πab

class imgpro
{
public:
	imgpro();

	//椭圆筛选
	bool isEllipse(const vector<cv::Point2f> &contours, const RotatedRect &canEll, const double maxERR);

	//椭圆检测
	void findEllipses(const vector< vector<cv::Point2f> >& contoursRaw, std::vector< int >& sublabEllipse, std::vector<RotatedRect>& rectEll);

	//椭圆检测
	bool DecodeMark1(Mat &image, std::vector<cv::Point2f>& vpUV, bool if_gray=false);

	//椭圆检测,记录长短轴
	bool DecodeMark1(Mat &image, mycircle &cir,bool if_gray = false);

	//椭圆鉴别hsv
	void identify_cir_hsv(Mat &image, vector<cv::Point2f>& vpUV, vector<int>& hsv);

	//椭圆心鉴别hsv, 并分为group类
	void identify_cir_hsv(Mat &image, vector<cv::Point2f>& vpUV, vector<int>& hsv, vector<int>& id , int group=2);

	//加噪点
	void salt(Mat &image, int n);

	//极线匹配
	bool matchEllipse(vector<Point2f>& cir_cev_l, vector<Point2f>& cir_cev_r, vector<Point2f>& cir_match_cev_l, vector<Point2f>& cir_match_cev_r, Mat& F_2_1);
	bool matchEllipse(mycircle &cir_l, mycircle &cir_r, Mat& F_2_1);

	//由凸包面积法匹配
	bool matchEllipse_convexHull(mycircle &cir_l, mycircle &cir_r, int mini_area=100);
	//由凸包面积法匹配、单个图像
	bool imgpro::matchEllipse_convexHull(mycircle &cir_l, int mini_area = 100);

	//由双目像素坐标计算世界坐标
	bool pointTo3D(vector<Point2f>& vec_uvLeft, vector<Point2f>& vec_uvRight, Mat& mLeftIntrinsic, Mat& mRightIntrinsic, Mat& mRightRT, vector<Point3f>& vec_3d);

	bool tack_flow(Mat& pregray, Mat& gray, vector<Point2f>& pre, vector<Point2f>& now, vector<uchar>& status,vector<float>& err);

	~imgpro();
};

imgpro.cpp

#include"imgpro.h"

double minArea = 130;//400--450之间
double maxArea = 30000;
double minLen = 40;
double maxLen = 6000;

TermCriteria termcrit(CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 30, 0.01);

bool SortByM1(const Point2f &v1, const Point2f &v2)//注意：本函数的参数的类型一定要与vector中元素的类型一致  
{
	return v1.x < v2.x;//升序排列  
}

imgpro::imgpro()
{
}

//椭圆筛选
bool imgpro::isEllipse(const vector<cv::Point2f> &contours, const RotatedRect &canEll, const double maxERR)
{
	bool isEll = true;
	double x0 = canEll.center.x;
	double y0 = canEll.center.y;
	double a = canEll.size.width > canEll.size.height ? canEll.size.width*0.5 : canEll.size.height*0.5;
	double a2 = a * 2;
	double b = canEll.size.width < canEll.size.height ? canEll.size.width*0.5 : canEll.size.height*0.5;//

	double err = 3;

	double angle = canEll.angle;
	double c = sqrt(a*a - b*b);
	Vec2f c1((float)cos(angle)*c + x0, (float)sin(angle)*c + y0);
	Vec2f c2(-(float)cos(angle)*c + x0, -(float)sin(angle)*c + y0);
	///Calculate max and min err
	for (size_t i = 0; i < contours.size(); i++)
	{
		double len1 = sqrt(pow(abs(c1[0] - contours[i].x), 2) + pow(abs(c1[1] - contours[i].y), 2));
		double len2 = sqrt(pow(abs(c2[0] - contours[i].x), 2) + pow(abs(c2[1] - contours[i].y), 2));
		double len3 = sqrt(pow(abs(x0 - contours[i].x), 2) + pow(abs(y0 - contours[i].y), 2));

		double errTmp = fabs(len1 + len2 - a2);
		//

		//第一次筛选
		if (errTmp > maxERR*a2)//两个筛选条件
			//if (errTmp >0.1* a2&&fabs(a - b)>err)
		{
			isEll = false;
		}
		//第二次（面积）筛选
		double trueArea = PI*a*b;
		double trueLen = 2 * PI*b + 4 * (a - b);
		/
		if (fabs(trueArea - abs(contourArea(contours)))>3)
		{
			isEll = false;
		}
		//第三次（周长）筛选
		//if (trueLen > abs(contourArea(contours)))
		//{
		//	isEll = false;
		//}
	}
	return isEll;
}

//椭圆检测
void imgpro::findEllipses(const vector< vector<cv::Point2f> >& contoursRaw, std::vector< int >& sublabEllipse, std::vector<RotatedRect>& rectEll)
{
	const size_t minContourPoint = 6;
	const double maxErrRate = 0.3;
	int itmps1 = contoursRaw.size();
	for (size_t i = 0; i < contoursRaw.size(); i++)
	{
		if (contoursRaw[i].size() < minContourPoint)
		{
			continue;
		}
		//临时拟合rect信息
		RotatedRect tmpRotatedRect;
		tmpRotatedRect = fitEllipse(contoursRaw[i]);

		///If the contour is a Ellipse 
		if (isEllipse(contoursRaw[i], tmpRotatedRect, maxErrRate))
		{
			sublabEllipse.push_back(i);
			rectEll.push_back(tmpRotatedRect);
			//aid_point.push_back(contoursRaw[i][0]);
		}
		else
		{
		}

	}

	return;
}

//椭圆检测,提取圆心到vpUV
bool imgpro::DecodeMark1(Mat &image, std::vector<cv::Point2f>& vpUV, bool if_gray)
{
	vpUV.clear();

	bool found = false;
	mycircle temp;

	DecodeMark1(image, temp, if_gray);

	int size = temp.cir_idle_ab_detect.size();
	for (size_t i = 0; i < size; i++)
	{
		vpUV.push_back(temp.cir_idle_ab_detect[i].center);
	}

	return found;
}

//椭圆检测,记录长短轴
bool imgpro::DecodeMark1(Mat &image, mycircle &cir,bool if_gray)
{
	bool found = false;
	const double resizeFactor = 1.0;

	//
	//图像处理
	//灰度+8U+边缘  用于轮廓检测
	//image.convertTo(image, CV_8UC1);
	Mat imGary;
	if (!if_gray)
	cvtColor(image, imGary, CV_BGR2GRAY);
	else
	{
		imGary = image;
	}
	//cv::GaussianBlur(imGary, imBlur, Size(5,5), 1, 1);

	Mat imCanny;
	Canny(imGary, imCanny, 30, 50, 3);
	//imshow("图像处理①Canny", imCanny);
	//waitKey(0);

	//
	//检测轮廓饭
	vector<vector<cv::Point2f> > contours;
	vector<vector<cv::Point>> contour1;

	//只检测外轮廓. CV_CHAIN_APPROX_SIMPLE压缩水平方向，垂直方向，对角线方向的元素，只保留该方向的终点坐标
	//CV_CHAIN_APPROX_NONE储存所有点
	//检测完轮廓 imcanny已经改变

	//cv::findContours(imCanny, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
	cv::findContours(imCanny, contour1, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);

	for (size_t i = 0; i < contour1.size(); i++)
	{
		double a = abs(contourArea(contour1[i]));
		double b = abs(arcLength(contour1[i], true));

		if (a >= minArea&&a <= maxArea&&b >= minLen&&b <= maxLen)
		{
			//亚像素角点检测
			vector<Point2f> tmp;
			for (size_t j = 0; j < contour1[i].size(); j++)
			{
				tmp.push_back(contour1[i][j]);
			}
			/*cornerSubPix(imCanny, tmp, Size(5, 5),
			Size(-1, -1), TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));*/
			contours.push_back(tmp);
		}
		else
		{
			continue;
		}

	}

	if (!contours.empty())
	{
		found = true;
		//drawContours(image, contours,)
	}
	else
	{
		found = false;
		return found;
	}
	Mat contoursIm;
	image.copyTo(contoursIm);

	//
	//筛选轮廓
	//轮廓中是通过了椭圆筛选的序号
	std::vector<int > EllipseContour;
	//轮廓拟合rect信息
	//std::vector<RotatedRect> EllipseRect;
	//筛选下椭圆
	cir.cir_idle_ab_detect.clear();
	cir.cir_vec.clear();

	findEllipses(contours, EllipseContour, cir.cir_idle_ab_detect);

	int size = cir.cir_idle_ab_detect.size();
	for (size_t i = 0; i < size; i++)
	{
		cir.cir_vec.push_back(cir.cir_idle_ab_detect[i].center);
	}
	//
	return found;
}

//椭圆鉴别hsv
void imgpro::identify_cir_hsv(Mat &image, vector<cv::Point2f>& vpUV, vector<int>& hsv)
{
	if (hsv.size()>0)hsv.clear();

	int size = vpUV.size();
	for (size_t i = 0; i < size; i++)
	{
		Mat temm(image, Rect(vpUV[i].x - 2, vpUV[i].y - 2, 5, 5));
		Mat temmp;
		cvtColor(temm, temmp, CV_BGR2HSV);

		/* 注意 Mat::at 函数是个模板函数, 需要指明参数类型, 因为这张图是具有红蓝绿三通道的图,
		所以它的参数类型可以传递一个 Vec3b, 这是一个存放 3 个 uchar 数据的 Vec(向量). 这里
		提供了索引重载, [2]表示的是返回第三个通道, 在这里是 Red 通道, 第一个通道(Blue)用[0]返回 */
		int bgr[3] = { 0, 0, 0 };

		for (int k = 0; k < 5; k++)
			for (int l = 0; l < 5; l++)
			{
				bgr[2] += (int)temmp.at<Vec3b>(k, l)[2];
				bgr[1] += (int)temmp.at<Vec3b>(k, l)[1];
				bgr[0] += (int)temmp.at<Vec3b>(k, l)[0];
			}
		bgr[2] /= 25;
		bgr[1] /= 25;
		bgr[0] /= 25;

		hsv.push_back(bgr[0]);
	}
}

//椭圆心鉴别hsv, 并分为group类
void imgpro::identify_cir_hsv(Mat &image, vector<cv::Point2f>& vpUV, vector<int>& hsv, vector<int>& id, int group)
{
	this->identify_cir_hsv(image, vpUV, hsv);

	int size = hsv.size();

	int step = 180 / group / 3;

	for (size_t i = 0; i < size; i++)
	{

		int not_sure = -10;

		int num = hsv[i] / step + 1;

		int k = num / 3;
		if (k == group)k = 0;

		if (num % 3 != 2)not_sure = k;
		id.push_back(not_sure);
	}
}

//加噪点
void imgpro::salt(Mat &image, int n)
{
	int i, j;
	for (int k = 0; k<n; k++)
	{
		// rand()是随机数生成器  
		i = rand() % image.cols;
		j = rand() % image.rows;
		if (image.type() == CV_8UC1)
		{ // 灰度图像  
			image.at<uchar>(j, i) = 255;
		}
		else if (image.type() == CV_8UC3)
		{ // 彩色图像  
			image.at<cv::Vec3b>(j, i)[0] = 255;
			image.at<cv::Vec3b>(j, i)[1] = 255;
			image.at<cv::Vec3b>(j, i)[2] = 255;
		}
	}
}

//椭圆的匹配
bool imgpro::matchEllipse(vector<Point2f>& cir_cev_l, vector<Point2f>& cir_cev_r, vector<Point2f>& cir_match_cev_l, vector<Point2f>& cir_match_cev_r, Mat& F_2_1)
{
	std::vector<cv::Vec<float, 3>> line1, line2, line3;
	cir_match_cev_l.clear();
	cir_match_cev_r.clear();

	//vpUVcenter3.clear();
	//先检测到椭圆

	//在图2上画极线
	if (cir_cev_l.size()>0 && cir_cev_r.size()>0 && F_2_1.data)
	{
		computeCorrespondEpilines(cir_cev_l, 1, F_2_1, line1);//计算对应点的外极线line1是一个三元组(a,b,c)，表示点在另一视图中对应的外极线ax+by+c=0;  

		//cv::circle(image1, cir_cev_l[9], 4, Scalar(0, 0, 255), -1);//在视图1中把关键点用圆圈画出来，然后再绘制在对应点处的外极线 
		//cv::line(image2, cv::Point(0, -line1[9][2] / line1[9][1]), cv::Point(image2.cols, -(line1[9][2] + line1[9][0] * image2.cols) / line1[9][1]), Scalar(0, 255, 0));


		for (size_t n = 0; n < cir_cev_l.size(); n++)
		{
			//
			double a = 0;
			double	b = 0;
			double	c = 0;
			double dis = 0;
			vector<Point2f> Epilines1_2;
			a = line1[n][0];
			b = line1[n][1];
			c = line1[n][2];

			double stddis = 2.00;
			//float stdSredis = 80;
			// 距离公式为d = |A*x0 + B*y0 + C|/√(A^2 + B^2)
			第一次筛选
			//在图2极线上筛选椭圆
			vector<int> numR;
			int m = 0;
			for (size_t i = 0; i < cir_cev_r.size(); i++)
			{

				dis = abs(a *cir_cev_r[i].x + b * cir_cev_r[i].y + c) / sqrt(a * a + b * b);
				float absx = abs(cir_cev_r[i].x - cir_cev_l[n].x);
				float absy = abs(cir_cev_r[i].y - cir_cev_l[n].y);
				if (dis < stddis /*&& absx<stdSredis && absy<stdSredis*/)//增加像素区域约束和极线误差约束
				{

					//if (cir_cev_l[n].x>cir_cev_r[i].x)//在合理夹角下，对于左图的点来说，右图对应的点X坐标值一定大于它的X坐标值
					//{
					//	break;
					//}
					Epilines1_2.push_back(cir_cev_r[i]);
					numR.push_back(i);
					m = i;
					break;
				}
			}
#if 0
			画出第n个点的反算极线
			if (n == 0)
			{
				cv::circle(image1, cir_cev_l[n], 4, Scalar(0, 0, 255), -1);//在视图1中把第0个关键点用圆圈画出来，然后再绘制在对应点处的外极线 
				cv::line(image2, cv::Point(0, -line1[n][2] / line1[n][1]), cv::Point(image2.cols, -(line1[n][2] + line1[n][0] * image2.cols) / line1[n][1]), Scalar(0, 255, 0), 3, 8);

				if (Epilines1_2.size()>0)
				{
					std::vector<cv::Vec<float, 3>>  linen;
					computeCorrespondEpilines(Epilines1_2, 2, F_2_1, linen);
					if (Epilines1_2.size() == linen.size())
					{
						for (size_t i = 0; i < Epilines1_2.size(); i++)
						{
							cv::line(image1, cv::Point(0, -linen[i][2] / linen[i][1]), cv::Point(image1.cols, -(linen[i][2] + linen[i][0] * image1.cols) / linen[i][1]), Scalar(255, 0, 0), 1, 8);
						}

					}
				}
			}
			//
#endif
			第二次筛选
			if (Epilines1_2.size() == 0 && numR.size() == 0)//极线约束没找到对应点
			{
				continue;
			}
			else if (Epilines1_2.size() == 1)//只找到一个
			{
				cir_match_cev_l.push_back(cir_cev_l[n]);
				cir_match_cev_r.push_back(Epilines1_2[0]);

				if (cir_cev_r.size() > 1)//如果极限约束匹配点为一个，将其从右相机待匹配点剔除
				{
					vector <Point2f>::iterator Iter = cir_cev_r.begin();
					cir_cev_r.erase(Iter + m);

				}
				//circle(image2, cir_match_cev_r[n], 4, Scalar(0, 255, 255), -1);
			}
			//else if (Epilines1_2.size()>1)//找到两个以上
			//{
			//	computeCorrespondEpilines(Epilines1_2, 1, F_2_1, line2);

			//	if (Epilines1_2.size() == line2.size())
			//	{
			//		for (size_t i = 0; i < Epilines1_2.size(); i++)
			//		{
			//			bool isOK = false;
			//			double a = 0;
			//			double	b = 0;
			//			double	c = 0;


			//			a = line2[i][0];
			//			b = line2[i][1];
			//			c = line2[i][2];

			//			dis = abs(a *cir_cev_l[n].x + b * cir_cev_l[n].y + c) / sqrt(a * a + b * b);

			//			if (dis < stddis)
			//			{
			//				cir_match_cev_l.push_back(cir_cev_l[n]);
			//				cir_match_cev_r.push_back(Epilines1_2[i]);
			//				isOK = true;
			//				break;
			//			}


			//		}
			//	}


			//}


		}
		return true;
	}
	return false;

}

//由极线约束匹配
bool imgpro::matchEllipse(mycircle &cir_l, mycircle &cir_r, Mat& F_2_1)
{
	return matchEllipse(cir_l.cir_vec, cir_r.cir_vec, cir_l.cir_match_vec, cir_r.cir_match_vec, F_2_1);
}

bool SortByArea(const RotatedRect &v1, const RotatedRect &v2)//注意：本函数的参数的类型一定要与vector中元素的类型一致  
{
	return v1.size.area() > v2.size.area();//降序排列  
}

//椭圆圆心化, 面积阈值化
int convert_Ellipse_center(vector<RotatedRect>& elli, vector<Point2f>& enter, int mini_area)
{
	enter.clear();
	for (auto &i : elli)
	{
		if (mini_area<=i.size.area())
		enter.push_back(i.center);
	}
	return enter.size();
}

//由凸包面积法匹配
bool imgpro::matchEllipse_convexHull(mycircle &cir_l, mycircle &cir_r, int mini_area)
{
	//return matchEllipse(cir_l.cir_vec, cir_r.cir_vec, cir_l.cir_match_vec, cir_r.cir_match_vec, F_2_1);
	sort(cir_l.cir_idle_ab_detect.begin(), cir_l.cir_idle_ab_detect.end(), SortByArea);
	convert_Ellipse_center(cir_l.cir_idle_ab_detect, cir_l.cir_match_vec, mini_area);

	sort(cir_r.cir_idle_ab_detect.begin(), cir_r.cir_idle_ab_detect.end(), SortByArea);
	convert_Ellipse_center(cir_r.cir_idle_ab_detect, cir_r.cir_match_vec, mini_area);

	if (cir_l.cir_match_vec.size() != cir_r.cir_match_vec.size())return false;

	convexHull(cir_l.cir_match_vec, cir_l.cir_match_vec);
	convexHull(cir_r.cir_match_vec, cir_r.cir_match_vec);

	int size = cir_l.cir_match_vec.size();

	for (int i = 0; i < size; i++)
	{
		if (cir_l.cir_idle_ab_detect[0].center != cir_l.cir_match_vec[0])
		{
			cir_l.cir_match_vec.push_back(cir_l.cir_match_vec[0]);
			cir_l.cir_match_vec.erase(cir_l.cir_match_vec.begin());
		}
		else
		{
			break;
		}
	}
	for (int i = 0; i < size; i++)
	{
		if (cir_r.cir_idle_ab_detect[0].center != cir_r.cir_match_vec[0])
		{
			cir_r.cir_match_vec.push_back(cir_r.cir_match_vec[0]);
			cir_r.cir_match_vec.erase(cir_r.cir_match_vec.begin());
		}
		else
		{
			break;
		}
	}
	return true;

	//int max1=-1, max2=-2;

	//for (int i = 0; i < size; i++)
	//{
	//	if (cir_l.cir_idle_ab_detect[0].center == cir_l.cir_match_vec[i])
	//	{
	//		max1 = i;
	//		break;
	//	}
	//}
	//for (int i = 0; i < size; i++)
	//{
	//	if (cir_r.cir_idle_ab_detect[0].center == cir_r.cir_match_vec[i])
	//	{
	//		max2 = i;
	//		break;
	//	}
	//}

	//if (max1 == max2)return true;
	//if (max1<0 || max2<0)return false;
	//
	//int num = max1 - max2;

	//if (num > 0)  //左相机最大面积在其凸包中顺序大
	//	for (int i = 0; i < num; i++)
	//	{
	//		cir_l.cir_match_vec.push_back(cir_l.cir_match_vec[0]);
	//		cir_l.cir_match_vec.erase(cir_l.cir_match_vec.begin());
	//	}
	//if (num < 0)  //右相机最大面积在其凸包中顺序大
	//{
	//	num = 0 - num;
	//	for (int i = 0; i < num; i++)
	//	{
	//		cir_r.cir_match_vec.push_back(cir_r.cir_match_vec[0]);
	//		cir_r.cir_match_vec.erase(cir_r.cir_match_vec.begin());
	//	}
	//}
	//return true;
}

//由凸包面积法匹配、单个图像
bool imgpro::matchEllipse_convexHull(mycircle &cir_l, int mini_area)
{
	//return matchEllipse(cir_l.cir_vec, cir_r.cir_vec, cir_l.cir_match_vec, cir_r.cir_match_vec, F_2_1);
	sort(cir_l.cir_idle_ab_detect.begin(), cir_l.cir_idle_ab_detect.end(), SortByArea);
	convert_Ellipse_center(cir_l.cir_idle_ab_detect, cir_l.cir_match_vec, mini_area);

	convexHull(cir_l.cir_match_vec, cir_l.cir_match_vec);

	int size = cir_l.cir_match_vec.size();

	for (int i = 0; i < size; i++)
	{
		if (cir_l.cir_idle_ab_detect[0].center != cir_l.cir_match_vec[0])
		{
			cir_l.cir_match_vec.push_back(cir_l.cir_match_vec[0]);
			cir_l.cir_match_vec.erase(cir_l.cir_match_vec.begin());
		}
		else
		{
			break;
		}
	}
	return true;
}

//由双目像素坐标计算世界坐标
bool imgpro::pointTo3D(vector<Point2f>& vec_uvLeft, vector<Point2f>& vec_uvRight, Mat& mLeftIntrinsic, Mat& mRightIntrinsic, Mat& mRightRT, vector<Point3f>& vec_3d)
{
	//  [u1]      |X|                     [u2]      |X|  
	//Z*|v1| = Ml*|Y|                   Z*|v2| = Mr*|Y|  
	//  [ 1]      |Z|                     [ 1]      |Z|  
	//            |1|                               |1|  

	if (vec_uvLeft.size() != vec_uvRight.size())return false;
	vec_3d.clear();

	Mat mLeftRT = (Mat_<float>(3, 4) <<
		1, 0, 0, 0,
		0, 1, 0, 0,
		0, 0, 1, 0);
	mLeftRT.convertTo(mLeftRT, CV_32F);
	mRightRT.convertTo(mRightRT, CV_32F);

	mLeftIntrinsic.convertTo(mLeftIntrinsic, CV_32F);
	mRightIntrinsic.convertTo(mRightIntrinsic, CV_32F);


	Mat mLeftM = Mat(3, 4, CV_32F);
	Mat mRightM = Mat(3, 4, CV_32F);
	mLeftM = mLeftIntrinsic * mLeftRT;
	mRightM = mRightIntrinsic * mRightRT;

	int size = vec_uvLeft.size();
	for (size_t i = 0; i < size; i++)
	{
		Point2f uvLeft = vec_uvLeft[i];
		Point2f uvRight = vec_uvRight[i];
		//最小二乘法A矩阵  
		Mat A = Mat(4, 3, CV_32F);
		A.at<float>(0, 0) = uvLeft.x * mLeftM.at<float>(2, 0) - mLeftM.at<float>(0, 0);
		A.at<float>(0, 1) = uvLeft.x * mLeftM.at<float>(2, 1) - mLeftM.at<float>(0, 1);
		A.at<float>(0, 2) = uvLeft.x * mLeftM.at<float>(2, 2) - mLeftM.at<float>(0, 2);

		A.at<float>(1, 0) = uvLeft.y * mLeftM.at<float>(2, 0) - mLeftM.at<float>(1, 0);
		A.at<float>(1, 1) = uvLeft.y * mLeftM.at<float>(2, 1) - mLeftM.at<float>(1, 1);
		A.at<float>(1, 2) = uvLeft.y * mLeftM.at<float>(2, 2) - mLeftM.at<float>(1, 2);

		A.at<float>(2, 0) = uvRight.x * mRightM.at<float>(2, 0) - mRightM.at<float>(0, 0);
		A.at<float>(2, 1) = uvRight.x * mRightM.at<float>(2, 1) - mRightM.at<float>(0, 1);
		A.at<float>(2, 2) = uvRight.x * mRightM.at<float>(2, 2) - mRightM.at<float>(0, 2);

		A.at<float>(3, 0) = uvRight.y * mRightM.at<float>(2, 0) - mRightM.at<float>(1, 0);
		A.at<float>(3, 1) = uvRight.y * mRightM.at<float>(2, 1) - mRightM.at<float>(1, 1);
		A.at<float>(3, 2) = uvRight.y * mRightM.at<float>(2, 2) - mRightM.at<float>(1, 2);

		//最小二乘法B矩阵  
		Mat B = Mat(4, 1, CV_32F);
		B.at<float>(0, 0) = mLeftM.at<float>(0, 3) - uvLeft.x * mLeftM.at<float>(2, 3);
		B.at<float>(1, 0) = mLeftM.at<float>(1, 3) - uvLeft.y * mLeftM.at<float>(2, 3);
		B.at<float>(2, 0) = mRightM.at<float>(0, 3) - uvRight.x * mRightM.at<float>(2, 3);
		B.at<float>(3, 0) = mRightM.at<float>(1, 3) - uvRight.y * mRightM.at<float>(2, 3);

		Mat XYZ = Mat(3, 1, CV_32F);
		//采用SVD最小二乘法求解XYZ  
		solve(A, B, XYZ, DECOMP_SVD);

		//cout<<"空间坐标为 = "<<endl<<XYZ<<endl;  

		//世界坐标系中坐标  
		Point3f world;
		world.x = XYZ.at<float>(0, 0);
		world.y = XYZ.at<float>(1, 0);
		world.z = XYZ.at<float>(2, 0);
		vec_3d.push_back(world);
	}
	return true;
}

bool imgpro::tack_flow(Mat& pregray, Mat& gray, vector<Point2f>& pre, vector<Point2f>& now, vector<uchar>& status, vector<float>& err)
{
	calcOpticalFlowPyrLK(pregray, gray, pre, now, status, err);
	return true;
}

imgpro::~imgpro()
{
}

mycircle.h

#pragma once
#include <opencv2\opencv.hpp>
#include <iostream>
#ifndef PI
#define PI 3.141592653
using namespace cv;
using namespace std;
#endif

class mycircle
{
public:

	vector<Point2f> cir_idle;//理论圆心
	vector<int> cir_id_idle;//理论id
	vector<RotatedRect> cir_idle_ab;//理论圆心

	vector<Point2f> cir_vec,cir_match_vec;//检测到圆心,匹配过的

	vector<int> cir_hsv;//鉴别hsv
	vector<int> cir_id;//鉴别id
	vector<RotatedRect> cir_idle_ab_detect;//检测椭圆

public:
	mycircle();

	//画检测到圆心 circle(img, cir_vec[i], 3, Scalar(255, 255, 255), CV_FILLED);
	Mat& show_circle(Mat& img);

	//画检测到圆顺序/id  sprintf(s, "%d", cir_id[i]);
	Mat& show_circle_id(Mat& img);

	//画匹配结果
	Mat& show_match_order(Mat& img);

	//画椭圆,默认填充 黑色 thickness=-1
	Mat& genarate_ellipse(Mat& img, vector<RotatedRect> ¢er_ab_size, Scalar corlor = Scalar(0,0,0), int thickness=-1);

	//分组画椭圆  默认填充 黑色 thickness=-1
	Mat& genarate_ellipse_maiker(Mat& img, vector<RotatedRect> ¢er_ab_size, int group, int thickness = -1);

	//画圆
	Mat& genarate_cir(Mat& img, vector<Point2f> center_idle, int radius);

	//排序椭圆面积大小


	~mycircle();
};

mycircle.cpp

#include "mycircle.h"

mycircle::mycircle()
{

}

//画检测到圆心 circle(img, cir_vec[i], 3, Scalar(255, 255, 255), CV_FILLED);
Mat& mycircle::show_circle(Mat& img)
{
	if (!img.data)
	{
		cout << "no data 1232";
		return img;
	}
	IplImage tstim = img;
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN | CV_FONT_ITALIC, 2.0, 2.0, 1, 1);
	for (size_t i = 0; i < cir_vec.size(); i++)
	{
		char s[100];
		sprintf(s, "%d", i);
		//cvPutText(&tstim, s, cir_vec[i], &font, CV_RGB(0, 0, 0));
		circle(img, cir_vec[i], 3, Scalar(255, 255, 255), CV_FILLED);//
	}
	return img;
}

//画检测到圆顺序/id or (hsv没有分类)  sprintf(s, "%d", cir_id[i]);
Mat& mycircle::show_circle_id(Mat& img)
{
	if (!img.data) 
	{
		cout << "no data 1232\n";
		return img;
	}

	//没有分类，直接显示 hsv 数值
	if (cir_id.size() != cir_hsv.size())
	{
		cir_id = cir_hsv;
	}

	//不可能不等，除非没鉴别
	if (cir_id.size() != cir_vec.size())
	{
		cout << "cir_vec.size!=cir_id.size\n";
		return img;
	}

	IplImage tstim = img;
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN | CV_FONT_ITALIC, 2.0, 2.0, 1, 1);
	for (size_t i = 0; i < cir_vec.size(); i++)
	{
		char s[100];
		sprintf(s, "%d", cir_id[i]);
		cvPutText(&tstim, s, cir_vec[i], &font, CV_RGB(0, 0, 0));
	}
	return img;
}

//画匹配结果
Mat& mycircle::show_match_order(Mat& img)
{
	if (!img.data)
	{
		cout << "no data 1232";
		return img;
	}
	IplImage tstim = img;
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_PLAIN | CV_FONT_ITALIC, 2.0, 2.0, 1, 1);
	for (size_t i = 0; i < cir_match_vec.size(); i++)
	{
		char s[100];
		sprintf(s, "%d", i);
		cvPutText(&tstim, s, cir_match_vec[i], &font, CV_RGB(0, 0, 0));
		circle(img, cir_match_vec[i], 3, Scalar(255, 255, 255), CV_FILLED);//
	}
	return img;
}

//画椭圆,默认黑色corlor（0,0,0），默认填充thickness=-1
Mat& mycircle::genarate_ellipse(Mat& img, vector<RotatedRect> ¢er_ab_size, Scalar corlor, int thickness)
{
	int size = center_ab_size.size();
	for (size_t i = 0; i < size; i++)
	{
		//int thickness = -1;
		int lineType = 8;
		ellipse(img,
			Point(center_ab_size[i].center.x, center_ab_size[i].center.y),
			Size(center_ab_size[i].size.width / 2, center_ab_size[i].size.height / 2),
			center_ab_size[i].angle,
			0,
			360,
			corlor,
			thickness,
			lineType,
			0);
	}
	return img;
}

//注意分组group
Mat& mycircle::genarate_ellipse_maiker(Mat& img, vector<RotatedRect> ¢er_ab_size, int group, int thickness)
{
	Scalar corlor;
	int size = center_ab_size.size();
	for (size_t i = 0; i < size; i++)
	{
		corlor[0] = 180 / group * (rand() % group);
		corlor[1] = 255;
		corlor[2] = 255;

		//int thickness = -1;
		int lineType = 8;

		//先画大黑圆
		ellipse(img,
			Point(center_ab_size[i].center.x, center_ab_size[i].center.y),
			Size(center_ab_size[i].size.width / 2, center_ab_size[i].size.height / 2),
			center_ab_size[i].angle,
			0,
			360,
			Scalar(0, 0, 0),
			thickness,
			lineType,
			0);

		//再画同心圆
		ellipse(img,
			Point(center_ab_size[i].center.x, center_ab_size[i].center.y),
			Size(center_ab_size[i].size.width / 4, center_ab_size[i].size.height / 4),
			center_ab_size[i].angle,
			0,
			360,
			corlor,
			thickness,
			lineType,
			0);
	}
	return img;
}

//画圆
Mat& mycircle::genarate_cir(Mat& img, vector<Point2f> center_idle, int radius)
{
	int size = center_idle.size();
	for (size_t i = 0; i < size; i++)
	{
		int thickness = 2;
		int lineType = 8;
		circle(img,
			Point(center_idle[i].x, center_idle[i].y),
			radius,
			Scalar(255, 0, 255),
			thickness,
			lineType);
	}
	return img;
}

mycircle::~mycircle()
{

}

res_c_r_w.h

#pragma once
#include <opencv2\opencv.hpp>
#include <iostream>
#include <fstream>
#include <string>
#ifndef PI
#define PI 3.141592653
using namespace cv;
using namespace std;
#endif

struct myQuaternion
{
	float x;
	float y;
	float z;
	float w;
};

class res_c_r_w
{
public:


public:
	res_c_r_w();

	//2d点欧氏距离
	float dis_comput_point(Point2f a, Point2f b);

	//3d点欧氏距离
	float dis_comput_point(Point3f a, Point3f b);

	//输出3d点
	int cout_3d_point(const vector<Point3f> &point3d);

	//2d点乱序，最小距离
	int comput_err2d_random_order(const vector<Point2f> &detected, const vector<Point2f> &idle, vector<float> &err);

	//2d点乱序，最小距离，//空的，日后填
	int comput_err2d_random_order(const vector<RotatedRect> &detected, const vector<RotatedRect> &idle, vector<float> &err);

	//记录vector
	int txt_record(string& filneme, vector<Point2f> &point2d);
	int txt_record(string& filneme, vector<float> &point);

	//读取txt
	int txt_read(string& filneme, vector<Point3f> &point3d);
	int txt_read(string& filneme, vector<myQuaternion> &myQuater);

	//读取xml
	bool xml_read(char filneme[], char mat_name[], Mat& mat);

	//融合两个矩阵
	Mat mergeRows(Mat& A, Mat& B);//行[A ; B]
	Mat mergecols(Mat& A, Mat& B);//列[A   B]

	//r计算四元数
	void QuaternionFromMatrix(const Mat &R, float quat[]);

	void MatrixFromQuaternion(const myQuaternion &s, Mat& res);

	//use svd to cumpute r t 
	bool my_svd(vector<Point3f> &pre, vector<Point3f> &now, Mat &R, Mat &t);

	~res_c_r_w();
};

res_c_w.cpp

#include "res_c_r_w.h"

res_c_r_w::res_c_r_w()
{
}

//2d点欧氏距离
float res_c_r_w::dis_comput_point(Point2f a, Point2f b)
{
	return sqrt(pow((a.x - b.x), 2) + pow((a.y - b.y), 2));
}

//3d点欧氏距离
float res_c_r_w::dis_comput_point(Point3f a, Point3f b)
{
	return sqrt(pow((a.x - b.x), 2) + pow((a.y - b.y), 2) + pow((a.z - b.z), 2));
}

//2d点乱序，最小距离
int res_c_r_w::comput_err2d_random_order(const vector<Point2f> &detected, const vector<Point2f> &idle, vector<float> &err)
{
	int num_match = 0;
	int seze_idle = idle.size();
	int seze_detected = detected.size();
	for (int i = 0; i < seze_detected; i++)
	{
		float min = 9999;
		for (int j = 0; j < seze_idle; j++)
		{
			float dis = this->dis_comput_point(detected[i], idle[j]);//2d点欧氏距离
			if (dis < min)min = dis;//距离最小点为误差
		}
		err.push_back(min);
		num_match++;
	}
	return num_match;
}

//空的，日后填
int res_c_r_w::comput_err2d_random_order(const vector<RotatedRect> &detected, const vector<RotatedRect> &idle, vector<float> &err)
{
	return 0;
}

//记录vector
int res_c_r_w::txt_record(string& filneme, vector<Point2f> &point2d)
{
	ofstream out(filneme);
	int size = point2d.size();
	for (int i = 0; i < size; i++)
		out << point2d[i].x << " " << point2d[i].y << endl;
	return size;
}
int res_c_r_w::txt_record(string& filneme, vector<float> &point)
{
	ofstream out(filneme);
	int size = point.size();
	for (int i = 0; i < size; i++)
		out << point[i] << endl;
	return size;
}

//读取txt,四元数
int res_c_r_w::txt_read(string& filneme, vector<myQuaternion> &myQuater)
{
	ifstream in(filneme);
	string line;

	int nest = -1;
	int num = 0;

	while (getline(in, line)) // line中不包括每行的换行符
	{
		float temp[4];
		int i = 0;
		while (true)
		{
			temp[i] = stof(line);
			//cout << temp[i] << " ";
			i++;
			nest = line.find(" ");                 //找到下一个逗号的位置
			if (nest == -1)nest = line.find(",");
			if (nest == -1)break;
			line = line.substr(nest + 1, line.size());//去掉已读数据与" "
		}
		myQuater.push_back(myQuaternion{ temp[0], temp[1], temp[2], temp[3]});
	}
	return myQuater.size();
}

//读取txt
int res_c_r_w::txt_read(string& filneme, vector<Point3f> &point3d)
{
	ifstream in(filneme);
	string line;

	int nest = -1;
	int num = 0;

	while (getline(in, line)) // line中不包括每行的换行符
	{
		float temp[3];
		int i = 0;
		while (true)
		{
			temp[i] = stof(line);
			//cout << temp[i] << " ";
			i++;
			nest = line.find(" ");                 //找到下一个逗号的位置
			if (nest == -1)nest = line.find(",");
			if (nest == -1)break;
			line = line.substr(nest + 1, line.size());//去掉已读数据与" "
		}
		point3d.push_back(Point3f{ temp[0], temp[1], temp[2] });
	}
	return point3d.size();
}

//读取xml
bool res_c_r_w::xml_read(char filneme[], char mat_name[], Mat& mat)
{
	FileStorage fs(filneme, FileStorage::READ);
	if (fs.isOpened())
	{
		fs[mat_name] >> mat;
		fs.release();
	}
	return mat.data;
}

int fps()
{
	/*
	#include<ctime>

	int main()
	{
	long start, finish;
	start = clock();

	cout << "HW .... " << endl;

	finish = clock();

	cout << finish - start << "/" << CLOCKS_PER_SEC << " (s) " << endl;

	return 0;
	}
	*/
	return 0;
}

res_c_r_w::~res_c_r_w()
{
}

Mat res_c_r_w::mergeRows(Mat& A, Mat& B)//行[A ; B]
{
	//CV_ASSERT(A.cols == B.cols&&A.type() == B.type());
	int totalRows = A.rows + B.rows;

	Mat mergedDescriptors(totalRows, A.cols, B.type());
	Mat submat = mergedDescriptors.rowRange(0, A.rows);
	A.copyTo(submat);
	submat = mergedDescriptors.rowRange(A.rows, totalRows);
	B.copyTo(submat);
	return mergedDescriptors;
}

Mat res_c_r_w::mergecols(Mat& A, Mat& B)//列[A   B]
{
	//CV_ASSERT(A.cols == B.cols&&A.type() == B.type());
	int totalCols = A.cols + B.cols;

	Mat mergedDescriptors(A.rows, totalCols, B.type());
	Mat submat = mergedDescriptors.colRange(0, A.cols);
	A.copyTo(submat);
	submat = mergedDescriptors.colRange(A.cols, totalCols);
	B.copyTo(submat);
	return mergedDescriptors;
}

//输出3d点
int res_c_r_w::cout_3d_point(const vector<Point3f> &point3d)
{
	int size = point3d.size();
	for (int i = 0; i < size; i++)
		cout << point3d[i].x << " " << point3d[i].y << " " << point3d[i].z <<endl;
	return size;
}

int sign(float x)
{
	return x >= 0 ? 1 : -1;
}
float myMax(float x, float y)
{
	return x >y ? x : y;
}

//计算四元数
void res_c_r_w::QuaternionFromMatrix(const Mat &R, float quat[])
{
	// Adapted from: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
	/*quat[0] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] + R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2;

	quat[1] = (float)sqrt(myMax(0.0, 1 + R.ptr<float>(0)[0] - R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2;
	quat[2] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] + R.ptr<float>(1)[1] - R.ptr<float>(2)[2]))/2;
	quat[3] = (float)sqrt(myMax(0.0, 1 - R.ptr<float>(0)[0] - R.ptr<float>(1)[1] + R.ptr<float>(2)[2]))/2;

	quat[1] *= sign(R.ptr<float>(2)[1] - R.ptr<float>(1)[2]);
	quat[2] *= sign(R.ptr<float>(0)[2] - R.ptr<float>(2)[0]);
	quat[3] *= sign(R.ptr<float>(1)[0] - R.ptr<float>(0)[1]);*/
	//R.convertTo(R, CV_32F);
	quat[0] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) + R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;

	quat[1] = (float)sqrt(myMax(0.0, 1 + R.at<float>(0, 0) - R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
	quat[2] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) + R.at<float>(1, 1) - R.at<float>(2, 2))) / 2;
	quat[3] = (float)sqrt(myMax(0.0, 1 - R.at<float>(0, 0) - R.at<float>(1, 1) + R.at<float>(2, 2))) / 2;

	quat[1] *= sign(R.at<float>(2, 1) - R.at<float>(1, 2));
	quat[2] *= sign(R.at<float>(0, 2) - R.at<float>(2, 0));
	quat[3] *= sign(R.at<float>(1, 0) - R.at<float>(0, 1));
}

// Convert to Matrix
void res_c_r_w::MatrixFromQuaternion(const myQuaternion &s, Mat& res)
{
	float x = s.x;
	float y = s.y;
	float z = s.z;
	float w = s.w;

	float x2 = x * x;
	float y2 = y * y;
	float z2 = z * z;
	float xy = x * y;
	float xz = x * z;
	float yz = y * z;
	float wx = w * x;
	float wy = w * y;
	float wz = w * z;

	// This calculation would be a lot more complicated for non-unit length quaternions
	// Note: The constructor of Matrix4 expects the Matrix in column-major format like expected by
	//   OpenGL
	float zroe[9] = { 1.0f - 2.0f * (y2 + z2), 2.0f * (xy - wz), 2.0f * (xz + wy),
		2.0f * (xy + wz), 1.0f - 2.0f * (x2 + z2), 2.0f * (yz - wx),
		2.0f * (xz - wy), 2.0f * (yz + wx), 1.0f - 2.0f * (x2 + y2) };

	res = Mat(3, 3, CV_32F, zroe).clone();
	//cout << res << endl;
}

//去质心化
void Remove_center(vector<Point3f>& a, Mat& ccc)
{
	int size = a.size();
	if (size == 0)return;

	Point3f taltal = { 0, 0, 0 };
	for (auto b : a)
	{
		taltal.x += b.x;
		taltal.y += b.y;
		taltal.z += b.z;
	}
	taltal.x /= size;
	taltal.y /= size;
	taltal.z /= size;
	for (auto& b : a)
	{
		b.x -= taltal.x;
		b.y -= taltal.y;
		b.z -= taltal.z;
	}
	ccc = Mat(3, 1, CV_32F, &taltal);
}

//svd
bool res_c_r_w::my_svd(vector<Point3f> &p1, vector<Point3f> &p2, Mat &R, Mat &t)
{
	Mat lc, rc;
	Remove_center(p1, lc);
	Remove_center(p2, rc);

	float zroe[9] = { 0, 0, 0, 0, 0, 0, 0, 0, 0 };
	Mat res = Mat(3, 3, CV_32F, zroe);
	cout << res << endl;

	for (int i = 0; i < p1.size(); i++)
	{
		Mat l = Mat(3, 1, CV_32F, &p1[i]);
		//cout << l;
		Mat r = Mat(1, 3, CV_32F, &p2[i]);
		res += l*r;
	}

	SVD thissvd(res, SVD::FULL_UV);
	Mat u = thissvd.u;

	Mat vt = thissvd.vt;

	Mat w = thissvd.w;

	if (w.at<float>(2, 0) == 0)return false;

	R = u*vt;
	//cout << R << endl;

	t = lc - R*rc;
	cout << t << endl;

	//Remove_center已经去质心化，优化函数计算的只是旋转矩阵，平移矩阵t = lc - R*rc需集成，另需考虑模型建立是否优化平移矩阵；

	return true;
}

virtualcam.h

#include <opencv2\opencv.hpp>
//#include <pylon/PylonIncludes.h>
//#include <pylon/PylonGUI.h>

//using namespace Pylon;
using namespace cv;

class virtualcam
{
public:
	int wide_frame, hight_frame;
	int which_cam;

public:
	virtualcam(){};
	//打开相机，单目，双目
	virtual bool open_cam(int which_cam = 0) = 0;
	//抓取一帧
	virtual bool grab_frame() = 0;
	//关闭相机
	virtual bool close_cam() = 0;

	~virtualcam(){};
};

class cam_monocular :public virtualcam
{
private:
	VideoCapture cam_bas_mono;

public:
	Mat frame;

public:
	//初始化，设置相机参数
	cam_monocular(int wideth = 640, int highth = 480);

	//打开单目
	virtual bool open_cam(int which_cam = 0);

	//抓取一帧
	virtual bool grab_frame();

	//关闭相机
	virtual bool close_cam();

	~cam_monocular();
};

class cam_stereo :public virtualcam
{
private:
	VideoCapture cam_cv_stereo;

public:
	Mat frame_l, frame_r, raw;

public:
	//初始化，设置相机参数,默认宽高 int wideth=640, int highth=480
	cam_stereo(int wideth = 640, int highth = 480);

	//int which_cam = 0
	virtual bool open_cam(int which_cam = 0);

	//抓取一帧
	virtual bool grab_frame();

	//关闭相机
	virtual bool close_cam();

	//可视化一帧，左右图像
	bool show_stereo_fram();

	~cam_stereo();
};

//class Basler_monocular :public virtualcam
//{
//public:
//
//	Mat frame;
//
//	CInstantCamera baslerL;
//	// This smart pointer will receive the grab result data.
//	CGrabResultPtr ptrGrabResultL;
//
//public:
//	//打开工业相机单目
//	virtual bool open_cam(int serialL)
//	{
//		return true;
//	}
//	virtual bool open_cam(String_t serialL);
//
//	//抓取工业相机一帧
//	virtual bool grab_frame();
//
//	//关闭工业相机
//	virtual bool close_cam();
//
//	bool CameraState();
//
//	~Basler_monocular();
//};

virtualcam.cpp

#include"virtualcam.h"


//初始化，设置相机参数int wideth = 640, int highth = 480
cam_monocular::cam_monocular(int wideth, int highth)
{
	hight_frame = highth;//默认640 X 480
	wide_frame = wideth;
}

//打开单目
bool cam_monocular::open_cam(int which_cam)
{
	int faile = 0;

	//打开相机
	bool res = cam_bas_mono.open(which_cam);
	while (faile<5 && (res == false))
	{
		if (cam_bas_mono.isOpened())
		{
			res = true;
			break;
		}
		else cam_bas_mono.open(which_cam);
		faile++;
		waitKey(100);
	}

	cam_bas_mono.set(CV_CAP_PROP_FRAME_WIDTH, wide_frame);
	cam_bas_mono.set(CV_CAP_PROP_FRAME_HEIGHT, hight_frame);

	return res;
}

//抓取一帧
bool cam_monocular::grab_frame()
{
	cam_bas_mono >> frame;
	return frame.data;
}

//关闭相机
bool cam_monocular::close_cam()
{
	cam_bas_mono.release();
	return true;
}

cam_monocular::~cam_monocular()
{

}

//


//初始化，设置相机参数,默认宽高 int wideth=640, int highth=480
cam_stereo::cam_stereo(int wideth, int highth)
{
	hight_frame = highth;//默认640 X 480
	wide_frame = 2 * wideth;
}

bool cam_stereo::open_cam(int which_cam)
{
	int faile = 0;
	//打开相机
	bool res = cam_cv_stereo.open(which_cam);
	while (faile<5 && (res == false))
	{
		if (cam_cv_stereo.isOpened())
		{
			res = true;
			break;
		}
		else cam_cv_stereo.open(which_cam);
		faile++;
		waitKey(100);
	}
	cam_cv_stereo.set(CV_CAP_PROP_FRAME_WIDTH, wide_frame);
	cam_cv_stereo.set(CV_CAP_PROP_FRAME_HEIGHT, hight_frame);

	return res;
}

//抓取一帧
bool cam_stereo::grab_frame()
{
	cam_cv_stereo >> raw;

	frame_l = raw(cv::Rect(0, 0, raw.cols / 2, raw.rows));
	frame_r = raw(cv::Rect(raw.cols / 2, 0, raw.cols / 2, raw.rows));

	return (frame_l.data&&frame_r.data);
}

//关闭相机
bool cam_stereo::close_cam()
{
	return false;
}

//可视化一帧，左右图像
bool cam_stereo::show_stereo_fram()
{
	if (frame_l.data&&frame_r.data)
	{
		imshow("frame_l", frame_l);
		imshow("frame_r", frame_r);
		waitKey(1);
	}
	return (frame_l.data&&frame_r.data);
}

cam_stereo::~cam_stereo()
{

}


//打开工业相机单目
//bool Basler_monocular::open_cam(String_t serialL)
//{
//	try
//	{
//		CDeviceInfo infoL;
//		DeviceInfoList_t filter;
//		infoL.SetSerialNumber(serialL);
//		filter.push_back(infoL);
//
//		CTlFactory& tlFactory = CTlFactory::GetInstance();
//		if (tlFactory.EnumerateDevices(filter) < 1)
//		{
//			if (baslerL.IsPylonDeviceAttached())
//			{
//				baslerL.DetachDevice();
//			}
//			return 0;
//		}
//
//		baslerL.Attach(tlFactory.CreateDevice(infoL));
//		baslerL.StartGrabbing();
//	}
//	catch (...)
//	{
//		if (baslerL.IsPylonDeviceAttached())
//		{
//			baslerL.DetachDevice();
//		}
//		return false;
//	}
//
//	return true;
//}
//
抓取工业相机一帧
//bool Basler_monocular::grab_frame()
//{
//	try
//	{
//		baslerL.RetrieveResult(5000, ptrGrabResultL, TimeoutHandling_ThrowException);
//		if (ptrGrabResultL->GrabSucceeded())
//		{
//			CPylonImage targetImageL;
//
//			CImageFormatConverter converter;
//			converter.OutputPixelFormat = PixelType_RGB8packed;
//			// Conversion is needed.
//			Mat frameBL;
//			converter.Convert(targetImageL, ptrGrabResultL);
//
//			frameBL = Mat(targetImageL.GetHeight(), targetImageL.GetWidth(), CV_8UC3, targetImageL.GetBuffer());
//
//			cv::cvtColor(frameBL, frame, CV_RGB2BGR);
//			//memcpy(img,frameRGB.data,frameRGB.cols*frameRGB.rows*frameRGB.channels()*sizeof(uchar));	
//		}
//
//	}
//	catch (...)
//	{
//		if (baslerL.IsPylonDeviceAttached())
//		{
//			baslerL.DetachDevice();
//		}
//		return 0;
//	}
//	return 0;
//}
//
关闭工业相机
//bool Basler_monocular::close_cam()
//{
//	if (baslerL.IsPylonDeviceAttached())
//	{
//		baslerL.DetachDevice();
//	}
//	return true;
//}
//
//bool Basler_monocular::CameraState()
//{
//	return (baslerL.IsPylonDeviceAttached());
//}
//
//Basler_monocular::~Basler_monocular(){
//
//}

extrinsics_1-3.yml

%YAML:1.0
R: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9959104248003017e-001, 1.9166997731104172e-002,
       2.1222016672844374e-002, -1.8428362910765935e-002,
       9.9923581339501355e-001, -3.4470054674091588e-002,
       -2.1866486551701522e-002, 3.4064870861073349e-002,
       9.9918038478500104e-001 ]
T: !!opencv-matrix
   rows: 3
   cols: 1
   dt: d
   data: [ 8.1690925213239451e-001, -1.2584479554166531e+002,
       2.4710087788415848e+001 ]
R1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9969738592232038e-001, 2.2877525203349226e-002,
       9.0418704176306371e-003, -2.0236706282166474e-002,
       9.7380757230750770e-001, -2.2647138414247231e-001,
       -1.3986146679068771e-002, 2.2621987303755620e-001,
       9.7397585018517963e-001 ]
R2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 9.9991893235707585e-001, 4.1255825086614368e-003,
       -1.2046089940304055e-002, -6.3696422901967113e-003,
       9.8124281196591046e-001, -1.9267062989030725e-001,
       1.1025260585609972e-002, 1.9273173982039660e-001,
       9.8118954340882691e-001 ]
P1: !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [ -2.3204885193537793e+003, 0., 6.1762542724609375e+001, 0., 0.,
       -2.3204885193537793e+003, -1.2964654409885406e+003, 0., 0., 0.,
       1., 0. ]
P2: !!opencv-matrix
   rows: 3
   cols: 4
   dt: d
   data: [ -2.3204885193537793e+003, 0., 6.1762542724609375e+001, 0., 0.,
       -2.3204885193537793e+003, -1.2964654409885406e+003,
       2.9760361015006673e+005, 0., 0., 1., 0. ]
Q: !!opencv-matrix
   rows: 4
   cols: 4
   dt: d
   data: [ 1., 0., 0., -6.1762542724609375e+001, 0., 1., 0.,
       1.2964654409885406e+003, 0., 0., 0., -2.3204885193537793e+003, 0.,
       0., 7.7972458673591771e-003, 0. ]
F: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 2.9813350879573163e-006, -2.6937748703588859e-005,
       -9.5695054799860471e-002, 2.2977465898410983e-005,
       4.1440226708800177e-007, -7.5395225304681979e-003,
       9.5740371765424964e-002, 1.0964014902456132e-002, 1. ]

intrinsics_1-3.yml

%YAML:1.0
M1: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 8.8330927960072984e+002, 0., 3.1248507591955087e+002, 0.,
       8.8330927960072984e+002, 2.8902228488593829e+002, 0., 0., 1. ]
D1: !!opencv-matrix
   rows: 1
   cols: 8
   dt: d
   data: [ -7.0635944943046292e-001, 3.0267571731940777e+001, 0., 0., 0.,
       0., 0., 3.8964226605568587e+002 ]
M2: !!opencv-matrix
   rows: 3
   cols: 3
   dt: d
   data: [ 8.8330927960072984e+002, 0., 3.1302857265075750e+002, 0.,
       8.8330927960072984e+002, 2.8745084586316977e+002, 0., 0., 1. ]
D2: !!opencv-matrix
   rows: 1
   cols: 8
   dt: d
   data: [ 9.4896000731754346e-002, -2.6823808396061288e+000, 0., 0., 0.,
       0., 0., -1.4166046218036655e+001 ]