Conscience Core
Namespaces | Classes | Typedefs | Enumerations | Functions | Variables
conscience_core::lidar Namespace Reference

Namespaces

 directions
 

Classes

struct  CartographyChangesQueue
 
struct  CartographyConfig
 
class  Csc3dCartographyProcessor
 
class  CscEntityPositionManager
 
class  CscLidar2DEngineSimulated
 
class  CscLidar3DEngineSimulated
 
class  CscLidarEngine
 
class  CscLidarEngineRplidar
 
class  CscPlaceCartography
 
class  CscPointCloud3d
 
class  CscPointCloudMatcher
 
class  CscRealEntity3dLidarValues
 
class  CscSlam3dMap
 
struct  CscSlam3dMapVoxelChanges
 
struct  CscSlam3dMapVoxelChangesListener
 
struct  CscSlam3dMapVoxelDiff
 
struct  CscSlam3dMapVoxelGrid
 
struct  CscSlam3dMapVoxelKey
 
struct  CscSlam3dMapVoxelKeyHash
 
struct  CscSlam3dProbabilisticVoxel
 
class  CscSlam3dProbabilisticVoxelGrid
 
class  CscSlamEngine
 
struct  FrameMetadata
 
struct  LidarExtrinsics
 
struct  LidarExtrinsicsSidecar
 
struct  LidarImuData
 
class  LidarValues
 
class  LidarValuesSnapshot
 
struct  LikelihoodMonitor
 
struct  OdoDelta
 
struct  OdometryData
 
struct  OdometryDeltaSidecar
 
struct  Pose2D
 
struct  VoxelHash
 

Typedefs

typedef pcl::PointXYZI PointT
 
typedef pcl::PointCloud< PointTPointCloudT
 
typedef uint64_t CscLidarMode
 
typedef PointMatcher< float > PM
 
typedef PM::DataPoints DP
 
typedef PointMatcherIO< float > PMIO
 
typedef Eigen::MatrixXf Matrix
 

Enumerations

enum  CscEntityPositionSource { Entity = 0, LidarLocalization = 1, Simulator = 2, CoreInstruction = 3 }
 
enum  CscLidarModeList : CscLidarMode { cscLidarModePaused = 1ULL << 0, cscLocalisation = 1ULL << 2, cscSlam = 1ULL << 4 }
 
enum  CscSlamCellOccupancyStatus : uint8_t { CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Unknown, CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Free, CscSlamCellOccupancyStatus::SlamCellOccupancyStatus_Occupied }
 

Functions

static bool isStaticOdometryDelta (const FrameMetadata &meta, float maxTranslationCm, float maxYawDeg)
 
template<typename T >
static T clampValue (T v, T lo, T hi)
 
static Eigen::Matrix4f makeYawOffsetTransform (float yawRad)
 
Eigen::Matrix4f makeRobotFromLidarTransform (const LidarExtrinsicsSidecar &e)
 
Eigen::Matrix4f makeRobotDeltaTransform (const OdometryDeltaSidecar &odo)
 
Eigen::Matrix4f makeLidarDeltaFromMetadata (const FrameMetadata &meta)
 
static bool readFile (const fs::path &path, std::string &out)
 
static double extractDouble (const std::string &text, const std::string &key, double fallback=0.0)
 
static int extractInt (const std::string &text, const std::string &key, int fallback=0)
 
static std::vector< OdoDeltaloadOdometryJsons (const fs::path &folder)
 
static std::vector< Pose2DintegrateOdometry (const std::vector< OdoDelta > &deltas, bool invertYaw, bool invertDelta)
 
static float extractYawY (const Eigen::Matrix3f &R)
 
static Eigen::Matrix4f projectToPlanarMotion (const Eigen::Matrix4f &T)
 
void toSimulatorPoint (CscPoint3dOriented *point)
 
void convertOdometryDeltaToRobotReferenceFrame (double &deltaPositionX, double &deltaPositionY, double &deltaPositionZ, double previousYaw)
 
