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;
}