aruco_detect

// aruco_detect.cpp : Defines the entry point for the console application.
//
#include "aruco.h"
#include <iostream>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/imgproc.hpp>
#include <string>
#include <stdexcept>

using namespace cv;
using namespace std;
using namespace aruco;
// class for parsing command line
// operator [](string cmd) return  whether cmd is present //string operator ()(string cmd) return the value as a string:
// -cmd value
class CmdLineParser { int argc; char** argv; public:CmdLineParser(int _argc, char** _argv) : argc(_argc), argv(_argv) {}   bool operator[](string param) { int idx = -1;  for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param)idx = i; return (idx != -1); }    string operator()(string param, string defvalue = "-1") { int idx = -1; for (int i = 0; i < argc && idx == -1; i++)if (string(argv[i]) == param)idx = i; if (idx == -1)return defvalue; else return (argv[idx + 1]); } };

cv::Mat __resize(const cv::Mat& in, int width) {
	if (in.size().width <= width)
		return in;
	float yf = float(width) / float(in.size().width);
	cv::Mat im2;
	cv::resize(in, im2, cv::Size(width, static_cast<int>(in.size().height * yf)));
	return im2;
}

struct Ret {
	bool ok;
	Mat img;
	vector<Marker> markers;
	Mat r, t;
};

Ret loadAndRectify(CameraParameters &camParam, MarkerMap &theMarkerMapConfig,string file) {
	Ret ret;
	Mat src = imread(file, 0);
	Size newSize;
	newSize.width = src.size[0];
	newSize.height = src.size[1];
	Rect roi;
	Mat optMat = cv::getOptimalNewCameraMatrix(camParam.CameraMatrix, camParam.Distorsion, newSize, 1, newSize, &roi);
	Mat dst;
	cout << "undistort..." << endl;
	undistort(src, dst, optMat, camParam.Distorsion);
	cout << "detecting..." << endl;
	MarkerDetector MDetector;
	MDetector.setDictionary(theMarkerMapConfig.getDictionary());
	ret.markers = MDetector.detect(dst);
	for (unsigned int i = 0; i < ret.markers.size(); i++) {
		cout << ret.markers[i] << endl;
		ret.markers[i].draw(dst, Scalar(0, 0, 255), 2);
	}
	if (camParam.isValid())
		for (unsigned int i = 0; i < ret.markers.size(); i++) {
			CvDrawingUtils::draw3dCube(dst, ret.markers[i], camParam);
		}
	ret.ok = false;
	if (theMarkerMapConfig.isExpressedInMeters() && camParam.isValid()) {
		MarkerMapPoseTracker MSPoseTracker;  // tracks the pose of the marker map
		MSPoseTracker.setParams(camParam, theMarkerMapConfig);
		if (MSPoseTracker.estimatePose(ret.markers)) {  // if pose correctly computed, print the reference system
			Mat rvec = MSPoseTracker.getRvec();
			Mat tvec = MSPoseTracker.getTvec();
			cout << "tvec=" << tvec << endl;
			aruco::CvDrawingUtils::draw3dAxis(dst, camParam,rvec , tvec,
				theMarkerMapConfig[0].getMarkerSize() * 2);
			ret.ok = true;
			ret.r = rvec;
			ret.t = tvec;
			ret.img = dst;
		} 
	}
	return ret;
}
static float distancePointLine(const cv::Point2f point, const cv::Vec3f& line) {
	//Line is given as a*x + b*y + c = 0
	return std::fabsf(line(0)*point.x + line(1)*point.y + line(2))
		/ std::sqrt(line(0)*line(0) + line(1)*line(1));
}

static void drawEpipolarLines(const std::string& title, Mat& F,
	const cv::Mat& img1, const cv::Mat& img2,
	const std::vector<Point2f> points1,
	const std::vector<Point2f> points2,
	const float inlierDistance = -1) {
	CV_Assert(img1.size() == img2.size() && img1.type() == img2.type());
	cv::Mat outImg(img1.rows, img1.cols * 2, CV_8UC3);
	cv::Rect rect1(0, 0, img1.cols, img1.rows);
	cv::Rect rect2(img1.cols, 0, img1.cols, img1.rows);
	/*
	* Allow color drawing
	*/
	if (img1.type() == CV_8U) {
		cv::cvtColor(img1, outImg(rect1), CV_GRAY2BGR);
		cv::cvtColor(img2, outImg(rect2), CV_GRAY2BGR);
	} else {
		img1.copyTo(outImg(rect1));
		img2.copyTo(outImg(rect2));
	}
	vector<cv::Vec3f> epilines1, epilines2;
	computeCorrespondEpilines(points1, 1, F, epilines1); //Index starts with 1
	computeCorrespondEpilines(points2, 2, F, epilines2);
	CV_Assert(points1.size() == points2.size() &&
		points2.size() == epilines1.size() &&
		epilines1.size() == epilines2.size());
	cv::RNG rng(0);
	for (size_t i = 0; i<points1.size(); i++) {
		if (inlierDistance > 0) {
			if (distancePointLine(points1[i], epilines2[i]) > inlierDistance ||
				distancePointLine(points2[i], epilines1[i]) > inlierDistance) {
				continue;
			}
		}
		cv::Scalar color(rng(256), rng(256), rng(256));
		cv::line(outImg(rect2),
			cv::Point(0, -epilines1[i][2] / epilines1[i][1]),
			cv::Point(img1.cols, -(epilines1[i][2] + epilines1[i][0] * img1.cols) / epilines1[i][1]),
			color);
		cv::circle(outImg(rect1), points1[i], 3, color, -1, CV_AA);

		cv::line(outImg(rect1),
			cv::Point(0, -epilines2[i][2] / epilines2[i][1]),
			cv::Point(img2.cols, -(epilines2[i][2] + epilines2[i][0] * img2.cols) / epilines2[i][1]),
			color);
		cv::circle(outImg(rect2), points2[i], 3, color, -1, CV_AA);
	}
	imwrite("epipolar.jpg", outImg);
}


int main(int argc, char** argv) {
	try {
		CmdLineParser cml(argc, argv);
		CameraParameters camParam;
		camParam.readFromXMLFile(argv[2]);
		MarkerMap theMarkerMapConfig;  // configuration of the map
		theMarkerMapConfig.readFromFile(argv[3]);
		theMarkerMapConfig = theMarkerMapConfig.convertToMeters(0.038);
		Ret ret1 = loadAndRectify(camParam, theMarkerMapConfig, "shot9.jpg");
		Ret ret2 = loadAndRectify(camParam, theMarkerMapConfig, "shot13.jpg");
		vector<Point2f> pts1, pts2;
		for (Marker m1 : ret1.markers) {
			for (Marker m2 : ret2.markers) {
				if (m1.id == m2.id) {
					for (Point2f p : m1) pts1.push_back(p);
					for (Point2f p : m2) pts2.push_back(p);
					/*					copy(m1.begin(), m1.end(), pts1);
					copy(m2.begin(), m2.end(), pts2);*/
				}
			}
		}
		Mat F= findFundamentalMat(pts1, pts2);
		drawEpipolarLines("999", F, ret1.img, ret2.img, pts1, pts2);

		cout << "F=" << F << endl;
		imwrite("dst9.jpg", ret1.img);
		imwrite("dst13.jpg", ret2.img);
		fgetc(stdin);
	}
	catch (std::exception& ex)
	{
		cout << "Exception :" << ex.what() << endl;
	}
}