Example detect_objects.cpp

/******************************************************************************
 *
 * Slamcore Confidential
 * ---------------------
 *
 * Slamcore Limited
 * All Rights Reserved.
 * (C) Copyright 2023
 *
 * NOTICE:
 *
 * All information contained herein is, and remains the property of Slamcore
 * Limited and its suppliers, if any. The intellectual and technical concepts
 * contained herein are proprietary to Slamcore Limited and its suppliers and
 * may be covered by patents in process, and are protected by trade secret or
 * copyright law. Dissemination of this information or reproduction of this
 * material is strictly forbidden unless prior written permission is obtained
 * from Slamcore Limited.
 *
 ******************************************************************************/

/**
 * @file
 * @ingroup slamcore_sdk_examples
 * @brief API example to detect objects
 *
 * Demonstrates the following:
 *
 *   - Create and run the person instance segmentation API
 *   - Count the number of objects in the scene
 *   - Compute their distance from the sensor body
 */

#include "slamcore/semantic/panoptic_bounding_box_3d_list.hpp"
#include "slamcore/slam/slam_create.hpp"
#include "slamcore/subsystems/panoptic_segmentation.hpp"

#include <cmath>
#include <iomanip>
#include <iostream>

/**
 * Compute Euclidean distance.
 */
static inline double l2Dist(const slamcore::Vector<double, 3>& vec)
{
  return std::sqrt(vec.x() * vec.x() + vec.y() * vec.y() + vec.z() * vec.z());
}

int main(int /*argc*/, char* /*argv*/[])
{
  // ******************************************************************
  // Initialise Slamcore API
  // ******************************************************************
  slamcore::slamcoreInit(slamcore::LogSeverity::Info,
                         [](const slamcore::LogMessageInterface& message)
                         {
                           const time_t time = slamcore::host_clock::to_time_t(message.getTimestamp());
                           struct tm tm;
                           localtime_r(&time, &tm);

                           std::cerr << "[" << message.getSeverity() << " "
                                     << std::put_time(&tm, "%FT%T%z") << "] "
                                     << message.getMessage() << "\n";
                         });

  // ******************************************************************
  // Create/Connect SLAM System
  // ******************************************************************
  slamcore::v0::SystemConfiguration sysCfg;

  // Turn on the colour sensor required for segmentation
  sysCfg.SensorEnableMap[slamcore::SensorIDT(slamcore::SensorType::Visible, 0)] = true;
  // Turn on the depth sensor required for computing 3D co-ordinates
  sysCfg.SensorEnableMap[slamcore::SensorIDT(slamcore::SensorType::Depth, 0)] = true;

  auto slam = slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::cout << "Starting SLAM..." << '\n';

  // ******************************************************************
  // Open the device
  // ******************************************************************
  slam->open();

  const auto panopticSegmentationSubsystem =
    slam->getSubsystem<slamcore::PanopticSegmentationSubsystemInterface>();

  panopticSegmentationSubsystem->registerDefaultPlugin();

  // Check if the plugin was loaded correctly
  const auto hasPanopticRunner = panopticSegmentationSubsystem->getRunner() != nullptr;
  if (!hasPanopticRunner)
  {
    std::cerr << "Error loading segmentation plugin!" << '\n';
    slamcore::slamcoreDeinit();
    return -1;
  }

  // ******************************************************************
  // Print versions
  // ******************************************************************
  const auto slam_version = slam->getProperty<std::string>(slamcore::Property::FirmwareVersion);
  const auto slam_build_ver = slam->getProperty<std::string>(slamcore::Property::FirmwareBuildVersion);
  const auto slam_build_type = slam->getProperty<std::string>(slamcore::Property::FirmwareBuildType);

  std::cout << "Client Version: " << slamcore::getVersion() << "/" << slamcore::getBuildVersion()
            << "/" << slamcore::getBuildType() << '\n';
  std::cout << "SLAM Version: " << slam_version << "/" << slam_build_ver << "/" << slam_build_type
            << '\n';

  // ******************************************************************
  // Enable all the streams
  // ******************************************************************
  slam->setStreamEnabled(slamcore::Stream::Pose, true);
  slam->setStreamEnabled(slamcore::Stream::PanopticBoundingBox3D, true);
  // *****************************************************************
  // Register callbacks!
  // *****************************************************************

  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const auto rc = errorObj->getValue();
      std::cout << "Received: ErrorCode" << '\n';
      std::cout << "\t" << rc.message() << " / " << rc.value() << " / " << rc.category().name()
                << '\n';
    });

  slam->registerCallback<slamcore::Stream::PanopticBoundingBox3D>(
    [](const slamcore::PanopticBoundingBox3DList::CPtr& bboxList)
    {
      std::cout << "Detected " << bboxList->size() << " objects in the scene:" << '\n';
      for (const auto& panopticBBox : *bboxList)
      {
        const auto translation = panopticBBox.getBBox().centerPose.translation;
        std::cout << "\t Detected a " << panopticBBox.getLabel().name()
                  << " at a distance of: " << l2Dist(translation) << " from the "
                  << panopticBBox.getReferenceFrame() << " frame of reference." << '\n';
      }
    });

  // ******************************************************************
  // Start streaming
  // ******************************************************************
  slam->start();

  // ******************************************************************
  // Main receiving loop
  // ******************************************************************
  while (slam->spin())
  {
    ;
  }

  // ******************************************************************
  // Stop SLAM
  // ******************************************************************
  slam->stop();

  // ******************************************************************
  // Disconnect/Close SLAM
  // ******************************************************************
  slam->close();

  slam.reset();

  // ******************************************************************
  // Deinitialise Slamcore API
  // ******************************************************************
  slamcore::slamcoreDeinit();

  std::cout << "We're Done Here." << '\n';

  return 0;
}