Rig.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2017 AliceVision contributors.
// 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/.
 
#pragma once
 
#include <aliceVision/types.hpp>
#include <aliceVision/geometry/Pose3.hpp>
 
#include <vector>
#include <cassert>
#include <algorithm>
#include <stdexcept>
 
namespace aliceVision {
namespace sfmData {
 
enum class ERigSubPoseStatus : std::uint8_t
{
    UNINITIALIZED = 0,
    ESTIMATED = 1,
    CONSTANT = 2
};
 
inline std::string ERigSubPoseStatus_enumToString(ERigSubPoseStatus rigSubPoseStatus)
{
    switch (rigSubPoseStatus)
    {
        case ERigSubPoseStatus::UNINITIALIZED:
            return "uninitialized";
        case ERigSubPoseStatus::ESTIMATED:
            return "estimated";
        case ERigSubPoseStatus::CONSTANT:
            return "constant";
    }
    throw std::out_of_range("Invalid rigSubPoseStatus enum");
}
 
inline ERigSubPoseStatus ERigSubPoseStatus_stringToEnum(const std::string& rigSubPoseStatus)
{
    std::string status = rigSubPoseStatus;
    std::transform(status.begin(), status.end(), status.begin(), ::tolower);  // tolower
 
    if (status == "uninitialized")
        return ERigSubPoseStatus::UNINITIALIZED;
    if (status == "estimated")
        return ERigSubPoseStatus::ESTIMATED;
    if (status == "constant")
        return ERigSubPoseStatus::CONSTANT;
 
    throw std::out_of_range("Invalid rigSubPoseStatus : " + rigSubPoseStatus);
}
 
struct RigSubPose
{
    ERigSubPoseStatus status = ERigSubPoseStatus::UNINITIALIZED;
    geometry::Pose3 pose;
 
    RigSubPose(const geometry::Pose3& pose = geometry::Pose3(), ERigSubPoseStatus status = ERigSubPoseStatus::UNINITIALIZED)
      : pose(pose),
        status(status)
    {}
 
    bool operator==(const RigSubPose& other) const { return (status == other.status && pose == other.pose); }
};
 
class Rig
{
  public:
    explicit Rig(unsigned int nbSubPoses = 0) { _subPoses.resize(nbSubPoses); }
 
    bool operator==(const Rig& other) const { return _subPoses == other._subPoses; }
 
    bool isInitialized() const
    {
        for (const RigSubPose& subPose : _subPoses)
        {
            if (subPose.status != ERigSubPoseStatus::UNINITIALIZED)
                return true;
        }
        return false;
    }
 
    bool isFullyCalibrated() const
    {
        for (const RigSubPose& subPose : _subPoses)
        {
            if (subPose.status == ERigSubPoseStatus::UNINITIALIZED)
                return false;
        }
        return true;
    }
 
    std::size_t getNbSubPoses() const { return _subPoses.size(); }
 
    const std::vector<RigSubPose>& getSubPoses() const { return _subPoses; }
 
    std::vector<RigSubPose>& getSubPoses() { return _subPoses; }
    const RigSubPose& getSubPose(IndexT index) const { return _subPoses.at(index); }
 
    RigSubPose& getSubPose(IndexT index) { return _subPoses.at(index); }
 
    void setSubPose(IndexT index, const RigSubPose& rigSubPose)
    {
        assert(_subPoses.size() > index);
        _subPoses[index] = rigSubPose;
    }
 
    void reset()
    {
        for (RigSubPose& subPose : _subPoses)
        {
            subPose.status = ERigSubPoseStatus::UNINITIALIZED;
            subPose.pose = geometry::Pose3();
        }
    }
 
  private:
    std::vector<RigSubPose> _subPoses;
};
 
}  // namespace sfmData
}  // namespace aliceVision