working rectification in calibration visualizatio

applications/gui2/src/modules/calibration/calibration.hpp

@@ -323,6 +323,7 @@ public:
 		FIX_PRINCIPAL_POINT = 16,
 		FIX_DISTORTION = 32,
 		LOSS_CAUCHY = 64,
+		NONMONOTONIC_STEP = 128,
 	};
 
 	void setFlags(int flags);

applications/gui2/src/modules/calibration/extrinsic.cpp

@@ -94,8 +94,15 @@ void ExtrinsicCalibration::start(unsigned int fsid, std::vector<FrameID> sources
 		running_ = false;
 		filter->remove();
 		if (fs_current_ == nullptr) { return; }
+
+		// change mode only once per frame (cameras contain same frame twice)
+		std::unordered_set<uint32_t> fids;
 		for (const auto camera : state_.cameras) {
-			setCalibrationMode((*fs_current_)[camera.id.source()], true);
+			fids.insert(camera.id.source());
+		}
+
+		for (const auto i : fids) {
+			setCalibrationMode((*fs_current_)[i], true);
 		}
 	});
 	state_.capture = true;
@@ -272,7 +279,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
 	CHECK(l.intrinsic.resolution == r.intrinsic.resolution);
 
 	auto size = l.intrinsic.resolution;
-	cv::Mat T = l.extrinsic.matrix() * inverse(r.extrinsic.matrix());
+	cv::Mat T = r.extrinsic.matrix() * inverse(l.extrinsic.matrix());
 	cv::Mat R, t, R1, R2, P1, P2, Q, map1, map2;
 
 	getRotationAndTranslation(T, R, t);
@@ -280,7 +287,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
 	cv::stereoRectify(
 		l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
 		r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(), size,
-		R, t, R1, R2, P1, P2, Q, 0, 1.0);
+		R, t, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1.0);
 
 	cv::initUndistortRectifyMap(l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
 		R1, P1, size, CV_32FC1, map1, map2);
@@ -288,7 +295,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
 	state_.maps2[cl].upload(map2);
 
 	cv::initUndistortRectifyMap(r.intrinsic.matrix(), r.intrinsic.distCoeffs.Mat(),
-		R1, P1, size, CV_32FC1, map1, map2);
+		R2, P2, size, CV_32FC1, map1, map2);
 	state_.maps1[cr].upload(map1);
 	state_.maps2[cr].upload(map2);
 }
