CameraToWorld.cpp

#include "pch_LightBarrier.h"
#include "CameraToWorld.h"

#include "ofxRulr/Exception.h"

#include "ofxRulr/Nodes/Item/Camera.h"
#include "ofxRulr/Nodes/Item/AbstractBoard.h"

#include "ofxCvGui/Widgets/Button.h"
#include "ofxCvGui/Widgets/Toggle.h"
#include "ofxCvGui/Widgets/Slider.h"
#include "ofxCvGui/Widgets/Spacer.h"
#include "ofxCvGui/Widgets/LiveValue.h"

#include "ofxGraycode.h"

using namespace ofxRulr;
using namespace ofxRulr::Nodes;
using namespace ofxRulr::Utils;

using namespace ofxCv;
using namespace cv;

namespace LightBarrier {
#pragma mark Model
	//----------
	unsigned int CameraToWorld::Model::getParameterCount() const {
		return 9;
	}

	//----------
	void CameraToWorld::Model::getResidual(DataPoint dataPoint, double & residual, double * gradient) const {
		if (gradient) {
			throw(ofxRulr::Exception("CameraToWorld::Model doesn't support gradient based optimisers"));
		}
		auto evaluated = dataPoint;
		this->evaluate(evaluated);
		residual = (double)(evaluated.world - dataPoint.world).length();
	}

	//----------
	void CameraToWorld::Model::evaluate(DataPoint & dataPoint) const {
		auto parameters = this->getParameters();
		if (parameters) {
			dataPoint.world.x = parameters[0] * dataPoint.camera.x + parameters[1] * dataPoint.camera.y + parameters[2];
			dataPoint.world.y = parameters[3] * dataPoint.camera.x + parameters[4] * dataPoint.camera.y + parameters[5];
			dataPoint.world.z = parameters[6] * dataPoint.camera.x + parameters[7] * dataPoint.camera.y + parameters[8];
		}
	}

#pragma mark CameraToWorld
	//----------
	CameraToWorld::CameraToWorld() {
		RULR_NODE_INIT_LISTENER;
	}

	//----------
	string CameraToWorld::getTypeName() const {
		return "LightBarrier::CameraToWorld";
	}

	//----------
	void CameraToWorld::init() {
		RULR_NODE_UPDATE_LISTENER;
		RULR_NODE_DRAW_WORLD_LISTENER;
		RULR_NODE_INSPECTOR_LISTENER;
		RULR_NODE_SERIALIZATION_LISTENERS;

		auto cameraInput = this->addInput<Item::Camera>();
		this->addInput<Item::AbstractBoard>();

		this->view = MAKE(ofxCvGui::Panels::World);
		this->view->onDrawWorld += [this](ofCamera &) {
			auto cameraNode = this->getInput<Item::Camera>();
			if (cameraNode) {
				cameraNode->drawWorldStage();
			}

			this->drawWorldStage();
		};

		auto & camera = this->view->getCamera();
		camera.setPosition(ofVec3f(1, -1, 0));
		camera.lookAt(ofVec3f(0.0f, 0.0f, 4.0f), ofVec3f(0.0f, -1.0f, 0.0f));

		cameraInput->onNewConnection += [this](shared_ptr<Item::Camera> camera) {
			auto cameraView = camera->getPanel();
			if (cameraView) {
				auto cameraWeak = weak_ptr<Item::Camera>(camera);
				cameraView->onMouse.addListener([this, cameraWeak](ofxCvGui::MouseArguments & args) {
					auto camera = cameraWeak.lock();
					if (args.action == ofxCvGui::MouseArguments::Action::Moved && args.isLocal()) {
						try {
							this->testValue = this->cameraToWorld(args.localNormalized * ofVec2f(camera->getWidth(), camera->getHeight()));
						}
						catch (...) {
							//if the camera has no calibration, then cameraToWorld may throw an exception 
							//just ignore it
						}
					}
				}, this);
			}
		};
		cameraInput->onDeleteConnection += [this](shared_ptr<Item::Camera> camera) {
			if (camera) {
				auto cameraView = camera->getPanel();
				if (cameraView) {
					cameraView->onMouse.removeListeners(this);
				}
			}
		};

		this->parameterCallbackEnabled = true;
		this->residual = 0.0;

		this->cachedCameraWidth = 0.0f;
		this->cachedCameraHeight = 0.0f;
	}

