CscSlamEngine.cpp File Reference

#include "Lidar/CscSlamEngine.h"
#include <mrpt/bayes/CParticleFilter.h>
#include <Simulator/CscEnvironmentSimulator.h>
#include <Axiomes/CscTimestamp.h>
#include <Lidar/CscLidarEngine.h>
#include <Util/geom.h>
#include <Util/json.h>
#include <fstream>
#include "Util/system.h"
#include <Configuration/CscKeyDirectories.h>
#include <pcl/io/pcd_io.h>
#include <mrpt/config/CConfigFileMemory.h>
#include <mrpt/img/CImage.h>
#include <mrpt/maps/COccupancyGridMap2D.h>
#include <mrpt/obs/CActionCollection.h>
#include <mrpt/obs/CActionRobotMovement2D.h>
#include <mrpt/io/CFileGZOutputStream.h>
#include <mrpt/io/CFileGZInputStream.h>
#include <mrpt/serialization/archiveFrom_std_vector.h>
#include <mrpt/obs/CObservation2DRangeScan.h>
#include <mrpt/poses/CPose2D.h>
#include <mrpt/slam/CMetricMapBuilderRBPF.h>
#include <mrpt/slam/CMonteCarloLocalization2D.h>
#include <opencv2/opencv.hpp>
#include <mrpt/maps/CSimplePointsMap.h>
#include <mrpt/slam/CICP.h>
#include <mrpt/poses/CPose3D.h>
#include <unordered_set>
#include <fast_gicp/gicp/fast_vgicp.hpp>

Classes
struct	conscience_core::lidar::LikelihoodMonitor
struct	conscience_core::lidar::directions::VoxelAccum

Namespaces
	conscience_core
	conscience_core::lidar
	conscience_core::lidar::directions

Macros
#define	USE_FAST_VGICP   1
#define	USE_FAST_GICP   0
#define	SAVE_SLAM_MAP_TO_PNG   0

Functions
OdometryData	conscience_core::lidar::composeLocalOdometryDelta (const OdometryData &a, const OdometryData &b)
ptr< CObservation2DRangeScan >	conscience_core::lidar::lidarValuesToMrptObservation (const std::map< double, double > &scanDegreesCentimeters, double maxDistanceMeters=8.0)
CActionCollection::Ptr	conscience_core::lidar::createOdometryActionFromDiff (const CPose2D &incr, bool isSimulatedEntity)
static std::string	conscience_core::lidar::PFAlgorithmToString (CParticleFilter::TParticleFilterAlgorithm algo)
void	conscience_core::lidar::resetSampling (size_t particlesCount, const ptr< COccupancyGridMap2D > &slamMap, CMonteCarloLocalization2D *slamLocalization, CscLogger *logger)
void	conscience_core::lidar::injectRandomPosesPartial (mrpt::slam::CMonteCarloLocalization2D &pf, mrpt::maps::COccupancyGridMap2D &grid, const mrpt::bayes::CParticleFilter::TParticleFilterOptions &pfOpts, double percent)
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)
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)
string	conscience_core::lidar::mrptPoseToJson (const TPose2D &pose)
string	conscience_core::lidar::mrptPointToJson (const TPoint2D &point)
CscPoint2dOriented	conscience_core::lidar::mrptPoseToCscPointOriented (const TPose2D &pose)
CscPoint2d	conscience_core::lidar::mrptPointToCscPoint (const TPoint2D &point)
static std::string	conscience_core::lidar::pose2dToString (const mrpt::poses::CPose2D &p)
pcl::PointCloud< pcl::PointXYZ >::Ptr	conscience_core::lidar::buildPclCloudFromCurrentFrameVoxelized (const std::vector< CscPoint3d > &cloudCm, float voxelSizeMeters, size_t maxPoints, const CscSlamEngine::Slam3dParams &slam3dParams)
pcl::PointCloud< pcl::PointXYZ >::Ptr	conscience_core::lidar::buildLocalPclCloudFromProbabilisticVoxels (const CscSlam3dMap &map, const CPose3D &centerPose, double radiusMeters, float voxelSizeMeters, size_t maxPoints)
double	conscience_core::lidar::directions::estimateFreeDistanceForLidarScan (double directionYawRads, const mrpt::obs::CObservation2DRangeScan &scan, double entityWidthMeters, double entityLengthMeters, double minClusterDistanceMeters, int minClusterSize, double maxDepthMeters)
optional< pair< int, double > >	conscience_core::lidar::directions::getBestDirectionForScan (CObservation2DRangeScan *scan, const vector< CscAvailableDirection > &authorizedDirections, double entityWidth, double entityLength, map< int, double > *computedDirections)
void	conscience_core::lidar::directions::saveScanWithBestDirectionAsPng (const mrpt::obs::CObservation2DRangeScan &scan, double bestYaw, const std::string &bestDirName, const fs::path &filename, double entityWidth, double entityLength)
double	conscience_core::lidar::directions::cmToMeters (double v)
int	conscience_core::lidar::directions::voxelIndex (double v, double voxelSizeMeters)
double	conscience_core::lidar::directions::estimateFreeDistanceFrontMetersForLidarScan3d (const LidarValuesSnapshot &scan, const CscSize3d &entitySizeCm, double voxelSizeMeters, int minVoxelPointCount, double maxDepthMeters, double minObstacleYMeters, double maxObstacleYMeters)

Variables
constexpr double	conscience_core::lidar::mrptGridSideMeters = 16
constexpr double	conscience_core::lidar::mrptGridResolution = 0.04
const string	conscience_core::lidar::CARTOGRAPHY_STORAGE_CARTO_PREFIX = "carto__"
const string	conscience_core::lidar::CARTOGRAPHY_STORAGE_DIRECTORY = string(CSC_DEFAULT_OUT_DIR) + "/CscSLAMCartographies"
static const float	conscience_core::lidar::SLAM_3D_MAP_VOXEL_SIZE_METERS = 0.015
static const float	conscience_core::lidar::SLAM_3D_ICP_VOXEL_SIZE_METERS = 0.05f
static CParticleFilter::TParticleFilterAlgorithm	conscience_core::lidar::PF_algorithm = CParticleFilter::pfAuxiliaryPFOptimal
const double	conscience_core::lidar::localizationOccupancyThreshold = 0.25
static LikelihoodMonitor	conscience_core::lidar::lkmon
long long	conscience_core::lidar::perturbationCounter = -1

Macro Definition Documentation

SAVE_SLAM_MAP_TO_PNG

#define SAVE_SLAM_MAP_TO_PNG   0

USE_FAST_GICP

#define USE_FAST_GICP   0

USE_FAST_VGICP

#define USE_FAST_VGICP   1