#pragma once

#include "SG_algo_Export.h"
#include <vector>
#include <opencv2/opencv.hpp>


//滤除离群点:z跳变门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier(
    SVzNL3DPosition* lineData, 
    int dataSize, 
    SSG_outlierFilterParam filterParam, 
    std::vector<SVzNL3DPosition>& filerData, 
    std::vector<int>& noisePts);
//滤除离群点：z跳变门限方法,改变原始数据
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_changeOriginData(
    SVzNL3DPosition* lineData, 
    int dataSize, 
    SSG_outlierFilterParam filterParam);
//滤除离群点：z跳变门限方法, vecotr对象
SG_APISHARED_EXPORT void wd_vectorDataRemoveOutlier_overwrite(
    std::vector<SVzNL3DPosition>& a_line,
    SSG_outlierFilterParam filterParam);
//滤除离群点：点距离门限方法（大于门限视为不连续，根据连续段点数量判断噪声）
SG_APISHARED_EXPORT void sg_lineDataRemoveOutlier_ptDistMethod(
    SVzNL3DPosition* lineData,
    int dataSize,
    SSG_outlierFilterParam filterParam,
    std::vector<SVzNL3DPosition>& filerData,
    std::vector<int>& noisePts);

//平滑
SG_APISHARED_EXPORT void sg_lineDataSmoothing(
    std::vector<SVzNL3DPosition>& input, 
    int smoothWin, 
    std::vector<SVzNL3DPosition>& output);
//分段平滑，这样不会影响分段端点
SG_APISHARED_EXPORT void sg_lineSegSmoothing(
    std::vector<SVzNL3DPosition>& input,
    double seg_y_deltaTh, //分段的Y间隔。大于此间隔，为新的分段
    double seg_z_deltaTh,//分段的Z间隔。大于此间隔，为新的分段
    int smoothWin,
    std::vector<SVzNL3DPosition>& output);

//VZ_APISHARED_EXPORT void sg_getLineMeanVar();
SG_APISHARED_EXPORT void sg_getLineLVFeature(
    SVzNL3DPosition* lineData, 
    int dataSize, 
    int lineIdx, 
    const SSG_slopeParam slopeParam, 
    const SSG_VFeatureParam valleyPara, 
    SSG_lineFeature* line_features);

//获取扫描线拐点特征
SG_APISHARED_EXPORT void sg_getLineCornerFeature(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
    SSG_lineFeature* line_features);

// 对激光线上由Mask(nPointIdx转义定义）指定的点提取激光线上的拐点特征
SG_APISHARED_EXPORT void sg_maskData_getLineCornerFeature(
    std::vector< SVzNL3DPosition>& lineData,
    int lineIdx,
    int maskID,
    const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
    std::vector<SSG_basicFeature1D>& features);

SG_APISHARED_EXPORT void sg_getLineCornerFeature_BQ(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    double refSteppingZ,
    const SSG_cornerParam cornerPara, 
    std::vector<SSG_basicFeature1D>& line_features);

SG_APISHARED_EXPORT void wd_getLineDataIntervals(
    std::vector<SVzNL3DPosition>& lineData,
    const SSG_lineSegParam lineSegPara,
    std::vector<SSG_RUN>& segs);

// 最小点集数量（小于此数无法拟合直线）
const int MIN_POINT_COUNT = 3;
//使用端点直线，检查点到直线的距离，大于门限的分割
SG_APISHARED_EXPORT void split(
    SSG_RUN a_run,
    std::vector< SVzNL3DPosition>& lineData,
    const double maxError,
    std::vector< SSG_RUN>& lineSegs);

/// <summary>
/// 提取激光线上的特征：跳变、低于z阈值、V及L型，用于粒径检测（PSM)
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
SG_APISHARED_EXPORT void wd_getLineCornerFeature_PSM(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const double groundZ,
    const SSG_cornerParam cornerPara,
    SSG_lineFeature* line_features);

/// 提取激光线上的圆环的上半段弧。宽度由圆环的宽度确定
/// seg端点：z距离大于门限
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
SG_APISHARED_EXPORT void wd_getRingArcFeature(
    std::vector< SVzNL3DPosition>& lineData,
    int lineIdx,
    const SSG_cornerParam cornerPara,
    double ringArcWidth, //定子的环宽度
    std::vector<SWD_segFeature>& line_ringArcs  //环
);

