Bug/intrinsic scaling

applications/calibration-multi/src/main.cpp

@@ -174,8 +174,8 @@ void visualizeCalibration(	MultiCameraCalibrationNew &calib, Mat &out,
 	vector<int> markers = {cv::MARKER_SQUARE, cv::MARKER_DIAMOND};
 
 	for (size_t c = 0; c < rgb.size(); c++) {
-		cv::rectangle(rgb[c], roi[c], Scalar(24, 224, 24), 2);
 		cv::remap(rgb[c], rgb[c], map1[c], map2[c], cv::INTER_CUBIC);
+		cv::rectangle(rgb[c], roi[c], Scalar(24, 224, 24), 2);
 
 		for (int r = 50; r < rgb[c].rows; r = r+50) {
 			cv::line(rgb[c], cv::Point(0, r), cv::Point(rgb[c].cols-1, r), cv::Scalar(0,0,255), 1);
@@ -241,7 +241,8 @@ void calibrate(	MultiCameraCalibrationNew &calib, vector<string> &uri_cameras,
 			size_t pos1 = uri_cameras[c/2].find("node");
 			size_t pos2 = uri_cameras[c/2].find("#", pos1);
 			node_name = uri_cameras[c/2].substr(pos1, pos2 - pos1);
-		
+			//LOG(INFO) << c << ":" << calib.getCameraMatNormalized(c, 1280, 720);
+			//LOG(INFO) << c + 1 << ":" << calib.getCameraMatNormalized(c + 1, 1280, 720);
 			if (params.save_extrinsic) {
 				saveExtrinsics(params.output_path + node_name + "-extrinsic.yml", R_c1c2, T_c1c2, R1, R2, P1, P2, Q);
 				LOG(INFO) << "Saved: " << params.output_path + node_name + "-extrinsic.yml";
@@ -295,7 +296,7 @@ void calibrateFromPath(	const string &path,
 	cv::FileStorage fs(path + filename, cv::FileStorage::READ);
 	fs["uri"] >> uri_cameras;
 
-	//params.idx_cameras = {6, 7};
+	//params.idx_cameras = {2, 3};//{0, 1, 4, 5, 6, 7, 8, 9, 10, 11};
 	params.idx_cameras.resize(uri_cameras.size() * 2);
 	std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
 

applications/calibration-multi/src/multicalibrate.cpp

@@ -81,16 +81,18 @@ double CalibrationTarget::estimateScale(vector<Point3d> points) {
 }
 
 MultiCameraCalibrationNew::MultiCameraCalibrationNew(
-			size_t n_cameras, size_t reference_camera, CalibrationTarget target, int fix_intrinsics) :
+			size_t n_cameras, size_t reference_camera, Size resolution,
+			CalibrationTarget target, int fix_intrinsics) :
 		
 	target_(target),
 	visibility_graph_(n_cameras),
 	is_calibrated_(false),
 	n_cameras_(n_cameras),
 	reference_camera_(reference_camera),
-	min_visible_points_(25),
+	min_visible_points_(50),
 	fix_intrinsics_(fix_intrinsics == 1 ? 5 : 0),
 
+	resolution_(resolution),
 	K_(n_cameras),
 	dist_coeffs_(n_cameras),
 	R_(n_cameras),
@@ -116,6 +118,23 @@ Mat MultiCameraCalibrationNew::getCameraMat(size_t idx) {
 	return K;
 }
 
+
+Mat MultiCameraCalibrationNew::getCameraMatNormalized(size_t idx, double scale_x, double scale_y)
+{
+	Mat K = getCameraMat(idx);
+	CHECK((scale_x != 0.0 && scale_y != 0.0) || ((scale_x == 0.0) && scale_y == 0.0));
+
+	scale_x = scale_x / (double) resolution_.width;
+	scale_y = scale_y / (double) resolution_.height;
+
+	Mat scale(Size(3, 3), CV_64F, 0.0);
+	scale.at<double>(0, 0) = scale_x;
+	scale.at<double>(1, 1) = scale_y;
+	scale.at<double>(2, 2) = 1.0;
+	
+	return (scale * K);
+}
+
 Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
 	DCHECK(idx < n_cameras_);
 	Mat D;
@@ -149,6 +168,7 @@ void MultiCameraCalibrationNew::addPoints(vector<vector<Point2d>> points, vector
 
 void MultiCameraCalibrationNew::reset() {
 	is_calibrated_ = false;
+	weights_ = vector(n_cameras_, vector(points2d_[0].size(), 0.0));
 	inlier_ = vector(n_cameras_, vector(points2d_[0].size(), 0));
 	points3d_ = vector(n_cameras_, vector(points2d_[0].size(), Point3d()));
 	points3d_optimized_ = vector(points2d_[0].size(), Point3d());
@@ -163,6 +183,7 @@ void MultiCameraCalibrationNew::saveInput(const string &filename) {
 }
 
 void MultiCameraCalibrationNew::saveInput(cv::FileStorage &fs) {
+	fs << "resolution" << resolution_;
 	fs << "K" << K_;
 	fs << "points2d" << points2d_;
 	fs << "visible" << visible_;
@@ -182,6 +203,7 @@ void MultiCameraCalibrationNew::loadInput(const std::string &filename, const vec
 	fs["K"] >> K;
 	fs["points2d"] >> points2d;
 	fs["visible"] >> visible;
+	fs["resolution"] >> resolution_;
 	fs.release();
 	
 	vector<size_t> cameras;
@@ -287,13 +309,13 @@ void MultiCameraCalibrationNew::updatePoints3D(size_t camera, Point3d new_point,
 		// 			instead store all triangulations and handle outliers
 		//			(perhaps inverse variance weighted mean?)
 		
-		if (euclideanDistance(point, new_point) > 1.0) {
+		if (euclideanDistance(point, new_point) > 10.0) {
 			LOG(ERROR) << "bad value (skipping) " << "(" << point << " vs " << new_point << ")";
 			f = -1;
 		}
 		else {
 			point = (point * f + new_point) / (double) (f + 1);
-			f = f + 1;
+			f++;
 		}
 	}
 	else {
@@ -365,8 +387,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
 	// cameras possibly lack line of sight?
 	DCHECK(points1.size() > 8);
 
-	Mat K1 = K_[camera_from];
-	Mat K2 = K_[camera_to];
+	Mat &K1 = K_[camera_from];
+	Mat &K2 = K_[camera_to];
 
 	vector<uchar> inliers;
 	Mat F, E;
@@ -462,12 +484,12 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
 	// Bundle Adjustment
 	// vector<Point3d> points3d_triangulated;
 	// points3d_triangulated.insert(points3d_triangulated.begin(), points3d.begin(), points3d.end());
-	
+	LOG(INFO) << K1;
 	double err;
 	cvsba::Sba sba;
 	{
-		sba.setParams(cvsba::Sba::Params(cvsba::Sba::TYPE::MOTION, 200, 1.0e-30, fix_intrinsics_, 5, false));
-
+		sba.setParams(cvsba::Sba::Params(cvsba::Sba::TYPE::MOTIONSTRUCTURE, 200, 1.0e-30, 5, 5, false));
+		
 		Mat rvec1, rvec2;
 		cv::Rodrigues(R1, rvec1);
 		cv::Rodrigues(R2, rvec2);
@@ -488,16 +510,21 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
 		cv::Rodrigues(r[1], R2);
 		t1 = t[0];
 		t2 = t[1];
-		err = sba.getFinalReprjError();
 
+		// intrinsic parameters should only be optimized at final BA
+		//K1 = K[0];
+		//K2 = K[1];
+
+		err = sba.getFinalReprjError();
 		LOG(INFO) << "SBA reprojection error before BA " << sba.getInitialReprjError();
 		LOG(INFO) << "SBA reprojection error after BA " << err;
 	}
 
 	calculateTransform(R2, t2, R1, t1, rmat, tvec);
-	
+
 	// Store and average 3D points for both cameras (skip garbage)
 	if (err < 10.0) {
+		Mat rmat1, tvec1;
 		updatePoints3D(camera_from, points3d, idx, R1, t1);
 		updatePoints3D(camera_to, points3d, idx, R2, t2);
 	}
@@ -507,11 +534,8 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
 					<< "), not updating points!";
 	}
 
-	// LOG(INFO) << "Final error: " << reprojectionError(points3d, points2, K2, rmat, tvec);
-	//if (reprojectionError(points3d, points2, K2, rmat, tvec) > 10.0) {
-		// TODO: should ignore results
-		// LOG(ERROR) << "pairwise calibration failed! RMS: " << reprojectionError(points3d, points2, K2, rmat, tvec);
-	//};
+	//LOG(INFO) << reprojectionError(points3d, points1, K1, R1, t1);
+	//LOG(INFO) << reprojectionError(points3d, points2, K2, R2, t2);
 
 	return err;
 }
@@ -598,16 +622,26 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
 			);
 			continue;
 		}
-		LOG(INFO) << "Running pairwise calibration for cameras " << c1 << " and " << c2;
+
 		size_t n_visible = getVisiblePointsCount({c1, c2});
-		
-		if (n_visible < min_visible_points_) continue;
+
+		if (n_visible < min_visible_points_) {
+			LOG(INFO)	<< "Not enough (" << min_visible_points_ << ") points  between "
+						<< "cameras " << c1 << " and " << c2 << " (" << n_visible << " points), "
+						<< "skipping";
+			continue;
+		}
+		LOG(INFO)	<< "Running pairwise calibration for cameras "
+					<< c1 << " and " << c2 << "(" << n_visible << " points)";
+
 		if (transformations.find(make_pair(c2, c1)) != transformations.end()) {
 			continue;
 		}
 		Mat R, t, R_i, t_i;
 
-		if (calibratePair(c1, c2, R, t) > 10.0) {
+		// TODO: threshold parameter, 16.0 possibly too high
+
+		if (calibratePair(c1, c2, R, t) > 16.0) {
 			LOG(ERROR)	<< "Pairwise calibration failed, skipping cameras "
 						<< c1 << " and " << c2;
 			continue;

applications/calibration-multi/src/multicalibrate.hpp

@@ -34,7 +34,8 @@ private:
 class MultiCameraCalibrationNew {
 public:
 	MultiCameraCalibrationNew(	size_t n_cameras, size_t reference_camera,
-								CalibrationTarget target, int fix_intrinsics=1);
+								Size resolution, CalibrationTarget target,
+								int fix_intrinsics=1);
 	
 	void setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs);
 	void setCameraParameters(size_t idx, const Mat &K);
@@ -56,6 +57,8 @@ public:
 	void saveInput(const std::string &filename);
 
 	Mat getCameraMat(size_t idx);
+	Mat getCameraMatNormalized(size_t idx, double scale_x = 1.0, double scale_y = 1.0);
+
 	Mat getDistCoeffs(size_t idx);
 
 	double calibrateAll(int reference_camera = -1);
@@ -133,6 +136,7 @@ private:
 	size_t min_visible_points_;
 	int fix_intrinsics_;
 
+	Size resolution_;
 	vector<Mat> K_;
 	vector<Mat> dist_coeffs_;
 	vector<Mat> R_;
@@ -143,6 +147,7 @@ private:
 	vector<vector<Point2d>> points2d_;
 	vector<vector<int>> visible_;
 	vector<vector<int>> inlier_; // "inlier"
+	vector<vector<double>> weights_;
 
 	int fm_method_;
 	double fm_ransac_threshold_;

applications/calibration/src/common.cpp

@@ -89,7 +89,7 @@ bool loadIntrinsics(const string &ifile, vector<Mat> &K1, vector<Mat> &D1) {
 	
 	LOG(INFO) << "Intrinsics from: " << ifile;
 
-	fs["M"] >> K1;
+	fs["K"] >> K1;
 	fs["D"] >> D1;
 
 	return true;

applications/calibration/src/lens.cpp

@@ -33,12 +33,13 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	const Size image_size = Size(	getOptionInt(opt, "width", 1280),
 							getOptionInt(opt, "height", 720));
 	const int n_cameras = getOptionInt(opt, "n_cameras", 2);
-	const int iter = getOptionInt(opt, "iter", 60);
-	const int delay = getOptionInt(opt, "delay", 750);
+	const int iter = getOptionInt(opt, "iter", 40);
+	const int delay = getOptionInt(opt, "delay", 1000);
 	const double aperture_width = getOptionDouble(opt, "aperture_width", 6.2);
 	const double aperture_height = getOptionDouble(opt, "aperture_height", 4.6);
 	const string filename_intrinsics = getOptionString(opt, "profile", FTL_LOCAL_CONFIG_ROOT "/intrinsics.yml");
 	CalibrationChessboard calib(opt);
+	const bool use_guess = getOptionInt(opt, "use_guess", 1);
 
 	LOG(INFO) << "Intrinsic calibration parameters";
 	LOG(INFO) << "         profile: " << filename_intrinsics;
@@ -49,12 +50,25 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 	LOG(INFO) << "           delay: " << delay;
 	LOG(INFO) << "  aperture_width: " << aperture_width;
 	LOG(INFO) << " aperture_height: " << aperture_height;
+	LOG(INFO) << "       use_guess: " << use_guess;
 	LOG(INFO) << "-----------------------------------";
 
-	int calibrate_flags =	cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO |
-							cv::CALIB_FIX_PRINCIPAL_POINT;
+	int calibrate_flags =	cv::CALIB_ZERO_TANGENT_DIST | cv::CALIB_FIX_ASPECT_RATIO;
+	if (use_guess) { calibrate_flags |= cv::CALIB_USE_INTRINSIC_GUESS; }
+	//						cv::CALIB_FIX_PRINCIPAL_POINT;
 	// PARAMETERS
 
+
+	vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
+
+	if (use_guess) {
+		camera_matrix.clear();
+		vector<Mat> tmp;
+		//dist_coeffs.clear();
+		loadIntrinsics(filename_intrinsics, camera_matrix, tmp);
+		CHECK(camera_matrix.size() == n_cameras); // (camera_matrix.size() == dist_coeffs.size())
+	}
+
 	vector<cv::VideoCapture> cameras;
 	cameras.reserve(n_cameras);
 	for (int c = 0; c < n_cameras; c++) { cameras.emplace_back(c); }
@@ -102,8 +116,6 @@ void ftl::calibration::intrinsic(map<string, string> &opt) {
 
 		cv::waitKey(delay);
 	}
-	
-	vector<Mat> camera_matrix(n_cameras), dist_coeffs(n_cameras);
 
 	for (int c = 0; c < n_cameras; c++) {
 		LOG(INFO) << "Calculating intrinsic paramters for camera " << std::to_string(c);

components/rgbd-sources/src/calibrate.cpp

@@ -89,6 +89,8 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 		D[1].copyTo(D2_);
 	}
 
+	fs.release();
+
 	CHECK(M1_.size() == Size(3, 3));
 	CHECK(M2_.size() == Size(3, 3));
 	CHECK(D1_.size() == Size(5, 1));
@@ -116,15 +118,18 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 	fs["P1"] >> P1_;
 	fs["P2"] >> P2_;
 	fs["Q"] >> Q_;
+
+	fs.release();
+
 	img_size_ = img_size;
 
 	// TODO: normalize calibration
-	double scale_x = 1.0 / 1280.0;
-	double scale_y = 1.0 / 720.0;
-
+	double scale_x = ((double) img_size.width) / 1280.0;
+	double scale_y = ((double) img_size.height) / 720.0;
+	
 	Mat scale(cv::Size(3, 3), CV_64F, 0.0);
-	scale.at<double>(0, 0) = (double) img_size.width * scale_x;
-	scale.at<double>(1, 1) = (double) img_size.height * scale_y;
+	scale.at<double>(0, 0) = scale_x;
+	scale.at<double>(1, 1) = scale_y;
 	scale.at<double>(2, 2) = 1.0;
 
 	M1_ = scale * M1_;
@@ -132,9 +137,10 @@ bool Calibrate::_loadCalibration(cv::Size img_size, std::pair<Mat, Mat> &map1, s
 	P1_ = scale * P1_;
 	P2_ = scale * P2_;
 
-	Q_.at<double>(0, 3) = Q_.at<double>(3, 2) * (double) img_size.width * scale_x;
-	Q_.at<double>(1, 3) = Q_.at<double>(3, 2) * (double) img_size.height * scale_y;
-	Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * (double) img_size.width * scale_x;
+	Q_.at<double>(0, 3) = Q_.at<double>(0, 3) * scale_x;
+	Q_.at<double>(1, 3) = Q_.at<double>(1, 3) * scale_y;
+	Q_.at<double>(2, 3) = Q_.at<double>(2, 3) * scale_x; // TODO: scaling?
+	Q_.at<double>(3, 3) = Q_.at<double>(3, 3) * scale_x;
 
 	// cv::cuda::remap() works only with CV_32FC1
 	initUndistortRectifyMap(M1_, D1_, R1_, P1_, img_size_, CV_32FC1, map1.first, map2.first);