diff --git a/include/g2o_opt_type.h b/include/g2o_opt_type.h new file mode 100644 index 0000000000000000000000000000000000000000..6bba219724386231269056f1ead5ed104b9943f8 --- /dev/null +++ b/include/g2o_opt_type.h @@ -0,0 +1,231 @@ +#ifndef G2O_OPT_TYPE_H_ +#define G2O_OPT_TYPE_H_ + +#include "../Thirdparty/g2o/g2o/core/base_vertex.h" +#include "../Thirdparty/g2o/g2o/types/types_sba.h" +#include "../Thirdparty/g2o/g2o/types/types_six_dof_expmap.h" +#include "../Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h" +#include "../Thirdparty/g2o/g2o/core/sparse_optimizer.h" +#include "../Thirdparty/g2o/g2o/core/block_solver.h" +#include "../Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h" + + +class QuatPlane +{ +public: + Eigen::Quaterniond _plane; + Eigen::Vector3d _normal; + double _dis; + +public: + + QuatPlane(const Eigen::Vector4d& pl) + { + double norm = pl.head(3).norm(); + + _normal = pl.head(3)/norm; + + _dis = -pl[3]/norm; + _plane = normAndDToQuat(_dis, _normal); + // _plane = Eigen::Quaterniond(_dis, _normal[0], _normal[1], _normal[2]); + _plane.normalize(); + + } + + QuatPlane() + { + _plane = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + } + + QuatPlane(const Eigen::Quaterniond& q_): _plane(q_) + { + _dis = q_.w() / std::sqrt(1 - q_.w() * q_.w()); + + _normal = q_.vec() * std::sqrt(1 + _dis * _dis); + + } + + inline Eigen::Vector3d plane3d() const + { + return _normal * _dis; + } + + + QuatPlane operator*(const Eigen::Quaterniond q_) const + { + Eigen::Quaterniond est_pt = this->_plane; + + Eigen::Matrix4d m; + m << est_pt.w(), est_pt.z(), -est_pt.y(), est_pt.x(), + -est_pt.z(), est_pt.w(), -est_pt.x(), est_pt.y(), + est_pt.y(), -est_pt.x(), est_pt.w(), est_pt.z(), + -est_pt.x(), -est_pt.y(), -est_pt.z(), est_pt.w(); + /* m << est_pt.w(), -est_pt.z(), est_pt.y(), est_pt.x(), + est_pt.z(), est_pt.w(), -est_pt.x(), est_pt.y(), + -est_pt.y(), est_pt.x(), est_pt.w(), est_pt.z(), + -est_pt.x(), -est_pt.y(), -est_pt.z(), est_pt.w();*/ + Eigen::Vector4d update_to_vector = Eigen::Vector4d(q_.x(), q_.y(), q_.z(), q_.w()); + Eigen::Vector4d v = m * update_to_vector; + Eigen::Quaterniond q = Eigen::Quaterniond(v[3], v[0], v[1], v[2]); + + q.normalize(); + + return QuatPlane(q); + } + + QuatPlane& operator*=(const Eigen::Quaterniond q_) + { + QuatPlane est_pt(*this); + + Eigen::Matrix4d m; + /* + m << q_.w(), -q_.z(), q_.y(), q_.x(), + q_.z(), q_.w(), -q_.x(), q_.y(), + -q_.y(), q_.x(), q_.w(), q_.z(), + -q_.x(), -q_.y(), -q_.z(), q_.w(); //left multi + */ + m << q_.w(), q_.z(), -q_.y(), q_.x(), + -q_.z(), q_.w(), q_.x(), q_.y(), + q_.y(), -q_.x(), q_.w(), q_.z(), + -q_.x(), -q_.y(), -q_.z(), q_.w(); + Eigen::Vector4d update_to_vector = Eigen::Vector4d(q_.x(), q_.y(), q_.z(), q_.w()); + Eigen::Vector4d v = m * update_to_vector; + _plane = Eigen::Quaterniond(v[3], v[0], v[1], v[2]); + + return *this; + } + Eigen::Quaterniond normAndDToQuat(double d, Eigen::Vector3d norm) { + Eigen::Quaterniond res; + norm.normalize(); + res.x() = norm[0]; + res.y() = norm[1]; + res.z() = norm[2]; + res.w() = -d; + + res.normalize(); + // Fun::normalizeAndUnify(res); + return res; + } + Eigen::Vector4d HesseToHomogenous(const Eigen::Vector3d &norm,const double &dis){ + Eigen::Vector4d homo_para; + double n_norm = 1; + norm.normalized(); + homo_para[0] = norm[0] * n_norm; + homo_para[1] = norm[1] * n_norm; + homo_para[2] = norm[2] * n_norm; + homo_para[3] = -1 * dis * n_norm; + return homo_para; + } + + Eigen::Quaterniond HomogeneousToQuaternion(const Eigen::Vector4d &homo_para){ + Eigen::Vector4d v = homo_para / homo_para.norm(); + Eigen::Quaterniond q(v[3], v[0], v[1], v[2]); + return q; + } + + + + +public: + + // Jr, right jacobian of SO(3) + static Eigen::Matrix3d JacobianR(const Eigen::Vector3d& w); + // Jr^(-1) + static Eigen::Matrix3d JacobianRInv(const Eigen::Vector3d& w); + // Jl, left jacobian of SO(3), Jl(x) = Jr(-x) + static Eigen::Matrix3d JacobianL(const Eigen::Vector3d& w); + // Jl^(-1) + static Eigen::Matrix3d JacobianLInv(const Eigen::Vector3d& w); + + static Eigen::Vector3d log(const QuatPlane & QuatPlane); + + static Eigen::Vector3d logAndTheta(const QuatPlane & QuatPlane, double * theta); + static QuatPlane exp(const Eigen::Vector3d & omega); + + static QuatPlane expAndTheta(const Eigen::Vector3d & omega,double * theta); + //static Eigen::Vector3d log_map(const QuatPlane q); + + +}; + +namespace g2o +{ + +class VertexQuatPlane: public g2o::BaseVertex<3, QuatPlane> +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW; + + VertexQuatPlane() { } + + virtual bool read(std::istream& is) { } + virtual bool write(std::ostream& os) const { } + + virtual void setToOriginImpl() + { + _estimate = QuatPlane(); + } + + virtual void oplusImpl(const double* update_) + {/* + Eigen::Vector4d v; + v << 0.5 * update_[0], 0.5 * update_[1], 0.5 * update_[2], 1.0f; + + Eigen::Quaterniond q(v); + QuatPlane estimation(estimate()._plane * q); + setEstimate(estimation);*/ + //std::cout << "update in " << std::endl; + Eigen::Map update(update_); + + //std::cout << "update plane: " << update << std::endl; + + QuatPlane tmp_1 = QuatPlane::exp(update/update.norm()); + + // Fun::normalizeAndUnify(tmp_q); + + QuatPlane estimation(tmp_1._plane*estimate()._plane); + setEstimate(estimation); + //std::cout << "update out " << std::endl; + + } +}; + +class EdgeSE3ProjectPlane: public BaseBinaryEdge<3, QuatPlane, VertexQuatPlane, 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; + +public: + double _fx; + double _fy; + double _cx; + double _cy; + +}; + + Eigen::Matrix3d hat(const Eigen::Vector3d& v) + { + Eigen::Matrix3d m; + m << 0.0, -v[2], v[1], + v[2], 0.0, -v[0], + -v[1], v[0], 0.0; + + return m; + } +} + +#endif \ No newline at end of file