Example ascii_image.cpp

/******************************************************************************
 *
 * SLAMcore Confidential
 * ---------------------
 *
 * SLAMcore Limited
 * All Rights Reserved.
 * (C) Copyright 2022
 *
 * 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 show how to obtain image raw pixels with helper.
 */

#include <slamcore/const_image_view.hpp>
#include <slamcore/slamcore.hpp>

#include <atomic>
#include <cmath>
#include <cstdlib>
#include <cstring>
#include <iomanip>
#include <iostream>

#include <sys/ioctl.h>

namespace
{
void logMsg(const slamcore::LogMessageInterface& message)
{
  const time_t time = std::chrono::system_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";
}

struct Args
{
  std::string dataset_path;
  std::string config_file;
  uint8_t sensor_index{0};
};

bool parseArgs(const int argc, char* const argv[], Args& args)
{
  int i{1};
  bool success{true};
  while (i < argc)
  {
    if (strcmp(argv[i], "-u") == 0)
    {
      if (++i < argc)
      {
        args.dataset_path = argv[i];
      }
      else
      {
        success = false;
        break;
      }
    }
    else if (strcmp(argv[i], "-c") == 0)
    {
      if (++i < argc)
      {
        args.config_file = argv[i];
      }
      else
      {
        success = false;
        break;
      }
    }
    else
    {
      args.sensor_index = atoi(argv[i]);
    }
    ++i;
  }

  if (!success)
  {
    std::cerr << "Usage: " << argv[0]
              << " [sensor-index=0] [-u <dataset-path>] [-c <config-file>]" << std::endl;
  }

  return success;
}

char getAsciiChar(const uint8_t val)
{
  constexpr std::array<char, 10> asciiChar{'@', '%', '#', '*', '+', '=', '-', ':', '.', ' '};
  return asciiChar[static_cast<size_t>(std::round(asciiChar.size() / 255.0 * val))];
}

void printSensors(const std::vector<slamcore::SensorIDT>& sensors, const uint8_t sensor_index)
{
  for (uint8_t i = 0; i < sensors.size(); ++i)
  {
    const auto sensor = sensors[i];
    if (i == sensor_index)
    {
      std::cout << " * ";
    }
    else
    {
      std::cout << "   ";
    }
    std::cout << sensor.first << " " << sensor.second << std::endl;
  }
}

template <typename T>
uint8_t getValue(T pixel);

template <>
uint8_t getValue<uint8_t>(uint8_t pixel)
{
  return pixel;
}

template <>
uint8_t getValue<float>(float pixel)
{
  return pixel * 255.0;
}

struct RGB_8
{
  uint8_t r{0};
  uint8_t g{0};
  uint8_t b{0};
};

template <>
uint8_t getValue<RGB_8>(RGB_8 pixel)
{
  return 0.333 * pixel.r + 0.333 * pixel.g + 0.333 * pixel.b;
}

template <class T>
void printImage(const slamcore::ImageInterface& image,
                int width,
                int height,
                int widthStep,
                int heightStep)
{
  const slamcore::ConstImageView<T> imageView{image};
  for (int i = 0; i < height; ++i)
  {
    const int ii = i * heightStep;
    for (int j = 0; j < width; ++j)
    {
      const int jj = j * widthStep;
      const auto pixel = imageView(ii, jj);
      const uint8_t value = getValue<T>(pixel);
      char asciiChar = getAsciiChar(value);
      std::cout << asciiChar;
    }
    std::cout << std::endl;
  }
}

void printImage(const slamcore::ImageInterface& image, const uint8_t n_sensors)
{
  winsize w;
  // For headless runs, like redirecting to a file
  static constexpr unsigned short minWinSize{10};
  w.ws_row = minWinSize;
  w.ws_col = minWinSize;
  ioctl(fileno(stdout), TIOCGWINSZ, &w);
  const int asciiImageHeight = w.ws_row - n_sensors;
  const int asciiImageWidth = w.ws_col;
  const int heightStep = std::floor(image.getHeight() / asciiImageHeight);
  const int widthStep = std::floor(image.getWidth() / asciiImageWidth);

  // Clear screen
  for (int ii = 0; ii <= w.ws_row; ++ii)
  {
    std::cout << "\n";
  }

  // Print image with specific format
  if (image.getFormat() == slamcore::ImageFormat::Mono_8)
  {
    printImage<uint8_t>(image, asciiImageWidth, asciiImageHeight, widthStep, heightStep);
  }
  else if (image.getFormat() == slamcore::ImageFormat::RGB_8)
  {
    printImage<RGB_8>(image, asciiImageWidth, asciiImageHeight, widthStep, heightStep);
  }
  else if (image.getFormat() == slamcore::ImageFormat::Mono_F)
  {
    printImage<float>(image, asciiImageWidth, asciiImageHeight, widthStep, heightStep);
  }
  else
  {
    std::cout << "I don't know how to print " << image.getFormat() << std::endl;
  }
}

}  // namespace

