上传新文件

include/g2o_opt_type.h

+#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<const Eigen::Vector3d> 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