几何模块

namespace: geometry

几何模块是整个系统的一个基础。这里面定义了一些很基础的数据结构，实现了基础的运算和算法，包括对于点，变换矩阵（Transformation），李群李代数（SE(3), se(3)）之间的转换等等，还有点云，三角网格等常用数据结构及相关算法的定义和实现。

Geometry.h

在Geometry.h中，首先要注意的一点就是：

typedef float scalar; 

这里默认将float作为标量类型，因此之后点云，矩阵等，格式都是单精度的，如果你想改变整个库使用的浮点数精度，使用双精度浮点数，在CMakeLists.txt文件中，对CMAKE_CXX_FLAGS变量添加-DUSING_FLOAT64选项。

typedef Vector6 Se3;
typedef Matrix4 SE3;

李群李代数被定义为SE3和Se3，分别是四维方阵和6维矩阵，可以通过函数：

Matrix4 Se3ToSE3(const Vector6 &input);
Vector6 SE3ToSe3(const Matrix4 &input);

来互相转换，这个函数内部实现是通过Sophus库完成的（在Open3D中，从6维向量到4维方阵的互相变换，对于平移部分是完全不变的，但是从数学推导上来说，二者是有微小区别的，所以理论上Sophus库是更准确的变换，如下图）。

struct VoxelGridHasher;
struct PixelGridHasher;

上面两个分别是三维和二维整型向量的哈希函数，空间哈希是很高效的数据结构。

std::tuple<Point3, double, double> FitPlane(const Point3List & _points);
std::tuple<Vector2, double, double> FitLine(const Point2List & _points);

上述函数分别用来拟合一组3D点的平面，以及2D点的直线。例如三维坐标系下，平面的参数方程为：\(ax+by+cz+d = 0\)，其中\(n=\{a, b, c\}^T\)是平面的法向量。上述函数返回值类型为std::tuple，其中第一个为平面或者直线的法向量，第二个为\(d\)，第三个是次大奇异值与最大奇异值的一个比值(使用SVD来拟合平面或者直线，可以得到奇异值)，可以用来作为对原始点与拟合平面误差的指标，如果所有点都在平面上，该值为0。也就是平面fit得越好，该值越小。

OnePiece使用Point3List来表示一组Eigen::Matrix<geometry::scalar, 3, 1>，类似的， PointXList表示一组Eigen::Matrix<scalar, Eigen::Dynamic, 1>。另外，有了CPP11标准，我们可以给一个模板类取别名，比如Eigen::Matrix<geometry::Scalar, T, 1>通过下面方式命名：

    template <int T>
        using Vector = Eigen::Matrix<scalar, T, 1>;

这样可以通过Vector<10>来声明一个10维度的向量。因此，我们使用PointList来表示模板向量的数组。 这意味着你可以使用PointList<100>来做std::vector<Eigen::Matrix<scalar, 100, 1>, Eigen::aligned_allocator<Eigen::Matrix<scalar, 100, 1>>>的别名。这个非常有用。

在Geometry.h中还定义了一些其他的别名，用于之后的特征匹配等，这些别名之所以被定义也正是因为起别名更便于理解，如果对基础知识足够了解，一看就知道指的是什么了。

Geometry2d.h

Geometry2d.h实现了一些2d上的几何算法，主要是计算几何的一些非常非常基础的内容。这部分是为了实现之后Algorithm中DCEL(doubly connected edge list)而写的。对于这部分中比较重要的函数有：

//检查一个点是否在一个凸多边形内
int CheckPointInConvexPoly(const geometry::Point2List &points, const Point2 &p);
//计算一个凸多边形的面积
float ComputeAreaConvexPoly(const Point2List &points);

PointCloud.h

PointCloud是点云相关的数据结构与一些简单的处理算法。点云包含点，颜色，法向（后面二者是可选的），相关算法包含了点云的下采样，法向量估计等等。下面列出一些基本的成员函数：

//从PLY文件读取点云
void LoadFromPLY(const std::string &filename);
//从OBJ文件读取点云
void LoadFromOBJ(const std::string &filename);
//从RGBD图读取点云
void LoadFromRGBD(const cv::Mat &rgb, const cv::Mat & depth, const camera::PinholeCamera &camera );
void LoadFromRGBD(const RGBDFrame &rbgd, const camera::PinholeCamera &camera );
//估计法向量，通过对某个点的周围点进行平面拟合。radius代表了搜索半径，knn代表拟合点的数量
void EstimateNormals(float radius = 0.1, int knn = 30);
//根据变换矩阵对点云进行旋转平移
void Transform(const TransformationMatrix &T);
//对点云进行下采样
std::shared_ptr<PointCloud> DownSample(double grid_len);
//将点云写到PLY文件中
bool WriteToPLY(const std::string &fileName) const;
//将点云写到OBJ文件中
bool WriteToOBJ(const std::string &fileName) const;
//与另外一个点云合并
void MergePCD(const PointCloud & another_pcd);

TriangleMesh.h

