OpenVulkano/openVulkanoCpp/AR/ArFrameMetadata.cpp

/*
 * This Source Code Form is subject to the terms of the Mozilla Public
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at https://mozilla.org/MPL/2.0/.
 */

#include "ArFrameMetadata.hpp"
#include <pugixml.hpp>
#include <yaml-cpp/yaml.h>
#include <sstream>

namespace OpenVulkano::AR
{
	namespace
	{
		template<typename T, int S>
		T ReadMat(const pugi::xml_node& matrixNode)
		{
			T mat(0);
			auto rowNode = matrixNode.first_child();
			for (int row = 0; row < S && rowNode; row++, rowNode = rowNode.next_sibling())
			{
				auto columnNode = rowNode.first_child();
				for (int column = 0; column < S && columnNode; column++, columnNode = columnNode.next_sibling())
				{
					mat[column][row] = columnNode.text().as_float();
				}
			}
			return mat;
		}

		template<glm::length_t SIZE, typename T, glm::qualifier Q>
		void MatToXML(const glm::mat<SIZE, SIZE, T, Q>& mat, std::ostream& stream, const std::string& nl = "\n")
		{
			auto nlRow = nl + "\t";
			stream <<  nl;
			for(int r = 0 ; r < SIZE; r++)
			{
				stream << "<row>" << nlRow;
				for (int c = 0; c < SIZE; c++)
				{
					stream << "<column>" << mat[c][r] << "</column>";
				}
				stream << nl << "</row>";
			}
		}

		template<glm::length_t SIZE, typename T, glm::qualifier Q>
		void MatToYaml(const glm::mat<SIZE, SIZE, T, Q>& mat, std::ostream& stream, const std::string& nl = "\n")
		{
			for(int r = 0 ; r < SIZE; r++)
			{
				stream << nl << "- [";
				for (int c = 0; c < SIZE; c++)
				{
					if (c) stream << ',';
					stream << ' ' << mat[c][r];
				}
				stream << " ]";
			}
		}
	}

	ArFrameMetadata ArFrameMetadata::FromXML(const char* xml, size_t length)
	{
		ArFrameMetadata frameData;

		pugi::xml_document doc;
		pugi::xml_parse_result result = doc.load_buffer(xml, length);
		if (!result) throw std::runtime_error("Failed to parse frame metadata");
		pugi::xml_node nodeFrame = doc.child("arframe");

		pugi::xml_node nodeCamera = nodeFrame.child("camera");
		frameData.transformation = ReadMat<Math::Matrix4f, 4>(nodeCamera.child("transform"));
		frameData.projection = ReadMat<Math::Matrix4f, 4>(nodeCamera.child("projection"));
		pugi::xml_node nodeRes = nodeCamera.child("resolution");
		Math::Vector2i resolution = { nodeRes.child("width").text().as_int(), nodeRes.child("height").text().as_int() };
		frameData.intrinsic = { ReadMat<Math::Matrix3f, 3>(nodeCamera.child("intrinsics")), resolution };
		frameData.exposureTime = nodeCamera.child("exposureDuration").text().as_float();
		frameData.exposureOffset = nodeCamera.child("exposureOffset").text().as_float();

		pugi::xml_node nodeLight = nodeFrame.child("light");
		frameData.lightIntensity = nodeLight.child("ambientIntensity").text().as_float();
		frameData.lightColorTemp = nodeLight.child("ambientColorTemp").text().as_float();

		frameData.timestamp = nodeFrame.child("timestamp").text().as_double();
		frameData.trackingState = ArTrackingState::GetFromName(nodeFrame.child("trackingState").child("camera").text().as_string());

		return frameData;
	}

	ArFrameMetadata ArFrameMetadata::FromYaml(const char* xml, size_t length)
	{
		ArFrameMetadata frameData;
		//TODO
		return frameData;
	}

	std::string ArFrameMetadata::ToYaml() const
	{
		std::stringstream meta;
		meta << std::setprecision(std::numeric_limits<double>::digits10);

		meta << "camera:\n  transform: ";
		MatToYaml(transformation, meta, "\n    ");
		meta << "\n  projection";
		MatToYaml(projection, meta, "\n    ");
		meta << "\n  resolution:\n    width: " << intrinsic.GetResolution().x << "\n    height: " << intrinsic.GetResolution().y;
		meta << "\n  intrinsics: ";
		MatToYaml(intrinsic.cameraMatrix, meta, "\n    ");
		meta << "\n  exposureDuration: " << exposureTime << "\n  exposureOffset: " << exposureOffset;
		meta << "\ntimestamp: " << timestamp.GetSeconds();
		meta << "\ntimestampDepth: " << timestampDepth.GetNanos();
		meta << "\ntrackingState: \n  camera: " << trackingState.GetName();
		meta << "\nlight:\n  intensity: " << lightIntensity << "\n  colorTemp: " << lightColorTemp;

		return meta.str();
	}

	std::string ArFrameMetadata::ToXML() const
	{
		std::stringstream meta;
		meta << std::setprecision(std::numeric_limits<double>::digits10);

		meta << "<arframe>\n\t<camera>\n\t\t<transform>";
		MatToXML(transformation, meta, "\n\t\t\t");
		meta << "\n\t\t</transform>\n\t\t<projection>";
		MatToXML(projection, meta, "\n\t\t\t");
		meta << "\n\t\t</projection>\n\t\t<resolution>\n\t\t\t<width>" << intrinsic.GetResolution().x << "</width>\n\t\t\t";
		meta << "<height>" << intrinsic.GetResolution().y << "</height>\n\t\t</resolution>\n\t\t<intrinsics>";
		MatToXML(intrinsic.cameraMatrix, meta, "\n\t\t\t");
		meta << "</intrinsics>\n\t\t<exposureDuration>" << exposureTime << "</exposureDuration>\n\t\t<exposureOffset>" << exposureOffset;
		meta << "</exposureOffset>\n\t</camera>\n\t";
		meta << "<timestamp>" << timestamp.GetSeconds() << "</timestamp>\n\t";
		meta << "<timestampDepth>" << timestampDepth.GetSeconds() << "</timestampDepth>\n\t";
		meta << "<trackingState>" << "\n\t\t<camera>" << trackingState.GetName() << "</camera>" << "\n\t</trackingState>";
		meta << "<light>\n\t\t<ambientIntensity>" << lightIntensity << "</ambientIntensity>\n\t\t";
		meta << "<ambientColorTemp>" << lightColorTemp << "</ambientColorTemp>\n\t</light>";
		meta << "\n</arframe>";

		return meta.str();
	}
}