Program Listing for File lidar.h¶
↰ Return to documentation for file (map-structure/sensors/include/sensors/lidar.h
)
#ifndef SENSORS_LIDAR_H_
#define SENSORS_LIDAR_H_
#include <memory>
#include <string>
#include <vector>
#include <aslam/common/pose-types.h>
#include <aslam/common/sensor.h>
#include <maplab-common/macros.h>
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <resources-common/point-cloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <yaml-cpp/yaml.h>
#include "sensors/measurement.h"
#include "sensors/sensor-types.h"
namespace vi_map {
class VIMap;
class MeasurementsTest_TestAccessorsLidar_Test;
class Lidar final : public aslam::Sensor {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MAPLAB_POINTER_TYPEDEFS(Lidar);
Lidar() = default;
explicit Lidar(const aslam::SensorId& sensor_id);
Lidar(const aslam::SensorId& sensor_id, const std::string& topic);
Sensor::Ptr cloneAsSensor() const {
return std::dynamic_pointer_cast<Sensor>(aligned_shared<Lidar>(*this));
}
Lidar* cloneWithNewIds() const {
Lidar* cloned_lidar = new Lidar();
*cloned_lidar = *this;
aslam::SensorId new_id;
aslam::generateId(&new_id);
cloned_lidar->setId(new_id);
return cloned_lidar;
}
uint8_t getSensorType() const override {
return SensorType::kLidar;
}
std::string getSensorTypeString() const override {
return static_cast<std::string>(kLidarIdentifier);
}
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;
}
};
template <typename PointCloudType>
class LidarMeasurement : public Measurement {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
MAPLAB_POINTER_TYPEDEFS(LidarMeasurement<PointCloudType>);
LidarMeasurement() = default;
LidarMeasurement(
const aslam::SensorId& sensor_id, const int64_t timestamp_nanoseconds,
const PointCloudType& point_cloud)
: Measurement(sensor_id, timestamp_nanoseconds),
point_cloud_(point_cloud) {
CHECK(isValid());
}
LidarMeasurement(
const aslam::SensorId& sensor_id, const int64_t timestamp_nanoseconds)
: Measurement(sensor_id, timestamp_nanoseconds) {
CHECK(isValid());
}
~LidarMeasurement() = default;
const PointCloudType& getPointCloud() const {
return point_cloud_;
}
PointCloudType* getPointCloudMutable() {
return &point_cloud_;
}
bool operator==(const LidarMeasurement& other) const {
return Measurement::operator==(other) &&
point_cloud_ == other.getPointCloud();
}
private:
explicit LidarMeasurement(const aslam::SensorId& sensor_id)
: Measurement(sensor_id) {}
bool isValidImpl() const override;
void setRandomImpl() override;
PointCloudType point_cloud_;
};
} // namespace vi_map
// Adds a new Ouster LIDAR point type to PCL.
namespace pcl {
struct OusterPointType {
PCL_ADD_POINT4D
int time_offset_us;
uint16_t reflectivity;
uint16_t signal;
uint8_t ring;
float intensity;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
} // namespace pcl
#if !defined(DOXYGEN_SHOULD_SKIP_THIS)
// clang-format off
POINT_CLOUD_REGISTER_POINT_STRUCT(
pcl::OusterPointType,
(float, x, x)
(float, y, y)
(float, z, z)
(int, time_offset_us, time_offset_us)
(uint16_t, reflectivity, reflectivity)
(uint16_t, signal, intensity)
(uint8_t, ring, ring))
// clang-format on
#endif // DOXYGEN_SHOULD_SKIP_THIS
namespace vi_map {
typedef LidarMeasurement<resources::PointCloud> MaplabLidarMeasurement;
typedef LidarMeasurement<pcl::PointCloud<pcl::PointXYZI>> PclLidarMeasurement;
typedef LidarMeasurement<pcl::PointCloud<pcl::OusterPointType>>
OusterLidarMeasurement;
typedef LidarMeasurement<sensor_msgs::PointCloud2> RosLidarMeasurement;
DEFINE_MEAUREMENT_CONTAINERS(MaplabLidarMeasurement);
DEFINE_MEAUREMENT_CONTAINERS(PclLidarMeasurement);
DEFINE_MEAUREMENT_CONTAINERS(RosLidarMeasurement);
DEFINE_MEAUREMENT_CONTAINERS(OusterLidarMeasurement);
} // namespace vi_map
DEFINE_MEASUREMENT_HASH(MaplabLidarMeasurement)
DEFINE_MEASUREMENT_HASH(PclLidarMeasurement)
DEFINE_MEASUREMENT_HASH(RosLidarMeasurement)
DEFINE_MEASUREMENT_HASH(OusterLidarMeasurement)
#endif // SENSORS_LIDAR_H_