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

#ifndef NODE_HPP_
#define NODE_HPP_

#include <vector>
#include <memory>
#include <cassert>

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

namespace mrob{


class Node{
  public:
    enum nodeMode{STANDARD = 0, ANCHOR, SCHUR_MARGI};

    Node(uint_t dim, nodeMode mode = STANDARD);
    virtual ~Node();
    virtual void update(VectRefConst &dx) = 0;
    virtual void update_from_auxiliary(VectRefConst &dx) = 0;
    virtual void set_state(MatRefConst &x) = 0;
    virtual void set_auxiliary_state(MatRefConst &x) = 0;
    virtual const MatRefConst get_state() const = 0;
    virtual MatRefConst get_auxiliary_state() const = 0;
    virtual void print() const {}
    factor_id_t get_id() const {return id_;}
    void set_id(factor_id_t id) {id_ = id;}
    factor_id_t get_dim(void) const {return dim_;}

     bool is_connected_to_EF() const {return isConnected2EF_;}
     void set_connected_to_EF(bool state = true) {isConnected2EF_ = state;}


    void set_node_mode(nodeMode mode){node_mode_ = mode;}
    nodeMode get_node_mode(){return node_mode_;}


  protected:
    factor_id_t id_;
    uint_t dim_;
    nodeMode node_mode_;
     // This variable is to know if there is an Eigen Factor connected to this node. The alternative
     // is a list of factors connected, but removed this option since it was not really necessary.
     bool isConnected2EF_;
};

double wrap_angle(double angle);


}



#endif /* NODE_HPP_ */