MarkerRotator.hpp

Go to the documentation of this file.

#pragma once

#include "../utility/TableHelper.hpp"
#include "../utility/GeometryHelper.hpp"

#include <string>
#include <vector>
#include <fstream>


namespace ISM
{

class MarkerRotator
{
    public:

        void rotateMarker(const std::string& sourceFile, const std::string& targetFile)
        {
            // copy source to target, so that only the copy will be modified
            std::ifstream source(sourceFile, std::fstream::binary);
            std::ofstream target(targetFile, std::fstream::trunc|std::fstream::binary);
            target << source.rdbuf();

            TableHelper helper(targetFile);
            std::vector<std::pair<int, ObjectPtr>> objectsWithDbIds = helper.getAllMarkerObjects();
            std::cout << "Objects count: " << objectsWithDbIds.size() << std::endl;
            Eigen::Quaterniond eigenQuat;
            Eigen::Matrix3d A, B, C;
            for (std::pair<int, ObjectPtr> objectPair : objectsWithDbIds)
            {
                ObjectPtr object = objectPair.second;
                std::cout << objectPair.first << ": " << object->type << std::endl;
                eigenQuat =  GeometryHelper::quatToEigenQuat(object->pose->quat);
                A = eigenQuat.toRotationMatrix();
                B = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitX())
                        * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitY())
                        * Eigen::AngleAxisd(0, Eigen::Vector3d::UnitZ());
                std::cout << B << std::endl;
                C = A * B;
                object->pose->quat = GeometryHelper::eigenQuatToQuat(Eigen::Quaterniond(C));
                helper.updateObjectQuaternion(objectPair.first, object);
            }
        }
};
}