Home

Welcome to the slambench3 wiki!

Dummy library template:

#include <SLAMBenchAPI.h>
#include <io/sensor/CameraSensorFinder.h>
#include <io/sensor/CameraSensor.h>
#include <opencv2/core/mat.hpp>

// Define parameters
int example_param, default_example_param = 0;

// Define sensors
slambench::io::CameraSensor *rgb_sensor;

// Define outputs
slambench::outputs::Output *pose_output;

// Define OpenCV image to hold data
cv::Mat *cv_rgb;

/**
 * Setup all the outputs for this SLAM algorithm
 * @param slam_settings
 */
void setup_outputs(SLAMBenchLibraryHelper *slam_settings)
{
  pose_output = new slambench::outputs::Output("KinectFusion Pose", slambench::values::VT_POSE, true);
  slam_settings->GetOutputManager().RegisterOutput(pose_output);
}

/**
 * Setup all sensors for this SLAM algorithm
 * @param slam_settings
 * @return
 */
bool setup_sensors(SLAMBenchLibraryHelper *slam_settings)
{
  slambench::io::CameraSensorFinder sensor_finder;
  rgb_sensor = sensor_finder.FindOne(slam_settings->get_sensors(), {{"camera_type", "rgb"}});
  if(rgb_sensor == nullptr)
  {
    std::cerr << "RGB sensor not found." << std::endl;
    return false;
  }
  if(rgb_sensor->PixelFormat != slambench::io::pixelformat::RGB_III_888)
  {
    std::cerr << "RGB sensor is not in RGB_III_888 format" << std::endl;
    return false;
  }
  if(rgb_sensor->FrameFormat != slambench::io::frameformat::Raster)
  {
    std::cerr << "RGB sensor is not in raster format" << std::endl;
    return false;
  }
  cv_rgb = new cv::Mat(rgb_sensor->Height, rgb_sensor->Width, CV_8UC3);

  return true;
}

/**
 * Declare all command line parameters here
 * @param slam_settings
 * @return
 */
bool sb_new_slam_configuration(SLAMBenchLibraryHelper *slam_settings)
{
  // Declare parameters
  slam_settings->addParameter(TypedParameter<int>("short", "long-option", "Description", &example_param, &default_example_param));
  return true;
}

/**
 * Setup all the sensors, outputs, and initialize the SLAM system
 * @param slam_settings
 * @return Whether or not the initialisation is successful
 */
bool sb_init_slam_system(SLAMBenchLibraryHelper *slam_settings)
{
  setup_outputs(slam_settings);
  setup_sensors(slam_settings);
  return true;
}

/**
 * Receive a new frame from the sensors
 * @param frame new input frame (from any sensor type, not necessarily camera)
 * @return true if the frame matches any of the algorithm's sensors, false otherwise
 */
bool sb_update_frame(SLAMBenchLibraryHelper*, slambench::io::SLAMFrame *frame)
{
  if(frame->FrameSensor == rgb_sensor)
  {
    memcpy(cv_rgb->data, frame->GetData(), frame->GetSize());
    frame->FreeData();
    return true;
  }

  return false;
}

/**
 * Execute SLAM pipeline with
 * @param slam_settings
 * @return Whether or not the execution was successful
 */
bool sb_process_once (SLAMBenchLibraryHelper *slam_settings)
{
  return true;
}

/**
 *
 * @param lib
 * @param ts current time stamp
 * @return
 */
bool sb_update_outputs(SLAMBenchLibraryHelper *lib, const slambench::TimeStamp *ts)
{

  if(pose_output->IsActive())
  {
    Eigen::Matrix4f mat;
    std::lock_guard<FastLock> lock (lib->GetOutputManager().GetLock());
    pose_output->AddPoint(*ts, new slambench::values::PoseValue(mat));
  }

  return true;
}

/**
 * Clean up any allocated global pointers
 * @return
 */
bool sb_clean_slam_system()
{
  delete rgb_sensor;
  delete pose_output;
  delete cv_rgb;
  return true;
}