KeyframeCollection.cpp

/**
 * @copyright Copyright (c) 2017 B-com http://www.b-com.com/
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "datastructure/KeyframeCollection.h"
#include <xpcf/core/helpers.h>
#include "core/Log.h"

BOOST_CLASS_EXPORT_IMPLEMENT(SolAR::datastructure::KeyframeCollection);

namespace xpcf = org::bcom::xpcf;

namespace SolAR {
namespace datastructure {

FrameworkReturnCode KeyframeCollection::addKeyframe(const SRef<Keyframe> keyframe, bool defineKeyframeId)
{
    addKeyframeInternal(keyframe, defineKeyframeId);
    regularizeReferenceKeyframes();
    return FrameworkReturnCode::_SUCCESS;
}

FrameworkReturnCode KeyframeCollection::addKeyframe(const Keyframe & keyframe, bool defineKeyframeId)
{
	SRef<Keyframe> keyframe_ptr = xpcf::utils::make_shared<Keyframe>(keyframe);
    return addKeyframe(keyframe_ptr, defineKeyframeId);
}

FrameworkReturnCode KeyframeCollection::addKeyframes(const std::vector<SRef<SolAR::datastructure::Keyframe>>& keyframes, bool defineKeyframeId)
{
    for (const auto& keyframe : keyframes) {
        addKeyframeInternal(keyframe, defineKeyframeId);
    }
    if (!keyframes.empty()) {
        regularizeReferenceKeyframes();
    }
    return FrameworkReturnCode::_SUCCESS;
}

FrameworkReturnCode KeyframeCollection::addKeyframes(const std::vector<SolAR::datastructure::Keyframe>& keyframes, bool defineKeyframeId)
{
    std::vector<SRef<SolAR::datastructure::Keyframe>> keyframesPtrs;
    keyframesPtrs.reserve(keyframes.size());
    for (const auto& keyframe : keyframes) {
        keyframesPtrs.push_back(xpcf::utils::make_shared<Keyframe>(keyframe));
    }
    return addKeyframes(keyframesPtrs, defineKeyframeId);
}

void KeyframeCollection::addKeyframeInternal(const SRef<Keyframe> keyframe, bool defineKeyframeId)
{
    if (defineKeyframeId) {
        keyframe->setId(m_id++);
    }
    m_keyframes[keyframe->getId()] = keyframe;
    if (keyframe->getReferenceKeyframe()) {
        m_refKeyframeToKeyframes[keyframe->getReferenceKeyframe()->getId()].insert(keyframe->getId());
    }
}

FrameworkReturnCode KeyframeCollection::getKeyframe(const uint32_t id, SRef<Keyframe> & keyframe) const
{
	std::map< uint32_t, SRef<Keyframe>>::const_iterator keyframeIt = m_keyframes.find(id);
	if (keyframeIt != m_keyframes.end()) {
		keyframe = keyframeIt->second;
		return FrameworkReturnCode::_SUCCESS;
	}
	else {
		LOG_DEBUG("Cannot find keyframe with id {} to get", id);
		return FrameworkReturnCode::_ERROR_;
	}
}

FrameworkReturnCode KeyframeCollection::getKeyframes(const std::vector<uint32_t>& ids, std::vector<SRef<Keyframe>>& keyframes) const
{
	for (auto &it : ids) {
		std::map< uint32_t, SRef<Keyframe>>::const_iterator keyframeIt = m_keyframes.find(it);
		if (keyframeIt == m_keyframes.end()) {
			LOG_DEBUG("Cannot find keyframe with id {} to get", it);
			return FrameworkReturnCode::_ERROR_;
		}
		keyframes.push_back(keyframeIt->second);
	}
	return FrameworkReturnCode::_SUCCESS;
}

FrameworkReturnCode KeyframeCollection::getAllKeyframes(std::vector<SRef<Keyframe>>& keyframes) const
{
    keyframes.clear();
    keyframes.reserve(m_keyframes.size());
    for (const auto & kf: m_keyframes) {
        keyframes.push_back(kf.second);
    }
	return FrameworkReturnCode::_SUCCESS;
}

FrameworkReturnCode KeyframeCollection::getAllKeyframesWithoutImages(std::vector<SRef<Keyframe>>& keyframes) const
{
    keyframes.clear();
    keyframes.reserve(m_keyframes.size());
    std::map<uint32_t, SRef<Keyframe>> newKeyframesMap;
    for (const auto& [id, kf]: m_keyframes) {
        SRef<Keyframe> keyframeWithoutImage = xpcf::utils::make_shared<Keyframe>(kf);
        // Remove image
        keyframeWithoutImage->setView(nullptr);
        newKeyframesMap[id] = keyframeWithoutImage;
    }
    // Update the reference keyframe for each keyframe
    for (const auto& [id, kf]: newKeyframesMap) {
        // Retrieve reference keyframe in new keyframe map
        if (kf->getReferenceKeyframe()) {
            kf->setReferenceKeyframe(newKeyframesMap.at(kf->getReferenceKeyframe()->getId()));
        }
        // Add keyframe in result vector
        keyframes.push_back(kf);
    }

    return FrameworkReturnCode::_SUCCESS;
}

FrameworkReturnCode KeyframeCollection::suppressKeyframe(const uint32_t id)
{
    std::map<uint32_t, SRef<Keyframe>>::iterator keyframeIt = m_keyframes.find(id);
    if (keyframeIt == m_keyframes.end()) {
        LOG_DEBUG("KeyframeCollection::suppressKeyframe - cannot find keyframe with id {} to suppress", id);
        return FrameworkReturnCode::_ERROR_;
    }
    // id is the reference keyframe of other keyframes
    auto refKeyframeIt = m_refKeyframeToKeyframes.find(id);
    if (refKeyframeIt != m_refKeyframeToKeyframes.end()) { 
        for (const auto& kfId : refKeyframeIt->second) {
            auto iterTmp = m_keyframes.find(kfId);
            if (iterTmp == m_keyframes.end()) {
                throw std::logic_error("KeyframeCollection::suppressKeyframe - keyframe (whose reference keyframe is " + std::to_string(id) + ") not found in keyframe list.");
            }
            iterTmp->second->setReferenceKeyframe(nullptr);
        }
        m_refKeyframeToKeyframes.erase(refKeyframeIt);
    }
    // other_keyframe is the reference keyframe of id
    if (auto refKeyframeOfKfId = keyframeIt->second->getReferenceKeyframe(); refKeyframeOfKfId != nullptr) {
        auto iterTmp = m_refKeyframeToKeyframes.find(refKeyframeOfKfId->getId());
        if (iterTmp == m_refKeyframeToKeyframes.end()) {
            throw std::logic_error("KeyframeCollection::suppressKeyframe - reference keyframe of keyframe " + std::to_string(id) + " not found in reference keyframe lookup table.");
        }
        if (iterTmp->second.erase(id) != 1) {
            throw std::logic_error("KeyframeCollection::suppressKeyframe - keyframe " + std::to_string(id) + " not found in list of keyframe associated with its reference keyframe.");
        }
    }
    // remove keyframe_id
    m_keyframes.erase(keyframeIt);
    return FrameworkReturnCode::_SUCCESS;
}

DescriptorType KeyframeCollection::getDescriptorType() const
{
	return m_descriptorType;
}

FrameworkReturnCode KeyframeCollection::setDescriptorType(const DescriptorType & type)
{
	m_descriptorType = type;
	return FrameworkReturnCode::_SUCCESS;
}

bool KeyframeCollection::isExistKeyframe(const uint32_t id) const
{
	if (m_keyframes.find(id) != m_keyframes.end())
		return true;
	else
		return false;
}

int KeyframeCollection::getNbKeyframes() const
{
	return static_cast<int>(m_keyframes.size());
}

uint32_t KeyframeCollection::getNbKeyframesHavingImage() const
{
    uint32_t n = 0;
    for (const auto& [id, keyframe] : m_keyframes) {
        if (keyframe && keyframe->getView()) {
            n++;
        }
    }
    return n;
}

void KeyframeCollection::nextSerializationWithoutKeyframeImages()
{
    for (const auto& [id, kf]: m_keyframes) {
        kf->nextSerializationWithoutImage();
    }
}

void KeyframeCollection::regularizeReferenceKeyframes()
{
    m_refKeyframeToKeyframes.clear();
    for (auto& [id, kf] : m_keyframes) {
        auto refKf = kf->getReferenceKeyframe();
        if (!refKf) {
            continue;
        }
        if (m_keyframes.find(refKf->getId()) == m_keyframes.end()) {
            kf->setReferenceKeyframe(nullptr);
        }
        else {
            m_refKeyframeToKeyframes[refKf->getId()].insert(id);
        }
    }
}

template <typename Archive>
void KeyframeCollection::serialize(Archive &ar, const unsigned int version)
{
	ar & m_id;
	ar & m_descriptorType;
	ar & m_keyframes;
    if (version == 0) { // load an old keyframeCollection (version == 0)
        regularizeReferenceKeyframes(); // fill m_refKeyframeToKeyframes
    }
    else {
        ar & m_refKeyframeToKeyframes;
    }
}

IMPLEMENTSERIALIZE(KeyframeCollection);

} // end of namespace datastructure
} // end of namespace SolAR