cartographer 3 Dポイントクラウド構築チュートリアル

git clone https://github.com/BlueWhaleRobot/ceres-solver.git
cd ceres-solver
mkdir build
cd build
cmake ..
make -j
sudo make install

git clone https://github.com/google/protobuf.git
cd protobuf
git checkout v3.6.1
mkdir build
cd build
cmake  \
  -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
  -DCMAKE_BUILD_TYPE=Release \
  -Dprotobuf_BUILD_TESTS=OFF \
  ../cmake
make -j 2
sudo make install

git clone https://github.com/googlecartographer/cartographer.git
cd cartographer
mkdir build
cd build
cmake .. -G Ninja
ninja
CTEST_OUTPUT_ON_FAILURE=1 ninja test
sudo ninja install

mkdir catkin_ws
cd catkin_ws
wstool init src
wstool merge -t src https://raw.githubusercontent.com/googlecartographer/cartographer_ros/master/cartographer_ros.rosinstall
wstool update -t src

catkin_make_isolated --install --use-ninja

rosrun cartographer_ros cartographer_pbstream_to_ros_map  -map_filestem=./aa_map -pbstream_filename=/media/jintain/wd/ros/slam/rosbags/cartographer_paper_deutsches_museum.bag.pbstream

 roslaunch cartographer_ros assets_writer_my_rslidar_3d.launch bag_filenames:=`pwd`/../rosbags/2018-08-11-13-20-34.bag    pose_graph_filename:=`pwd`/../rosbags/test_3d.pbstream

pcl_ply2pcd 2018-08-11-13-20-34.bag_points.ply test_3d.pcd

#include 
#include 
#include 
// #include 
#include 

int
main (int argc, char** argv)
{
  pcl::PointCloud<:pointxyz>::Ptr cloud (new pcl::PointCloud<:pointxyz>);

  if (pcl::io::loadPCDFile<:pointxyz> ("../ct_lx_rmp.pcd", *cloud) == -1) //* load the file
  {
    PCL_ERROR ("Couldn't read file test_pcd.pcd 
");
    return (-1);
  }
  std::cout << "Loaded "
            << cloud->width * cloud->height
            << " data points from test_pcd.pcd with the following fields: "
            << std::endl;
  for (size_t i = 0; i < cloud->points.size (); ++i)
    std::cout << "    " << cloud->points[i].x
              << " "    << cloud->points[i].y
              << " "    << cloud->points[i].z << std::endl;


    // visualize it
    pcl::visualization::CloudViewer viewer("ff");
    viewer.showCloud(cloud);
    while (!viewer.wasStopped())     {

    }
  return (0);
}

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

project(pcd_read)

find_package(PCL REQUIRED)

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

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

#include 
#include 
#include 
#include 
#include 

using namespace pcl;
using namespace pcl::io;
using namespace pcl::console;

void
printHelp (int, char **argv)
{
  print_error ("Syntax is: %s [-format 0|1] input.ply output.pcd
", argv[0]);
}

bool
loadCloud (const std::string &filename, pcl::PCLPointCloud2 &cloud)
{
  TicToc tt;
  print_highlight ("Loading "); print_value ("%s ", filename.c_str ());

  pcl::PLYReader reader;
  tt.tic ();
  if (reader.read (filename, cloud) < 0)
    return (false);
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]
");
  print_info ("Available dimensions: "); print_value ("%s
", pcl::getFieldsList (cloud).c_str ());

  return (true);
}

void
saveCloud (const std::string &filename, const pcl::PCLPointCloud2 &cloud, bool format)
{
  TicToc tt;
  tt.tic ();

  print_highlight ("Saving "); print_value ("%s ", filename.c_str ());
  
  pcl::PCDWriter writer;
  writer.write (filename, cloud, Eigen::Vector4f::Zero (), Eigen::Quaternionf::Identity (), format);
  
  print_info ("[done, "); print_value ("%g", tt.toc ()); print_info (" ms : "); print_value ("%d", cloud.width * cloud.height); print_info (" points]
");
}

/* ---[ */
int
main (int argc, char** argv)
{
  print_info ("Convert a PLY file to PCD format. For more information, use: %s -h
", argv[0]);

  if (argc < 3)
  {
    printHelp (argc, argv);
    return (-1);
  }

  // Parse the command line arguments for .pcd and .ply files
  std::vector pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
  std::vector ply_file_indices = parse_file_extension_argument (argc, argv, ".ply");
  if (pcd_file_indices.size () != 1 || ply_file_indices.size () != 1)
  {
    print_error ("Need one input PLY file and one output PCD file.
");
    return (-1);
  }

  // Command line parsing
  bool format = 1;
  parse_argument (argc, argv, "-format", format);
  print_info ("PCD output format: "); print_value ("%s
", (format ? "binary" : "ascii"));

  // Load the first file
  pcl::PCLPointCloud2 cloud;
  if (!loadCloud (argv[ply_file_indices[0]], cloud)) 
    return (-1);

  // Convert to PLY and save
  saveCloud (argv[pcd_file_indices[0]], cloud, format);

  return (0);
}

-- Copyright 2016 The Cartographer Authors
--
-- Licensed under the Apache License, Version 2.0 (the "License");
-- you may not use this file except in compliance with the License.
-- You may obtain a copy of the License at
--
--      http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS,
-- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-- See the License for the specific language governing permissions and
-- limitations under the License.

VOXEL_SIZE = 5e-2

include "transform.lua"

options = {
  tracking_frame = "laserbase_link",
  pipeline = {
    {
      action = "min_max_range_filter",
      min_range = 1.,
      max_range = 20.,
    },
    {
      action = "dump_num_points",
    },
    {
      action = "fixed_ratio_sampler",
      sampling_ratio = 0.01,
    },

    {
      action = "write_ply",
      filename = "points.ply"
    },

    -- Gray X-Rays. These only use geometry to color pixels.
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_yz_all",
      transform = YZ_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xy_all",
      transform = XY_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xz_all",
      transform = XZ_TRANSFORM,
    },

    -- Now we recolor our points by frame and write another batch of X-Rays. It
    -- is visible in them what was seen by the horizontal and the vertical
    -- laser.
    {
      action = "color_points",
      frame_id = "rslidar",
      color = { 255., 0., 0. },
    },

    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_yz_all_color",
      transform = YZ_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xy_all_color",
      transform = XY_TRANSFORM,
    },
    {
      action = "write_xray_image",
      voxel_size = VOXEL_SIZE,
      filename = "xray_xz_all_color",
      transform = XZ_TRANSFORM,
    },
  }
}

return options