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

#ifndef NODEPOSE3D_HPP_
#define NODEPOSE3D_HPP_

#include "mrob/matrix_base.hpp"
#include "mrob/SE3.hpp" //requires including and linking SE3 library
#include "mrob/node.hpp"

namespace mrob{

class NodePose3d : public Node
{
  public:
    NodePose3d(const Mat4 &initial_x, Node::nodeMode mode = STANDARD);
    NodePose3d(const SE3 &initial_x, Node::nodeMode mode = STANDARD);
    ~NodePose3d()  override = default;
    virtual void update(VectRefConst &dx);
    virtual void update_from_auxiliary(VectRefConst &dx);
    virtual void set_state(MatRefConst &x);
    virtual void set_auxiliary_state(MatRefConst &x);
    virtual MatRefConst get_state() const {return state_.T();};
    virtual MatRefConst get_auxiliary_state() const {return auxiliaryState_.T();};
    void print() const;

  protected:
    SE3 state_;
    SE3 auxiliaryState_; //an auxiliary vector for undoing updates

  public:
    //EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen


};


}


#endif /* NODEPOSE3D_HPP_ */