#ifndef G2O_OPT_TYPE_H_
#define G2O_OPT_TYPE_H_

#include "g2o/g2o/core/base_vertex.h"
#include "g2o/g2o/types/types_sba.h"
#include "g2o/g2o/types/types_six_dof_expmap.h"
#include "g2o/g2o/core/optimization_algorithm_levenberg.h"
#include "g2o/g2o/core/sparse_optimizer.h"
#include "g2o/g2o/core/block_solver.h"
#include "g2o/g2o/solvers/linear_solver_eigen.h"
#include "mylib/QuatPlane.h"
namespace g2o {
    Eigen::Matrix3d hat(const Eigen::Vector3d &v);

/**
 * \brief Plane vertex, omg(XYZ)
 */
    class VertexSBAPlaneomg : public BaseVertex<3, Eigen::Vector3d> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        VertexSBAPlaneomg(){}

        virtual bool read(std::istream &is){};

        virtual bool write(std::ostream &os) const{};

        virtual void setToOriginImpl() ;

        virtual void oplusImpl(const double *update) ;
    };

    /***********************************************************************/

class EdgeSE3ProjectPlane : public BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPlaneomg, g2o::VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;


        EdgeSE3ProjectPlane() {}

        bool read(std::istream &is) {}

        bool write(std::ostream &os) const {}

        virtual void computeError();

        virtual void linearizeOplus();

        //Vector2d cam_project(const Vector3d & pc) const;

        Eigen::Matrix3d skew(const Eigen::Vector3d &v) const;


    };



//EdgeSE3ProjectPlane

}
#endif