CameraPose.hpp

// This file is part of the AliceVision project.
// Copyright (c) 2018 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>
 
namespace aliceVision {
namespace sfmData {
 
class CameraPose
{
  public:
    using ptr = CameraPose*;
    using sptr = std::shared_ptr<CameraPose>;
  public:
    CameraPose() = default;
 
    explicit CameraPose(const geometry::Pose3& transform, bool locked = false)
      : _transform(transform),
        _locked(locked)
    {}
 
    CameraPose::ptr clone() const
    {
        return new CameraPose(*this);
    }
 
    inline const geometry::Pose3& getTransform() const { return _transform; }
 
    inline bool isLocked() const { return _locked; }
 
    inline bool operator==(const CameraPose& other) const { return (_transform == other._transform && _locked == other._locked); }
 
    inline void setTransform(const geometry::Pose3& transform) { _transform = transform; }
 
    inline void lock() { _locked = true; }
 
    inline void unlock() { _locked = false; }
 
    void initializeState()
    {
        if (_locked)
        {
            _state = EEstimatorParameterState::CONSTANT;
        }
        else
        {
            _state = EEstimatorParameterState::REFINED;
        }
    }
 
    EEstimatorParameterState getState() const { return _state; }
 
    void setState(EEstimatorParameterState state) { _state = state; }
 
    bool isRotationOnly() const
    {
        return _rotationOnly;
    }   
 
    void setRotationOnly(bool partial)
    {
        _rotationOnly = partial;
    }
 
    void setRemovable(bool removable)
    {
        _removable = removable;
    }
 
    bool isRemovable() const
    {
        return _removable;
    }
 
    inline void updateFromEstimator(const std::array<double, 6> & data) 
    {
        // do not update a camera pose set as Ignored or Constant in the Local strategy
        if (getState() != EEstimatorParameterState::REFINED)
        {
            return;
        }
        
        const Vec3 r_refined(data.at(0), data.at(1), data.at(2));
        const Vec3 c_refined(data.at(3), data.at(4), data.at(5));
 
        const Mat3 R_refined = SO3::expm(r_refined);
 
        // update the pose
        setTransform(geometry::Pose3(R_refined, c_refined));
    }
 
  private:
    geometry::Pose3 _transform;
    bool _locked = false;
    bool _rotationOnly = false;
    bool _removable = true;
    EEstimatorParameterState _state = EEstimatorParameterState::REFINED;
};
 
}  // namespace sfmData
}  // namespace aliceVision