Linking pcl library with ROS

Question

in pcd_register.cpp

I have code,

#include <ros/ros.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <terminal_tools/print.h>
#include <terminal_tools/parse.h>
#include <terminal_tools/time.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/gicp.h>
#include <pcl/registration/transforms.h>

#include <pcl/filters/voxel_grid.h> // TEMP: currently used instead of octomap

#include "pcl/registration/types.h"

using namespace pcl::registration;

int main (int argc, char** argv) {

  ros::init (argc, argv, "pcd_register");
  ros::Time::init();

  terminal_tools::TicToc tictoc;

  // Downsampling
  bool downsampling_enabled = true;
  double downsampling_leaf_size = 0.001;
  terminal_tools::parse_argument(argc, argv, "-d", downsampling_enabled);
  terminal_tools::parse_argument(argc, argv, "-D", downsampling_leaf_size);

  // read point clouds
  vector<PointCloud> data;
  std::string extension (".pcd");
  for (int i = 1; i < argc; i++)
  {
    string fname = string (argv[i]);
    if (fname.size () <= extension.size ())
      continue;
    transform (fname.begin (), fname.end (), fname.begin (), (int(*)(int))tolower);
    if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0)
    {
      PointCloud cloud;
      pcl::io::loadPCDFile (argv[i], cloud);
      if (cloud.points.size() == 0) continue;
      data.push_back(cloud);
      terminal_tools::print_highlight("Read point cloud from "); terminal_tools::print_value("%s", argv[i]); terminal_tools::print_info(" containing "); terminal_tools::print_value("%d", cloud.points.size()); terminal_tools::print_info(" points.\n");
    }
  }

  if (data.empty ())
  {
    ROS_ERROR ("Syntax is: %s <source.pcd> <target.pcd> [*]", argv[0]);
    ROS_ERROR ("[*] - multiple files can be added. The registration results of (i, i+1) will be registered against (i+2), etc");
    ROS_INFO ("Example: %s `rospack find pcl`/test/bun0.pcd `rospack find pcl`/test/bun4.pcd", argv[0]);
    return (-1);
  }
  terminal_tools::print_highlight(" Loaded "); terminal_tools::print_value("%d ", (int)data.size ()); terminal_tools::print_info("datasets.\n");

  pcl::GeneralizedIterativeClosestPoint<pcl::registration::Point, pcl::registration::Point> reg;
  PointCloud cloud_input, cloud_output, cloud_model;

  char filename[255];
  unsigned int number_clouds_processed = 0;
  for (unsigned int i = 0; i < data.size(); ++i) {

    // some filtering or whatever here ...
    if (downsampling_enabled) {
      pcl::VoxelGrid<Point> sor;
      sor.setInputCloud(boost::make_shared<PointCloud>(data[i]));
      sor.setLeafSize(downsampling_leaf_size, downsampling_leaf_size, downsampling_leaf_size);
      sor.filter(cloud_input);
    }
    else
      cloud_input = data[i];

    // write input files
    sprintf(filename, "registration_input_%04d.pcd", number_clouds_processed);
    pcl::io::savePCDFileBinary(filename, cloud_input);

    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    if (number_clouds_processed == 0) { // no registration needed
      cloud_output = cloud_input;
      cloud_model = cloud_input;
      terminal_tools::print_highlight("Adding first scan to model.\n");
      // TODO: simply use first point cloud to update/initialize world mode, get cloud_model for next registration from world model
    }
    else {
      //terminal_tools::print_highlight("Registering scan %04d (%d points) against model (%d points)", number_clouds_processed, cloud_input.points.size(), cloud_model.size());
      terminal_tools::print_highlight("Registering scan "); terminal_tools::print_value("%04d ", number_clouds_processed); 
      terminal_tools::print_info("("); terminal_tools::print_value("%d", cloud_input.points.size()); terminal_tools::print_info(" points) against model ("); terminal_tools::print_value("%d ", cloud_model.size()); terminal_tools::print_info("points)"); 
      tictoc.tic();

      reg.setInputCloud(boost::make_shared<const PointCloud>(cloud_input));
      reg.setInputTarget(boost::make_shared<const PointCloud>(cloud_model));

      reg.setMaximumIterations(2);  // only one iteration, gicp implementation has inner iterations in optimization
      reg.align(cloud_output);      // cloud_output = registered point cloud

      terminal_tools::print_info("[done, "); terminal_tools::print_value("%g", tictoc.toc()); terminal_tools::print_info("s]\n"); 

      // setting up new model
      //cloud_model_normals += cloud_output; // TEMP
      double model_subsampling_leaf_size = downsampling_leaf_size; // TEMP
      PointCloud cloud_temp; // TEMP
      cloud_model += cloud_output; // TEMP
      pcl::VoxelGrid<Point> grid; // TEMP
      grid.setInputCloud(boost::make_shared<PointCloud>(cloud_model)); // TEMP
      grid.setLeafSize(model_subsampling_leaf_size, model_subsampling_leaf_size, model_subsampling_leaf_size); // TEMP
      grid.filter(cloud_temp); // TEMP
      cloud_model = cloud_temp; // TEMP
      // TODO: use cloud_output to udpate world model, get cloud_model from world model
    }
    ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


    // write output files
    sprintf(filename, "registration_output_%04d.pcd", number_clouds_processed);
    pcl::io::savePCDFileBinary(filename, cloud_output);

    ++number_clouds_processed;
  }

  pcl::io::savePCDFileBinary("registration_final_model.pcd", cloud_model);

  return 0;
}

but when i am compiling it does not know all the terminal_tools header files. but i have downloaded ROS.

Do I have to specify or something on CMakeLists.txt? this is my cmake

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(pcd_register)

find_package(PCL 1.2 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (pcd_register pcd_register.cpp)
target_link_libraries (pcd_register ${PCL_LIBRARIES})

Answer 1

I have a ROS running a similar node using the PCL library and your CMakeLists.txt looks good regarding PCL. If cmake is not finding the terminal tools headers, it is because you have not specified to add those include directories within cmake. In general, if you know how to compile with g++ myprogram.cpp -I${INCLUDE_DIRS} -L${LIB_DIRS} -l${LIBS}, then you need to add the following lines to your CMakeLists:

include_directories(${INCLUDE_DIRS})

link_directories(${LIB_DIRS})

target_link_libraries(pcd_register ${LIBS}).

In your case, you need to specify the include directories of terminal_tools with an include_directories line.