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/slamcore.hpp>
#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 log(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";
}
} // namespace
int main(int argc, char* argv[])
try
{
if (argc < 2)
{
std::cout << "Usage: " << argv[0] << " <dataset-path> [<config-file>]" << std::endl;
return -1;
}
// Initialise SLAMcore API
slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::log);
// Create/Connect SLAM System
slamcore::v0::SystemConfiguration sysCfg;
sysCfg.Source = 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();
// 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);
}
});
// Enable saving trajectories so they can be fetched by the subsystem
slam->setProperty(slamcore::Property::SaveTrajectories, true);
// Get trajectory subsystem
const std::shared_ptr<slamcore::TrajectorySubsystemInterface> trajectorySubsystem =
slam->getSubsystem<slamcore::TrajectorySubsystemInterface>();
// Start streaming
slam->start();
// Main receiving loop
while (slam->spinOnce() && !s_finished)
{
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
// 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
slamcore::TrajectorySubsystemInterface::TrajectoryPaths paths =
trajectorySubsystem->write("trajectories");
std::cout << "Unoptimised trajectory written to " << paths.UnoptimisedTrajectory
<< std::endl;
if (trajectorySubsystem->getOptimisedTrajectory())
{
std::cout << "Optimised trajectory written to " << paths.OptimisedTrajectory << std::endl;
}
// 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 << "system_error 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;
}