#include <iostream>
#include <fstream>
using namespace std;

#include <Eigen/Dense>

#include <opencv2/opencv.hpp>

// libicp
//#include "icpPointToPlane.h"

// PCL
#include <pcl/io/pcd_io.h>
#include <pcl/common/common.h>
#include <pcl/common/geometry.h>
#include <pcl/filters/filter.h>
#include <pcl/registration/icp.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_one_to_one.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/correspondence_rejection_organized_boundary.h>
#include <pcl/registration/transformation_estimation_point_to_plane.h>
#include <pcl/registration/transformation_estimation_point_to_plane_weighted.h>
#include <pcl/registration/transformation_validation_euclidean.h>
#include <pcl/visualization/pcl_visualizer.h>

int loadPointCloudPcd(string path, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud)
{
    if(pcl::io::loadPCDFile<pcl::PointXYZ>(path, *cloud) == -1) {
        cerr << "Couldn't read file " << path << endl;
        return -1;
    }
    return 0;
}

// Ref: http://pointclouds.org/documentation/tutorials/iterative_closest_point.php
//template <typename PointT>
bool lm_pose(pcl::PointCloud<pcl::PointXYZ>::Ptr pcSrc,
            pcl::PointCloud<pcl::PointXYZ>::Ptr pcTrg,
            Eigen::Matrix4d &trans)
{
    pcl::registration::TransformationEstimationLM<pcl::PointXYZ, pcl::PointXYZ, double> transEstLM;
    transEstLM.estimateRigidTransformation(*pcSrc, *pcTrg, trans);

    return true;
}

int main(int argc, char** argv)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr sourceCloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr targetCloud(new pcl::PointCloud<pcl::PointXYZ>);

    std::string sourceFile = "sourceCloud.pcd";
    std::string targetFile = "targetCloud.pcd";

    Eigen::Matrix4d trans = Eigen::Matrix4d::Identity();

    loadPointCloudPcd(sourceFile, sourceCloud);
    loadPointCloudPcd(targetFile, targetCloud);

    lm_pose(sourceCloud, targetCloud, trans);

    std::cout << "transformation: " << trans << std::endl;
}