//直线特征提取：对split-and-merge法作了简化，以起点终点直线代替拟合直线
SG_APISHARED_EXPORT void wd_surfaceLineSegment(
    std::vector< SVzNL3DPosition>& lineData,
    int lineIdx,
    SVzNLRangeD lineLenRange,
    const SSG_lineSegParam lineSegPara,
    std::vector<SSG_featureSemiCircle>& lineSegs,
    std::vector<SSG_featureSemiCircle>& invlaidLineSegs);

/// <summary>
/// 提取激光线上的Jumping特征
/// nPointIdx被重新定义成Feature类型
/// 算法流程：
/// （1）逐点计算前向角和后向角
/// （2）逐点计算拐角，顺时针为负，逆时针为正
/// （3）搜索正拐角的极大值。
/// （4）判断拐角是否为跳变
/// </summary>
SG_APISHARED_EXPORT void sg_getLineJumpFeature_cornerMethod(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const SSG_cornerParam cornerPara, //scale通常取bagH的1/4
    std::vector< SSG_basicFeature1D>& jumpFeatures);

SG_APISHARED_EXPORT void wd_getLineGloveArcs(
    std::vector<SVzNL3DPosition>& lineData,
    int lineIdx,
    const SSG_gloveArcParam arcPara,
    std::vector<SSG_basicFeature1D>& gloveArcs);

/// 提取激光线上的圆柱形特征
SG_APISHARED_EXPORT void sg_getLineUpperSemiCircleFeature(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const double sieveDiameter,
    const SSG_slopeParam slopePara, 
    std::vector< SSG_featureSemiCircle>& line_features,
    cv::Mat& holeMask);

/// <summary>
/// 提取激光线上的极值点（极大值点和极小值点）
/// 
/// </summary>
SG_APISHARED_EXPORT void sg_getLineLocalPeaks(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const double scaleWin,
    std::vector< SSG_basicFeature1D>& localMax,
    std::vector< SSG_basicFeature1D>& localMin);

SG_APISHARED_EXPORT void sg_getFlatLineLocalPeaks_vector(
    std::vector<SVzNL3DPosition>& lineData,
    int lineIdx,
    const double scaleWin,
    const double minPkHeighth,
    const double holeR,
    std::vector< SSG_basicFeature1D>& localMax);

SG_APISHARED_EXPORT void sg_getLineDownJumps(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const double jumpTh,
    std::vector< SSG_basicFeature1D>& downJumps);

//对feature进行生长
SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees(
    std::vector<SSG_lineFeature>& lineFeatures,
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam);

//对feature进行生长:不比较type，只判断位置是否相邻
SG_APISHARED_EXPORT void wd_getFeatureGrowingTrees_noTypeMatch(
    std::vector<SSG_lineFeature>& lineFeatures,
    std::vector<SSG_featureTree>& feature_trees,
    std::vector<SSG_featureTree>& ending_trees,
    SSG_treeGrowParam growParam);

//对segment feature进行生长
SG_APISHARED_EXPORT void wd_getSegFeatureGrowingTrees(
    std::vector<std::vector<SWD_segFeature>>& all_lineFeatures,
    std::vector<SWD_segFeatureTree>& feature_trees,
    SSG_treeGrowParam growParam);

SG_APISHARED_EXPORT void sg_lineFeaturesGrowing(
    int lineIdx,
    bool isLastLine,
    std::vector<SSG_basicFeature1D>& features,
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam);

//SG_APISHARED_EXPORT void sg_getTreeROI(SSG_featureTree* a_tree);

//对ending进行生长
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees(
    std::vector<SSG_2DValueI>& lineEndings,
    SVzNL3DLaserLine* laser3DPoints,
    bool isVScan,
    int featureType,
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam);

//对ending进行生长，垂直扫描时只进行水平生长；水平扫描时只进行垂直生长
SG_APISHARED_EXPORT void sg_getEndingGrowingTrees_angleCheck(
    std::vector<SSG_2DValueI>& lineEndings,
    SVzNL3DLaserLine* laser3DPoints,
    bool isVScan,
    int featureType,
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam,
    double angleCheckScale);

//对扫描线上的
SG_APISHARED_EXPORT void sg_LVFeatureGrowing(
    std::vector<SSG_lineFeature>& lineFeatures, 
    std::vector<SSG_featureTree>& trees, 
    SSG_bagParam bagParam, 
    SSG_treeGrowParam growParam, 
    std::vector<SSG_2DValueI>& edgePts_0,
    std::vector<SSG_2DValueI>& edgePts_1);

SG_APISHARED_EXPORT void sg_peakFeatureGrowing(
    std::vector<std::vector< SSG_basicFeature1D>>& lineFeatures,
    std::vector<SSG_featureTree>& trees,
    SSG_treeGrowParam growParam);

