# pragma once

// 各种头文件

// C++标准库

#include <fstream>

#include <vector>

using namespace std;

// OpenCV

#include <opencv2/core/core.hpp>

#include <opencv2/highgui/highgui.hpp>

//PCL

#include <pcl/io/pcd_io.h>

#include <pcl/point_types.h>

// 类型定义

typedef pcl::PointXYZRGBA PointT;

typedef pcl::PointCloud<PointT> PointCloud;

// 相机内参结构

struct CAMERA_INTRINSIC_PARAMETERS

{

    double cx, cy, fx, fy, scale;

};

// 函数接口

// image2PonitCloud 将rgb图和深度图转换为点云

PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera );

// point2dTo3d 将单个点从图像坐标转换为空间坐标

// input: 3维点Point3f (u,v,d)

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera );

#include "slamBase.h"

//传入rgb, depth, 和相机内参

PointCloud::Ptr image2PointCloud( cv::Mat& rgb, cv::Mat& depth, CAMERA_INTRINSIC_PARAMETERS& camera )

{

    PointCloud::Ptr cloud ( new PointCloud );

    for (int m = ; m < depth.rows; m++)

        for (int n=; n < depth.cols; n++)

        {

            // 获取深度图中(m,n)处的值

            ushort d = depth.ptr<ushort>(m)[n];

            // d 可能没有值，若如此，跳过此点

            if (d == )

                continue;

            // d 存在值，则向点云增加一个点

            PointT p;

            // 计算这个点的空间坐标

            p.z = double(d) / camera.scale;

            p.x = (n - camera.cx) * p.z / camera.fx;

            p.y = (m - camera.cy) * p.z / camera.fy;

            // 从rgb图像中获取它的颜色

            // rgb是三通道的BGR格式图，所以按下面的顺序获取颜色

            p.b = rgb.ptr<uchar>(m)[n*];

            p.g = rgb.ptr<uchar>(m)[n*+];

            p.r = rgb.ptr<uchar>(m)[n*+];

            // 把p加入到点云中

            cloud->points.push_back( p );

        }

    // 设置并保存点云

    cloud->height = ;

    cloud->width = cloud->points.size();

    cloud->is_dense = false;

    return cloud;

}

//像素坐标转为3维点

cv::Point3f point2dTo3d( cv::Point3f& point, CAMERA_INTRINSIC_PARAMETERS& camera )

{

    cv::Point3f p; // 3D 点

    p.z = double( point.z ) / camera.scale;  //point.z  d

    p.x = ( point.x - camera.cx) * p.z / camera.fx;  //point.x  u

    p.y = ( point.y - camera.cy) * p.z / camera.fy;  //point.y  v

    return p;

}

# 增加PCL库的依赖

FIND_PACKAGE( PCL REQUIRED COMPONENTS common io )

list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") # use this in Ubuntu 16.04

# 增加opencv的依赖

FIND_PACKAGE( OpenCV REQUIRED )

INCLUDE_DIRECTORIES( ${OpenCV_INCLUDE_DIRS} )

# 添加头文件和库文件

ADD_DEFINITIONS( ${PCL_DEFINITIONS} )

INCLUDE_DIRECTORIES( ${PCL_INCLUDE_DIRS}  )

LINK_LIBRARIES( ${PCL_LIBRARY_DIRS} )

#把slamBase.cpp编译成 slamBase 库，并把该库里调到的OpenCV和PCL的部分，和相应的库链接起来

ADD_LIBRARY( slambase slamBase.cpp )   # 实现文件  slamBase.cpp

TARGET_LINK_LIBRARIES( slambase

    ${OpenCV_LIBS}

    ${PCL_LIBRARIES} )

ADD_EXECUTABLE( detectFeatures detectFeatures.cpp ) #  可执行程序  detectFeatures.cpp

TARGET_LINK_LIBRARIES( detectFeatures

    slambase

    ${OpenCV_LIBS}

    ${PCL_LIBRARIES} )

#include<iostream>

#include "slamBase.h" //

using namespace std;

// OpenCV 特征检测模块

#include <opencv2/features2d/features2d.hpp>

// #include <opencv2/nonfree/nonfree.hpp> // use this if you want to use SIFT or SURF

#include <opencv2/calib3d/calib3d.hpp>

int main( int argc, char** argv )

