You’re reading an older version of the Slamcore SDK documenation. The latest one is 23.04.
Example write_trajectory.cpp
/******************************************************************************
*
* Slamcore Confidential
* ---------------------
*
* Slamcore Limited
* All Rights Reserved.
* (C) Copyright 2021
*
* 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 use the trajectory subsystem and helpers.
*/
#include "slamcore/errors.hpp"
#include "slamcore/slam/slam_create.hpp"
#include "slamcore/subsystems/optimised_trajectory.hpp"
#include <errno.h>
#include <string.h>
#include <sys/stat.h>
#include <atomic>
#include <ctime>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>
#include <thread>
// Flag variable indicating processing completed or stopped.
static std::atomic<bool> s_finished{false};
namespace
{
void logInfo(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";
}
bool isDirectory(const char* directory)
{
struct stat st;
if (0 == stat(directory, &st))
{
return S_ISDIR(st.st_mode);
}
const auto statErrno = errno;
std::cerr << "Failed to stat '" << directory << "'. Error: " << strerror(statErrno) << std::endl;
return false;
}
bool createOutputDirectory(const char* directory)
{
int status = mkdir(directory, S_IRWXU | S_IRWXG | S_IROTH | S_IXOTH);
if (status != 0)
{
const auto mkdirError = errno;
if (EEXIST == mkdirError && isDirectory(directory))
{
return true;
}
std::cerr << "Failed to create output directory '" << directory
<< "'. Error: " << strerror(mkdirError) << std::endl;
return false;
}
return true;
}
} // namespace
int main(int argc, char* argv[])
{
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " <dataset-path> [<config-file>]" << std::endl;
return -1;
}
const auto outputDir = "trajectories";
if (!createOutputDirectory(outputDir))
{
return -1;
}
// Initialise Slamcore API
slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::logInfo);
// Create/Connect SLAM System
slamcore::v0::SystemConfiguration sysCfg;
sysCfg.Sources.push_back(slamcore::DataSource::Dataset);
sysCfg.DatasetPath = argv[1];
if (argc > 2)
{
sysCfg.ConfigFilePath = argv[2];
}
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();
// Enable streams of interest
slam->setStreamEnabled(slamcore::Stream::Pose, true);
slam->setStreamEnabled(slamcore::Stream::MetaData, true);
slam->setStreamEnabled(slamcore::Stream::FrameSync, true);
// Catch end of dataset
slam->registerCallback<slamcore::Stream::ErrorCode>(
[](const slamcore::ErrorCodeInterface::CPtr& errorObj)
{
const std::error_code rc = errorObj->getValue();
if (rc == make_error_code(slamcore::errc::end_of_data))
{
s_finished.store(true);
}
});
// Accumulate trajectory and tracking status
slamcore::TrajectoryHelper trajectoryHelper;
const auto mainCallback = [&trajectoryHelper](const slamcore::ConstTaggedObject& obj) -> void
{
trajectoryHelper.feed(obj);
};
slam->registerCallback<slamcore::Stream::Pose>(mainCallback);
slam->registerCallback<slamcore::Stream::MetaData>(mainCallback);
slam->registerCallback<slamcore::Stream::FrameSync>(mainCallback);
// Get trajectory subsystem
const std::shared_ptr<slamcore::OptimisedTrajectorySubsystemInterface> trajectorySubsystem =
slam->getSubsystem<slamcore::OptimisedTrajectorySubsystemInterface>();
// Start streaming
slam->start();
// Main receiving loop
while (slam->spinOnce() && !s_finished)
;
// Stop SLAM
slam->stop();
// Update subsystem's internal state with the most recent trajectories
trajectorySubsystem->fetch();
// Write trajectories as csv files in given output directory
trajectoryHelper.writeTrajectories(outputDir, trajectorySubsystem->getTrajectory().get(), &std::cout);
// Disconnect/Close SLAM
slam->close();
// Deinitialise Slamcore API
slamcore::slamcoreDeinit();
std::cout << "We're Done Here." << std::endl;
}