	//----------
	void CameraToWorld::serialize(Json::Value & json) {
		auto & jsonDataPoints = json["dataPoints"];
		int pointIndex = 0;
		for (const auto & dataPoint : this->dataPoints) {
			auto & jsonDataPoint = jsonDataPoints[pointIndex++];

			auto & jsonWorldPoint = jsonDataPoint["world"];
			jsonWorldPoint["x"] = dataPoint.world.x;
			jsonWorldPoint["y"] = dataPoint.world.y;
			jsonWorldPoint["z"] = dataPoint.world.z;

			auto & jsonCameraPoint = jsonDataPoint["camera"];
			jsonCameraPoint["x"] = dataPoint.camera.x;
			jsonCameraPoint["y"] = dataPoint.camera.y;
		}

		auto & jsonAdditions = json["additions"];
		int additionIndex = 0;
		for (auto & addition : this->additions) {
			jsonAdditions[additionIndex++] = addition;
		}

		auto & jsonFitParameters = json["fitParameters"];
		for (int i = 0; i < 9; i++) {
			jsonFitParameters[i] = this->fitParameters[i].get();
		}
	}

	//----------
	void CameraToWorld::deserialize(const Json::Value & json) {
		this->dataPoints.clear();
		const auto & jsonDataPoints = json["dataPoints"];
		for (const auto & jsonDataPoint : jsonDataPoints) {
			auto & jsonWorldPoint = jsonDataPoint["world"];
			DataPoint dataPoint;

			dataPoint.world.x = jsonWorldPoint["x"].asFloat();
			dataPoint.world.y = jsonWorldPoint["y"].asFloat();
			dataPoint.world.z = jsonWorldPoint["z"].asFloat();

			auto & jsonCameraPoint = jsonDataPoint["camera"];
			dataPoint.camera.x = jsonCameraPoint["x"].asFloat();
			dataPoint.camera.y = jsonCameraPoint["y"].asFloat();

			this->dataPoints.push_back(dataPoint);
		}

		this->additions.clear();
		const auto & jsonAdditions = json["additions"];
		for (const auto & jsonAddition : jsonAdditions) {
			this->additions.push_back(jsonAddition.asInt());
		}

		auto & jsonFitParameters = json["fitParameters"];
		this->parameterCallbackEnabled = false;
		for (int i = 0; i < 9; i++) {
			this->fitParameters[i].set(jsonFitParameters[i].asFloat());
		}
		this->parameterCallbackEnabled = true;
		float dummy;
		this->callbackModelParameter(dummy);

		this->updatePreview();
	}
	//----------
	ofxCvGui::PanelPtr CameraToWorld::getPanel() {
		return this->view;
	}

	//----------
	void CameraToWorld::update() {
		auto camera = this->getInput<Item::Camera>();
		if (camera) {
			if (camera->getWidth() != this->cachedCameraWidth || camera->getHeight() != this->cachedCameraHeight) {
				try {
					this->updatePreview();
				}
				RULR_CATCH_ALL_TO_ERROR
			}
		}
	}