CscQuaternion extractNavigationQuaternionFromEntityQuaternion (const CscQuaternion &entityQuaternion)
 
double extractNavigationYawFromEntityQuaternion (const CscQuaternion &entityQuaternion)
 
vector< CscPoint3dtransformPointsLidarLocalToWorld (const vector< CscPoint3d > &pointCloud, const CscPoint3d &entityPosition, const CscQuaternion &entityRotationQuaternion)
 
CscPoint3d transformPointLidarLocalToWorld (const CscPoint3d &localPoint, const CscPoint3d &entityPosition, const CscQuaternion &entityRotationQuaternion)
 
CscPoint3d transformPointWorldToLidarLocal (const CscPoint3d &worldPoint, const CscPoint3d &entityPosition, const CscQuaternion &entityRotationQuaternion)
 
void * updateLidarValuesThreadActionLoop (void *argument)
 
bool checkRPLIDARHealth (RPlidarDriver *drv)
 
DP cscPointsToPMDataPoints (const vector< const CscPoint3d * > &points, bool XZTo2DOnly)
 
void unpack_row_col_7_11_ptr (const uint8_t *&ptr, uint16_t &row, uint16_t &col)
 
uint16_t read_u16_le_ptr (const uint8_t *&ptr)
 
OdometryData composeLocalOdometryDelta (const OdometryData &a, const OdometryData &b)
 
ptr< CObservation2DRangeScan > lidarValuesToMrptObservation (const std::map< double, double > &scanDegreesCentimeters, double maxDistanceMeters=8.0)
 
CActionCollection::Ptr createOdometryActionFromDiff (const CPose2D &incr, bool isSimulatedEntity)
 
static std::string PFAlgorithmToString (CParticleFilter::TParticleFilterAlgorithm algo)
 
void resetSampling (size_t particlesCount, const ptr< COccupancyGridMap2D > &slamMap, CMonteCarloLocalization2D *slamLocalization, CscLogger *logger)
 
void injectRandomPosesPartial (mrpt::slam::CMonteCarloLocalization2D &pf, mrpt::maps::COccupancyGridMap2D &grid, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &pfOpts, double percent)
 
double scanMapConsistency_LL_per_ray (const std::shared_ptr< mrpt::maps::COccupancyGridMap2D > &grid, const std::shared_ptr< mrpt::obs::CObservation2DRangeScan > &scan, const mrpt::poses::CPose2D &entityPose)
 
bool hasNearbySupportedStructureDT (const std::vector< std::pair< int, int >> &clusterCells, const std::vector< uint8_t > &support, int W, int H, int maxShiftCells)
 
string mrptPoseToJson (const TPose2D &pose)
 
string mrptPointToJson (const TPoint2D &point)
 
CscPoint2dOriented mrptPoseToCscPointOriented (const TPose2D &pose)
 
CscPoint2d mrptPointToCscPoint (const TPoint2D &point)
 
static std::string pose2dToString (const mrpt::poses::CPose2D &p)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr buildPclCloudFromCurrentFrameVoxelized (const std::vector< CscPoint3d > &cloudCm, float voxelSizeMeters, size_t maxPoints, const CscSlamEngine::Slam3dParams &slam3dParams)
 
pcl::PointCloud< pcl::PointXYZ >::Ptr buildLocalPclCloudFromProbabilisticVoxels (const CscSlam3dMap &map, const CPose3D &centerPose, double radiusMeters, float voxelSizeMeters, size_t maxPoints)
 
 ENUM (CscSlam3dMapVoxelDiffType, SLAM3D_VOXEL_DIFF_TYPE_ADD, SLAM3D_VOXEL_DIFF_TYPE_REMOVE)
 
 ENUM (CscSlam3dMapVoxelChangesMode, SLAM3D_VOXEL_CHANGES_MODE_DIFFS, SLAM3D_VOXEL_CHANGES_MODE_FULL_REBUILD)
 
