Aruco标志码打印、识别、测量及控制构建

头文件：
#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp" 
#include "opencv2/video.hpp"
#include "opencv2/objdetect.hpp"
#include "opencv2/imgcodecs.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/ml.hpp"
#include "opencv.hpp"
#include <string>
#include <iostream>
#include <fstream>
#include<stack>
#include<aruco.hpp>
#include"dictionary.hpp"
#include"charuco.hpp"
#include <ctime>
#include<math.h>

using namespace cv;
using namespace std;
using namespace aruco;

class ARUCO_FINDER
{

public:
	ARUCO_FINDER()
	{
		cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
		Ptr<CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04f, 0.02f, dictionary);


	};


public:

	Ptr<cv::aruco::Dictionary> dictionary;
	vector<Mat> Printimage_all;//多个某字典的aruco
	Ptr<CharucoBoard> board;
	vector<Vec3d > rvecs_ARUCO, tvecs_ARUCO;//from the marker to the camera  pose，Vec3d相当于三通道Mat//其中的rvec三个参数分别为欧拉角表征的绕z，绕新y，绕新x 轴旋转的欧拉角 及 RPY表征下的rz。ry 。rx
	std::vector<int> ids;//marker的id
	std::vector<std::vector<cv::Point2f> > corners;//角的列表
	std::vector<std::vector<cv::Point2f> >rejected;//返回所有marker候选，含不是marker的方形

public:
	//标定步骤的成员变量
	Mat cameraMatrix, distCoeffs;
	std::vector<cv::Mat> rvecs_CH, tvecs_CH;

public:
	void aruco_detect2estimate_pose(char filename_temp[100]);
	void use_Charuco_calibrate(cv::Ptr<aruco::CharucoBoard> Charucoboard);
	void print_Charuco();
	void print_aruco();

};
struct id_corner_RT
{
	int id;
	vector<cv::Point2f> corner;//每个corner有四个角点，四个角点是xy二维
	Vec3d rvec, tvec;

};

class GUIDE_ROBOT
{
	//这个也应该参考着aruco去写,同时也需要标定，所以标定测charuco程序也应该在里面

public:
	GUIDE_ROBOT()
	{
		cv::Ptr<cv::aruco::Dictionary> dictionary = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_50);
		Ptr<CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04f, 0.02f, dictionary);
	};


public:

	Ptr<cv::aruco::Dictionary> dictionary;
	vector<Mat> Printimage_all;//多个某字典的aruco
	vector<Vec3d > rvecs_ARUCO, tvecs_ARUCO;//from the marker to the camera  pose，Vec3d相当于三通道Mat//其中的rvec三个参数分别为欧拉角表征的绕z，绕新y，绕新x 轴旋转的欧拉角 及 RPY表征下的rz。ry 。rx
	std::vector<int> ids;//marker的id
	std::vector<std::vector<cv::Point2f> > corners;//角的列表
	std::vector<std::vector<cv::Point2f> >rejected;//返回所有marker候选，含不是marker的方形

private:
	//建立一个struct，让id里的标号和id的值对应,以及rt
	id_corner_RT  id_c_rt[6];//每个aruco对应一个 	id_corner_RT的结构体
	vector<vector<Vec3d>>aruco_controler;
	//补充： 第一层vec 包含 三个控制向量， 第二程vec 包含一个控制向量，第三层vec是两个向量端点。



public:
	//标定步骤的成员变量
	Mat cameraMatrix, distCoeffs;
	std::vector<cv::Mat> rvecs_CH, tvecs_CH;


public:

	void guide_robot_func(int index);
	void  detect_aruco_vecs(char filename_temp[100]);

private:
	Physical_dimension Laser_device;
	Physical_dimension Xianweijing_device;


public:
	//最终指导的运动R和 t
	HOW2MOVE how_move1;//运动到激光器的
	HOW2MOVE how_move2;//运动到显微镜的


};


CPP:
using namespace cv;
using namespace std;
using namespace aruco;

