Визуализация PCD, содержащая пользовательскую структуру с двумя точками - PullRequest
Новая
       47

Визуализация PCD, содержащая пользовательскую структуру с двумя точками

0 голосов
/ 20 апреля 2020

Я создал пользовательский тип двойной точки для сохранения положения точки в файле PCD. Мне потребовался двойной тип данных, поскольку мои точки находятся в глобальных координатах и ​​имеют очень большие значения (порядка от 10 ^ 6 до 10 ^ 7) и требуют хорошей точности. Поскольку значения большие и точность по float32 умолчанию ограничена, существует значительное приближение данных, которая также видна при визуализации.

1002 * Я создал этот PCD, превращая необработанный PointCloud с первоначальной глобальной ссылкой координаты от GPS в сумка данных, которая у меня есть. Я использую точность 15 точек.

Я создал отдельный скрипт для визуализации этого пользовательского PCD типа точки. Но визуально сравнивая, я не вижу значительной разницы между FLOAT32 и двойными PCD типа данных.

Raw_float_pcd_visualization

Transformed_float_pcd_visualization

Transformed_double_pcd_visualization

Вы можете видеть, что PCD transformed_double и transformed_float довольно похожи и приблизительны. Хотя raw_float PCD довольно хорош по сравнению с этими двумя.

Я прилагаю файлы PCD для справки:

raw_float

transformed_float

transformed_double

Я думаю, что я пропускаю некоторые вещи при загрузке pointcloud, и есть еще некоторые изменения, которые необходимо сделать, чтобы визуализировать точки с двойной точностью.

Я использовал «pcl_viewer» из pcl_tools для визуализации PCD типа FLOAT.

Код для визуализации пользовательской двойной структуры PCD: 

#define PCL_NO_PRECOMPILE
#include <iostream>

// #include "double_viz/pcl_double.h"
#include <pcl-1.7/pcl/common/common.h>
#include <pcl-1.7/pcl/io/pcd_io.h>
#include <pcl-1.7/pcl/visualization/pcl_visualizer.h>
#include <pcl-1.7/pcl/console/parse.h>
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/point_types.h>

namespace pcl
{
  #define PCL_ADD_UNION_POINT4D_DOUBLE \
    union EIGEN_ALIGN16 { \
      double data[4]; \
      struct { \
        double x; \
        double y; \
        double z; \
      }; \
    };

  struct _PointXYZDouble
  {
    PCL_ADD_UNION_POINT4D_DOUBLE; // This adds the members x,y,z which can also be accessed using the point (which is float[4])

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };

  struct EIGEN_ALIGN16 PointXYZDouble : public _PointXYZDouble
  {
    inline PointXYZDouble (const _PointXYZDouble &p)
    {
      x = p.x; y = p.y; z = p.z; data[3] = 1.0;
    }

    inline PointXYZDouble ()
    {
      x = y = z = 0.0;
      data[3] = 1.0;
    }

    inline PointXYZDouble (double _x, double _y, double _z)
    {
      x = _x; y = _y; z = _z;
      data[3] = 1.0;
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  };
}

  POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZDouble,
    (double, x, x)
    (double, y, y)
    (double, z, z)
  )
  POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZDouble, pcl::_PointXYZDouble)


// This function displays the help
void
showHelp(char * program_name)
{
  std::cout << std::endl;
  std::cout << "Usage: " << program_name << " cloud_filename.[pcd]" << std::endl;
  std::cout << "-h:  Show this help." << std::endl;
}

// This is the main function
int
main (int argc, char** argv)
{

  // Show help
  if (pcl::console::find_switch (argc, argv, "-h") || pcl::console::find_switch (argc, argv, "--help")) 
  {
    showHelp (argv[0]);
    return 0;
  }

  // Fetch point cloud filename in arguments | Works with PCD
  std::vector<int> filenames;

  if (filenames.size () != 1)  
  {
    filenames = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");

    if (filenames.size () != 1) 
    {
      showHelp (argv[0]);
      return -1;
    } 
  }

  // Load file | Works with PCD and PLY files
  pcl::PointCloud<pcl::PointXYZDouble>::Ptr source_cloud (new pcl::PointCloud<pcl::PointXYZDouble> ());

    if (pcl::io::loadPCDFile (argv[filenames[0]], *source_cloud) < 0)  
    {
      std::cout << "Error loading point cloud " << argv[filenames[0]] << std::endl << std::endl;
      showHelp (argv[0]);
      return -1;
    }
  // Visualization
//   printf(  "\nPoint cloud colors :  white  = original point cloud\n"
//       "                        red  = transformed point cloud\n");
  pcl::visualization::PCLVisualizer viewer ("Visualize double PCL");

   // Define R,G,B colors for the point cloud
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZDouble> source_cloud_color_handler (source_cloud, 100, 100, 100);
  // We add the point cloud to the viewer and pass the color handler
  viewer.addPointCloud (source_cloud, source_cloud_color_handler, "original_cloud");

  viewer.addCoordinateSystem (1.0, "cloud", 0);
  viewer.setBackgroundColor(0.05, 0.05, 0.05, 0); // Setting background to a dark grey
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY, 1, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "original_cloud");
  viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_LINE_WIDTH, 1, "original_cloud");
  //viewer.setPosition(800, 400); // Setting visualiser window position

  while (!viewer.wasStopped ()) // Display the visualiser until 'q' key is pressed
  { 
    viewer.spinOnce ();
  }

  return 0;
}

1 Ответ

0 голосов
/ 21 апреля 2020

В файле raw_float поле размера было определено как 4 байта каждое: SIZE 4 4 4 4, чтобы быть прочитанным как двойное, оно должно быть SIZE 8 8 8 8.

С вашим текущим реализация каждого поля читается как Float32

...