Program Listing for File sensor-manager.h¶
↰ Return to documentation for file (map-structure/vi-map/include/vi-map/sensor-manager.h
)
#ifndef VI_MAP_SENSOR_MANAGER_H_
#define VI_MAP_SENSOR_MANAGER_H_
#include <algorithm>
#include <string>
#include <unordered_set>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <aslam/cameras/ncamera.h>
#include <aslam/common/memory.h>
#include <aslam/common/sensor.h>
#include <aslam/common/unique-id.h>
#include <aslam/common/yaml-file-serialization.h>
#include <glog/logging.h>
#include <maplab-common/accessors.h>
#include <maplab-common/macros.h>
#include <sensors/absolute-6dof-pose.h>
#include <sensors/gps-utm.h>
#include <sensors/gps-wgs.h>
#include <sensors/imu.h>
#include <sensors/lidar.h>
#include <sensors/loop-closure-sensor.h>
#include <sensors/odometry-6dof-pose.h>
#include <sensors/pointcloud-map-sensor.h>
#include <sensors/sensor-types.h>
#include <sensors/wheel-odometry-sensor.h>
#include "vi-map/unique-id.h"
namespace vi_map {
class SensorManager : public aslam::YamlFileSerializable {
public:
MAPLAB_POINTER_TYPEDEFS(SensorManager);
SensorManager() = default;
~SensorManager() = default;
SensorManager(const SensorManager& other);
void operator=(const SensorManager&) = delete;
bool isEqual(const SensorManager& other, const bool verbose = false) const;
bool operator==(const SensorManager& other) const;
bool operator!=(const SensorManager& other) const;
SensorManager::Ptr clone() const {
return aligned_shared<SensorManager>(*this);
}
// Merge by cloning the sensors and extrinsics from another sensor manager.
void merge(const SensorManager& other);
// Swap the contents of two sensor managers.
void swap(SensorManager* other);
// Adds a sensor to the manager, which takes over the sensor but offers shared
// ownership if retreived later.
template <typename DerivedSensor>
void addSensor(
typename DerivedSensor::UniquePtr sensor,
const aslam::SensorId& base_sensor_id,
const aslam::Transformation& T_B_S);
template <typename DerivedSensor>
void addSensor(
typename DerivedSensor::UniquePtr sensor,
const aslam::SensorId& base_sensor_id, const aslam::Position3D& p_B_S);
template <typename DerivedSensor>
void addSensorAsBase(typename DerivedSensor::UniquePtr sensor);
// Removes a sensor and its extrinsics from the node. The function will check
// fail if the sensor id does not exist or if the sensor is a base sensor and
// still has any other sensors defined with respect to it
void removeSensor(const aslam::SensorId& sensor_id);
template <typename DerivedSensor>
const DerivedSensor& getSensor(const aslam::SensorId& sensor_id) const;
template <typename DerivedSensor>
typename DerivedSensor::Ptr getSensorPtr(
const aslam::SensorId& sensor_id) const;
void getAllSensorIds(aslam::SensorIdSet* all_sensor_ids) const;
void getAllSensorIdsOfType(
const vi_map::SensorType& sensor_type,
aslam::SensorIdSet* all_sensor_ids) const;
size_t getNumSensors() const;
size_t getNumSensorsOfType(const vi_map::SensorType& sensor_type) const;
// Check if a sensor with a given id exists.
bool hasSensor(const aslam::SensorId& sensor_id) const;
// Check if a sensor with a given type exists.
bool hasSensorOfType(const vi_map::SensorType& sensor_type) const;
// Get the type of a sensor.
SensorType getSensorType(const aslam::SensorId& sensor_id) const;
// Check if a sensor is a base reference sensor.
bool isBaseSensor(const aslam::SensorId& base_sensor_id) const;
// Get the base reference sensor id of a sensor, if it is defined.
const aslam::SensorId& getBaseSensorId(
const aslam::SensorId& sensor_id) const;
// Get the base sensor id, if there is only one, returns false otherwise.
bool getBaseSensorIdIfUnique(aslam::SensorId* sensor_id) const;
// Set the sensors calibration transformation to the base sensor
void setSensor_T_B_S(
const aslam::SensorId& sensor_id, const aslam::Transformation& T_B_S);
// Assumes no rotation in the extrinsics with respect to the base sensor.
void setSensor_T_B_S(
const aslam::SensorId& sensor_id, const aslam::Position3D& p_B_S);
// Returns the relative transformation T_B_S to the base sensor.
const aslam::Transformation& getSensor_T_B_S(
const aslam::SensorId& sensor_id) const;
aslam::Transformation& getSensor_T_B_S(const aslam::SensorId& sensor_id);
void serialize(YAML::Node* yaml_node) const override;
bool deserialize(const YAML::Node& yaml_node) override;
private:
template <typename DerivedSensor>
void addSensor(
const typename DerivedSensor::Ptr& sensor,
const aslam::SensorId& base_sensor_id,
const aslam::Transformation& T_B_S);
template <typename DerivedSensor>
void addSensorAsBase(typename DerivedSensor::Ptr sensor);
AlignedUnorderedMap<aslam::SensorId, aslam::Sensor::Ptr> sensors_;
AlignedUnorderedMap<aslam::SensorId, aslam::SensorId> base_sensor_id_map_;
AlignedUnorderedMap<aslam::SensorId, aslam::Transformation> T_B_S_map_;
};
} // namespace vi_map
#include "./sensor-manager-inl.h"
#endif // VI_MAP_SENSOR_MANAGER_H_