double distancePoints (const TPoint2D &pointA, const TPoint2D &pointB)
 
double distancePoints (const TPose2D &poseA, const TPose2D &poseB)
 
CscPoint2d toMrptReferenceFrame (const CscPoint3d &point)
 
CscPoint3d toMrptReferenceFrame3d (double x, double y, double z)
 
CscPoint3d fromMrptReferenceFrame3d (double mx, double my, double mz)
 

Variables

static std::unique_ptr< CscLoggerlogger = CscLogger::getForCategory("CscLidarEngine")
 
static const string cartographyPointsFilename = "allPoints.csv"
 
const string PLACE_TAG_FROM_CARTOGRAPHY = "FromCartography"
 
constexpr double mrptGridSideMeters = 16
 
constexpr double mrptGridResolution = 0.04
 
const string CARTOGRAPHY_STORAGE_CARTO_PREFIX = "carto__"
 
const string CARTOGRAPHY_STORAGE_DIRECTORY = string(CSC_DEFAULT_OUT_DIR) + "/CscSLAMCartographies"
 
static const float SLAM_3D_MAP_VOXEL_SIZE_METERS = 0.015
 
static const float SLAM_3D_ICP_VOXEL_SIZE_METERS = 0.05f
 
static CParticleFilter::TParticleFilterAlgorithm PF_algorithm = CParticleFilter::pfAuxiliaryPFOptimal
 
const double localizationOccupancyThreshold = 0.25
 
static LikelihoodMonitor lkmon
 
long long perturbationCounter = -1
 

Typedef Documentation

◆ CscLidarMode

◆ DP

typedef PM::DataPoints conscience_core::lidar::DP

◆ Matrix

typedef Eigen::MatrixXf conscience_core::lidar::Matrix

◆ PM

typedef PointMatcher<float> conscience_core::lidar::PM

◆ PMIO

typedef PointMatcherIO<float> conscience_core::lidar::PMIO

◆ PointCloudT

typedef pcl::PointCloud<PointT> conscience_core::lidar::PointCloudT

◆ PointT

typedef pcl::PointXYZI conscience_core::lidar::PointT

Enumeration Type Documentation

◆ CscEntityPositionSource

Enumerator
Entity 

position was sent by entity and its localisation system

LidarLocalization 

position was evaluated by lidar localisation

Simulator 

position is coming from core's environment simulator

CoreInstruction 

a process requested this position to be applied

◆ CscLidarModeList

Enumerator
cscLidarModePaused 
cscLocalisation 
cscSlam 

new lidar mode which allows concurrent cartography and localisation

◆ CscSlamCellOccupancyStatus

Enumerator
SlamCellOccupancyStatus_Unknown 
SlamCellOccupancyStatus_Free 
SlamCellOccupancyStatus_Occupied 

Function Documentation

◆ buildLocalPclCloudFromProbabilisticVoxels()

pcl::PointCloud<pcl::PointXYZ>::Ptr conscience_core::lidar::buildLocalPclCloudFromProbabilisticVoxels ( const CscSlam3dMap map,
const CPose3D &  centerPose,
double  radiusMeters,
float  voxelSizeMeters,
size_t  maxPoints 
)

◆ buildPclCloudFromCurrentFrameVoxelized()

pcl::PointCloud<pcl::PointXYZ>::Ptr conscience_core::lidar::buildPclCloudFromCurrentFrameVoxelized ( const std::vector< CscPoint3d > &  cloudCm,
float  voxelSizeMeters,
size_t  maxPoints,
const CscSlamEngine::Slam3dParams slam3dParams 
)

◆ checkRPLIDARHealth()

bool conscience_core::lidar::checkRPLIDARHealth ( RPlidarDriver *  drv)

◆ clampValue()

template<typename T >
static T conscience_core::lidar::clampValue ( v,
lo,
hi 
)
static

◆ composeLocalOdometryDelta()

