wip: more output

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

@@ -287,8 +287,8 @@ void ExtrinsicCalibration::updateCalibration(int c) {
 void ExtrinsicCalibration::stereoRectify(int cl, int cr,
 	const CalibrationData::Calibration& l, const CalibrationData::Calibration& r) {
 
-	CHECK(l.extrinsic.tvec != r.extrinsic.tvec);
-	CHECK(l.intrinsic.resolution == r.intrinsic.resolution);
+	CHECK_NE(l.extrinsic.tvec, r.extrinsic.tvec);
+	CHECK_EQ(l.intrinsic.resolution, r.intrinsic.resolution);
 
 	auto size = l.intrinsic.resolution;
 	cv::Mat T = r.extrinsic.matrix() * inverse(l.extrinsic.matrix());
@@ -308,8 +308,7 @@ void ExtrinsicCalibration::stereoRectify(int cl, int cr,
 	cv::Vec3d rvec2;
 	cv::Rodrigues(R1 * l.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), rvec1);
 	cv::Rodrigues(R2 * r.extrinsic.matrix()(cv::Rect(0, 0, 3, 3)), rvec2);
-	LOG(INFO) << rvec1 << ", " << rvec2;
-	CHECK(cv::norm(rvec1, rvec2) < 0.01);
+	CHECK_LT(cv::norm(rvec1, rvec2), 0.01);
 
 	cv::initUndistortRectifyMap(l.intrinsic.matrix(), l.intrinsic.distCoeffs.Mat(),
 		R1, P1, size, CV_32FC1, map1, map2);

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

 #include "extrinsicview.hpp"
 #include "visualization.hpp"
+#include "widgets.hpp"
+
 #include "../../screen.hpp"
 #include "../../widgets/window.hpp"
 
@@ -10,6 +12,7 @@
 #include <nanogui/checkbox.h>
 #include <nanogui/label.h>
 #include <nanogui/formhelper.h>
+#include <nanogui/tabwidget.h>
 
 using ftl::gui2::ExtrinsicCalibrationStart;
 using ftl::gui2::ExtrinsicCalibrationView;
@@ -404,14 +407,16 @@ class ExtrinsicCalibrationView::ResultsWindow : public FixedWindow {
 public:
 	ResultsWindow(nanogui::Widget* parent, ExtrinsicCalibrationView* view);
 	virtual void draw(NVGcontext* ctx) override;
+	void update();
 
 private:
 	ExtrinsicCalibrationView* view_;
 	ExtrinsicCalibration* ctrl_;
 
-	nanogui::Button* bcalibrate_;
-	nanogui::Button* bpause_;
-	nanogui::Button* bresults_;
+	std::vector<ftl::calibration::CalibrationData::Calibration> calib_;
+	std::vector<std::string> names_;
+
+	nanogui::TabWidget* tabs_;
 
 public:
 	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
@@ -424,16 +429,33 @@ ExtrinsicCalibrationView::ResultsWindow::ResultsWindow(nanogui::Widget* parent,
 		[view = view_]() {
 		view->setMode(Mode::VIDEO);
 	});
-	setSize({300, 300});
+
+	tabs_ = new nanogui::TabWidget(this);
+	tabs_->createTab("Extrinsic");
+
+	setFixedSize({600, 400});
+	tabs_->setFixedSize(fixedSize());
+}
+
+void ExtrinsicCalibrationView::ResultsWindow::ResultsWindow::update() {
+	calib_.resize(ctrl_->cameraCount());
+	while (tabs_->tabCount() > 1) {
+		tabs_->removeTab(tabs_->tabCount() - 1);
+	}
+
+	for (int i = 0; i < ctrl_->cameraCount(); i++) {
+		calib_[i] = ctrl_->calibration(i);
+		auto* tab = tabs_->createTab(std::to_string(i));
+		auto* display = new IntrinsicDetails(tab);
+		display->update(calib_[i].intrinsic);
+	}
 }
 
 void ExtrinsicCalibrationView::ResultsWindow::draw(NVGcontext* ctx) {
 	FixedWindow::draw(ctx);
-	std::vector<ftl::calibration::CalibrationData::Extrinsic> calib(ctrl_->cameraCount());
-	for (int i = 0; i < ctrl_->cameraCount(); i++) {
-		calib[i] = ctrl_->calibration(i).extrinsic;
+	if (tabs_->activeTab() == 0) { // create a widget and move there
+		drawFloorPlan(ctx, tabs_->tab(0), calib_);
 	}
-	drawFloorPlan(ctx, this, calib);
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -606,7 +628,7 @@ void ExtrinsicCalibrationView::setMode(Mode mode) {
 			wcontrol_->setVisible(false);
 			wcalibration_->setVisible(false);
 			wresults_->setVisible(true);
-			//wresults_->update();
+			wresults_->update();
 			break;
 	}
 	screen()->performLayout();

applications/gui2/src/views/calibration/visualization.hpp

@@ -80,7 +80,7 @@ static void drawTriangle(NVGcontext* ctx, const ftl::calibration::CalibrationDat
 }
 
 static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
-		const std::vector<ftl::calibration::CalibrationData::Extrinsic>& calib,
+		const std::vector<ftl::calibration::CalibrationData::Calibration>& calib,
 		const std::vector<std::string>& names = {},
 		int origin=0) {
 
@@ -91,15 +91,16 @@ static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
 	cv::Vec3f center = {0.0f, 0.0f};
 	std::vector<cv::Point2f> points(calib.size());
 	for (unsigned int i = 0; i < points.size(); i++) {
+		const auto& extrinsic = calib[i].extrinsic;
 		// xz, assume floor on y-plane y = 0
-		float x = calib[i].tvec[0];
-		float y = calib[i].tvec[2];
+		float x = extrinsic.tvec[0];
+		float y = extrinsic.tvec[2];
 		points[i] = {x, y};
 		minx = std::min(minx, x);
 		miny = std::min(miny, y);
 		maxx = std::max(maxx, x);
 		maxy = std::max(maxy, y);
-		center += calib[i].tvec;
+		center += extrinsic.tvec;
 	}
 	center /= float(points.size());
 	float w = parent->width();
@@ -112,6 +113,6 @@ static void drawFloorPlan(NVGcontext* ctx, nanogui::Widget* parent,
 	nanogui::Vector2f off{center[0], center[2]};
 
 	for (unsigned int i = 0; i < points.size(); i++) {
-		drawTriangle(ctx, calib[i], apos, off, s, 0.3);
+		drawTriangle(ctx, calib[i].extrinsic, apos, off, s, 0.3);
 	}
 }

applications/gui2/src/views/thumbnails.cpp

@@ -165,7 +165,7 @@ void Thumbnails::updateThumbnails() {
 				continue;
 			}
 
-			auto* tab = tabwidget_->createTab("Frameset " + std::to_string(fsid));
+			auto* tab = tabwidget_->createTab(framesets[fsid]->name());
 			tab->setLayout(new nanogui::BoxLayout
 				(nanogui::Orientation::Vertical, nanogui::Alignment::Middle, 40));
 			auto* panel = new nanogui::Widget(tab);

components/calibration/include/ftl/calibration/extrinsic.hpp

@@ -161,13 +161,16 @@ public:
 	/** add a single (uncalibrated) camera. Returns index of camera. */
 	unsigned int addCamera(const CalibrationData::Intrinsic &);
 
-	/** add a single calibrated camera. Returns index of camera. */
+	/** add a single calibrated camera (if valid calibration). Returns index of camera. */
 	unsigned int addCamera(const CalibrationData::Calibration &);
 
 	/** Add a stereo camera pair. Pairs always use other cameras to estimate
 	 * initial pose. Returns index of first camera. */
 	unsigned int addStereoCamera(const CalibrationData::Intrinsic &, const CalibrationData::Intrinsic &);
 
+	/** Add calibrated stereo camera (if contains valid calibration) */
+	unsigned int addStereoCamera(const CalibrationData::Calibration &, const CalibrationData::Calibration &);
+
 	const CalibrationData::Intrinsic& intrinsic(unsigned int c);
 	const CalibrationData::Extrinsic& extrinsic(unsigned int c);
 	const CalibrationData::Calibration& calibration(unsigned int c);

components/calibration/include/ftl/calibration/structures.hpp

@@ -32,6 +32,11 @@ struct CalibrationData {
 			double& operator[](unsigned i);
 			double operator[](unsigned i) const;
 
+			/** are radial distortion values are for rational model */
+			bool rationalModel() const;
+			/** is thin prism model is used (s1-s4 set) */
+			bool thinPrism() const;
+
 			/**
 			 * Return distortion parameters in cv::Mat. Shares same memory.
 			 */
@@ -51,6 +56,18 @@ struct CalibrationData {
 		/** New instance with scaled values for new resolution */
 		Intrinsic(const Intrinsic& other, cv::Size sz);
 
+		/* valid values (resolution is non-zero) */
+		bool valid() const;
+
+		/** horizontal field of view in degrees */
+		double fovx() const;
+		/** vertical field of view in degrees */
+		double fovy() const;
+		/** focal length in sensor size units */
+		double focal() const;
+		/** aspect ratio: fx/fy */
+		double aspectRatio() const;
+
 		/** Replace current values with new ones */
 		void set(const cv::Mat &K, cv::Size sz);
 		void set(const cv::Mat &K, const cv::Mat &D, cv::Size sz);
@@ -67,7 +84,7 @@ struct CalibrationData {
 		double cy;
 		DistortionCoefficients distCoeffs;
 
-		/** (optional) sensor size */
+		/** (optional) sensor size; Move elsehwere? */
 		cv::Size2d sensorSize;
 
 		MSGPACK_DEFINE(resolution, fx, fy, cx, cy, distCoeffs, sensorSize);
@@ -82,13 +99,16 @@ struct CalibrationData {
 
 		Extrinsic inverse() const;
 
+		/** valid calibration (values not NAN) */
+		bool valid() const;
+
 		/** get as a 4x4 matrix */
 		cv::Mat matrix() const;
 		/** get 3x3 rotation matrix */
 		cv::Mat rmat() const;
 
-		cv::Vec3d rvec = {0.0, 0.0, 0.0};
-		cv::Vec3d tvec = {0.0, 0.0, 0.0};
+		cv::Vec3d rvec = {NAN, NAN, NAN};
+		cv::Vec3d tvec = {NAN, NAN, NAN};
 		MSGPACK_DEFINE(rvec, tvec);
 	};
 
@@ -110,6 +130,7 @@ struct CalibrationData {
 	bool hasCalibration(ftl::codecs::Channel channel) const;
 
 private:
+	// TODO: identify cameras with unique ID string instead of channel.
 	std::map<ftl::codecs::Channel, Calibration> data_;
 
 public:

components/calibration/src/extrinsic.cpp

@@ -248,7 +248,7 @@ int recoverPose(const cv::Mat &E, const std::vector<cv::Point2d> &_points1,
 	cv::Mat points1(_points1.size(), 2, CV_64FC1);
 	cv::Mat points2(_points2.size(), 2, CV_64FC1);
 
-	CHECK(points1.size() == points2.size());
+	CHECK_EQ(points1.size(), points2.size());
 
 	for (size_t i = 0; i < _points1.size(); i++) {
 		auto p1 = points1.ptr<double>(i);
@@ -309,7 +309,7 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
 
 	// convert from homogenous coordinates
 	for (int col = 0; col < points3dh.cols; col++) {
-		CHECK(points3dh.at<double>(3, col) != 0);
+		CHECK_NE(points3dh.at<double>(3, col), 0);
 		cv::Point3d p = cv::Point3d(points3dh.at<double>(0, col),
 							points3dh.at<double>(1, col),
 							points3dh.at<double>(2, col))
@@ -345,8 +345,8 @@ double calibratePair(const cv::Mat &K1, const cv::Mat &D1,
 		ba.run(options);
 		error = ba.reprojectionError();
 	}
-	CHECK((cv::countNonZero(params1.rvec()) == 0) &&
-		  (cv::countNonZero(params1.tvec()) == 0));
+	CHECK_EQ(cv::countNonZero(params1.rvec()), 0);
+	CHECK_EQ(cv::countNonZero(params1.tvec()), 0);
 
 	return sqrt(error);
 }
@@ -377,6 +377,16 @@ unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Intrin
 	return idx;
 }
 
+unsigned int ExtrinsicCalibration::addStereoCamera(const CalibrationData::Calibration &c1, const CalibrationData::Calibration &c2) {
+	unsigned int idx = calib_.size();
+	calib_.push_back({c1.intrinsic, c1.extrinsic});
+	calib_.push_back({c2.intrinsic, c2.extrinsic});
+	is_calibrated_.push_back(c1.extrinsic.valid());
+	is_calibrated_.push_back(c2.extrinsic.valid());
+	mask_.insert({idx, idx + 1});
+	return idx;
+}
+
 std::string ExtrinsicCalibration::status() {
 	auto str = std::atomic_load(&status_);
 	if (str) { return *str; }

components/calibration/src/optimize.cpp

@@ -154,7 +154,7 @@ Mat Camera::distortionCoefficients() const {
 
 Mat Camera::rvec() const {
 	cv::Mat rvec(cv::Size(3, 1), CV_64FC1);
-	CHECK(rvec.step1() == 3);
+	CHECK_EQ(rvec.step1(), 3);
 	ceres::QuaternionToAngleAxis(data + Parameter::ROTATION,
 		(double*)(rvec.data));
 	return rvec;
@@ -166,7 +166,7 @@ Mat Camera::tvec() const {
 
 Mat Camera::rmat() const {
 	cv::Mat R(cv::Size(3, 3), CV_64FC1);
-	CHECK(R.step1() == 3);
+	CHECK_EQ(R.step1(), 3);
 	ceres::QuaternionToRotation<double>(data + Parameter::ROTATION,
 		ceres::RowMajorAdapter3x3<double>((double*)(R.data)));
 
@@ -334,8 +334,8 @@ struct ScaleError {
 double ftl::calibration::optimizeScale(const vector<Point3d> &object_points, vector<Point3d> &points) {
 
 	// use exceptions instead
-	CHECK(points.size() % object_points.size() == 0);
-	CHECK(points.size() % 2 == 0);
+	CHECK_EQ(points.size() % object_points.size(), 0);
+	CHECK_EQ(points.size() % 2, 0);
 
 	// initial scale guess from first two object points
 

components/calibration/src/structures.cpp

@@ -6,6 +6,8 @@
 #include <ftl/calibration/structures.hpp>
 #include <ftl/calibration/parameters.hpp>
 
+#include <cmath>
+
 using ftl::calibration::CalibrationData;
 
 CalibrationData::Intrinsic::DistortionCoefficients::DistortionCoefficients() :
@@ -84,8 +86,11 @@ cv::Mat CalibrationData::Intrinsic::matrix(cv::Size size) const {
 	return ftl::calibration::scaleCameraMatrix(matrix(), resolution, size);
 }
 
-////////////////////////////////////////////////////////////////////////////////
+bool CalibrationData::Intrinsic::valid() const {
+	return (resolution == cv::Size{0, 0});
+}
 
+////////////////////////////////////////////////////////////////////////////////
 
 void CalibrationData::Extrinsic::set(const cv::Mat& T) {
 	if (T.type() != CV_64FC1) {
@@ -165,6 +170,12 @@ cv::Mat CalibrationData::Extrinsic::rmat() const {
 	return R;
 }
 
+bool CalibrationData::Extrinsic::valid() const {
+	return !(
+		std::isnan(tvec[0]) || std::isnan(tvec[1]) || std::isnan(tvec[2]) ||
+		std::isnan(rvec[0]) || std::isnan(rvec[1]) || std::isnan(rvec[2]));
+}
+
 ////////////////////////////////////////////////////////////////////////////////
 
 CalibrationData CalibrationData::readFile(const std::string &path) {