Cvb/CppPointCloudAcquisition

// ----------------------------------------------------------------------------
// ----------------------------------------------------------------------------
 
#include <iostream>
#include <fstream>
#include <string>
#include <vector>
 
#include <cvb/device_factory.hpp>
#include <cvb/driver/driver.hpp>
#include <cvb/image.hpp>
#include <cvb/utilities/utilities.hpp>
#include <cvb/composite.hpp>
#include <cvb/driver/multi_part_image.hpp>
#include <cvb/plane_enumerator.hpp>
#include <cvb/point_cloud.hpp>
 
 
 
int main()
{
  try
  {
    const constexpr auto numElemsToAcquire = 10;
 
    auto infoList = Cvb::DeviceFactory::Discover(Cvb::DiscoverFlags::IgnoreVins);
    if (infoList.empty())
    {
      std::cout << "Discovered" << std::endl;
      throw std::runtime_error("There is no available device for this demonstration.");
    }
 
    // Attention: even if you have connected such a device it is not necessarily found first.
    // You may want to filter here.
    auto device = Cvb::DeviceFactory::Open<Cvb::GenICamDevice>(infoList[0].AccessToken(), Cvb::AcquisitionStack::GenTL);
 
    // Maybe adjust some values
    //auto nodeMap = device->NodeMap(CVB_LIT("Device"));
    //auto settingNodes = nodeMap->Node<Cvb::FloatNode>(CVB_LIT("Aperture"));
    //settingNodes->SetValue(5.66f);
    //auto exposureTime = nodeMap->Node<Cvb::IntegerNode>(CVB_LIT("Cust::ExposureTime"));
    //exposureTime->SetValue(40000);
 
    const auto numStreams = device->StreamCount();
    // We know the device is streaming point clouds including neighborhood information.
    // So we can directly acquire dense point clouds.
    auto stream = device->Stream<Cvb::PointCloudStream>(0);
    
    stream->Start();
 
    for (auto i = 0; i < numElemsToAcquire; i++)
    {
      Cvb::DensePointCloudPtr pointCloud;
      Cvb::WaitStatus waitStatus;
      Cvb::NodeMapEnumerator enumerator;
      std::tie(pointCloud, waitStatus, enumerator) = stream->Wait<Cvb::DensePointCloud>();
      if (waitStatus != Cvb::WaitStatus::Ok)
      {
        std::cout << "wait status not ok: " << static_cast<int>(waitStatus) << std::endl;
        break;
      }
 
      // Here we've got the point cloud (xyz), but there is more in the buffer as 
      // a multi part buffer is a composition of different parts.
      // You can access these parts through a composite, which is basically just another view on the point cloud.
      auto composite = Cvb::Composite::FromObject(pointCloud);
      if (composite->Purpose() == Cvb::CompositePurpose::PointCloud)
      {
        const auto numberOfElements = composite->ItemCount();
        std::cout << "Found " << numberOfElements << " point cloud parts" << std::endl;
 
        // check if the format is as expected.
        auto xyz = Cvb::get<Cvb::PlaneEnumeratorPtr>(composite->ItemAt(0));
        if (!xyz)
        {
          std::cout << "Composite xyz is not a plane enumerator" << std::endl;
          break;
        }
 
        if (!Cvb::holds_alternative<Cvb::PlanePtr>(composite->ItemAt(1)))
        {
          std::cout << "Composite confidence is not a plane" << std::endl;
          break;
        }
 
        if (!Cvb::holds_alternative<Cvb::ImagePtr>(composite->ItemAt(2)))
        {
          std::cout << "Composite image is not an image" << std::endl;
          break;
        }
 
        // Note: that items in composite are not automatically associated with each other.
        // E.g. the image inside a composite could refer to a totally different sensor. 
        // As there is no standard way to communicate the relation between individual parts, 
        // you have to create this association with your a priori knowledge.
        // This can be done by adding planes with specific roles to the composite/point cloud.
        // Be aware that no data is copied.
 
        // Get the RGB planes from the image. 
        // This is necessary because an image plane supports more memory layouts than a plane. 
        // For modern acquisition devices they are usually compatible.
        auto rgbImage = Cvb::get<Cvb::ImagePtr>(composite->ItemAt(2));
        auto rPlane = Cvb::Plane::FromImagePlane(rgbImage->Plane(0), Cvb::PlaneRole::PixRGB_R);
        auto gPlane = Cvb::Plane::FromImagePlane(rgbImage->Plane(1), Cvb::PlaneRole::PixRGB_G);
        auto bPlane = Cvb::Plane::FromImagePlane(rgbImage->Plane(2), Cvb::PlaneRole::PixRGB_B);
 
        // Add them at the end
        composite->InsertItemAt(composite->ItemCount(), rPlane);
        composite->InsertItemAt(composite->ItemCount(), gPlane);
        composite->InsertItemAt(composite->ItemCount(), bPlane);
 
        // remember the point cloud and the composite are the same thing
        pointCloud->Save("xyzrgb.pcd");
 
        // Note: when you load it again the image is gone, but XYZ and RGB planes can be restored!
      }
      else
      {
        std::cout << "The composite contains something unexpected" << std::endl;
        break;
      }
    }
    stream->Stop();
  }
  catch (const std::exception& e)
  {
    std::cout << e.what() << std::endl;
  }
  return 0;
}