factor_graph.hpp

/* Copyright (c) 2022, Gonzalo Ferrer
 *
 * 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.
 *
 *
 * factor_graph.hpp
 *
 *  Created on: Feb 12, 2018
 *      Author: Gonzalo Ferrer
 *              g.ferrer@skoltech.ru
 *              Mobile Robotics Lab, Skoltech 
 */

#ifndef FACTOR_GRAPH_HPP_
#define FACTOR_GRAPH_HPP_

//#include <unordered_map>
#include <deque>// for long allocations

#include "mrob/factor.hpp"
#include "mrob/node.hpp"

namespace mrob{
class FGraph{
public:
    FGraph();
    virtual ~FGraph();
    factor_id_t add_factor(std::shared_ptr<Factor> &factor);
    factor_id_t add_eigen_factor(std::shared_ptr<EigenFactor> &factor);
    factor_id_t add_node(std::shared_ptr<Node> &node);

    std::shared_ptr<Node>& get_node(factor_id_t key);

    std::shared_ptr<Factor>& get_factor(factor_id_t key);
    std::shared_ptr<EigenFactor>& get_eigen_factor(factor_id_t key);
    void print(bool complete = false) const;


    factor_id_t number_nodes() {return nodes_.size();};
    factor_id_t number_factors() {return factors_.size();};
    uint_t get_dimension_state() {return stateDim_;};
    uint_t get_dimension_obs() {return obsDim_;};

    //TODO serialization
    void save_graph() const;
    void load_graph();

protected:
    //All nodes in the system. The index Id corresponds to the position in this vector
    std::deque<std::shared_ptr<Node> >   nodes_;
    // Only active nodes, here the index does not guarantee anything.
    std::deque<std::shared_ptr<Node> >   active_nodes_;

    //std::unordered_set<std::shared_ptr<Factor> > factors_;
    std::deque<std::shared_ptr<Factor> > factors_; // no specific order needed

    // This requires a special list for the factors
    std::deque<std::shared_ptr<EigenFactor> > eigen_factors_;

    uint_t stateDim_, obsDim_;
};



}

#endif /* FACTOR_Graph_HPP_ */