/******************************************************************************
*
* 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 << "'" << std::endl;
}
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, 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)"
<< std::endl; // CSV header
else
std::cerr << "Could not open the file - '" << mem_cpu_file_name << "'\n"
<< "Corresponding data will not be stored into csv..." << std::endl;
}
else
std::cout << "No output folder provided. Skipping CSV writing..." << std::endl;
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" << std::endl;
// (opt) Dump to CSV file
if (mem_cpu_file.is_open())
mem_cpu_file << timestamp << ", " << memory << ", " << system_cpu << ", " << user_cpu
<< std::endl;
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 << std::endl;
}
std::cerr << "Usage: " << script << " " << usage << std::endl;
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 << std::endl;
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)" << std::endl;
else
std::cerr << "Could not open the file - '" << framerate_file_name << "'\n"
<< "Corresponding data will not be stored into csv..." << std::endl;
mem_cpu_file_name = params.output_folder + "/memory_cpu.csv";
}
else
std::cout << "No output folder provided. Skipping CSV writing..." << std::endl;
// 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!" << std::endl;
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() << std::endl;
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 << std::endl;
// (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 << std::endl;
// (opt) Dump to CSV file
if (framerate_file.is_open())
framerate_file << fps << std::endl;
}
});
// Start streaming
slam->start();
// Start looking for CPU/memory usage
printPerfThread.detach();
// Run until error or CTRL+C
while (!s_finished)
{
slam->spinOnce();
}
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 << std::endl;
break;
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
switch (sessionState)
{
case slamcore::TaskStatusInterface::TaskState::Success:
std::cout << "Saving session done!" << std::endl;
break;
case slamcore::TaskStatusInterface::TaskState::Progress:
slam->cancelAsyncTask(slamcore::TaskType::SaveSession, tid);
std::cout << "Saving session interrupted!" << std::endl;
return -1;
case slamcore::TaskStatusInterface::TaskState::Cancelled:
std::cout << "Saving session cancelled!" << std::endl;
return -1;
default:
std::cout << "Saving session error!" << std::endl;
return -1;
}
}
// Disconnect/Close SLAM
slam->close();
// Deinitialise Slamcore API
slamcore::slamcoreDeinit();
std::cout << "We're Done Here." << std::endl;
return 0;
}