pcl::PointCloud<RIFT32>::Ptr processRIFT(
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud) {
// ------------------------------------------------------------------
// -----Read ply file-----
// ------------------------------------------------------------------
//Asign pointer to the keypoints cloud
/*pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudColor(
 new pcl::PointCloud<pcl::PointXYZRGB>);
 pcl::PointCloud<pcl::PointXYZRGB>& point_cloud = *cloudColor;
 if (pcl::io::loadPLYFile(filename, point_cloud) == -1) {
 cerr << "Was not able to open file "" << filename << "".n";
 printUsage("");
 }*/

// Object for storing the point cloud with intensity value.
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIntensityGlobal(
        new pcl::PointCloud<pcl::PointXYZI>);
// Convert the RGB to intensity.
pcl::PointCloudXYZRGBtoXYZI(*cloud, *cloudIntensityGlobal);


pcl::PointCloud<pcl::PointWithScale> sifts = processSift(cloud);
//We find the corresponding point of the sift keypoint in the original
//cloud and store it with RGB so that it can be transformed into
intensity
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_Color(
        new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>& point_cloud_sift = *cloud_Color;
for (int i = 0; i < sifts.points.size(); ++i) {
    pcl::PointWithScale pointSift = sifts.points[i];
    pcl::PointXYZRGB point;
    for (int j = 0; j < cloud->points.size(); ++j) {
        point = cloud->points[j];
        /*if (pointSift.x == point.x && pointSift.y == point.y
         && pointSift.z == point.z) {*/

        if (sqrt(
                pow(pointSift.x - point.x, 2)
                        + pow(pointSift.y - point.y, 2)
                        + pow(pointSift.z - point.z, 2)) < 0.005) {
            point_cloud_sift.push_back(point);
            //std::cout << point.x << " " << point.y << " " << point.z
<< std::endl;
            break;
        }
    }
}

cout << "Keypoint cloud has " << point_cloud_sift.points.size()
        << " pointsn";

// Object for storing the point cloud with intensity value.
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIntensityKeypoints(
        new pcl::PointCloud<pcl::PointXYZI>);
// Object for storing the intensity gradients.
pcl::PointCloud<pcl::IntensityGradient>::Ptr gradients(
        new pcl::PointCloud<pcl::IntensityGradient>);
// Object for storing the normals.
pcl::PointCloud<pcl::Normal>::Ptr normals(new
pcl::PointCloud<pcl::Normal>);
// Object for storing the RIFT descriptor for each point.
pcl::PointCloud<RIFT32>::Ptr descriptors(new pcl::PointCloud<RIFT32>
());

// Convert the RGB to intensity.
pcl::PointCloudXYZRGBtoXYZI(*cloud_Color, *cloudIntensityKeypoints);
std::cout << "Size: " << cloudIntensityKeypoints->points.size() <<
"n";

// Estimate the normals.
pcl::NormalEstimation<pcl::PointXYZI, pcl::Normal> normalEstimation;
normalEstimation.setInputCloud(cloudIntensityGlobal);
//normalEstimation.setSearchSurface(cloudIntensityGlobal);
normalEstimation.setRadiusSearch(0.03);
pcl::search::KdTree<pcl::PointXYZI>::Ptr kdtree(
        new pcl::search::KdTree<pcl::PointXYZI>);
normalEstimation.setSearchMethod(kdtree);
normalEstimation.compute(*normals);

// Compute the intensity gradients.
pcl::IntensityGradientEstimation<pcl::PointXYZI, pcl::Normal,
        pcl::IntensityGradient,
        pcl::common::IntensityFieldAccessor<pcl::PointXYZI> > ge;
ge.setInputCloud(cloudIntensityGlobal);
//ge.setSearchSurface(cloudIntensityGlobal);
ge.setInputNormals(normals);
ge.setRadiusSearch(0.03);
ge.compute(*gradients);

// RIFT estimation object.
pcl::RIFTEstimation<pcl::PointXYZI, pcl::IntensityGradient, RIFT32>
rift;
rift.setInputCloud(cloudIntensityKeypoints);
rift.setSearchSurface(cloudIntensityGlobal);
rift.setSearchMethod(kdtree);
// Set the intensity gradients to use.
rift.setInputGradient(gradients);
// Radius, to get all neighbors within.
rift.setRadiusSearch(0.05);
// Set the number of bins to use in the distance dimension.
rift.setNrDistanceBins(4);
// Set the number of bins to use in the gradient orientation
dimension.
rift.setNrGradientBins(8);
// Note: you must change the output histogram size to reflect the
previous values.

rift.compute(*descriptors);
cout << "Computed " << descriptors->points.size() << " RIFT
descriptorsn";

//matchRIFTFeatures(*descriptors, *descriptors);
return descriptors;
}

int matchRIFTFeatures(pcl::PointCloud<RIFT32>::Ptr descriptors1,
    pcl::PointCloud<RIFT32>::Ptr descriptors2) {
int correspondences = 0;
for (int i = 0; i < descriptors1->points.size(); ++i) {
    RIFT32 des1 = descriptors1->points[i];
    double minDis = 100000000000000000;
    double actDis = 0;
    //Find closest descriptor
    for (int j = 0; j < descriptors2->points.size(); ++j) {
        actDis = distanceRIFT(des1, descriptors2->points[j]);
        //std::cout << "act distance: " << actDis << std::endl;
        if (actDis < minDis) {
            minDis = actDis;
        }
    }
    //std::cout << "min distance: " << minDis << std::endl;
    //If distance between both descriptors is less than threshold we
found correspondence
    if (minDis < 0.5)
        ++correspondences;
}
return correspondences;
}