to playback the tracker and save the data for each frame

playbackAndSaveData.cpp

#include <Eigen/Eigen>

#include <opencv2/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/imgcodecs.hpp>

#include <boost/filesystem.hpp>
#include <boost/program_options.hpp>
#include <boost/tokenizer.hpp>

#include <eesep/core/Mesh.hpp>
#include <eesep/core/barymaps.hpp>
#include <eesep/core/Logger.hpp>

#include <iostream>
#include <fstream>
#include <eesep/registration/TemplateTracker.hpp>
#include <eesep/core/Camera.hpp>
#include <eesep/core/geometry.hpp>


int main(int argc, char* argv[])
{
    namespace fs = boost::filesystem;
    namespace po = boost::program_options;

    std::string meshFilename{};
    std::string trackInfoFilename{};
    std::string cameraFilename{};
    std::string baseDirectory{};
    int imageWidth{-1};
    int imageHeight{-1};
    bool reflectYZ{false};
    std::size_t startFrameIndex{0};

    po::options_description desc("\n\nThis program takes as input a preop obj model, camera intrinsics and tracked info\n"
                                 "It exports for each frame the corresponding mask and depth map\n");
    desc.add_options()
            ("help", "Show the help message.\n")
            ("mesh", po::value<std::string>(&meshFilename)->required(),
             "The obj file containing the preop mesh")
             ("trackInfo", po::value<std::string>(&trackInfoFilename)->required(),
             "The yml file containing the tracking info")
            ("camera", po::value<std::string>(&cameraFilename)->required(),
             "The camera calibration file used (photolens)")
            ("imageWidth,w", po::value<int>(&imageWidth)->required(),
             "The width of the image in pixels")
            ("imageHeight,h", po::value<int>(&imageHeight)->required(),
             "The height of the image in pixels")
            ("reflectYZ", po::bool_switch(&reflectYZ),
             "Reflect the Y and Z coordinates of the model (ie multiply by -1). This must be used for models expressed in OpenGL convention with negative Z")
            ("startFrameIndex", po::value<std::size_t>(&startFrameIndex)->default_value(startFrameIndex),
             "The frame index from where to start")
            ("baseDir", po::value<std::string>(&baseDirectory)->default_value(baseDirectory),
             "The base directory in which all files will be saved");

    po::variables_map vm;

    try
    {
        po::store(po::parse_command_line(argc, argv, desc), vm);

        if (vm.count("help") || (argc == 1))
        {
            std::cout << desc << std::endl;
            return EXIT_SUCCESS;
        }

        po::notify(vm);
    }
    catch (boost::program_options::required_option& e)
    {
        EESEP_CERR("ERROR: " << e.what());
        EESEP_CERR("Usage:\n\n)" << desc);
        return EXIT_FAILURE;
    }
    catch (boost::program_options::error& e)
    {
        EESEP_CERR("ERROR: " << e.what());
        EESEP_CERR("Usage:\n\n)" << desc);
        return EXIT_FAILURE;
    }

    using namespace eesep::registration;

    // load the tracking info
    TrackingInfoExporter::FrameInfo trackingData = readFrameInfo(trackInfoFilename);


    Eigen::Matrix4f Rt = Eigen::Matrix4f::Identity();

    // calibration stuff - lot of data conversion
    eesep::core::CameraIntrinsics camIntrinsics{};
    loadCameraIntrinsics(cameraFilename, camIntrinsics);
    cv::Mat kMat;
    cv::Mat dist;
    camIntrinsics.getOpenCVCalibration(kMat, dist);
    Eigen::Matrix3f eigenMatK{};
    cv::cv2eigen(kMat, eigenMatK);
    const auto eigenMatKinv = eigenMatK.inverse();


    // this should not be needed
    const float coeff = reflectYZ ? -1.f : 1.f;
    const Eigen::DiagonalMatrix<float, 3> transform(1.f, coeff, coeff);

    eesep::core::Mesh mesh;
    eesep::core::loadMesh(meshFilename, mesh);


    // for each frame
    for(const auto& frameData : trackingData)
    {
        const auto frameNumber = frameData.first;

        // if we need to start from a given frame
        if(frameNumber < startFrameIndex)
        {
            continue;
        }

        // get the pose
        const auto infoFrame = frameData.second;

        // if tvec is close to 0 in norm the frame has not been tracked
        if(cv::norm(infoFrame.tvecRefined) < 100 * std::numeric_limits<double>::epsilon())
        {
            continue;
        }

        Eigen::Matrix4f eigenRotoTranslation{};
        cv::cv2eigen(eesep::core::composeRotoTranslation(infoFrame.rvecRefined, infoFrame.tvecRefined),
                     eigenRotoTranslation);

        const auto localMesh = eesep::core::applyRototranslation(mesh, eigenRotoTranslation);
        const auto vertices = localMesh.getMatVertices();
        const Eigen::Matrix2Xf imgPoints = (eigenMatK * vertices).colwise().hnormalized();

        eesep::core::BaryMap baryMap;
        eesep::core::DepthMap depthMap;
        eesep::core::RenderMask renderMask;
        eesep::core::computeBarymap(vertices,
                                    localMesh.getMatFaces(),
                                    localMesh.getMatNormals(),
                                    imgPoints,
                                    eigenMatKinv,
                                    imageWidth,
                                    imageHeight,
                                    baryMap,
                                    depthMap,
                                    renderMask);

        /// TODO save necessary data
        //  eesep::core::saveDepthMap(...) give a .png
        //eesep::core::saveRenderMask() give a .png

    }

    return EXIT_SUCCESS;

}