	//---------
	void CameraToWorld::drawWorldStage() {
		ofMesh points;
		for (auto & point : this->dataPoints) {
			points.addVertex(point.world);
			points.addColor(ofColor(abs(point.camera.x) * 255.0f, abs(point.camera.y) * 255.0f, 255.0f));
		}
		Utils::Graphics::pushPointSize(3.0f);
		{
			points.drawVertices();
		}
		Utils::Graphics::popPointSize();

		ofSphere(this->testValue, 0.05f);

		if (this->parameters.showResiduals) {
			ofMesh residuals;
			residuals.setMode(ofPrimitiveMode::OF_PRIMITIVE_LINES);
			for (const auto & dataPoint : this->dataPoints) {
				auto evaluated = dataPoint;
				this->model.evaluate(evaluated);

				residuals.addColor(ofColor(255, 50));
				residuals.addVertex(dataPoint.world);
				residuals.addColor(ofColor(255, 255));
				residuals.addVertex(evaluated.world);
			}
			residuals.draw();
		}

		ofPushStyle();
		{
			ofSetColor(255, 50);
			this->planePreview.draw();
		}
		ofPopStyle();
	}

	//---------
	ofVec3f CameraToWorld::cameraToWorld(const ofVec2f & camera) {
		auto cameraNode = this->getInput<Item::Camera>();
		if (!cameraNode) {
			throw(ofxRulr::Exception("Can't call cameraToWorld"));
		}
		DataPoint dataPoint;
		const auto cameraMatrix = cameraNode->getCameraMatrix();
		const auto distortionMatrix = cameraNode->getDistortionCoefficients();
		dataPoint.camera = ofxCv::undistortPoint(camera, cameraMatrix, distortionMatrix);
		this->model.evaluate(dataPoint);
		return dataPoint.world;
	}