TriangleMesh定义了三角网格数据结构以及相关的算法，三角网格与点云最大的区别就是有边和面，在三角网格中每个面都是三角形，由3个点构成。因此，除了点之外，三角网格中还有面表，里面存放的是构成面的3个点的索引。目前实现的网格，是不包含纹理的，因为这一块儿作者不懂。

//从PLY文件读取网格
void LoadFromPLY(const std::string &filename);
//从OBJ文件读取网格
void LoadFromOBJ(const std::string &filename);
/*
计算法向量，三角网格的法向量相对于点云的法向量估计就容易多了。
首先，三角网格已经有面了，也就可以直接根据三个点求平面得到面的法向量，
接着，我们只要对与点相连接的所有的面的法向量进行加权平均，
就得到点的法向量
*/
void ComputeNormals(float radius = 0.1, int knn = 30);
//根据变换矩阵对网格进行旋转平移
void Transform(const TransformationMatrix &T);
//将网格写到PLY文件中
bool WriteToPLY(const std::string &fileName) const;
//将网格写到OBJ文件中
bool WriteToOBJ(const std::string &fileName) const;
//基于二次误差的边坍塌简化网格
std::shared_ptr<geometry::TriangleMesh> QuadricSimplify(int target_num) const;
//基于体素的均匀网格简化，类似于点云下采样
std::shared_ptr<geometry::TriangleMesh> ClusteringSimplify(float grid_len) const;
//删除点比较少的碎片
std::shared_ptr<geometry::TriangleMesh> Prune(int min_points) const;

关于网格简化的原理，可以看论文：related_paper/quadrics.pdf。

网格简化效果对比：网格简化

Ransac.h

Ransac.h中主要实现了两个具体的利用ransac的算法，基于一个基本的Ransac框架GRANSAC。Ransac的好处是，它会随机抽取一些点来求某个变换或者模型，这样可以很大程度上排除outlier。比如在拟合平面时，如果利用Geometry.h中FitPlane直接拟合，那么outlier会对结果造成很大影响，而使用Ransac则能得到更好的效果。

//使用RANSAC来估计一组对应点之间的刚体变换
TransformationMatrix EstimateRigidTransformationRANSAC(const PointCorrespondenceSet &correspondence_set,
    PointCorrespondenceSet & inliers, std::vector<int> &inlier_ids, int max_iteration = 2000, float threshold = 0.1);
//使用RANSAC来拟合平面
std::tuple<geometry::Point3, double, double> FitPlaneRANSAC(const Point3List &points, 
    Point3List & inliers, std::vector<int> &inlier_ids, int max_iteration = 2000, float threshold = 0.05);

使用上述Ransac函数，还可以得到对应得inliers的集合。

RGBDFrame.h

RGBDFrame.h定义了RGBDFrame这个数据结构，它整合了深度图和颜色图，可以简化调用过程。这个数据结构主要是为了搭建一个RGBD SLAM系统来服务的。它可以保存一些已经提取的feature，pointcloud等等，这些在追踪时候需要用到。在搭建一个RGBD SLAM时候，这些内容不用重复计算，可以提高效率。Release()用来释放掉所有保存的内容。

KDTree.h

KDTree.h有封装了两个库，你可以选择底层为Opencv的实现，也可以选择nanoflann的实现（通过更改预定义NANO_IMPLEMENTATION），默认是nanoflann，到目前来说，nanoflann是完爆OpenCV的实现的。

OpenCV(不推荐)

对OpenCV中FLANN的封装简化了调用过程。这里发现OpenCV的flann有不少挺坑爹的地方，需要注意（OnePiece使用的是OpenCV-3.4.5）。

调用OpenCV的KDTree最麻烦的地方在于，eigen和opencv的互相转换，而实际上封装kdtree就是为了解决这个。

下面是一段用OpenCv的flann建立kdtree的代码（源自互联网）：

int main() {
    //用于构造kdtree的点集
    vector<cv::Point2f> features = { { 1,1 },{ 2, 2},{ 3, 3},{ 4, 4},{ 2, 4} };
    cv::Mat source = cv::Mat(features).reshape(1);
    source.convertTo(source, CV_32F);

    cv::flann::KDTreeIndexParams indexParams(2);
    cv::flann::Index kdtree(source, indexParams); 

    //预设knnSearch所需参数及容器
    int queryNum = 3;//用于设置返回邻近点的个数
    vector<float> vecQuery(2);//存放查询点的容器
    vector<int> vecIndex(queryNum);//存放返回的点索引
    vector<float> vecDist(queryNum);//存放距离
    cv::flann::SearchParams params(32);//设置knnSearch搜索参数

    //KD树knn查询
    vecQuery = { 3, 4};
    kdtree.knnSearch(vecQuery, vecIndex, vecDist, queryNum, params);

    cout << "vecDist: " << endl;
    for (auto&x : vecDist)
        cout << x << " ";
    cout << endl;

    cout << "vecIndex: " << endl;
    for (auto&x : vecIndex)
        cout << x << " ";

    return 0;
}

