You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.
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;
}