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

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

#ifndef SENSORS_ABSOLUTE_6DOF_POSE_H_
#define SENSORS_ABSOLUTE_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 Absolute pose sensor, that provides a transformation from
// the sensor frame to the world frame, which is assumed to be the mission
// frame.
class Absolute6DoF final : public aslam::Sensor {
 public:
  MAPLAB_POINTER_TYPEDEFS(Absolute6DoF);

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

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

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

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

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

  inline bool get_T_G_S_fixed_covariance(
      aslam::TransformationCovariance* T_G_S_fixed_covariance) const {
    if (has_fixed_T_G_S_covariance_) {
      *T_G_S_fixed_covariance = T_G_S_fixed_covariance_;
      return true;
    }
    return false;
  }

  inline void set_T_G_S_fixed_covariance(
      const aslam::TransformationCovariance& T_G_S_fixed_covariance) {
    T_G_S_fixed_covariance_ = T_G_S_fixed_covariance;
    has_fixed_T_G_S_covariance_ = true;
  }

  inline bool has_T_G_S_fixed_covariance() {
    return has_fixed_T_G_S_covariance_;
  }

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

  bool has_fixed_T_G_S_covariance_;

  // Can be set as a property of the sensor and be used as a fall-back in case
  // the measurements do not come with individual covariances.
  aslam::TransformationCovariance T_G_S_fixed_covariance_;
};

// Stores 6DoF absolute measurements, i.e. a transformation from the
// sensor frame (S) to the world frame (W)
class Absolute6DoFMeasurement : public Measurement {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  MAPLAB_POINTER_TYPEDEFS(Absolute6DoFMeasurement);

  Absolute6DoFMeasurement() {
    T_G_S_.setIdentity();
    T_G_S_covariance_.setZero();
    has_T_M_B_cached_ = false;
  }

  explicit Absolute6DoFMeasurement(const aslam::SensorId& sensor_id)
      : Measurement(sensor_id) {
    T_G_S_.setIdentity();
    T_G_S_covariance_.setZero();
    has_T_M_B_cached_ = false;
  }

  explicit Absolute6DoFMeasurement(
      const aslam::SensorId& sensor_id, const int64_t timestamp_nanoseconds,
      const aslam::Transformation& T_G_S,
      const aslam::TransformationCovariance& covariance)
      : Measurement(sensor_id, timestamp_nanoseconds),
        T_G_S_(T_G_S),
        T_G_S_covariance_(covariance) {
    has_T_M_B_cached_ = false;
  }

  inline const aslam::Transformation& get_T_G_S() const {
    return T_G_S_;
  }

  inline void set_T_G_S(const aslam::Transformation& T_G_S) {
    T_G_S_ = T_G_S;
  }

  inline const aslam::TransformationCovariance& get_T_G_S_covariance() const {
    return T_G_S_covariance_;
  }

  inline void set_T_G_S_covariance(
      const aslam::TransformationCovariance& measurement_covariance) {
    T_G_S_covariance_ = measurement_covariance;
  }

  inline bool isEqual(
      const Absolute6DoFMeasurement& other, const bool verbose = false) const {
    bool is_equal = true;
    if (other.getSensorId() != getSensorId()) {
      LOG_IF(WARNING, verbose)
          << "Absolute6DoFMeasurements have different sensor id: "
          << other.getSensorId() << " vs " << getSensorId();
      is_equal = false;
    }

    if (other.getTimestampNanoseconds() != getTimestampNanoseconds()) {
      LOG_IF(WARNING, verbose)
          << "Absolute6DoFMeasurements have different timestamps: "
          << other.getTimestampNanoseconds() << " vs "
          << getTimestampNanoseconds();
      is_equal = false;
    }

    constexpr double kPrecision = 1e-6;
    if ((other.get_T_G_S().getTransformationMatrix() -
         get_T_G_S().getTransformationMatrix())
            .cwiseAbs()
            .maxCoeff() > kPrecision) {
      LOG_IF(WARNING, verbose)
          << "Absolute6DoFMeasurements have different T_G_S: "
          << other.get_T_G_S() << " vs " << get_T_G_S();
      is_equal = false;
    }

    if ((other.get_T_G_S_covariance() - get_T_G_S_covariance())
            .cwiseAbs()
            .maxCoeff() > kPrecision) {
      LOG_IF(WARNING, verbose)
          << "Absolute6DoFMeasurements have different T_G_S_covariance: "
          << other.get_T_G_S_covariance() << " vs " << get_T_G_S_covariance();
      is_equal = false;
    }
    return is_equal;
  }

  inline bool has_T_M_B_cached() const {
    return has_T_M_B_cached_;
  }
  inline bool get_T_M_B_cached(aslam::Transformation* T_M_B_cached) const {
    if (has_T_M_B_cached_) {
      *T_M_B_cached = T_M_B_cached_;
      return true;
    }
    return false;
  }
  inline void set_T_M_B_cached(const aslam::Transformation& T_M_B_cached) {
    has_T_M_B_cached_ = true;
    T_M_B_cached_ = T_M_B_cached;
  }

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

  inline void setRandomImpl() override {
    T_G_S_.setRandom();
    T_G_S_covariance_.setRandom();
  }

  aslam::Transformation T_G_S_;
  aslam::TransformationCovariance T_G_S_covariance_;

  // If the absolute pose constraints arrive in real-time, we can use the
  // pose-lookup buffer to already interpolate the odometry estimate at the time
  // of the measurement and don't need to interpolate it later based on the map
  // state.
  bool has_T_M_B_cached_;
  aslam::Transformation T_M_B_cached_;
};

DEFINE_MEAUREMENT_CONTAINERS(Absolute6DoFMeasurement)

}  // namespace vi_map

DEFINE_MEASUREMENT_HASH(Absolute6DoFMeasurement)

#endif  // SENSORS_ABSOLUTE_6DOF_POSE_H_