OdometryData conscience_core::lidar::composeLocalOdometryDelta ( const OdometryData a,
const OdometryData b 
)

◆ convertOdometryDeltaToRobotReferenceFrame()

void conscience_core::lidar::convertOdometryDeltaToRobotReferenceFrame ( double &  deltaPositionX,
double &  deltaPositionY,
double &  deltaPositionZ,
double  previousYaw 
)
inline

parameters in Conscience ref frame

Parameters
previousYawstandard Conscience yaw -> positive to the right if looking from above

◆ createOdometryActionFromDiff()

CActionCollection::Ptr conscience_core::lidar::createOdometryActionFromDiff ( const CPose2D &  incr,
bool  isSimulatedEntity 
)

◆ cscPointsToPMDataPoints()

DP conscience_core::lidar::cscPointsToPMDataPoints ( const vector< const CscPoint3d * > &  points,
bool  XZTo2DOnly 
)

◆ distancePoints() [1/2]

double conscience_core::lidar::distancePoints ( const TPoint2D &  pointA,
const TPoint2D &  pointB 
)
inline

◆ distancePoints() [2/2]

double conscience_core::lidar::distancePoints ( const TPose2D &  poseA,
const TPose2D &  poseB 
)
inline

◆ ENUM() [1/2]

conscience_core::lidar::ENUM ( CscSlam3dMapVoxelChangesMode  ,
SLAM3D_VOXEL_CHANGES_MODE_DIFFS  ,
SLAM3D_VOXEL_CHANGES_MODE_FULL_REBUILD   
)

◆ ENUM() [2/2]

conscience_core::lidar::ENUM ( CscSlam3dMapVoxelDiffType  ,
SLAM3D_VOXEL_DIFF_TYPE_ADD  ,
SLAM3D_VOXEL_DIFF_TYPE_REMOVE   
)

◆ extractDouble()

static double conscience_core::lidar::extractDouble ( const std::string &  text,
const std::string &  key,
double  fallback = 0.0 
)
static

◆ extractInt()

static int conscience_core::lidar::extractInt ( const std::string &  text,
const std::string &  key,
int  fallback = 0 
)
static

◆ extractNavigationQuaternionFromEntityQuaternion()

CscQuaternion conscience_core::lidar::extractNavigationQuaternionFromEntityQuaternion ( const CscQuaternion entityQuaternion)
inline

use this to retrieve transformation quaternion from entity current quaternion, it extracts actual entity rotation when "still" and looking forward (removing base rotation which is not relevant when rotating sensor with no rotation on the robot -> base rotation is not part of the actual rotation in sensors rotations)

Parameters
entityQuaternionentity current rotation quaternion, as returned by CscEnvironmentSimulator::getEntityPositionAndRotationQuaternion(entitySN)
Returns
rotation quaternion of entity without base rotation (pi/2 on X axis)
See also
CscEnvironmentSimulator::getEntityPositionAndRotationQuaternion(string)

◆ extractNavigationYawFromEntityQuaternion()

double conscience_core::lidar::extractNavigationYawFromEntityQuaternion ( const CscQuaternion entityQuaternion)
inline

use this instead of buggy CscPoint3dOriented::toStandardEulerY(), it extracts actual entity rotation when "still" and looking forward

Parameters
entityQuaternionentity current rotation quaternion, as returned by CscEnvironmentSimulator::getEntityPositionAndRotationQuaternion(entitySN)
Returns
yaw euler (hence on axis Y) from entity quaternion - following Conscience standard
Warning
yaw following Conscience standard -> from above -> yaw positive = robot turning to the right
See also
CscEnvironmentSimulator::getEntityPositionAndRotationQuaternion(string)

◆ extractYawY()

static float conscience_core::lidar::extractYawY ( const Eigen::Matrix3f &  R)
static

◆ fromMrptReferenceFrame3d()

CscPoint3d conscience_core::lidar::fromMrptReferenceFrame3d ( double  mx,
double  my,
double  mz 
)
inline