	//----------
	void CameraToWorld::populateInspector(ofxCvGui::InspectArguments & inspectArguments) {
		auto inspector = inspectArguments.inspector;

		inspector->addParameterGroup(this->parameters);
		
		auto clearButton = MAKE(ofxCvGui::Widgets::Button, "Clear", [this]() {
			this->additions.clear();
			this->dataPoints.clear();
		});
		clearButton->setHeight(100.0f);
		inspector->add(clearButton);

		auto findCornersButton = inspector->addButton("Find corners", [this]() {
			try {
				ScopedProcess scopedProcess("Finding corners");
				this->addBoard();
				this->fit();
				scopedProcess.end();
			}
			RULR_CATCH_ALL_TO_ERROR;
		}, ' ');
		findCornersButton->setHeight(100.0f);

		inspector->add(MAKE(ofxCvGui::Widgets::Button, "Delete last corners", [this]() {
			if (!this->additions.empty()) {
				this->dataPoints.resize(MAX(this->dataPoints.size() - this->additions.back(), 0));
				this->additions.pop_back();
			}
		}, OF_KEY_BACKSPACE));

		inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<size_t>, "Total correspondences points found", [this]() {
			return this->dataPoints.size();
		}));
		inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<size_t>, "Boards found", [this] {
			return this->additions.size();
		}));

		inspector->add(MAKE(ofxCvGui::Widgets::Spacer));

		inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<double>, "Residual [m]", [this]() {
			return this->residual;
		}));

		inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<ofVec3f>, "Test point", [this]() {
			return this->testValue;
		}));

		inspector->add(MAKE(ofxCvGui::Widgets::Spacer));

		for (int i = 0; i < 9; i++) {
			inspector->add(MAKE(ofxCvGui::Widgets::LiveValue<float>, "Parameter " + ofToString(i), [this, i] {
				return this->fitParameters[i];
			}));
		}

		inspector->add(MAKE(ofxCvGui::Widgets::Button, "Update preview", [this]() {
			this->updatePreview();
		}));

		inspector->add(MAKE(ofxCvGui::Widgets::Button, "Set fit to plane", [this]() {
			try
			{
				auto result = ofSystemTextBoxDialog("Plane depth Z (m)", "4.0");
				auto depth = ofToFloat(result);
				this->setFitToPlane(depth);
			}
			RULR_CATCH_ALL_TO_ALERT
		}));
	}

	//----------
	void CameraToWorld::addBoard() {
		this->throwIfMissingAnyConnection();
		auto cameraNode = this->getInput<Item::Camera>();
		auto boardNode = this->getInput<Item::AbstractBoard>();

		vector<cv::Point2f> imagePoints;
		vector<cv::Point3f> objectPoints;
		auto frame = cameraNode->getGrabber()->getFreshFrame();
		auto pixels = frame->getPixels();

		auto foundBoard = boardNode->findBoard(ofxCv::toCv(pixels)
			, imagePoints
			, objectPoints
			, this->parameters.findBoardMode
			, cameraNode->getCameraMatrix()
			, cameraNode->getDistortionCoefficients());

		if (!foundBoard) {
			throw(ofxRulr::Exception("No corners found"));
		}

		cout << "Object points " << objectPoints.size() << endl;
		cout << "Image points " << imagePoints.size() << endl;

		cv::Mat rotation, translation;
		cv::solvePnP(objectPoints
			, imagePoints
			, cameraNode->getCameraMatrix()
			, cameraNode->getDistortionCoefficients()
			, rotation
			, translation
			, false);

		auto boardTransform = ofxCv::makeMatrix(rotation, translation);

		for (int i = 0; i < imagePoints.size(); i++) {
			DataPoint dataPoint;
			dataPoint.world = (toOf(objectPoints[i]) * boardTransform) * cameraNode->getTransform();
			dataPoint.camera = ofxCv::undistortPoint(toOf(imagePoints[i])
				, cameraNode->getCameraMatrix()
				, cameraNode->getDistortionCoefficients());
			this->dataPoints.push_back(dataPoint);
		}
		this->additions.push_back(imagePoints.size());
	}

	//----------
	void CameraToWorld::setFitToPlane(float depth) {
		this->dataPoints.clear();
		this->additions.clear();

		double parameters[] = { depth, 0.0f, 0.0f,
									0.0f, depth, 0.0f,
									0.0f, 0.0f, depth };
		this->model.setParameters(parameters);
	}

	//----------
	void CameraToWorld::fit() {
		ofxNonLinearFit::Fit<Model> fit(ofxNonLinearFit::Algorithm(nlopt::LN_NEWUOA, ofxNonLinearFit::Algorithm::LocalGradientless));
		fit.optimise(this->model, &this->dataPoints, &this->residual);
		this->residual /= this->dataPoints.size();
		this->updateParameters();
		this->updatePreview();
	}

	//----------
	void CameraToWorld::callbackModelParameter(float &) {
		if (!this->parameterCallbackEnabled) {
			return;
		}

		double values[9];
		for (int i = 0; i < 9; i++) {
			values[i] = (double) this->fitParameters[i].get();
		}
		this->model.setParameters(values);
	}

	//----------
	void CameraToWorld::updateParameters() {
		this->parameterCallbackEnabled = false;
		const auto values = this->model.getParameters();
		for (int i = 0; i < 9; i++) {
			this->fitParameters[i].set((float)values[i]);
		}
		this->parameterCallbackEnabled = true;
	}

	//----------
	void CameraToWorld::updatePreview() {
		auto camera = this->getInput<Item::Camera>();
		if (camera) {
			this->planePreview.clear();
			this->planePreview.addVertex(this->cameraToWorld(ofVec2f(0, 0)));
			this->planePreview.addVertex(this->cameraToWorld(ofVec2f(camera->getWidth(), 0)));
			this->planePreview.addVertex(this->cameraToWorld(ofVec2f(camera->getWidth(), camera->getHeight())));
			this->planePreview.addVertex(this->cameraToWorld(ofVec2f(0, camera->getHeight())));
			this->planePreview.setMode(ofPrimitiveMode::OF_PRIMITIVE_TRIANGLE_FAN);

			this->cachedCameraWidth = camera->getWidth();
			this->cachedCameraHeight = camera->getHeight();
		}
	}
}