How to add GICP source code into CMakeLists.txt and use it in ROS

The source code for GICP can be found at

http://www.robots.ox.ac.uk/~avsegal/generalized_icp.html


Part of CMakeLists.txt

The gicp folder downloaded from the website is at /home/sai/workspace/

Add these lines in CMakeLists.txt

include_directories(/home/sai/workspace/gicp/ann_1.1.1/include/ANN/)
include_directories(/home/sai/workspace/gicp/)
link_directories(/home/sai/workspace/gicp/)

include_directories(/home/sai/workspace/gicp/ann_1.1.1/lib/)
link_directories(/home/sai/workspace/gicp/ann_1.1.1/lib/)


rosbuild_add_executable(gicp_testing src/gicp_testing.cpp)
target_link_libraries(gicp_testing ${PCL_LIBRARIES} libgicp.a libANN.a -lgsl -lgslcblas)


Add this line in your test file which in my case is gicp_testing.cpp

The original source code is at

http://www.pcl-users.org/file/n4019799/Main.cpp


gicp_testing.cpp file is as follows.


#include "pcl17/io/pcd_io.h"
#include "pcl17/filters/voxel_grid.h"
#include "pcl17/registration/gicp.h"
#include "pcl17/visualization/cloud_viewer.h"
#include "/home/sai/workspace/gicp/gicp.h"


pcl17::PointCloud::Ptr modelCloud(new pcl17::PointCloud);
pcl17::PointCloud::Ptr modelCloudDownsampled(new pcl17::PointCloud);
pcl17::PointCloud::Ptr dataCloud(new pcl17::PointCloud);
pcl17::PointCloud::Ptr dataCloudDownsampled(new pcl17::PointCloud);

pcl17::PointCloud::Ptr transformed(new pcl17::PointCloud);

void viewerOneOff(pcl17::visualization::PCLVisualizer& viewer)
{
    viewer.addPointCloud(modelCloudDownsampled, "model");
    //viewer.setPointCloudRenderingProperties(pcl17::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "model");
    viewer.addPointCloud(transformed, "transformed");
    //viewer.setPointCloudRenderingProperties(pcl17::visualization::PCL_VISUALIZER_COLOR, 0, 0, 255, "transformed");
}


int main(int argv, char **args)
{
    // load files and convert to point type pcl17::PointXYZ
    sensor_msgs::PointCloud2 cloudBlob;

    pcl17::io::loadPCDFile("temp1.pcd", cloudBlob);
    pcl17::fromROSMsg(cloudBlob, *modelCloud);
    pcl17::io::loadPCDFile("temp2.pcd", cloudBlob);
    pcl17::fromROSMsg(cloudBlob, *dataCloud);

    // make dense
    std::vector indices;
    pcl17::removeNaNFromPointCloud(*modelCloud, *modelCloud, indices);
    pcl17::removeNaNFromPointCloud(*dataCloud,  *dataCloud,  indices);

    // downsample clouds
    pcl17::VoxelGrid vg;
    vg.setInputCloud(modelCloud);
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter(*modelCloudDownsampled);

    vg.setInputCloud(dataCloud);
    vg.setLeafSize (0.01f, 0.01f, 0.01f);
    vg.filter(*dataCloudDownsampled);


    /* Segal-GICP */
    dgc::gicp::GICPPointSet ps_in, ps_out;
 
    for(int i=0; ipoints.size(); ++i)
    {
        dgc::gicp::GICPPoint p;
        p.x = dataCloudDownsampled->points[i].x;
        p.y = dataCloudDownsampled->points[i].y;
        p.z = dataCloudDownsampled->points[i].z;
     
        ps_in.AppendPoint(p);
    }
 
    for(int i=0; ipoints.size(); ++i)
    {
        dgc::gicp::GICPPoint p;
        p.x = modelCloudDownsampled->points[i].x;
        p.y = modelCloudDownsampled->points[i].y;
        p.z = modelCloudDownsampled->points[i].z;
     
        ps_out.AppendPoint(p);
    }  
 
    dgc_transform_t T_guess, T_delta, T_est;
    dgc_transform_identity(T_delta);
    dgc_transform_identity(T_guess);

    ps_in.BuildKDTree();
    ps_out.BuildKDTree();
    ps_in.ComputeMatrices();
    ps_out.ComputeMatrices();
    ps_out.SetDebug(0);
    //ps_out.SetMaxIterationInner(8);
 
    ps_out.AlignScan(&ps_in, T_guess, T_delta, 5);
    dgc_transform_copy(T_est, T_guess);
    dgc_transform_left_multiply(T_est, T_delta);        
 
    Eigen::Matrix4f em;
 
    for(int i=0; i<4 i="" p="">        for(int j=0; j<4 j="" p="">            em(i,j) = (float) T_est[i][j];
         
    std::cout << em << std::endl << std::endl;

    return 0;
}