diff --git a/include/mylib/g2o_opt_type.h b/include/mylib/g2o_opt_type.h new file mode 100644 index 0000000000000000000000000000000000000000..d22fcf631e895853ff286144d5909c3e0803f8e5 --- /dev/null +++ b/include/mylib/g2o_opt_type.h @@ -0,0 +1,62 @@ +#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 \ No newline at end of file