From 1b24e74192ad865ab1f21d2e35024a3325b8def7 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?=E8=82=A5=E9=BC=A0=E8=B7=AF=E6=98=93?=
 <guoxin960606@163.com>
Date: Tue, 22 Jun 2021 13:54:03 +0800
Subject: [PATCH] =?UTF-8?q?=E4=B8=8A=E4=BC=A0=E6=96=B0=E6=96=87=E4=BB=B6?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 include/mylib/QuatPlane.h | 74 +++++++++++++++++++++++++++++++++++++++
 1 file changed, 74 insertions(+)
 create mode 100644 include/mylib/QuatPlane.h

diff --git a/include/mylib/QuatPlane.h b/include/mylib/QuatPlane.h
new file mode 100644
index 0000000..bfecfd2
--- /dev/null
+++ b/include/mylib/QuatPlane.h
@@ -0,0 +1,74 @@
+#ifndef QUAT_PLANE_H
+#define QUAT_PLANE_H
+
+# include <mylib/commen_include.h>
+
+    class QuatPlane{
+    public: //public 修饰的成员变量和函数可以在类的内部和外部访问
+        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+    public:
+
+        QuatPlane(){}
+
+        QuatPlane(Eigen::Vector4d home_para):homo_para_(home_para){
+                HomogeneousToHesse(homo_para_, normal_, dis_);
+        }
+        QuatPlane(Eigen::Vector3d normal, double dis): normal_(normal), dis_(dis){
+            HesseToHomogenous(normal_, dis_, homo_para_);
+        }
+
+        inline Eigen::Vector3d plane3d() const
+        {
+            return normal_*dis_;
+        }
+        // 创建立平面上的点
+        Eigen::Vector4d CreatePlanebyPoints(Eigen::Vector4d pt1_,Eigen::Vector4d pt2_,Eigen::Vector4d pt3_,Eigen::Vector4d pt4_);
+        // popular Hesse notation formed by a unit normal vector n and the orthogonal distance to the origin d.
+        static void HomogeneousToHesse(const Eigen::Vector4d &homo_para, Eigen::Vector3d &norm, double &dis);
+        // 只要我们是标准化的，正规化的，我们就可以使用n_norm = 1.从Hesse形式转变为面的齐次形式
+        static void HesseToHomogenous(const Eigen::Vector3d &norm,const double &dis, Eigen::Vector4d &homo_para);
+
+        // 这里是平面的四元数表达法，只需要把Hesse形式进行一个小小的变形就可以得到单位四元数化的平面参数化形式，几何意义不明确
+        Eigen::Quaterniond HesseToQuaternion(Eigen::Vector3d norm,double d);
+
+        // 我们把Hesse单位法向量看成是一个单位圆球上的一点，可以把平面用球坐标参数化
+        Eigen::Vector3d SpherToHesse(const double &phi_,const double &theta,const double &dis_s);
+
+
+        Eigen::Matrix4d Leftq(const Eigen::Quaterniond q);
+        Eigen::Matrix4d Rightq(const Eigen::Quaterniond q);
+        Eigen::Quaterniond Conjugate(const Eigen::Quaterniond q);
+        //Eigen::Matrix3d hat(const Eigen::Vector3d &v);
+
+        static Eigen::Quaterniond ExpUpDateQuat(const Eigen::Vector3d &omg,const Eigen::Quaterniond q);
+
+        //Eigen::Matrix3d hat(const Eigen::Vector3d &omg);
+        //Eigen::Vector3d ahat(const Eigen::Matrix3d M);
+        static Eigen::Vector3d Rotation2Vector(Eigen::Matrix3d R);
+        static Eigen::Vector3d logQuat(Eigen::Quaterniond q);
+
+        // 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);
+
+    private:// 修饰的成员变量和函数只可以在类的内部访问
+        //unsigned long id_;
+        /*    Eigen::Vector3d normal_;
+    normal_ << 1, 1, 1;
+    double dis_ = 1.0; // hessian form, n^T * pt = d
+    int num_points_ = 100;
+    VecVector3d plane_points_;*/
+        Eigen::Vector4d homo_para_; // homogeneous parameters
+        Eigen::Vector3d normal_;
+        double dis_; // hessian form, n^T * pt = d
+        int num_points_ = 100;
+        Eigen::Vector3d plane_points_;
+        //std::vector<int> frame_ids_; // the index of the frame that observes this plane.
+    };
+
+#endif //QUAT_PLANE_H
-- 
GitLab