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