//semiCircle特征生长
SG_APISHARED_EXPORT void sg_getFeatureGrowingTrees_semiCircle(
    std::vector< SSG_featureSemiCircle>& lineFeatures,
    const int lineIdx,
    const int lineSize,
    std::vector<SSG_semiCircleFeatureTree>& trees,
    std::vector<SSG_semiCircleFeatureTree>& stopTrees,
    std::vector<SSG_semiCircleFeatureTree>& invalidTrees, //被移除的树，这些树可能将目标分成多个树，从而被移除。
    SSG_treeGrowParam growParam);

SSG_meanVar _computeMeanVar(double* data, int size);

///搜索局部最高点（z最小点）。
///搜索方法：每次步进搜索窗口长度的一半。对局部最高点进行标记，防止被重复记录。
SG_APISHARED_EXPORT void sg_getLocalPeaks(
    SVzNL3DLaserLine* scanLines, 
    int lineNum, 
    std::vector<SSG_2DValueI>& peaks, 
    SSG_localPkParam searchWin);

/// <summary>
/// 区域生长法：以局部最高点作为生长种子进行生长
/// 生长方法与一般的区域生长不同：以种子点为圆心作圆周扫描，记录扫描到的边界
/// </summary>
//SG_APISHARED_EXPORT void sg_peakPolarScan(cv::Mat& edgeMask, SVzNL2DPoint a_peak, SSG_polarScanParam polarScanParam, std::vector< SSG_2DValueI>& rgnContour);
//从Peak点进行水平垂直扫描得到区域边界
SG_APISHARED_EXPORT void sg_peakXYScan(
    SVzNL3DLaserLine* laser3DPoints, 
    int lineNum, 
    cv::Mat& featureEdgeMask, 
    SSG_2DValueI a_peak, 
    SSG_treeGrowParam growParam, 
    SSG_bagParam bagParam,
    bool rgnPtAsEdge,
    std::vector< SSG_lineConotours>& topContour,
    std::vector< SSG_lineConotours>& bottomContour,
    std::vector< SSG_lineConotours>& leftContour,
    std::vector< SSG_lineConotours>& rightContour,
    int* maxEdgeId_top, 
    int* maxEdgeId_btm, 
    int* maxEdgeId_left, 
    int* maxEdgeId_right);

//取出与给定edgeId相同的边界点
SG_APISHARED_EXPORT void sg_getContourPts(
    std::vector< SSG_lineConotours>& contour_all,
    int vldEdgeId,
    std::vector< SSG_2DValueI>& contourFilter, 
    int* lowLevelFlag);

//从边界点对中取出与给定edgeId相同的边界点
SG_APISHARED_EXPORT void sg_getPairingContourPts(
    std::vector<SSG_conotourPair>& contourPairs,
    std::vector<SSG_intPair>& idPairs,
    std::vector< SSG_conotourPair>& contourFilter,
    SVzNLRangeD range,
    bool isTBDir,
    int* lowLevelFlag_0,
    int* lowLevelFlag_1);

//取出最短距离的边界点
SG_APISHARED_EXPORT void sg_contourPostProc(
    std::vector< SSG_contourPtInfo>& contour, 
    int maxEdgeIdx, 
    double sameConturDistTh,
    std::vector< SSG_2DValueI>& contourFilter, 
    int sideID,  
    int* blockFlag);

//距离变换
//input, output均为float型
SG_APISHARED_EXPORT void sg_distanceTrans(const cv::Mat input, cv::Mat& output, int distType);

/// <summary>
/// 以5x5方式寻找localPeaks
/// </summary>
/// <param name="input"></param>
/// <param name="peaks"></param>
SG_APISHARED_EXPORT void sg_getLocalPeaks_distTransform(
    cv::Mat& input, 
    std::vector<SSG_2DValueI>& peaks, 
    SSG_localPkParam searchWin);

/// <summary>
/// 使用模板法提取直角特征
/// 水平向下直角特征：拐点左侧deltaZ在一个很小的范围内；拐点右侧deltaY在一个很小的范围内
/// </summary>
SG_APISHARED_EXPORT void sg_getLineRigthAngleFeature(
    SVzNL3DPosition* lineData,
    int dataSize,
    int lineIdx,
    const SSG_lineRightAngleParam templatePara_HF,
    const SSG_lineRightAngleParam templatePara_FH,
    const SSG_lineRightAngleParam templatePara_HR,
    const SSG_lineRightAngleParam templatePara_RH,
    SSG_lineFeature* line_features);