需要注意的点： 1. OpenCV的flann不支持double类型 2. 上述代码中，query是vector<float>，如果用cv::Mat(features[0])，会出问题。 3. 对于建立kdtree引入的Input matrix，需要一直维护着，不能释放掉，也不能对它进行别的操作。如果想要将建立kdtree与search分成两个函数，那么就一定要克隆一份input matrix，再用它来建立kdtree，使用中不可以释放掉它。 4. knnSearch不会返回什么值，并且indices与dists需要在开始提前resize(k)，而radiusSearch中，开始时候不需要对indices与dists进行resize，并且会返回查找到的点有多少。一定要根据这个值来访问indices与dists，而不是indices.size()。

不过上面的这些问题已经通过封装解决了。OnePiece中，KDTree使用很简单。它是一个模板类，也就是你需要指定放入树中的数据的维度。建立树时，直接传入geometry中提供的PointList系列（本质上是Eigen向量的vector），可以是动态向量组，也可以是静态向量组。如果是动态向量组，需要让动态向量的维度等于设定模板的维度。此外，通过RadiusSearch与KnnSearch来进行查找，也是可以直接根据Eigen的向量来查询。调用方式类似于OpenCV的方式，只是少了很多需要考虑的麻烦事。

nanoflann

nanoflann是一个c++11的库，非常轻量级，只有一个hpp文件。相对于OpenCV的实现，nanoflann更高效内存占用也更少。在nanoflann中，对于每种需要搜索的点都要重新建立一个类，需要包含某些函数。这些东西也被OnePiece封装好了。nanoflann的radiusSearch原本不能指定max_neighbors，也就是它会找到所有在半径内的点。这个在某些情况下是没有必要的。OnePiece在这里修改了nanoflann的源码，好让它拥有这个feature（当max_neighbors=0时会搜索所有点），提高运行效率。

在调用上，OnePiece将OpenCV与Nanoflann封装成一样的调用方式：

    class SearchParameter
    {
        public:
        //kdtree parameter
        SearchParameter(int _checks = 256, 
            float _eps = 1e-8, bool _sorted = true)
        {
            checks = _checks;
            eps = _eps;
            sorted = _sorted;
        }
        //返回结果根据距离排序
        bool sorted = true;
        //定义EPS
        float eps = 1e-8;
        //递归迭代的次数（搜索深度，越大结果越准确，但是耗时越多）
        int checks = 32;


    };
    template<int T = 3>
    class KDTree
    {
        public:
        KDTree(int _trees = 4)
        {
            trees = 4;
        }
        //从一组高维向量中，建立KDTree，可以选择动态向量的vector，也可以选择静态向量的vector
        void BuildTree(const geometry::PointXList &points);

        void BuildTree(const geometry::PointList<T> &points);
        //strange rules in opencv flann: 
        //when you do knnsearch, you need to resize(k) at beginning.
        //when you do radius search, you need to get the returned points_count and do a resize for the results.
        //半径搜索，可以搜索动态向量和静态向量
        void RadiusSearch(const geometry::VectorX &point, std::vector<int> &indices, 
            std::vector<float > &dists, double radius, int max_result, 
            const SearchParameter &sp = SearchParameter());
        void RadiusSearch(const geometry::Vector<T> &point, std::vector<int> &indices, 
            std::vector<float > &dists, double radius, int max_result, 
            const SearchParameter sp = SearchParameter());
        //knn搜索，可以搜索动态向量和静态向量
        void KnnSearch(const geometry::VectorX &point, std::vector<int> &indices, 
            std::vector<float > &dists, int k, 
            const SearchParameter &sp = SearchParameter());
        void KnnSearch(const geometry::Vector<T> &point, std::vector<int> &indices, 
            std::vector<float > &dists, int k, 
            const SearchParameter &sp = SearchParameter());


#if NANO_IMPLEMENTATION
        //only nanoflann based KDTree supports
        void RadiusSearch(const geometry::VectorX &point, std::vector<size_t> &indices, 
            std::vector<float > &dists, double radius, int max_result, 
            const SearchParameter &sp = SearchParameter());
        void RadiusSearch(const geometry::Vector<T> &point, std::vector<size_t> &indices, 
            std::vector<float > &dists, double radius, int max_result, 
            const SearchParameter sp = SearchParameter());
        //search the k nearest points for a dynamic vector or static vector 
        void KnnSearch(const geometry::VectorX &point, std::vector<size_t> &indices, 
            std::vector<float > &dists, int k, 
            const SearchParameter &sp = SearchParameter());
        void KnnSearch(const geometry::Vector<T> &point, std::vector<size_t> &indices, 
            std::vector<float > &dists, int k, 
            const SearchParameter &sp = SearchParameter());
#endif
    };

基于nanoflann的实现，可以将上述indices换成std::vector<size_t>，来支持更大点云的搜索。

使用KDTree的例子可以查看example/GetLabelUsingKDTree.cpp，输入是两个模型：original model（不带语义信息）与reference model（带有语义信息），通过KDTree找到最近点，从而获得original model中的语义信息。