上传新文件

src/planar.cpp

+#include "mylib/planar.h"
+/*
+    封装的含义：
+    1.对属性和方法进行封装
+    2.对属性和方法进行访问
+    类封装了成员变量和成员函数，使用指针的方法可以用->进行调用
+*/
+
+    /*这里创建点有些问题，法向量和深度信息*/
+
+void Plane::createPlanePoints(){
+    // unsigned long id_;
+    // Eigen::Vector4d homo_para_; // homogeneous parameters
+    // obtain a seed from the timer
+    unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
+    std::mt19937 gen(seed);
+    std::uniform_real_distribution<double> d_x{0.0, 4.0};
+    std::uniform_real_distribution<double> d_y{0.0, 5.0};
+
+    for (int i = 0; i < num_points_; ++i) {
+        Eigen::Vector3d pt;
+        std::vector<int> frame_ids_;
+        pt[0] = d_x(gen);
+        pt[1] = d_y(gen);
+        pt[2] = (dis_ - normal_[0]*pt[0] - normal_[1]*pt[1]) / normal_[2];
+        plane_points_.push_back(pt);
+        // std::cout<<"create plane_points successfully"<<std::endl;
+    }
+}
+// popular Hesse notation formed by a unit normal vector n and the orthogonal distance to the origin d.
+void Plane::HomogeneousToHesse(const Eigen::Vector4d &homo_para, Eigen::Vector3d &norm, double &dis){
+    Eigen::Vector3d n(homo_para[0], homo_para[1], homo_para[2]);
+
+    norm[0] = homo_para[0] / n.norm();
+    norm[1] = homo_para[1] / n.norm();
+    norm[2] = homo_para[2] / n.norm();
+
+    dis = -1 * homo_para[3] / n.norm();
+}
+// 只要我们是标准化的，正规化的，我们就可以使用n_norm = 1.从Hesse形式转变为面的齐次形式
+void Plane::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;
+}
+
+// 这里是平面的四元数表达法，只需要把Hesse形式进行一个小小的变形就可以得到单位四元数化的平面参数化形式，几何意义不明确
+Eigen::Quaterniond Plane::HesseToQuaternion(Eigen::Vector3d norm,double d) {
+    Eigen::Quaterniond quat;
+    norm.normalize();
+    quat.x() = norm[0];
+    quat.y() = norm[1];
+    quat.z() = norm[2];
+    quat.w() = -d;
+    quat.normalize();
+    return quat;
+}
+
+// 我们把Hesse单位法向量看成是一个单位圆球上的一点，可以把平面用球坐标参数化
+Eigen::Vector3d Plane::SpherToHesse(const double &phi_,const double &theta,const double &dis_s){
+    EIGEN_USING_STD(sin)
+    EIGEN_USING_STD(cos)
+    dis_ = dis_s;
+    // [\cos \phi \cos \theta \quad \cos \phi \sin \theta \quad \sin \phi] \mathbf{P}_{\mathrm{f}}-d=0
+    normal_[0] = cos(phi_)*cos(theta);
+    normal_[1] = cos(phi_)*sin(theta);
+    normal_[2] = sin(phi_);
+    return normal_;
+}