Program Listing for File odometry-6dof-pose.h

↰ Return to documentation for file (map-structure/sensors/include/sensors/odometry-6dof-pose.h)

#ifndef SENSORS_ODOMETRY_6DOF_POSE_H_
#define SENSORS_ODOMETRY_6DOF_POSE_H_

#include <string>

#include <aslam/common/sensor.h>
#include <maplab-common/macros.h>
#include <yaml-cpp/yaml.h>

#include "sensors/measurement.h"
#include "sensors/sensor-types.h"

namespace vi_map {

// Represents a 6DoF Odometry sensor, that provides a transformation from the
// odometry sensor frame to the odometry base frame, which is assumed to be the
// mission frame.
class Odometry6DoF final : public aslam::Sensor {
 public:
  MAPLAB_POINTER_TYPEDEFS(Odometry6DoF);

  Odometry6DoF();
  explicit Odometry6DoF(
      const aslam::SensorId& sensor_id, const std::string& topic);

  Sensor::Ptr cloneAsSensor() const {
    return std::dynamic_pointer_cast<Sensor>(
        aligned_shared<Odometry6DoF>(*this));
  }

  Odometry6DoF* cloneWithNewIds() const {
    Odometry6DoF* cloned_odom_sensor = new Odometry6DoF();
    *cloned_odom_sensor = *this;
    aslam::SensorId new_id;
    aslam::generateId(&new_id);
    cloned_odom_sensor->setId(new_id);
    return cloned_odom_sensor;
  }

  uint8_t getSensorType() const override {
    return SensorType::kOdometry6DoF;
  }

  std::string getSensorTypeString() const override {
    return static_cast<std::string>(kOdometry6DoFIdentifier);
  }

  inline bool has_T_St_Stp1_fixed_covariance() {
    return has_T_St_Stp1_fixed_covariance_;
  }

  inline bool get_T_St_Stp1_fixed_covariance(
      aslam::TransformationCovariance* T_St_Stp1_fixed_covariance) const {
    if (has_T_St_Stp1_fixed_covariance_) {
      *T_St_Stp1_fixed_covariance = T_St_Stp1_fixed_covariance_;
      return true;
    }
    return false;
  }

  inline void set_T_St_Stp1_fixed_covariance(
      const aslam::TransformationCovariance& T_St_Stp1_fixed_covariance) {
    T_St_Stp1_fixed_covariance_ = T_St_Stp1_fixed_covariance;
    has_T_St_Stp1_fixed_covariance_ = true;
  }

 private:
  bool loadFromYamlNodeImpl(const YAML::Node& sensor_node) override;
  void saveToYamlNodeImpl(YAML::Node* sensor_node) const override;

  bool isValidImpl() const override {
    return true;
  }

  void setRandomImpl() override {}

  bool isEqualImpl(
      const Sensor& /*other*/, const bool /*verbose*/) const override {
    return true;
  }

  aslam::TransformationCovariance T_St_Stp1_fixed_covariance_;
  bool has_T_St_Stp1_fixed_covariance_;
};

// Stores 6DoF Odometry measurements, i.e. a transformation from the odometry
// sensor frame (S) to the odometry base frame (O), which is assumed to be the
// mission frame (M).
class Odometry6DoFMeasurement : public Measurement {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  MAPLAB_POINTER_TYPEDEFS(Odometry6DoFMeasurement);

  Odometry6DoFMeasurement() {
    T_O_S_.setIdentity();
    T_O_S_covariance_.setZero();
  }

  explicit Odometry6DoFMeasurement(
      const aslam::SensorId& sensor_id, const int64_t timestamp_nanoseconds,
      const aslam::Transformation& T_O_S,
      const aslam::TransformationCovariance& covariance)
      : Measurement(sensor_id, timestamp_nanoseconds),
        T_O_S_(T_O_S),
        T_O_S_covariance_(covariance) {}

  const aslam::Transformation& get_T_O_S() const {
    return T_O_S_;
  }

  void set_T_O_S(const aslam::Transformation& T_O_S) {
    T_O_S_ = T_O_S;
  }

  inline bool has_T_O_S_covariance() {
    return has_T_O_S_covariance_;
  }

  inline bool get_T_O_S_covariance(
      aslam::TransformationCovariance* T_O_S_covariance) const {
    if (has_T_O_S_covariance_) {
      *T_O_S_covariance = T_O_S_covariance_;
      return true;
    }
    return false;
  }

  inline void set_T_O_S_covariance(
      const aslam::TransformationCovariance& T_O_S_covariance) {
    T_O_S_covariance_ = T_O_S_covariance;
    has_T_O_S_covariance_ = true;
  }

 private:
  bool isValidImpl() const {
    return true;
  }

  void setRandomImpl() override {
    T_O_S_.setRandom();
    T_O_S_covariance_.setRandom();
  }

  aslam::Transformation T_O_S_;
  aslam::TransformationCovariance T_O_S_covariance_;
  bool has_T_O_S_covariance_;
};

DEFINE_MEAUREMENT_CONTAINERS(Odometry6DoFMeasurement)

constexpr char kYamlFieldNameFixedCovariance[] = "T_St_Stp1_fixed_covariance";

}  // namespace vi_map

DEFINE_MEASUREMENT_HASH(Odometry6DoFMeasurement)

#endif  // SENSORS_ODOMETRY_6DOF_POSE_H_