//计算扫描ROI
SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum);
//计算扫描ROI: vecotr格式
SG_APISHARED_EXPORT SVzNL3DRangeD sg_getScanDataROI_vector(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines
);

//计算点云的ROI和scale: vecotr格式
SG_APISHARED_EXPORT SWD_pointCloudPara wd_getPointCloudPara(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines);

//XY平面直线拟合
SG_APISHARED_EXPORT void lineFitting(
    std::vector< SVzNL3DPoint>& inliers,
    double* _k,
    double* _b);

//拟合成通用直线方程，包括垂直
SG_APISHARED_EXPORT void lineFitting_abc(
    std::vector< SVzNL3DPoint>& inliers,
    double* _a,
    double* _b,
    double* _c);

//圆最小二乘拟合
SG_APISHARED_EXPORT double fitCircleByLeastSquare(
    const std::vector<SVzNL3DPoint>& pointArray,
    SVzNL3DPoint& center,
    double& radius);

//抛物线最小二乘拟合 y=ax^2 + bx + c
SG_APISHARED_EXPORT bool leastSquareParabolaFitEigen(
    const std::vector<cv::Point2d>& points,
    double& a, double& b, double& c,
    double& mse, double& max_err);

//计算Z均值
SG_APISHARED_EXPORT double computeMeanZ(std::vector< SVzNL3DPoint>& pts);

//计算角度差值，在0-180度范围
SG_APISHARED_EXPORT double computeAngleDiff(double theta1, double theta2);

//计算直线交点
SG_APISHARED_EXPORT SVzNL3DPoint computeLineCrossPt_abs(
    double a1, double b1, double c1,
    double a2, double b2, double c2);

//计算过2点的直线方程
SG_APISHARED_EXPORT void compute2ptLine(
    SVzNL3DPoint pt1, 
    SVzNL3DPoint pt2, 
    double* _a, double* _b, double* _c);

//计算过2点的直线方程,不使用结构体
SG_APISHARED_EXPORT void compute2ptLine_2(
    double x1, double y1,
    double x2, double y2,
    double* _a, double* _b, double* _c);

//旋转45度后的直线方程
SG_APISHARED_EXPORT void rotateLine45Deg(
    double _a, double _b, double _c,
    double x0, double y0,
    double* r_a, double* r_b, double* r_c);

//计算直线角度
SG_APISHARED_EXPORT double getLineAngle(const double _a, const double _b, const double _c);

//计算两点的2D距离
SG_APISHARED_EXPORT double compute2DLen(SVzNL3DPoint pt1, SVzNL3DPoint pt2);

//计算点到直线距离 
SG_APISHARED_EXPORT double computePtDistToLine(double x0, double y0, double a, double b, double c);

//计算垂足点，直线方程：y = kx + b
SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint(
    double x0,
    double y0,
    double k,
    double b);

//计算垂足点，直线方程：ax+by+c = 0
SG_APISHARED_EXPORT SVzNL2DPointD sx_getFootPoint_abc(
    double x0,
    double y0,
    double A,
    double B,
    double C);

//Bresenham算法
SG_APISHARED_EXPORT void drawLine(
    int x0, 
    int y0, 
    int x1, 
    int y1, 
    std::vector<SVzNL2DPoint>& pts);

/// <summary>
/// 两步法标注
/// </summary>
/// <param name="bwImg"> 目标点为“1”， 空白点为“0”</param>
/// <param name="labImg"> 标注结果。每个点为rgnID, ID从2开始 </param>
/// <param name="labelRgns"></param>
SG_APISHARED_EXPORT void SG_TwoPassLabel(
    const cv::Mat& bwImg, 
    cv::Mat& labImg, 
    std::vector<SSG_Region>& labelRgns,
    int connectivity = 4);

//计算面参数: z = Ax + By + C
//res: [0]=A, [1]= B, [2]=-1.0, [3]=C,
SG_APISHARED_EXPORT void vzCaculateLaserPlane(
    std::vector<cv::Point3f> Points3ds,
    std::vector<double>& res);

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum);

SG_APISHARED_EXPORT SSG_planeCalibPara sg_HCameraVScan_getGroundCalibPara(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines);

//将一个平面调整为水平平面(z为固定值），
SG_APISHARED_EXPORT SSG_planeCalibPara adjustPlaneToXYPlane(
    double plane_A, double plane_B, double plane_C  //平面法向量
);

//计算一个平面调平参数。
//数据输入中可以有一个地平面和参考调平平面，以最高的平面进行调平
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara2(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines);

