Example measure_slam_performance.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 measure some of the system performance like FPS, memory and CPU usage.
 */

#include "slamcore/slam/slam_create.hpp"

#include <unistd.h>

#include "sys/times.h"

#include <atomic>
#include <chrono>
#include <csignal>
#include <cstdio>
#include <cstring>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <sstream>
#include <thread>

static constexpr auto version = "1.0.0";

// Flag variable indicating processing completed or stopped.
static std::atomic<bool> s_finished{false};

// Callback to be executed on SIGINT.
static void handleSignal(int)
{
  s_finished.store(true);
}

// Extract the integer value from a string
int extractValue(const std::string& line)
{
  std::stringstream ss;
  ss << line;
  std::string tmp;
  int value = -1;

  // Parse the string
  while (!ss.eof())
  {
    ss >> tmp;

    // Check if the current chunk is the integer
    if (std::stringstream(tmp) >> value)
    {
      break;
    }
    tmp = "";
  }
  return value;
}

/*
Get the physical memory currently used by current process by
extracting the value VmRSS from /proc/self/status
*/
int getMemoryUsedByCurrentProcess()
{
  std::string filename("/proc/self/status");
  std::ifstream file(filename);
  std::string line;
  int result = -1;

  if (!file.is_open())
  {
    std::cerr << "Could not open the file - '" << filename << "'" << '\n';
  }

  while (std::getline(file, line))
  {
    if (line.find("VmRSS") != std::string::npos)
    {
      result = extractValue(line); // kB
      break;
    }
  }

  file.close();
  return result;
}

// Get the System CPU time already used by this process
double getCurrentSystemCpuTime()
{
  struct tms timeSample;
  times(&timeSample);
  return timeSample.tms_stime; // ms
}

// Get the User CPU time already used by this process
double getCurrentUserCpuTime()
{
  struct tms timeSample;
  times(&timeSample);
  return timeSample.tms_utime; // ms
}

void printPerf(int interval, const std::string& mem_cpu_file_name = "")
{
  // File initialization
  std::ofstream mem_cpu_file;
  if (!mem_cpu_file_name.empty())
  {
    mem_cpu_file.open(mem_cpu_file_name);

    if (mem_cpu_file.is_open())
    {
      mem_cpu_file << "timestamp (ns), memory_usage (kB), system_cpu_usage (ms), user_cpu_usage (ms)"
                   << '\n'; // CSV header
    }
    else
    {
      std::cerr << "Could not open the file - '" << mem_cpu_file_name << "'\n"
                << "Corresponding data will not be stored into csv..." << '\n';
    }
  }
  else
  {
    std::cout << "No output folder provided. Skipping CSV writing..." << '\n';
  }

  while (true)
  {
    const auto p1 = std::chrono::system_clock::now();
    auto timestamp =
      std::chrono::duration_cast<std::chrono::nanoseconds>(p1.time_since_epoch()).count();
    // Get memory usage
    int memory = getMemoryUsedByCurrentProcess();

    // Get CPU usage
    double system_cpu = getCurrentSystemCpuTime();
    double user_cpu = getCurrentUserCpuTime();

    // Print results on stdout
    std::cout << "[Mem/CPU] Timestamp: " << timestamp << "\n"
              << "[Mem/CPU] Memory_Used: " << memory << " kB\n"
              << "[Mem/CPU] System_CPU_time: " << system_cpu << " ms\n "
              << "[Mem/CPU] User_CPU_time: " << user_cpu << " ms" << '\n';

    // (opt) Dump to CSV file
    if (mem_cpu_file.is_open())
    {
      mem_cpu_file << timestamp << ", " << memory << ", " << system_cpu << ", " << user_cpu << '\n';
    }

    auto ms = std::chrono::steady_clock::now() + std::chrono::milliseconds(interval);
    std::this_thread::sleep_until(ms);
  }
  mem_cpu_file.close();
}

void usage_error(const std::string& script, const std::string& message = "")
{
  constexpr auto usage = "-u <euroc_dataset> "
                         "[-c <configuration-file>] "
                         "[-o <output_folder>] "
                         "[-i <capture_interval>] "
                         "[-s <save_session_name> | -l <load_session_path>]\n";
  if (!message.empty())
  {
    std::cerr << script << ": " << message << '\n';
  }
  std::cerr << "Usage: " << script << " " << usage << '\n';
  exit(1);
}

struct Parameters
{
  std::string dataset;
  std::string configuration_file;
  std::string output_folder;
  std::string save_session_name;
  std::string load_session_path;
  int capture_interval = 100; // ms
};

Parameters getParameters(int argc, char* argv[])
{
  int opt = 0;
  Parameters params{};

  while ((opt = getopt(argc, argv, "vu:c:o:i:s:l:")) != -1)
  {
    switch (opt)
    {
      case 'v':
        std::cout << "Measure SLAM performance v" << version << '\n';
        exit(0);
      case 'u':
        params.dataset = optarg;
        break;
      case 'c':
        params.configuration_file = optarg;
        break;
      case 'o':
        params.output_folder = optarg;
        break;
      case 'i':
        params.capture_interval = atoi(optarg);
        break;
      case 's':
        params.save_session_name = optarg;
        break;
      case 'l':
        params.load_session_path = optarg;
        break;
      default:
        usage_error(argv[0]);
    }
  }

  if (params.dataset.empty())
  {
    usage_error(argv[0], "no dataset specified");
  }

  if (!params.save_session_name.empty() && !params.load_session_path.empty())
  {
    usage_error(argv[0], "argument '-s' not allowed with argument '-l'");
  }
  return params;
}

