Example multisession.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 set up and run a SLAM system in multisession mode.
 *
 * This demonstrates how to perform SLAM with an already saved session file
 * from another execution of the software.
 */

#include <slamcore/slamcore.hpp>

#include <ctime>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <system_error>

namespace
{
/**
 * Logging callback.
 *
 * @param message Message received by the logging system.
 */
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

// Convenience struct to store Pose data.
struct Pose
{
  struct Translation
  {
    double x{0}, y{0}, z{0};
  } position;
  struct Rotation
  {
    double x{0}, y{0}, z{0}, w{1};
  } orientation;
};

// Convenience struct to store Metadata information.
struct MetaData
{
  slamcore::TrackingStatus trackingStatus{slamcore::TrackingStatus::NOT_INITIALISED};
  int64_t numFeatures{0}, trackedFeatures{0};
};

int main(int argc, char* argv[])
try
{
  if (argc < 2)
  {
    std::cout << "Usage: " << argv[0] << " <session-file>" << std::endl;
    return -1;
  }

  // Initialise SLAMcore API
  slamcore::slamcoreInit(slamcore::LogSeverity::Info, ::log);

  // Create/Connect SLAM System
  slamcore::v0::SystemConfiguration sysCfg;
  sysCfg.LoadSessionFilePath = argv[1];
  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
  slam->setStreamEnabled(slamcore::Stream::Pose, true);
  slam->setStreamEnabled(slamcore::Stream::MetaData, true);

  // Register callbacks
  Pose pose;
  slam->registerCallback<slamcore::Stream::Pose>(
    [&pose](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& poseObj) {
      pose.position.x = poseObj->getTranslation().x();
      pose.position.y = poseObj->getTranslation().y();
      pose.position.z = poseObj->getTranslation().z();
      pose.orientation.x = poseObj->getRotation().x();
      pose.orientation.y = poseObj->getRotation().y();
      pose.orientation.z = poseObj->getRotation().z();
      pose.orientation.w = poseObj->getRotation().w();
    });

  MetaData meta;
  slam->registerCallback<slamcore::Stream::MetaData>(
    [&meta](const slamcore::MetaDataInterface::CPtr& metaObj) {
      switch (metaObj->getID())
      {
      case slamcore::MetaDataID::TrackingStatus:
      {
        metaObj->getValue(meta.trackingStatus);
        break;
      }
      case slamcore::MetaDataID::NumFeatures:
      {
        metaObj->getValue(meta.numFeatures);
        break;
      }
      case slamcore::MetaDataID::TrackedFeatures:
      {
        metaObj->getValue(meta.trackedFeatures);
        break;
      }
      default:
      {
      }
      }
    });

  // Start streaming
  slam->start();

  // Main receiving loop
  while (slam->spinOnce())
  {
    std::cout << "Tracking Status: " << meta.trackingStatus << std::endl;
    std::cout << "Tracked Features: " << meta.trackedFeatures << "/" << meta.numFeatures
              << std::endl;
    if (meta.trackingStatus == slamcore::TrackingStatus::OK)
    {
      std::cout << "Position   : " << pose.position.x << ", " << pose.position.y << ", "
                << pose.position.z << std::endl;
      std::cout << "Orientation: " << pose.orientation.x << ", " << pose.orientation.y
                << ", " << pose.orientation.z << ", " << pose.orientation.w << std::endl;
    }
    else
    {
      std::cout << "Position   : ??, ??, ??" << std::endl;
      std::cout << "Orientation: ??, ??, ??, ??" << std::endl;
    }
  }

  // Stop SLAM
  slam->stop();

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