//aruco
void ARUCO_FINDER::aruco_detect2estimate_pose(char filename_temp[100])
{



	/*
	被注释的这部分是marker生成程序，为了不每次都生成就先注释起来，确定该程序段能用——————1.12
	Mat Printimage;
	vector<int>png_param;
	png_param.push_back(IMWRITE_PNG_COMPRESSION);
	png_param.push_back(9);




	int filenum=0;
	for (int i = 0; i < 15; i++)
	{
	char* print_path = new char[120];
	sprintf(print_path, "C:\\Users\\Administrator.PC-20180331UNLV\\Desktop\\print_img\\print_img(%d).png", filenum);
	cv::aruco::drawMarker(dictionary, i, 200, Printimage, 1);
	imwrite(print_path, Printimage, png_param);
	delete[] print_path;
	filenum++;
	}



	*/
	cv::Ptr<cv::aruco::DetectorParameters> parameters = aruco::DetectorParameters::create();//如果提示0*000000出错，这里的para出错了
	(*parameters).doCornerRefinement = true;
	/*

	cv::Ptr<cv::aruco::DetectorParameters> parameters;//如果要修改参数应该这么写，如果要用默认参数，应该用create

	(*parameters).adaptiveThreshConstant = 3;//
	(*parameters).adaptiveThreshWinSizeMax = 3;
	(*parameters).adaptiveThreshWinSizeMin = 3;
	(*parameters).adaptiveThreshWinSizeStep = 3;
	(*parameters).cornerRefinementMaxIterations = 3;
	(*parameters).cornerRefinementMinAccuracy = 3;
	(*parameters).cornerRefinementWinSize = 3;
	(*parameters).doCornerRefinement = 3;
	(*parameters).errorCorrectionRate = 3;
	(*parameters).markerBorderBits = 3;
	(*parameters).maxErroneousBitsInBorderRate = 3;
	(*parameters).maxMarkerPerimeterRate = 3;
	(*parameters).minCornerDistanceRate = 3;
	(*parameters).minDistanceToBorder = 3;
	(*parameters).minMarkerDistanceRate = 3;
	(*parameters).minMarkerPerimeterRate = 3;
	(*parameters).minOtsuStdDev = 3;
	(*parameters).perspectiveRemoveIgnoredMarginPerCell = 3;
	(*parameters).perspectiveRemovePixelPerCell = 3;
	(*parameters).polygonalApproxAccuracyRate = 3;


	*/
	//这里就不用直接给值的额，把待检测图片设为形参就行
	//char filename_temp[100];
	//sprintf(filename_temp, "D:/aruco标定图片序列/5.jpg");

	cv::Mat image = imread(filename_temp);
	Mat IMG_COPY;
	image.copyTo(IMG_COPY);

	cv::aruco::detectMarkers(image, dictionary, corners, ids, parameters, rejected);//需要调整参数
	// if at least one marker detected
	if (ids.size() > 0)


		cv::aruco::drawDetectedMarkers(IMG_COPY, corners, ids);
	cv::aruco::estimatePoseSingleMarkers(corners, 0.02f, cameraMatrix, distCoeffs, rvecs_ARUCO, tvecs_ARUCO);
	//检测pose, R|T表示从mark到cam的RT。The marker corrdinate system is centered on the middle of the marker, with the Z axis perpendicular to the marker plane.
	//The coordinates of the four corners of the marker in its own coordinate system are :	(-markerLength / 2, markerLength / 2, 0), (markerLength / 2, markerLength / 2, 0),	(markerLength / 2, -markerLength / 2, 0), (-markerLength / 2, -markerLength / 2, 0)

	for (int i = 0; i < ids.size(); i++)
	{
		cv::aruco::drawAxis(IMG_COPY, cameraMatrix, distCoeffs, rvecs_ARUCO[i], tvecs_ARUCO[i], 0.02f);

	}
	namedWindow("detect", WINDOW_AUTOSIZE);
	cv::imshow("detect", IMG_COPY);


}
void ARUCO_FINDER::use_Charuco_calibrate(cv::Ptr<aruco::CharucoBoard> Charucoboard)
{

	Ptr<aruco::DetectorParameters> detectorParams = aruco::DetectorParameters::create();
	Ptr<aruco::Board> board = Charucoboard.staticCast<aruco::Board>();




	vector< vector< vector< Point2f > > >allCorners;
	vector< vector< int > >  allIds;
	vector< Mat > allImgs;
	Size imgSize;

	//检测marker并记入上面几个vector中

	std::vector<int> markerIds;
	std::vector<std::vector<cv::Point2f>> markerCorners, rejected;

	char filename[1000];//读取多个view的图片，依次检测markers，将id和corner再压入另外总的vector
	char pic_num[100];
	for (int j = 1; j < 16; j++)
	{
		sprintf(filename, FLIEPATH_wait2charuco_cablication_jubu, j);

		cv::Mat inputImage = imread(filename);
		//imshow("lalala", inputImage);
		cv::aruco::detectMarkers(inputImage, dictionary, markerCorners, markerIds, detectorParams);
		//aruco::refineDetectedMarkers(inputImage, board, markerCorners, markerIds, rejected);

		Mat currentCharucoCorners, currentCharucoIds;//插入角点




		if (markerIds.size() > 0) {

			cv::aruco::interpolateCornersCharuco(markerCorners, markerIds, inputImage, Charucoboard, currentCharucoCorners, currentCharucoIds);
		}
		Mat result_img;
		inputImage.copyTo(result_img);
		if (markerIds.size() > 0) aruco::drawDetectedMarkers(result_img, markerCorners);
		if (currentCharucoCorners.total() > 0)
			aruco::drawDetectedCornersCharuco(result_img, currentCharucoCorners, currentCharucoIds);
		putText(result_img, "result~", Point(10, 20), FONT_HERSHEY_SIMPLEX, 0.5, Scalar(255, 0, 0), 2);
		sprintf(pic_num, "第%d标定图", j);
		namedWindow(pic_num, WINDOW_NORMAL);
		imshow(pic_num, result_img);

		allCorners.push_back(markerCorners);
		allIds.push_back(markerIds);
		allImgs.push_back(inputImage);
		imgSize = inputImage.size();
	}
	// Detect aruco board from several viewpoints and fill allCornersConcatenated, allIdsConcatenated and markerCounterPerFrame
	// After capturing in several viewpoints, start calibration



	int calibrationFlags = 0;
	float aspectRatio = 1;


	// prepare data for charuco calibration
	int nFrames = (int)allCorners.size();
	vector< Mat > allCharucoCorners;
	vector< Mat > allCharucoIds;
	vector< Mat > filteredImages;
	allCharucoCorners.reserve(nFrames);
	allCharucoIds.reserve(nFrames);

	for (int i = 0; i < nFrames; i++) {
		// interpolate using camera parameters
		Mat currentCharucoCorners, currentCharucoIds;
		aruco::interpolateCornersCharuco(allCorners[i], allIds[i], allImgs[i], Charucoboard,
			currentCharucoCorners, currentCharucoIds, cameraMatrix,
			distCoeffs);

		allCharucoCorners.push_back(currentCharucoCorners);
		allCharucoIds.push_back(currentCharucoIds);
		filteredImages.push_back(allImgs[i]);
	}



	// calibrate camera using charuco
	double repError =
		aruco::calibrateCameraCharuco(allCharucoCorners, allCharucoIds, Charucoboard, imgSize,
		cameraMatrix, distCoeffs, rvecs_CH, tvecs_CH, calibrationFlags);
	cout << "M = " << endl << " " << cameraMatrix << endl << endl;
	cout << "Dis = " << endl << " " << distCoeffs << endl << endl;



	// show interpolated charuco corners for debugging
	if (true) {
		for (unsigned int frame = 0; frame < filteredImages.size(); frame++) {
			Mat imageCopy = filteredImages[frame].clone();
			if (allIds[frame].size() > 0) {

				if (allCharucoCorners[frame].total() > 0) {
					aruco::drawDetectedCornersCharuco(imageCopy, allCharucoCorners[frame],
						allCharucoIds[frame]);
				}
			}
			namedWindow("out~", WINDOW_NORMAL);
			imshow("out~", imageCopy);

		}
	}



}
void ARUCO_FINDER::print_Charuco()
{

	cv::Ptr<cv::aruco::CharucoBoard> board = cv::aruco::CharucoBoard::create(5, 7, 0.04f, 0.02f, dictionary);
	cv::Mat boardImage;
	board->draw(cv::Size(5000, 7000), boardImage, 10, 1);
	char* print_path = new char[120];
	sprintf(print_path, FLIEPATH_PRINTCHARUCO);
	imwrite(print_path, boardImage);
	delete[] print_path;

}
void ARUCO_FINDER::print_aruco()
{

	//循环打印arcuo单个标志的

	Mat Printimage;
	vector<int>png_param;
	png_param.push_back(IMWRITE_PNG_COMPRESSION);
	png_param.push_back(9);
	int filenum = 0;
	for (int i = 0; i < 15; i++)
	{
		char* print_path = new char[120];
		sprintf(print_path, FLIEPATH_PRINTARUCO, filenum);
		cv::aruco::drawMarker(dictionary, i, 200, Printimage, 1);
		imwrite(print_path, Printimage, png_param);
		Printimage_all.push_back(Printimage);
		delete[] print_path;
		filenum++;
	}














}