scaling missing in calibration

applications/calibration-ceres/src/calibration.cpp

@@ -110,6 +110,8 @@ double ftl::calibration::computeExtrinsicParameters(const Mat &K1, const Mat &D1
 	for (size_t i = 0; i < points_out.size(); i++) {
 		ba.addPoint({points1[i], points2[i]}, points_out[i]);
 	}
+	
+	ba.addObject(object_points);
 
 	double error_before = ba.reprojectionError();
 

applications/calibration-multi/src/main.cpp

@@ -5,6 +5,8 @@
 #include <ftl/rgbd/source.hpp>
 #include <ftl/rgbd/group.hpp>
 
+#include <ftl/calibration.hpp>
+
 #include <ftl/master.hpp>
 #include <ftl/streams/receiver.hpp>
 #include <ftl/streams/netstream.hpp>
@@ -231,14 +233,18 @@ void calibrateRPC(	ftl::net::Universe* net,
 		auto *nstream = nstreams[c/2];
 		while(true) {
 			try {
-				if (params.optimize_intrinsic) {
+				if (params.save_intrinsic && params.optimize_intrinsic) {
 					// never update distortion during extrinsic calibration
 					setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {Mat(0, 0, CV_64FC1), Mat(0, 0, CV_64FC1)});
 				}
-				setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
-				setPoseRPC(net, nstream, getMat4x4(R[c], t[c]));
-				saveCalibrationRPC(net, nstream);
-				LOG(INFO) << "CALIBRATION SENT";
+				if (params.save_extrinsic) {
+					setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
+					setPoseRPC(net, nstream, getMat4x4(R[c], t[c]));
+				}
+				if (params.save_intrinsic || params.save_extrinsic) {
+					saveCalibrationRPC(net, nstream);
+					LOG(INFO) << "CALIBRATION SENT";
+				}
 				break;
 
 			} catch (std::exception &ex) {
@@ -382,8 +388,11 @@ void runCameraCalibration(	ftl::Configurable* root,
 				cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
 				cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
 				
-				camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera());
-				camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera());
+				camera_parameters[2*idx] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()),
+					rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
+				camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
+					rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
+
 				if (res.empty()) res = rgb_new[2*idx].size();
 			}
 		}
@@ -667,10 +676,13 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 
 				cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
 				cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
