/******************************************************************************
*
* 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, enabling all streams.
*
* Demonstrates the following:
*
* - Create and interact with the SLAM system
* (slamcore::SLAMSystemCallbackInterface::open, slamcore::SLAMSystemCallbackInterface::start,
* slamcore::SLAMSystemCallbackInterface::close etc.)
* - Enable/use data streams.
* - Toggling of slamcore::Property(s).
* - Receive and display data in a loop.
*/
#include "slamcore/errors.hpp"
#include "slamcore/slam/slam_create.hpp"
#include <ctime>
#include <iomanip>
#include <iostream>
#include <memory>
#include <stdexcept>
#include <system_error>
#include <thread>
int main(int argc, char* argv[])
{
// ******************************************************************
// Initialise Slamcore API
// ******************************************************************
slamcore::slamcoreInit(slamcore::LogSeverity::Info,
[](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";
});
// ******************************************************************
// Create/Connect SLAM System
// ******************************************************************
slamcore::v0::SystemConfiguration sysCfg;
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();
// ******************************************************************
// Print versions
// ******************************************************************
const std::string slam_version = slam->getProperty<std::string>(slamcore::Property::FirmwareVersion);
const std::string slam_build_ver =
slam->getProperty<std::string>(slamcore::Property::FirmwareBuildVersion);
const std::string slam_build_type =
slam->getProperty<std::string>(slamcore::Property::FirmwareBuildType);
std::cout << "Client Version: " << slamcore::getVersion() << "/" << slamcore::getBuildVersion()
<< "/" << slamcore::getBuildType() << std::endl;
std::cout << "SLAM Version: " << slam_version << "/" << slam_build_ver << "/" << slam_build_type
<< std::endl;
// ******************************************************************
// Enable all the streams
// ******************************************************************
slam->setStreamEnabled(slamcore::Stream::Pose, true);
slam->setStreamEnabled(slamcore::Stream::Video, true);
slam->setStreamEnabled(slamcore::Stream::IMU, true);
slam->setStreamEnabled(slamcore::Stream::ActiveMap, true);
slam->setStreamEnabled(slamcore::Stream::Velocity, true);
slam->setStreamEnabled(slamcore::Stream::MetaData, true);
slam->setStreamEnabled(slamcore::Stream::LocalPointCloud, true);
// *****************************************************************
// Register callbacks!
// *****************************************************************
slam->registerCallback<slamcore::Stream::ErrorCode>(
[](const slamcore::ErrorCodeInterface::CPtr& errorObj)
{
const auto rc = errorObj->getValue();
std::cout << "Received: ErrorCode" << std::endl;
std::cout << "\t" << rc.message() << " / " << rc.value() << " / " << rc.category().name()
<< std::endl;
});
slam->registerCallback<slamcore::Stream::Pose>(
[](const slamcore::PoseInterface<slamcore::camera_clock>::CPtr& poseObj)
{
std::cout << "Received: Pose" << std::endl;
std::cout << "\t" << poseObj->getTranslation().x() << "," << poseObj->getTranslation().y()
<< "," << poseObj->getTranslation().z() << std::endl;
});
slam->registerCallback<slamcore::Stream::Video>(
[](const slamcore::MultiFrameInterface::CPtr& multiFrameObj)
{
std::cout << "Received: MultiFrame" << std::endl;
std::cout << "\t" << multiFrameObj->size() << " "
<< (multiFrameObj->isKeyFrame() ? "KF" : "NON-KF") << std::endl;
for (const auto& frame : *multiFrameObj)
{
const auto& img = frame.image();
std::cout << "\t\t" << img.getWidth() << " x " << img.getHeight() << " at "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
img.getHWTimestamp().time_since_epoch())
.count()
<< " [ns]" << std::endl;
}
});
slam->registerCallback<slamcore::Stream::IMU>(
[](const slamcore::IMUListInterface::CPtr& imuObj)
{
std::cout << "Received: IMUList" << std::endl;
for (std::size_t idx = 0; idx < imuObj->size(); ++idx)
{
const auto& meas = imuObj->get(idx);
switch (meas.getSensorID().first)
{
case (slamcore::SensorType::Accelerometer):
{
std::cout << "\tTimestamp: "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
meas.getHWTimestamp().time_since_epoch())
.count();
std::cout << "\tAccelerometer: " << meas.getMeasurement().x() << ","
<< meas.getMeasurement().y() << "," << meas.getMeasurement().z() << std::endl;
break;
}
case (slamcore::SensorType::Gyroscope):
{
std::cout << "\tTimestamp: "
<< std::chrono::duration_cast<std::chrono::nanoseconds>(
meas.getHWTimestamp().time_since_epoch())
.count();
std::cout << "\tGyroscope: " << meas.getMeasurement().x() << ","
<< meas.getMeasurement().y() << "," << meas.getMeasurement().z() << std::endl;
break;
}
default:
std::cerr << "Unknown IMU measurement: " << meas.getSensorID().first << std::endl;
}
}
});
slam->registerCallback<slamcore::Stream::Velocity>(
[](const slamcore::VelocityInterface<slamcore::camera_clock>::CPtr& velObj)
{
std::cout << "Received: Velocities" << std::endl;
std::cout << "\tLinear: " << velObj->getLinear().x() << "," << velObj->getLinear().y() << ","
<< velObj->getLinear().z() << std::endl;
std::cout << "\tAngular: " << velObj->getAngular().x() << "," << velObj->getAngular().y()
<< "," << velObj->getAngular().z() << std::endl;
});
slam->registerCallback<slamcore::Stream::ActiveMap>(
[](const slamcore::SparseMapInterface::CPtr& mapObj)
{
std::cout << "Received: SparseMap" << std::endl;
std::cout << "\tLandmarks: " << mapObj->size() << std::endl;
});
slam->registerCallback<slamcore::Stream::MetaData>(
[](const slamcore::MetaDataInterface::CPtr& metaObj)
{
std::cout << "Received: MetaData" << std::endl << "\tID: ";
slamcore::MetaDataID ID = metaObj->getID();
if (ID >= slamcore::MetaDataID::Count)
{
std::cout << "Unknown MetaData ID";
}
else
{
std::cout << ID;
}
std::cout << std::endl << "\tValue: ";
switch (metaObj->getValueType())
{
case slamcore::MetaDataInterface::ValueType::Integer:
{
int64_t ival = 0;
metaObj->getValue(ival);
std::cout << ival << std::endl;
}
break;
case slamcore::MetaDataInterface::ValueType::Floating:
{
double dval = 0.0;
metaObj->getValue(dval);
std::cout << dval << std::endl;
}
break;
case slamcore::MetaDataInterface::ValueType::String:
{
std::string sval;
metaObj->getValue(sval);
std::cout << sval << std::endl;
}
break;
default:
{
std::cout << "Unknown data type" << std::endl;
}
break;
}
});
slam->registerCallback<slamcore::Stream::FrameSync>(
[](const slamcore::FrameSyncInterface::CPtr&)
{ std::cout << "Received: FrameSync" << std::endl; });
slam->registerCallback<slamcore::Stream::LocalPointCloud>(
[](const slamcore::PointCloudInterface::CPtr& pointCloudObj)
{
std::cout << "Received: LocalPointCloud" << std::endl;
std::cout << "\tNumber of Points: " << pointCloudObj->size() << std::endl;
});
// ******************************************************************
// Start streaming
// ******************************************************************
slam->start();
// ******************************************************************
// Main receiving loop
// ******************************************************************
while (slam->spinOnce())
;
// ******************************************************************
// Stop SLAM
// ******************************************************************
slam->stop();
// ******************************************************************
// Disconnect/Close SLAM
// ******************************************************************
slam->close();
slam.reset();
// ******************************************************************
// Deinitialise Slamcore API
// ******************************************************************
slamcore::slamcoreDeinit();
std::cout << "We're Done Here." << std::endl;
return 0;
}