{

    // 声明并从data文件夹里读取两个rgb与深度图

    cv::Mat rgb1 = cv::imread( "./data/rgb20.png");

    cv::Mat rgb2 = cv::imread( "./data/rgb21.png");

    cv::Mat depth1 = cv::imread( "./data/depth20.png", -);

    cv::Mat depth2 = cv::imread( "./data/depth21.png", -);

    // 声明特征提取器与描述子提取器

    cv::Ptr<cv::FeatureDetector> detector;

    cv::Ptr<cv::DescriptorExtractor> descriptor;

    // 构建提取器，默认两者都为 ORB

    // 如果使用 sift, surf ，之前要初始化nonfree模块

    // cv::initModule_nonfree();

    // _detector = cv::FeatureDetector::create( "SIFT" );

    // _descriptor = cv::DescriptorExtractor::create( "SIFT" );

    detector = cv::ORB::create();

    descriptor = cv::ORB::create();

    vector< cv::KeyPoint > kp1, kp2; //关键点

    detector->detect( rgb1, kp1 );  //提取关键点

    detector->detect( rgb2, kp2 );

    cout<<"Key points of two images: "<<kp1.size()<<", "<<kp2.size()<<endl;

    // 可视化， 显示关键点

    cv::Mat imgShow;

    cv::drawKeypoints( rgb1, kp1, imgShow, cv::Scalar::all(-), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS );

    cv::namedWindow("Keypoints",);

    cv::imshow( "keypoints", imgShow );

    cv::imwrite( "./data/keypoints.png", imgShow );

    cv::waitKey(); //暂停等待一个按键

    // 计算描述子

    cv::Mat desp1, desp2;

    descriptor->compute( rgb1, kp1, desp1 );

    descriptor->compute( rgb2, kp2, desp2 );

    // 匹配描述子

    vector< cv::DMatch > matches;

    cv::BFMatcher matcher;

    matcher.match( desp1, desp2, matches );

    cout<<"Find total "<<matches.size()<<" matches."<<endl;

    // 可视化：显示匹配的特征

    cv::Mat imgMatches;

    cv::drawMatches( rgb1, kp1, rgb2, kp2, matches, imgMatches );

    cv::namedWindow("matches",);

    cv::imshow( "matches", imgMatches );

    cv::imwrite( "./data/matches.png", imgMatches );

    cv::waitKey(  );

    // 筛选匹配，把距离太大的去掉

    // 这里使用的准则是去掉大于四倍最小距离的匹配

    vector< cv::DMatch > goodMatches;

    double minDis = ;

    for ( size_t i=; i<matches.size(); i++ )

    {

        if ( matches[i].distance < minDis )

            minDis = matches[i].distance;

    }

    cout<<"min dis = "<<minDis<<endl;

    for ( size_t i=; i<matches.size(); i++ )

    {

        if (matches[i].distance < *minDis)

            goodMatches.push_back( matches[i] );

    }

    // 显示 good matches

    cout<<"good matches="<<goodMatches.size()<<endl;

    cv::drawMatches( rgb1, kp1, rgb2, kp2, goodMatches, imgMatches );

    cv::namedWindow("good matches",);

    cv::imshow( "good matches", imgMatches );

    cv::imwrite( "./data/good_matches.png", imgMatches );

    cv::waitKey();

    // 计算图像间的运动关系

    // 关键函数：cv::solvePnPRansac()

    // 为调用此函数准备必要的参数

    // 第一个帧的三维点

    vector<cv::Point3f> pts_obj;

    // 第二个帧的图像点

    vector< cv::Point2f > pts_img;

    // 相机内参,使用到结构

    CAMERA_INTRINSIC_PARAMETERS C;

    C.cx = 682.3;

    C.cy = 254.9;

    C.fx = 979.8;

    C.fy = 942.8;

    C.scale = 1000.0;

    for (size_t i=; i<goodMatches.size(); i++)

    {

        // query 是第一个, train 是第二个

        cv::Point2f p = kp1[goodMatches[i].queryIdx].pt;

        // 获取d是要小心！x是向右的，y是向下的，所以y才是行，x是列！

        ushort d = depth1.ptr<ushort>( int(p.y) )[ int(p.x) ];

        if (d == )

            continue;

        pts_img.push_back( cv::Point2f( kp2[goodMatches[i].trainIdx].pt ) );

        // 将(u,v,d)转成(x,y,z)

        cv::Point3f pt ( p.x, p.y, d ); //   深度值/scale = z

        cv::Point3f pd = point2dTo3d( pt, C );

        pts_obj.push_back( pd );

    }

    //使用到结构

    double camera_matrix_data[][] = {

        {C.fx, , C.cx},

        {, C.fy, C.cy},

        {, , }

    };

    // 构建相机矩阵

    cv::Mat cameraMatrix( , , CV_64F, camera_matrix_data );

    cv::Mat rvec, tvec, inliers;

    // 求解pnp

    cv::solvePnPRansac( pts_obj, pts_img, cameraMatrix, cv::Mat(), rvec, tvec, false, , 1.0, 0.95, inliers ,cv::SOLVEPNP_ITERATIVE);

    // 求旋转向量和平移向量，旋转矩阵

    cout<<"inliers: "<<inliers.rows<<endl;

    cout<<"R="<<rvec<<endl;

    cout<<"t="<<tvec<<endl;
    //旋转矩阵
    cv::Mat R;
    cv::Rodrigues(rvec,R);
    cout<<"R_matrix="<<R<<endl;
 

    // 画出inliers匹配

    vector< cv::DMatch > matchesShow;

    for (size_t i=; i<inliers.rows; i++)

    {

        matchesShow.push_back( goodMatches[inliers.ptr<int>(i)[]] );

    }

    cv::drawMatches( rgb1, kp1, rgb2, kp2, matchesShow, imgMatches );

    cv::namedWindow("inlier matches",);

    cv::imshow( "inlier matches", imgMatches );

    cv::imwrite( "./data/inliers.png", imgMatches );

    cv::waitKey(  );

    return ;

}

CMAKE_MINIMUM_REQUIRED( VERSION 2.8 )

PROJECT( slam )

SET(CMAKE_CXX_COMPILER "g++")

SET( CMAKE_BUILD_TYPE Debug  )

SET(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) #可执行文件输出的文件夹

SET(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) #库函数编译输出位置

INCLUDE_DIRECTORIES( ${PROJECT_SOURCE_DIR}/include ) #头文件

LINK_DIRECTORIES( ${PROJECT_SOURCE_DIR}/lib) #使用编译好的库文件libslamBase.a

ADD_SUBDIRECTORY( ${PROJECT_SOURCE_DIR}/src ) #可执行程序