+				/*
+				camera_parameters[2*idx] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getLeftCamera()),
+					rgb_new[2*idx].size(), Size(fs.frames[i].getLeftCamera().width, fs.frames[i].getLeftCamera().height));
+				camera_parameters[2*idx+1] = ftl::calibration::scaleCameraMatrix(createCameraMatrix(fs.frames[i].getRightCamera()),
+					rgb_new[2*idx].size(), Size(fs.frames[i].getRightCamera().width, fs.frames[i].getRightCamera().height));
 				
-				camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera());
-				camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera());
-				if (res.empty()) res = rgb_new[2*idx].size();
+				if (res.empty()) res = rgb_new[2*idx].size();*/
 			}
 		}
 		catch (std::exception ex) {
@@ -687,14 +699,6 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 
 	stream->begin();
 	ftl::timer::start(false);
-	
-	while(true) {
-		if (!res.empty()) {
-			LOG(INFO) << "Camera resolution: " << params.size;
-			break;
-		}
-		std::this_thread::sleep_for(std::chrono::seconds(1));
-	}
 
 	for (auto *nstream: nstreams) {
 		bool res = true;
@@ -724,7 +728,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 	std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
 	calib.loadInput(path + filename, params.idx_cameras);
 
-	for (size_t i = 0; i < nstreams.size(); i++) {
+	/*for (size_t i = 0; i < nstreams.size(); i++) {
 		while(true) {
 			try {
 				if (camera_parameters[2*i].empty() || camera_parameters[2*i+1].empty()) {
@@ -742,7 +746,7 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 			}
 			catch (...) {}
 		}
-	}
+	}*/
 
 
 	Mat out;

applications/calibration-multi/src/multicalibrate.cpp

@@ -700,7 +700,6 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
 		calculateInverse(R_[c_from], t_[c_from], R, t);
 		LOG(INFO)	<< "Error before BA, cameras " << reference_camera_ << " and " << c_from << ": "
 					<< getReprojectionErrorOptimized(c_from, K_[c_from], R, t);
-	
 	}
 
 	double err;
@@ -739,13 +738,17 @@ double MultiCameraCalibrationNew::calibrateAll(int reference_camera) {
 			ba.addPoint(visible, points2d, p);
 		}
 
+		ba.addObject(object_points_);
+
 		ftl::calibration::BundleAdjustment::Options options;
+		options.loss = ftl::calibration::BundleAdjustment::Options::Loss::CAUCHY;
 		options.optimize_intrinsic = !fix_intrinsics_;
 		options.fix_distortion = true;
 		options.max_iter = 50;
 		options.fix_camera_extrinsic = {reference_camera};
 		options.verbose = true;
- 
+		options.max_iter = 500;
+		
 		err = ba.reprojectionError();
 		ba.run(options);
 

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

@@ -108,8 +108,7 @@ public:
 	void addPoint(const std::vector<cv::Point2d>& observations, cv::Point3d& point);
 	void addPoints(const std::vector<std::vector<cv::Point2d>>& observations, std::vector<cv::Point3d>& points);
 
-	void addConstraintPlane(int group_size);
-	void addConstraintObject(const std::vector<cv::Point3d>& object_points);
+	void addObject(const std::vector<cv::Point3d>& object_points);
 
 	/** @brief Perform bundle adjustment with custom options.
 	 */
@@ -119,11 +118,11 @@ public:
 	 */
 	void run();
 
-	/** @brief Calculate RMS error (for one camera)
+	/** @brief Calculate MSE error (for one camera)
 	 */
 	double reprojectionError(const int camera) const;
 
-	/** @brief Calculate RMS error for all cameras
+	/** @brief Calculate MSE error for all cameras
 	 */
 	double reprojectionError() const;
 
@@ -134,6 +133,11 @@ protected:
 	 */
 	void _reprojectionErrorMSE(const int camera, double &error, double &npoints) const;
 
+	/** @brief Set camera parametrization (fixed parameters/cameras)
+	 */
+	void _setCameraParametrization(ceres::Problem& problem, const BundleAdjustment::Options& options);
+	void _setStructureParametrization(ceres::Problem& problem, const BundleAdjustment::Options& options);
+
 	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);
@@ -150,24 +154,17 @@ private:
 		double* point;
 	};
 
-	// object shape based constraint for group of points from idx_start to idx_end
-	struct ConstraintObject {
+	// group of points with known structure; from idx_start to idx_end
+	struct Object {
 		int idx_start;
 		int idx_end;
 		std::vector<cv::Point3d> object_points;
 	};
 
-	// planar constraint for group of points from idx_start to idx_end
-	struct ConstraintPlane {
-		int idx_start;
-		int idx_end;
-		int group_size;
-	};
-
 	// camera paramters (as pointers)
 	std::vector<Camera*> cameras_;
 	std::vector<BundleAdjustment::Point> points_;
-	std::vector<BundleAdjustment::ConstraintObject> constraints_object_;
+	std::vector<BundleAdjustment::Object> objects_;
 };
 
 #endif

components/calibration/src/optimize.cpp

@@ -237,37 +237,13 @@ void BundleAdjustment::addPoints(const vector<vector<Point2d>>& observations, st
 	}
 }
 
-void BundleAdjustment::addConstraintObject(const vector<Point3d> &object_points) {
+void BundleAdjustment::addObject(const vector<Point3d> &object_points) {
 	if (points_.size() % object_points.size() != 0) { throw std::exception(); }
-	constraints_object_.push_back(BundleAdjustment::ConstraintObject {0, (int) points_.size(), object_points});
+	objects_.push_back(BundleAdjustment::Object {0, (int) points_.size(), object_points});
 }
 
-void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
-
-	ceres::LossFunction *loss_function = nullptr;
-
-	if (options.loss == Options::Loss::HUBER) {
-		loss_function = new ceres::HuberLoss(1.0);
-	}
-	else if (options.loss == Options::Loss::CAUCHY) {
-		loss_function = new ceres::CauchyLoss(1.0);
-	}
-
-	for (auto point : points_) {
-		for (size_t i = 0; i < point.observations.size(); i++) {
-			if (!point.visibility[i]) { continue; }
-			ceres::CostFunction* cost_function =
-				ReprojectionError::Create(point.observations[i]);
-
-			problem.AddResidualBlock(cost_function,
-						loss_function,
-						getCameraPtr(i),
-						point.point);
-		}
-	}
-
-	// apply options
-
+void BundleAdjustment::_setCameraParametrization(ceres::Problem &problem, const BundleAdjustment::Options &options) {
+	
 	vector<int> constant_camera_parameters;
 
 	// extrinsic paramters
@@ -336,6 +312,7 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
 			if (params.size() == Camera::n_parameters) {
 				// Ceres crashes if all parameters are set constant using
 				// SubsetParameterization()
+				// https://github.com/ceres-solver/ceres-solver/issues/347
 				problem.SetParameterBlockConstant(getCameraPtr(i));
 			}
 			else if (params.size() > 0) {
@@ -344,6 +321,35 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
 			}
 		}
 	}