//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SG_APISHARED_EXPORT SSG_planeCalibPara sg_getPlaneCalibPara_ROIs(
    SVzNL3DLaserLine* laser3DPoints,
    int lineNum,
    std::vector<SVzNL3DRangeD>& ROIs);

//计算一个平面调平参数。
//以数据输入中ROI以内的点进行平面拟合，计算调平参数
//旋转矩阵为调平参数，即将平面法向调整为垂直向量的参数
SSG_planeCalibPara sg_getPlaneCalibPara2_ROI(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines,
    SVzNL3DRangeD roi);

// 从旋转矩阵计算欧拉角（Z-Y-X顺序）
SG_APISHARED_EXPORT SSG_EulerAngles rotationMatrixToEulerZYX(const double R[3][3]);
// 从欧拉角计算旋转矩阵（Z-Y-X顺序）
SG_APISHARED_EXPORT void eulerToRotationMatrixZYX(const SSG_EulerAngles& angles, double R[3][3]);

//相机姿态调平，并去除地面
///camPoseR为3x3矩阵
SG_APISHARED_EXPORT void lineDataRT(
    SVzNL3DLaserLine* a_line,
    const double* camPoseR,
    double groundH);
SG_APISHARED_EXPORT void lineDataRT_vector(
    std::vector< SVzNL3DPosition>& a_line,
    const double* camPoseR, 
    double groundH);
SG_APISHARED_EXPORT void HCamera_lineDataRT_vector(
    std::vector< SVzNL3DPosition>& a_line,
    const double* camPoseR,
    double groundH);
SG_APISHARED_EXPORT void lineDataRT_RGBD(
    SVzNLXYZRGBDLaserLine* a_line,
    const double* camPoseR,
    double groundH);

//基于相邻点的聚类
SG_APISHARED_EXPORT void sg_pointClustering(
    std::vector< SVzNL3DPosition>& pts,
    double clusterDist,
    std::vector<std::vector< SVzNL3DPosition>>& objClusters  //result
);

//基于栅格上点的窗口内的相邻点的聚类，聚类条件由3D点的邻域决定
//使用vector构成2维结构体数组
SG_APISHARED_EXPORT void wd_pointClustering2D(
    std::vector<std::vector<SSG_featureClusteringInfo>>& featureMask,
    std::vector<std::vector<SVzNL3DPoint>>& feature3DInfo,
    int clusterCheckWin, //搜索窗口
    SSG_treeGrowParam growParam,//聚类条件
    int clusterID, //当前Cluster的ID
    std::vector< SVzNL2DPoint>& a_cluster,  //result
    SVzNL3DRangeD& clusterRoi  //roi3D
);

//对栅格化数据进行XY平面上的投影量化，并对量化产生的空白点进行插值
SG_APISHARED_EXPORT void pointClout2DProjection(
    std::vector< std::vector<SVzNL3DPosition>>& gridScanData,
    SVzNLRangeD x_range,
    SVzNLRangeD y_range,
    double scale,
    double cuttingGrndZ,
    int edgeSkip,
    double inerPolateDistTh,  //插值阈值。大于此阈值的不进行量化插值
    cv::Mat& projectionData,//投影量化数据，初始化为一个极大值1e+6
    cv::Mat& backIndexing  //标记坐标索引，用于回找3D坐标
);

//分水岭算法
SG_APISHARED_EXPORT void watershed(SWD_waterShedImage& img);
// 根据输入的种子点进行分水岭算法
SG_APISHARED_EXPORT void wd_seedWatershed(
    SWD_waterShedImage& img, 
    std::vector<SSG_2DValueI>& watershedSeeds, //种子点
    int maxLevel, //最大水位
    int startMakerID  //起始Marker的ID
);

//点云滤波预处理
SG_APISHARED_EXPORT void wd_noiseFilter(
    std::vector< std::vector<SVzNL3DPosition>>& scanLines,
    const SSG_outlierFilterParam filterParam,
    int* errCode);

// Eigen库实现：计算从pts1向pts2的旋转平移矩阵
//需要
SG_APISHARED_EXPORT void caculateRT(
    const std::vector<cv::Point3d>& pts1,
    const std::vector<cv::Point3d>& pts2,
    cv::Mat& R, cv::Mat& T,
    cv::Point3d& C1, cv::Point3d& C2);

//计算点旋转平移后的位置
SG_APISHARED_EXPORT void pointRT(const cv::Mat& R, const cv::Mat& T,
    const cv::Point3d& originPt, const cv::Point3d& rtOriginPT,  //RT(旋转平移）前后的质心
    const cv::Point3d& pt, cv::Point3d& rtPt);  //RT前后的点