stereo_problem.cpp

#include <string>
#include <iostream>
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp"

using namespace cv;
using namespace std;
struct View {
	string name;
	Mat img, t, r, out, F;
	Point3f optCenter;
};
struct Folder {
	Mat cm;
	vector<View> views;
};
Point2f backProject(Point3f&pt, Mat&r, Mat&t, Mat&cm) {
	vector<Point3f> opts;
	vector<Point2f> ppts;
	opts.push_back(pt);
	projectPoints(opts, r, t, cm, Mat(), ppts);
	return ppts[0];
}
View loadView(string folder,string name, Mat &cm) {
	View view;
	string storageName = folder + name + ".pos.xml";
	FileStorage pfs(storageName, FileStorage::READ);
	if (!pfs.state) {
		cerr << storageName + " not found";
		throw storageName + " not found";
	}
	cout << "loading view " << name << endl;
	Mat i1 = imread(folder + name, 1);
	view.name = name;
	view.img = i1;
	pfs["rvec"] >> view.r; pfs["tvec"] >> view.t;
	return view;
}
pair<Mat,Mat> calcR(View&v1, View&v2) {
	Mat R1,R2,R,T;
	Rodrigues(v1.r, R1);
	Rodrigues(v2.r, R2);
	R = R2*R1.inv();
	Mat RT1 = R*v1.t.t();
	T = v2.t.t() - RT1;
	return make_pair(R, T);
}
void handlePair(View&v1, View&v2,Folder&folder) {
	auto &cm = folder.cm;
	auto RT = calcR(v1, v2);
	Mat R1, R2, P1, P2, Q, RT_d;
	RT.first.convertTo(RT_d, CV_64F);
	cout << "R = " << RT_d << endl;
	Mat tt = RT.second;
	tt.convertTo(tt, CV_64F);
	cout << "cm = " << cm << endl;
	cout << "T = " << tt << endl;
	auto imageSize = Size(v1.img.cols, v1.img.rows);
	Rect validROI[2];
	stereoRectify(cm, Mat(), cm, Mat(), imageSize,
		RT_d, tt, R1, R2, P1, P2, Q, CALIB_ZERO_DISPARITY*0,.999, imageSize, &validROI[0], &validROI[1]);
	bool isVerticalStereo = fabs(P2.at<double>(1, 3)) > fabs(P2.at<double>(0, 3));
	Mat rmap[2][2];
	initUndistortRectifyMap(cm, Mat(), R1, P1, imageSize, CV_32FC1, rmap[0][0], rmap[0][1]);
	initUndistortRectifyMap(cm, Mat(), R2, P2, imageSize, CV_32FC1, rmap[1][0], rmap[1][1]);
	Mat canvas;
	double sf = 1;
	int w, h;
	if (!isVerticalStereo) {
		cout << "horisontal stereo" << endl;
		w = cvRound(imageSize.width*sf);
		h = cvRound(imageSize.height*sf);
		canvas.create(h, w * 2, CV_8UC3);
	}
	else {
		cout << "vertical stereo" << endl;
		w = cvRound(imageSize.width*sf);
		h = cvRound(imageSize.height*sf);
		canvas.create(h * 2, w, CV_8UC3);
		return;
	}
	String file;
	Mat rimg1, rimg2;
	remap(v1.img, rimg1, rmap[0][0], rmap[0][1], INTER_LINEAR);
	remap(v2.img, rimg2, rmap[1][0], rmap[1][1], INTER_LINEAR);
	Mat canvasPart = !isVerticalStereo ? canvas(Rect(0, 0, w, h)) : canvas(Rect(0, h, w, h));
	rimg1.copyTo(canvasPart);
	Mat canvasPart2 = !isVerticalStereo ? canvas(Rect(w, 0, w, h)) : canvas(Rect(0, h, w, h));
	rimg2.copyTo(canvasPart2);
	string fname = "pair_" + v1.name + "_" + v2.name + ".png";
	cout << "writing " << fname << endl;
	imwrite(fname, canvas);
}
void drawBorder(Mat&img,View&v,Folder&folder) {
	vector<Point3f> pts;
	vector<Point2f> pts2d;
	float dx = .12;
	float dy = .17;
	pts.push_back(Point3f(0,dy,0));
	pts.push_back(Point3f(0, 0, 0));
	pts.push_back(Point3f(dx, 0, 0));
	pts.push_back(Point3f(dx, dy, 0));
	pts.push_back(Point3f(0, 0, 0));
	for (int i = 0; i < pts.size(); i++) {
		Point2f p2d= backProject(pts[i], v.r, v.t, folder.cm);
		pts2d.push_back(p2d);		
	}
	for (int i = 0; i < pts2d.size();i++) {
		line(img, pts2d[i], pts2d[(i + 1) % 4], Scalar(255, 0, 0));
	}
	circle(img, pts2d[4],3, Scalar(255, 255, 0));
}
int main() {
	Folder folder;
	string name = "C:/Users/steve/Dropbox/Projects/kinnekt/data/car/rect/";
	FileStorage cmfs(name + "/calib.yml", FileStorage::READ);
	cmfs["camera_matrix"] >> folder.cm;
	folder.cm.convertTo(folder.cm, CV_64F);
	folder.views.push_back(loadView(name,"0.jpg",folder.cm));
	folder.views.push_back(loadView(name, "1.jpg", folder.cm));
	folder.views.push_back(loadView(name, "6.jpg", folder.cm));
	folder.views.push_back(loadView(name, "9.jpg", folder.cm));
	for(auto&v:folder.views){
		drawBorder(v.img, v,folder);
		auto saveFile = "border_" + v.name;
		cout << "saving " << saveFile << endl;
		imwrite(saveFile,v.img);
	}
	for (int i = 0; i < folder.views.size(); i++) {
		for (int j = 0; j < i; j++) {
			auto &v1 = folder.views[i];
			auto &v2 = folder.views[j];
			handlePair(v1, v2,folder);
		}
	}
    return 0;
}