+}
+
+void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
+
+	ceres::LossFunction *loss_function = nullptr;
+
+	if (options.loss == Options::Loss::HUBER) {
+		loss_function = new ceres::HuberLoss(1.0);
+	}
+	else if (options.loss == Options::Loss::CAUCHY) {
+		loss_function = new ceres::CauchyLoss(1.0);
+	}
+
+	for (auto point : points_) {
+		for (size_t i = 0; i < point.observations.size(); i++) {
+			if (!point.visibility[i]) { continue; }
+			ceres::CostFunction* cost_function =
+				ReprojectionError::Create(point.observations[i]);
+
+			problem.AddResidualBlock(cost_function,
+						loss_function,
+						getCameraPtr(i),
+						point.point);
+		}
+	}
+
+	// apply options
+
+	_setCameraParametrization(problem, options);
 
 	if (!options.optmize_structure) {
 		// do not optimize points
@@ -353,9 +359,23 @@ void BundleAdjustment::_buildBundleAdjustmentProblem(ceres::Problem &problem, co
 
 void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const BundleAdjustment::Options &options) {
 
-	for (auto &constraint : constraints_object_) {
-		int npoints = constraint.object_points.size();
-		auto &object_points = constraint.object_points;
+	// same idea as in scale optimization
+
+	ceres::LossFunction *loss_function = nullptr;
+
+	// should use separate configuration option
+	/*
+	if (options.loss == Options::Loss::HUBER) {
+		loss_function = new ceres::HuberLoss(1.0);
+	}
+	else if (options.loss == Options::Loss::CAUCHY) {
+		loss_function = new ceres::CauchyLoss(1.0);
+	}
+	*/
+
+	for (auto &object : objects_) {
+		int npoints = object.object_points.size();
+		auto &object_points = object.object_points;
 
 		vector<double> d;
 		for (int i = 0; i < npoints; i++) {
@@ -364,7 +384,7 @@ void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const Bundle
 			}
 		}
 
-		for (int p = constraint.idx_start; p < constraint.idx_end; p += npoints) {
+		for (int p = object.idx_start; p < object.idx_end; p += npoints) {
 			size_t i_d = 0;
 			for (size_t i = 0; i < object_points.size(); i++) {
 				for (size_t j = i + 1; j < object_points.size(); j++) {
@@ -373,7 +393,7 @@ void BundleAdjustment::_buildLengthProblem(ceres::Problem &problem, const Bundle
 
 					auto cost_function = LengthError::Create(d[i_d++]);
 
-					problem.AddResidualBlock(cost_function, nullptr, p1, p2);
+					problem.AddResidualBlock(cost_function, loss_function, p1, p2);
 				}
 			}
 		}
@@ -442,7 +462,7 @@ void BundleAdjustment::_reprojectionErrorMSE(const int camera, double &error, do
 double BundleAdjustment::reprojectionError(const int camera) const {
 	double error, ncameras;
 	_reprojectionErrorMSE(camera, ncameras, error);
-	return sqrt(error);
+	return error / ncameras;
 }
 
 double BundleAdjustment::reprojectionError() const {
@@ -454,5 +474,5 @@ double BundleAdjustment::reprojectionError() const {
 		error += e * n;
 		npoints += n;
 	}
-	return sqrt(error / npoints);
+	return error / npoints;
 }