◆ hasNearbySupportedStructureDT()

bool conscience_core::lidar::hasNearbySupportedStructureDT ( const std::vector< std::pair< int, int >> &  clusterCells,
const std::vector< uint8_t > &  support,
int  W,
int  H,
int  maxShiftCells 
)

◆ injectRandomPosesPartial()

void conscience_core::lidar::injectRandomPosesPartial ( mrpt::slam::CMonteCarloLocalization2D &  pf,
mrpt::maps::COccupancyGridMap2D &  grid,
const mrpt::bayes::CParticleFilter::TParticleFilterOptions &  pfOpts,
double  percent 
)

◆ integrateOdometry()

static std::vector<Pose2D> conscience_core::lidar::integrateOdometry ( const std::vector< OdoDelta > &  deltas,
bool  invertYaw,
bool  invertDelta 
)
static

◆ isStaticOdometryDelta()

static bool conscience_core::lidar::isStaticOdometryDelta ( const FrameMetadata meta,
float  maxTranslationCm,
float  maxYawDeg 
)
static

◆ lidarValuesToMrptObservation()

ptr<CObservation2DRangeScan> conscience_core::lidar::lidarValuesToMrptObservation ( const std::map< double, double > &  scanDegreesCentimeters,
double  maxDistanceMeters = 8.0 
)

◆ loadOdometryJsons()

static std::vector<OdoDelta> conscience_core::lidar::loadOdometryJsons ( const fs::path &  folder)
static

◆ makeLidarDeltaFromMetadata()

Eigen::Matrix4f conscience_core::lidar::makeLidarDeltaFromMetadata ( const FrameMetadata meta)

◆ makeRobotDeltaTransform()

Eigen::Matrix4f conscience_core::lidar::makeRobotDeltaTransform ( const OdometryDeltaSidecar odo)

◆ makeRobotFromLidarTransform()

Eigen::Matrix4f conscience_core::lidar::makeRobotFromLidarTransform ( const LidarExtrinsicsSidecar e)

◆ makeYawOffsetTransform()

static Eigen::Matrix4f conscience_core::lidar::makeYawOffsetTransform ( float  yawRad)
static

◆ mrptPointToCscPoint()

CscPoint2d conscience_core::lidar::mrptPointToCscPoint ( const TPoint2D &  point)

◆ mrptPointToJson()

string conscience_core::lidar::mrptPointToJson ( const TPoint2D &  point)

◆ mrptPoseToCscPointOriented()

CscPoint2dOriented conscience_core::lidar::mrptPoseToCscPointOriented ( const TPose2D &  pose)

◆ mrptPoseToJson()

string conscience_core::lidar::mrptPoseToJson ( const TPose2D &  pose)

◆ PFAlgorithmToString()

static std::string conscience_core::lidar::PFAlgorithmToString ( CParticleFilter::TParticleFilterAlgorithm  algo)
static

◆ pose2dToString()

static std::string conscience_core::lidar::pose2dToString ( const mrpt::poses::CPose2D &  p)
static

◆ projectToPlanarMotion()

static Eigen::Matrix4f conscience_core::lidar::projectToPlanarMotion ( const Eigen::Matrix4f &  T)
static

◆ read_u16_le_ptr()

uint16_t conscience_core::lidar::read_u16_le_ptr ( const uint8_t *&  ptr)
inline

◆ readFile()

static bool conscience_core::lidar::readFile ( const fs::path &  path,
std::string &  out 
)
static

◆ resetSampling()

void conscience_core::lidar::resetSampling ( size_t  particlesCount,
const ptr< COccupancyGridMap2D > &  slamMap,
CMonteCarloLocalization2D *  slamLocalization,
CscLogger logger 
)

◆ scanMapConsistency_LL_per_ray()