@@ -298,14 +305,6 @@ void ExtrinsicCalibration::run() {
 
 	future_ = ftl::pool.push([this](int id) {
 		try {
-			for (int c = 0; c < cameraCount(); c += 2) {
-				auto t1 = state_.calib.calibration(c).extrinsic.tvec;
-				auto t2 = state_.calib.calibration(c + 1).extrinsic.tvec;
-
-				LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
-						  << cv::norm(t1, t2);
-			}
-
 			auto opt = state_.calib.options();
 			opt.optimize_intrinsic = !(state_.flags & Flags::FIX_INTRINSIC);
 			opt.rational_model = state_.flags & Flags::RATIONAL_MODEL;
@@ -316,15 +315,11 @@ void ExtrinsicCalibration::run() {
 			opt.loss = (state_.flags & Flags::LOSS_CAUCHY) ?
 				ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY :
 				ftl::calibration::BundleAdjustment::Options::Loss::SQUARED;
+			opt.use_nonmonotonic_steps = state_.flags & Flags::NONMONOTONIC_STEP;
 
 			state_.calib.setOptions(opt);
 			state_.calib.run();
 
-			for (int c = 0; c < cameraCount(); c += 2) {
-				auto t1 = state_.calib.calibrationOptimized(c).extrinsic.tvec;
-				auto t2 = state_.calib.calibrationOptimized(c + 1).extrinsic.tvec;
-			}
-
 			// Rectification maps for visualization; stereo cameras assumed
 			// if non-stereo cameras added visualization/grouping (by index)
 			// has to be different.
@@ -336,6 +331,9 @@ void ExtrinsicCalibration::run() {
 				auto l = state_.calib.calibrationOptimized(c);
 				auto r = state_.calib.calibrationOptimized(c + 1);
 				stereoRectify(c, c + 1, l, r);
+
+				LOG(INFO) << "baseline (" << c << ", " << c + 1 << "): "
+						  << cv::norm(l.extrinsic.tvec, r.extrinsic.tvec);
 			}
 		}
 		catch (ftl::exception &ex) {

applications/gui2/src/views/calibration/extrinsicview.cpp

@@ -303,9 +303,19 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
 		else	{ flags &= ~ExtrinsicCalibration::Flags::LOSS_CAUCHY; }
 	});
 
+	auto* nstep = new nanogui::CheckBox(wfreeze, "Non-monotonic step");
+	nstep->setChecked(flags_ & ExtrinsicCalibration::Flags::NONMONOTONIC_STEP);
+	nstep->setCallback([&flags = flags_](bool v) {
+		if (v)	{ flags |= ExtrinsicCalibration::Flags::NONMONOTONIC_STEP; }
+		else	{ flags &= ~ExtrinsicCalibration::Flags::NONMONOTONIC_STEP; }
+	});
+
 	auto* fall = new nanogui::CheckBox(wfreeze, "Freeze all intrinsic paramters");
 	fall->setChecked(flags_ & ExtrinsicCalibration::Flags::FIX_INTRINSIC);
-	fall->setCallback([&flags = flags_](bool v) {
+	fall->setCallback([&flags = flags_, wfreeze](bool v) {
+		for (int i = 3; i < wfreeze->childCount(); i++) {
+			wfreeze->childAt(i)->setEnabled(!v);
+		}
 		if (v)	{ flags |= ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
 		else	{ flags &= ~ExtrinsicCalibration::Flags::FIX_INTRINSIC; }
 	});
@@ -338,10 +348,11 @@ void ExtrinsicCalibrationView::CalibrationWindow::build() {
 		else	{ flags &= ~ExtrinsicCalibration::Flags::ZERO_DISTORTION; }
 	});
 
-	fall->setCallback([wfreeze](bool value){
-		for (int i = 2; i < wfreeze->childCount(); i++) {
-			wfreeze->childAt(i)->setEnabled(!value);
-		}
+	auto* rdist = new nanogui::CheckBox(wfreeze, "Rational distortion model");
+	rdist->setChecked(flags_ & ExtrinsicCalibration::Flags::RATIONAL_MODEL);
+	rdist->setCallback([&flags = flags_](bool v) {
+		if (v)	{ flags |= ExtrinsicCalibration::Flags::RATIONAL_MODEL; }
+		else	{ flags &= ~ExtrinsicCalibration::Flags::RATIONAL_MODEL; }
 	});
 
 	/* Needs thinking: visualize visibility graph? Use earlier alignment (if

components/calibration/include/ftl/calibration/optimize.hpp

@@ -129,6 +129,8 @@ public:
 
 		Loss loss = Loss::SQUARED;
 
+		bool use_nonmonotonic_steps = false;
+
 		// fix_camera_extrinsic and fix_camera_intrinsic overlap with some of
 		// the generic options. The more generic setting is always used, the
 		// specific extrinsic/intrinsic options are applied on top of those.
@@ -150,7 +152,7 @@ public:
 		bool fix_distortion = true;
 		/// use distortion coefficients k4, k5, and k6; if false, set to zero
 		bool rational_model = true;
-		/// assume zero distortion during optimization
+		/// distortion set to zero
 		bool zero_distortion = false;
 
 		bool optimize_intrinsic = true;
@@ -217,7 +219,6 @@ protected:
 
 	void _buildProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
 	void _buildBundleAdjustmentProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
-	void _buildLengthProblem(ceres::Problem& problem, const BundleAdjustment::Options& options);
 
 private:
 	// point to be optimized and corresponding observations

components/calibration/src/extrinsic.cpp

@@ -613,7 +613,7 @@ double ExtrinsicCalibration::optimize() {
 				p = {px[m], py[m], pz[m]};
 			}
 
-			// TODO: desgin more meaningful check
+			// TODO: desgin better check
 			if (cv::norm(absdiff(px, py, pz)) > threshold_bad_) {
 				n_points_bad++;
 				continue;
@@ -641,6 +641,8 @@ double ExtrinsicCalibration::optimize() {
 
 	updateStatus_("Bundle adjustment");
 	options_.verbose = true;
+	options_.max_iter = 250; // should converge much earlier
+
 	LOG(INFO) << "fix intrinsics: " << (options_.optimize_intrinsic ? "no" : "yes");
 	LOG(INFO) << "fix focal: " << (options_.fix_focal ? "yes" : "no");
 	LOG(INFO) << "fix principal point: " << (options_.fix_principal_point ? "yes" : "no");
@@ -661,6 +663,7 @@ double ExtrinsicCalibration::optimize() {
 	}
 
 	rmse_total_ = ba.reprojectionError();
+	LOG(INFO) << "reprojection error (all cameras): " << rmse_total_;
 	return rmse_total_;
 }
 

components/calibration/src/optimize.cpp

@@ -154,6 +154,7 @@ Mat Camera::distortionCoefficients() const {
 
 Mat Camera::rvec() const {
 	cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
+	CHECK(rvec.step1() == 3);
 	ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
 		(double*)(rvec.data));
 	return rvec;
@@ -164,8 +165,11 @@ Mat Camera::tvec() const {
 }
 
 Mat Camera::rmat() const {
-	Mat R;
-	cv::Rodrigues(rvec(), R);
+	cv::Mat R(cv::Size(3, 3), CV_64FC1);
+	CHECK(R.step1() == 3);
+	ceres::QuaternionToRotation<double>(data + Parameter::ROTATION,
+		ceres::RowMajorAdapter3x3<double>((double*)(R.data)));
+
 	return R;
 }
 
@@ -213,8 +217,8 @@ struct ReprojectionError {
 		p[1] += camera[Camera::Parameter::TY];
 		p[2] += camera[Camera::Parameter::TZ];
 
-		T x = T(p[0]) / p[2];
-		T y = T(p[1]) / p[2];
+		T x = p[0] / p[2];
+		T y = p[1] / p[2];
 
 		// Intrinsic parameters
 		const T& f = camera[Camera::Parameter::F];
@@ -605,6 +609,8 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
 		options.num_threads = bundle_adjustment_options.num_threads;
 	}
 
+	options.use_nonmonotonic_steps = bundle_adjustment_options.use_nonmonotonic_steps;
+
 	ceres::Solver::Summary summary;
 	ceres::Solve(options, &problem, &summary);
 

components/calibration/src/structures.cpp

@@ -146,7 +146,7 @@ CalibrationData::Extrinsic::Extrinsic(cv::InputArray R, cv::InputArray t) {
 }
 
 cv::Mat CalibrationData::Extrinsic::matrix() const {
-	cv::Mat T(cv::Size(4, 4), CV_64FC1, cv::Scalar(0));
+	cv::Mat T(cv::Size(4, 4), CV_64FC1, cv::Scalar(0.0));
 	cv::Rodrigues(rvec, T(cv::Rect(0, 0, 3, 3)));
 	T.at<double>(0, 3) = tvec[0];
 	T.at<double>(1, 3) = tvec[1];
@@ -160,7 +160,7 @@ cv::Mat CalibrationData::Extrinsic::matrix() const {
 }
 
 cv::Mat CalibrationData::Extrinsic::rmat() const {
-	cv::Mat R(cv::Size(3, 3), CV_64FC1, cv::Scalar(0));
+	cv::Mat R(cv::Size(3, 3), CV_64FC1, cv::Scalar(0.0));
 	cv::Rodrigues(rvec, R);
 	return R;
 }