int main(int argc, char* argv[])
try
{
  // Parse args
  Args args;
  if (!::parseArgs(argc, argv, args))
  {
    return -1;
  }

  // Initialise SLAMcore API
  slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::logMsg);

  // Create/Connect SLAM System
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.Mode = slamcore::PositioningMode::SLAM;
  if (!args.config_file.empty())
  {
    sysCfg.ConfigFilePath = args.config_file;
  }
  if (!args.dataset_path.empty())
  {
    sysCfg.Source = slamcore::DataSource::Dataset;
    sysCfg.DatasetPath = args.dataset_path;
  }
  else
  {
    sysCfg.Source = slamcore::DataSource::RealSense;
  }

  // Enable color stream
  sysCfg.EnableColor = true;

  std::unique_ptr<slamcore::SLAMSystemCallbackInterface> slam =
    slamcore::createSLAMSystem(sysCfg);
  if (!slam)
  {
    std::cerr << "Error creating SLAM system!" << std::endl;
    slamcore::slamcoreDeinit();
    return -1;
  }

  std::cout << "Starting SLAM..." << std::endl;

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

  // Get cameras' information
  const auto cameraInfo = slam->getSubsystem<slamcore::CameraSensorsInfoInterface>();
  const auto sensors = cameraInfo->getCameraList();
  printSensors(sensors, args.sensor_index);
  if (args.sensor_index >= sensors.size())
  {
    std::cout << "Error: Sensor index: " << static_cast<unsigned>(args.sensor_index)
              << std::endl;
    slam->close();
    slamcore::slamcoreDeinit();
    return -1;
  }

  // Enable Video Stream
  slam->setStreamEnabled(slamcore::Stream::Video, true);

  // Register Image and Error Callback
  slamcore::MultiFrameInterface::CPtr multiFrame;
  slam->registerCallback<slamcore::Stream::Video>(
    [&multiFrame](const slamcore::MultiFrameInterface::CPtr& multiFrameObj)
    { multiFrame = multiFrameObj; });

  // Catch end of dataset
  std::atomic<bool> finished{false};
  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [&finished](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const std::error_code rc = errorObj->getValue();
      if (rc == make_error_code(slamcore::errc::end_of_data))
      {
        finished.store(true);
      }
    });

  // Start streaming
  slam->start();

  constexpr float refreshRate{4.0};  // Hz
  const auto refreshPeriod = std::chrono::seconds(1) / refreshRate;
  auto nextTime = std::chrono::steady_clock::now() + refreshPeriod;

  while (slam->spinOnce() && !finished)
  {
    const auto now = std::chrono::steady_clock::now();
    if (now >= nextTime)
    {
      const slamcore::ImageInterface& image = multiFrame->get(args.sensor_index);
      printImage(image, sensors.size());

      // Info about sensors.
      printSensors(sensors, args.sensor_index);

      nextTime = now + refreshPeriod;
    }
  }

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

  // Deinitialise SLAMcore API
  slamcore::slamcoreDeinit();

  std::cout << "We're Done Here." << std::endl;
}
catch (const slamcore::slam_exception& ex)
{
  std::cerr << "SLAM exception! " << ex.what() << " / " << ex.code().message() << " / "
            << ex.code().value() << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}
catch (const std::exception& ex)
{
  std::cerr << "Uncaught std::exception! " << ex.what() << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}
catch (...)
{
  std::cerr << "Uncaught unknown exception!" << std::endl;
  slamcore::slamcoreDeinit();
  return -1;
}