int main(int argc, char* argv[])
{
  const auto params = getParameters(argc, argv);

  // Handle Ctrl-C
  signal(SIGINT, handleSignal);

  // 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 SLAM System
  slamcore::v0::SystemConfiguration sysCfg;

  // Set datasource
  sysCfg.Sources.push_back(slamcore::DataSource::Dataset);
  sysCfg.DatasetPath = params.dataset;
  if (!params.configuration_file.empty())
  {
    sysCfg.ConfigFilePath = params.configuration_file;
  }

  // Init target CSV file if an output folder is provided
  std::ofstream framerate_file;
  std::string framerate_file_name, mem_cpu_file_name;
  if (!params.output_folder.empty())
  {
    framerate_file_name = params.output_folder + "/framerate.csv";
    framerate_file.open(framerate_file_name);

    if (framerate_file.is_open())
    {
      framerate_file << "timestamp (ns), framerate (frame/s)" << '\n';
    }
    else
    {
      std::cerr << "Could not open the file - '" << framerate_file_name << "'\n"
                << "Corresponding data will not be stored into csv..." << '\n';
    }

    mem_cpu_file_name = params.output_folder + "/memory_cpu.csv";
  }
  else
  {
    std::cout << "No output folder provided. Skipping CSV writing..." << '\n';
  }

  // Memory/CPU capture thread
  std::thread printPerfThread = std::thread(
    [params, mem_cpu_file_name]() { printPerf(params.capture_interval, mem_cpu_file_name); });

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

  // Open the device
  if (!params.load_session_path.empty())
  {
    slam->openWithSession(params.load_session_path.c_str());
  }
  else
  {
    slam->open();
  }

  // Register the error callback
  slam->registerCallback<slamcore::Stream::ErrorCode>(
    [](const slamcore::ErrorCodeInterface::CPtr& errorObj)
    {
      const std::error_code ec = errorObj->getValue();
      std::cerr << "Error: " << ec.message() << " / " << ec.value() << '\n';
      s_finished.store(true);
    });

  // Get streams
  slam->setStreamEnabled(slamcore::Stream::Video, true);
  slam->setStreamEnabled(slamcore::Stream::MetaData, true);

  slam->registerCallback<slamcore::Stream::Video>(
    [&framerate_file](const slamcore::MultiFrameInterface::CPtr& multiFrameObj)
    {
      // Acquire the timestamp of a multiframe
      // It will be very close (just before) the timestamp at which ProcessedFrameRate is computed
      auto timestamp =
        (*multiFrameObj)[0].image().getAcquisitionTimestamp().time_since_epoch().count();

      // Print it to stdout
      std::cout << "[Framerate] Timestamp: " << timestamp << '\n';

      // (opt) Dump to CSV file
      if (framerate_file.is_open())
      {
        framerate_file << timestamp << ", ";
      }
    });

  slam->registerCallback<slamcore::Stream::MetaData>(
    [&framerate_file](const slamcore::MetaDataInterface::CPtr& metaObj)
    {
      slamcore::MetaDataID ID = metaObj->getID();
      if (ID == slamcore::MetaDataID::ProcessedFrameRate)
      {
        // Get framerate
        double fps = 0.0;
        metaObj->getValue(fps);

        // Print it to stdout
        std::cout << "[Framerate] Frame per second: " << fps << '\n';

        // (opt) Dump to CSV file
        if (framerate_file.is_open())
        {
          framerate_file << fps << '\n';
        }
      }
    });

  // Start streaming
  slam->start();

  // Start looking for CPU/memory usage
  printPerfThread.detach();

  // Run until error or CTRL+C
  while (!s_finished)
  {
    slam->spin();
  }

  framerate_file.close();

  slam->stop();

  // Save session
  if (!params.save_session_name.empty())
  {
    const auto tid =
      slam->launchAsyncTask(slamcore::TaskType::SaveSession,
                            {{"filename", params.output_folder + "/" + params.save_session_name}});
    auto sessionState = slamcore::TaskStatusInterface::TaskState::Idle;

    while (true)
    {
      const auto info = slam->getTaskStatus(slamcore::TaskType::SaveSession, tid);
      if (info)
      {
        sessionState = info->getState();

        std::cout << std::unitbuf << "\rStatus: " << sessionState << std::flush;

        if (sessionState != slamcore::TaskStatusInterface::TaskState::Progress)
        {
          std::cout << '\n';
          break;
        }
      }
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    switch (sessionState)
    {
      case slamcore::TaskStatusInterface::TaskState::Success:
        std::cout << "Saving session done!" << '\n';
        break;
      case slamcore::TaskStatusInterface::TaskState::Progress:
        slam->cancelAsyncTask(slamcore::TaskType::SaveSession, tid);
        std::cout << "Saving session interrupted!" << '\n';
        return -1;
      case slamcore::TaskStatusInterface::TaskState::Cancelled:
        std::cout << "Saving session cancelled!" << '\n';
        return -1;
      default:
        std::cout << "Saving session error!" << '\n';
        return -1;
    }
  }

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

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

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

  return 0;
}