double conscience_core::lidar::scanMapConsistency_LL_per_ray ( const std::shared_ptr< mrpt::maps::COccupancyGridMap2D > &  grid,
const std::shared_ptr< mrpt::obs::CObservation2DRangeScan > &  scan,
const mrpt::poses::CPose2D &  entityPose 
)

◆ toMrptReferenceFrame()

CscPoint2d conscience_core::lidar::toMrptReferenceFrame ( const CscPoint3d point)
inline

◆ toMrptReferenceFrame3d()

CscPoint3d conscience_core::lidar::toMrptReferenceFrame3d ( double  x,
double  y,
double  z 
)
inline

◆ toSimulatorPoint()

void conscience_core::lidar::toSimulatorPoint ( CscPoint3dOriented point)

This method is used to convert the euler with a standard euler format to the one used in the simulator

◆ transformPointLidarLocalToWorld()

CscPoint3d conscience_core::lidar::transformPointLidarLocalToWorld ( const CscPoint3d localPoint,
const CscPoint3d entityPosition,
const CscQuaternion entityRotationQuaternion 
)
inline

◆ transformPointsLidarLocalToWorld()

vector<CscPoint3d> conscience_core::lidar::transformPointsLidarLocalToWorld ( const vector< CscPoint3d > &  pointCloud,
const CscPoint3d entityPosition,
const CscQuaternion entityRotationQuaternion 
)
inline

◆ transformPointWorldToLidarLocal()

CscPoint3d conscience_core::lidar::transformPointWorldToLidarLocal ( const CscPoint3d worldPoint,
const CscPoint3d entityPosition,
const CscQuaternion entityRotationQuaternion 
)
inline

◆ unpack_row_col_7_11_ptr()

void conscience_core::lidar::unpack_row_col_7_11_ptr ( const uint8_t *&  ptr,
uint16_t &  row,
uint16_t &  col 
)
inline

◆ updateLidarValuesThreadActionLoop()

void* conscience_core::lidar::updateLidarValuesThreadActionLoop ( void *  argument)

Variable Documentation

◆ CARTOGRAPHY_STORAGE_CARTO_PREFIX

const string conscience_core::lidar::CARTOGRAPHY_STORAGE_CARTO_PREFIX = "carto__"

◆ CARTOGRAPHY_STORAGE_DIRECTORY

const string conscience_core::lidar::CARTOGRAPHY_STORAGE_DIRECTORY = string(CSC_DEFAULT_OUT_DIR) + "/CscSLAMCartographies"

◆ cartographyPointsFilename

const string conscience_core::lidar::cartographyPointsFilename = "allPoints.csv"
static

◆ lkmon

LikelihoodMonitor conscience_core::lidar::lkmon
static

◆ localizationOccupancyThreshold

const double conscience_core::lidar::localizationOccupancyThreshold = 0.25

◆ logger

std::unique_ptr<CscLogger> conscience_core::lidar::logger = CscLogger::getForCategory("CscLidarEngine")
static

◆ mrptGridResolution

constexpr double conscience_core::lidar::mrptGridResolution = 0.04
constexpr

◆ mrptGridSideMeters

constexpr double conscience_core::lidar::mrptGridSideMeters = 16
constexpr

◆ perturbationCounter

long long conscience_core::lidar::perturbationCounter = -1

◆ PF_algorithm

CParticleFilter::TParticleFilterAlgorithm conscience_core::lidar::PF_algorithm = CParticleFilter::pfAuxiliaryPFOptimal
static

◆ PLACE_TAG_FROM_CARTOGRAPHY

const string conscience_core::lidar::PLACE_TAG_FROM_CARTOGRAPHY = "FromCartography"

◆ SLAM_3D_ICP_VOXEL_SIZE_METERS

const float conscience_core::lidar::SLAM_3D_ICP_VOXEL_SIZE_METERS = 0.05f
static

◆ SLAM_3D_MAP_VOXEL_SIZE_METERS

const float conscience_core::lidar::SLAM_3D_MAP_VOXEL_SIZE_METERS = 0.015
static