Global Namespace

Overview

// namespaces

namespace gtsam;
namespace math;
namespace mola;
    namespace mola::lidar_segmentation;
    namespace mola::rhodes;
namespace mp2p_icp;
namespace mp2p_icp_filters;
namespace mrpt;
    namespace mrpt::obs;
    namespace mrpt::serialization;
namespace poses;
namespace std;
namespace string_literals;

// typedefs

typedef mrpt::math::CMatrixDouble44 Matrix;
typedef std::vector<mrpt::math::TPoint3D> TPoints;
typedef std::vector<mp2p_icp::plane_patch_t> TPlanes;

// structs

struct HandlersContainer;
struct LoadedModules;
struct OLAE_LinearSystems;
struct errors;

// global variables

static const std::string ANNOTATION_NAME_PC_LAYERS = "lidar-pointcloud-layers";
static const char* yamlRotIntParams1 =     R"###(# Config for gtsam::RotationIntegrationParams gyroBias: [-1.0e-4, 2.0e-4, -3.0e-4] sensorLocationInVehicle:   quaternion: [0.0, 0.0, 0.0, 1.0]   translation: [0.0, 0.0, 0.0])###";
static std::string kitti_basedir;
static std::string result_path_file;
const float lengths[] = {100, 200, 300, 400, 500, 600, 700, 800};
const int32_t num_lengths = 8;
static const int GZ_COMPRESS_LEVEL = 0;
std::string MOLA_MAP_STORAGE_DIR =     mrpt::get_env<std::string>("MOLA_MAP_STORAGE_DIR", "/tmp");
static mola::MolaLauncherApp app;
static std::map<std::string, LoadedModules> loaded_lib_handled;
static mrpt::system::CTimeLogger timlog;
const std::string txt1 = R"###(# sample yaml a: 1.0 b: "foo" c:   - a   - b   - c d:   va: 'z')###";
constexpr const char* APP_NAME = "mp2p-icp-log-viewer";
static std::map<std::string, mrpt::obs::CRawlog::Ptr> rawlogsCache;
static mp2p_icp_filters::GeneratorSet generators;
static const uint8_t DIPI_SERIALIZATION_VERSION = 0;
static const uint8_t SERIALIZATION_VERSION = 0;
static const uint8_t SERIALIZATION_VERSION = 0;
auto& rnd = mrpt::random::getRandomGenerator();
static int NUM_REPS = mrpt::get_env<int>("NUM_REPS", 3);
static bool DO_SAVE_STAT_FILES =     mrpt::get_env<bool>("DO_SAVE_STAT_FILES", false);
static bool DO_PRINT_ALL = mrpt::get_env<bool>("DO_PRINT_ALL", false);
const std::string datasetDir = MP2P_DATASET_DIR;
static bool TEST_LARGE_ROTATIONS = nullptr != ::getenv("TEST_LARGE_ROTATIONS");
static bool DO_SAVE_STAT_FILES = nullptr != ::getenv("DO_SAVE_STAT_FILES");
static bool DO_PRINT_ALL = nullptr != ::getenv("DO_PRINT_ALL");
static bool SKIP_OUTLIERS = nullptr != ::getenv("SKIP_OUTLIERS");
static const size_t num_reps = mrpt::get_env<int>("NUM_REPS", 25);
const std::string datasetDir = MP2P_DATASET_DIR;

// global functions

MRPT_INITIALIZER(do_register_G2ODataset);
MRPT_INITIALIZER(do_register_LidarOdometry);

static void load_icp_set_of_params(
    LidarOdometry::Parameters::ICP_case& out,
    const mrpt::containers::yaml& cfg
    );

MRPT_INITIALIZER(do_register_WheelOdometry);
MRPT_INITIALIZER(do_register_Foo);
MRPT_INITIALIZER(do_register_mrpt_kernel);
MRPT_INITIALIZER(register_mola_lidar_segmentation);
MRPT_INITIALIZER(do_register_slam_gtsam);
MRPT_INITIALIZER(do_register_slam_rhodes);
MRPT_INITIALIZER(register_mp2p_icp);
MRPT_INITIALIZER(register_mola_lidar_segmentation);
static void test_rotation_integration();

int main(
    ] int argc,
    ] char** argv
    );

MRPT_INITIALIZER(do_register_EurocDataset);
MRPT_INITIALIZER(do_register_GenericSensor);
MRPT_INITIALIZER(do_register_KaistDataset);
static TCLAP::CmdLine cmd("mola-kitti-eval-error");

static TCLAP::ValueArg<std::string> arg_kitti_basedir(
    "k",
    "kitti-basedir",
    "Path to the kitti datasets. Overrides to the default,
    which is reading " "the env var `KITTI_BASE_DIR`.",
    false,
    "",
    "",
    cmd
    );

static TCLAP::ValueArg<std::string> arg_result_path(
    "r",
    "result-path",
    "The path file to evaluate",
    true,
    "result.txt",
    "result.txt",
    cmd
    );

static TCLAP::ValueArg<int> arg_seq(
    "s",
    "sequence",
    "The path file to evaluate",
    true,
    1,
    "01",
    cmd
    );

static bool eval();
static void do_kitti_eval_error();

int main(
    int argc,
    char** argv
    );

std::vector<Matrix> loadPoses_mrpt(std::string file_name);
std::vector<Matrix> loadPoses(string file_name);
vector<float> trajectoryDistances(vector<Matrix>& poses);

int32_t lastFrameFromSegmentLength(
    vector<float>& dist,
    int32_t first_frame,
    float len
    );

float rotationError(Matrix& pose_error);
float translationError(Matrix& pose_error);

vector<errors> calcSequenceErrors(
    vector<Matrix>& poses_gt,
    vector<Matrix>& poses_result
    );

void saveSequenceErrors(
    vector<errors>& err,
    string file_name
    );

void savePathPlot(
    vector<Matrix>& poses_gt,
    vector<Matrix>& poses_result,
    string file_name
    );

vector<int32_t> computeRoi(
    vector<Matrix>& poses_gt,
    vector<Matrix>& poses_result
    );

void plotPathPlot(
    string dir,
    vector<int32_t>& roi,
    int32_t idx
    );

void saveErrorPlots(
    vector<errors>& seq_err,
    string plot_error_dir,
    char* prefix
    );

void plotErrorPlots(
    string dir,
    char* prefix
    );

void saveStats(
    vector<errors> err,
    string dir
    );

MRPT_INITIALIZER(do_register_KittiOdometryDataset);

static void build_list_files(
    const std::string& dir,
    const std::string& file_extension,
    std::vector<std::string>& out_lst
    );

static void parse_calib_line(
    const std::string& line,
    Eigen::Matrix<double, 3, 4>& M
    );

MRPT_INITIALIZER(do_register_RawlogDataset);
MRPT_INITIALIZER(do_register_InputROS1);

IMPLEMENTS_VIRTUAL_SERIALIZABLE(
    EntityBase,
    mrpt::serialization::CSerializable,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    LandmarkPoint3,
    EntityBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    RefPose3,
    EntityBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    RelDynPose3KF,
    EntityBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    RelPose3,
    EntityBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    RelPose3KF,
    EntityBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    FactorDynamicsConstVel,
    FactorBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    FactorRelativePose3,
    FactorBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    FactorStereoProjectionPose,
    FactorBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    SmartFactorIMU,
    FactorBase,
    mola
    );

IMPLEMENTS_SERIALIZABLE(
    SmartFactorStereoProjectionPose,
    FactorBase,
    mola
    );

IMPLEMENTS_VIRTUAL_MRPT_OBJECT(
    FilterBase,
    mrpt::rtti::CObject,
    mp2p_icp_filters
    );

IMPLEMENTS_SERIALIZABLE(
    WorldModelData,
    mrpt::serialization::CSerializable,
    mola
    );

MRPT_TODO("win32: add SetConsoleCtrlHandler");
static TCLAP::CmdLine cmd("mola-cli");

static TCLAP::ValueArg<std::string> arg_yaml_cfg(
    "c",
    "config",
    "Input YAML config file(required)(*.yml)",
    false,
    "",
    "demo.yml",
    cmd
    );

static TCLAP::ValueArg<std::string> arg_verbosity_level(
    "v",
    "verbosity",
    "Verbosity level: ERROR|WARN|INFO|DEBUG(Default: INFO)",
    false,
    "",
    "INFO",
    cmd
    );

static TCLAP::SwitchArg arg_enable_profiler(
    "p",
    "profiler",
    "Enable time profiler by default in all modules(Default: NO)",
    cmd
    );

static TCLAP::SwitchArg arg_enable_profiler_whole(
    "",
    "profiler-whole",
    "Enable whole-history time profiler in all modules(Default: NO).**DO " "NOT** use in production,
    only to benchmark short runs(unbounded memory " "usage)",
    cmd
    );

static TCLAP::SwitchArg arg_rtti_list_all(
    "",
    "rtti-list-all",
    "Loads all MOLA modules,
    then list all classes registered via mrpt::rtti,
    " "and exits.",
    cmd
    );

static TCLAP::ValueArg<std::string> arg_rtti_list_children(
    "",
    "rtti-children-of",
    "Loads all MOLA modules,
    then list all known classes that inherit from the " "given one,
    and exits.",
    false,
    "",
    "mp2p_icp::ICP_Base",
    cmd
    );

static TCLAP::SwitchArg arg_list_modules(
    "",
    "list-modules",
    "Loads all MOLA modules,
    then list them. It also shows the list of paths " "in which the program looks for module dynamic libraries,
    then exits.",
    cmd
    );

static TCLAP::SwitchArg arg_list_module_shared_dirs(
    "",
    "list-module-shared-dirs",
    "Finds all MOLA module source/shared directories,
    then list them. Paths " "can be added with the environment variable MOLA_MODULES_SHARED_PATH.",
    cmd
    );

void mola_signal_handler(int s);
void mola_install_signal_handler();
static int mola_cli_launch_slam();
int mola_cli_rtti_list_all();
int mola_cli_rtti_list_child();
int mola_cli_list_modules();
int mola_cli_list_module_shared_dirs();

int main(
    int argc,
    char** argv
    );

int main(
    int argc,
    char** argv
    );

static TCLAP::CmdLine cmd("mola-yaml-parser");

static TCLAP::SwitchArg arg_no_includes(
    "",
    "no-includes",
    "Disables solving YAML `$include{}`s(Default: NO)",
    cmd
    );

static TCLAP::SwitchArg arg_no_cmd_runs(
    "",
    "no-cmd-runs",
    "Disables solving YAML `$(cmd)`s(Default: NO)",
    cmd
    );

static TCLAP::SwitchArg arg_no_env_vars(
    "",
    "no-env-vars",
    "Disables solving YAML `${xxx}`s(Default: NO)",
    cmd
    );

static TCLAP::UnlabeledValueArg<std::string> arg_input_files(
    "YAML_file",
    "Input YAML file(required)(*.yml)",
    true,
    "params.yml",
    "YAML files",
    cmd
    );

int main(
    int argc,
    char** argv
    );

MRPT_TODO("Improvement: automatic scanning libraries and extract their classes");
const std::map<std::string, LoadedModules>& get_loaded_modules();

void internal_load_lib_modules(
    mrpt::system::COutputLogger& app,
    const std::vector<std::string>& lib_search_paths
    );

static void internal_unload_lib_modules();

void internal_load_lib_modules(
    mrpt::system::COutputLogger& app,
    const std::vector<std::string>& lib_search_paths
    );

const std::map<std::string, LoadedModules>& get_loaded_modules();

static void from_env_var_to_list(
    const std::string& env_var_name,
    std::vector<std::string>& lst
    );

MRPT_TODO("Improve: allow changing FilterEdgesPlanes from cli flags");
static TCLAP::CmdLine cmd("test-mola-fe-lidar-segment-scan");

static TCLAP::ValueArg<std::string> arg_lidar_kitti_file(
    "k",
    "input-kitti-file",
    "Load 3D scan from a Kitti lidar(.bin) file",
    true,
    "",
    "./00000.bin",
    cmd
    );

static TCLAP::ValueArg<std::string> arg_lidar_pose(
    "",
    "lidar-pose",
    "Defines the 4x4 homogeneous matrix of the LiDAR sensor in the vehicle " "frame.\n" "Use Matlab format,
    quoted as a string. For example:\n" "For KAIST left_VLP :\n" " --lidar-pose \"\"\n" "For KAIST right_VLP:\n" " --lidar-pose \"\"\n" [-5.1406e-01 -7.0220e-01 -4.9259e-01 -4.4069e-01;" " 4.8648e-01 -7.1167e-01 5.0680e-01 3.9705e-01 ;" " -7.0644e-01 2.0893e-02 7.0745e-01 1.9095e+00 ;" " 0 0 0 1][-5.1215e-01 6.9924e-01 -4.9876e-01 -4.4988e-01;" " -4.9481e-01 -7.1485e-01 -4.9412e-01 -4.1671e-01;" " -7.0204e-01 -6.2641e-03 7.1210e-01 1.9129e+00;" " 0 0 0 1],
    false,
    "",
    "" [1 0 0 0;0 1 0 0;0 0 1 0;0 0 0 1],
    cmd
    );

static TCLAP::ValueArg<std::string> arg_params_file(
    "c",
    "config-file",
    "Load parameters from a YAML config file,
    containing a top-level YAML " "entry named `params` with all the parameters under it",
    true,
    "",
    "config.yml",
    cmd
    );

void do_scan_segment_test();

int main(
    int argc,
    char** argv
    );

IMPLEMENTS_MRPT_OBJECT(
    FilterEdgesPlanes,
    mp2p_icp_filters::FilterBase,
    mp2p_icp_filters
    );

MRPT_FILL_ENUM_MEMBER(
    mola::ASLAM_gtsam::StateVectorType,
    SE2
    );

MRPT_FILL_ENUM_MEMBER(
    mola::ASLAM_gtsam::StateVectorType,
    SE3
    );

MRPT_FILL_ENUM_MEMBER(
    mola::ASLAM_gtsam::StateVectorType,
    SE2Vel
    );

MRPT_FILL_ENUM_MEMBER(
    mola::ASLAM_gtsam::StateVectorType,
    SE3Vel
    );

MRPT_TODO(
    "Disabled temporarily to avoid segfault; also,
    it must be refactored"
    );

void test_gtsam_basic_graph();

int main(
    ] int argc,
    ] char** argv
    );

int main(
    ] int argc,
    ] char** argv
    );

static TCLAP::CmdLine cmd("mola-myapp");

static TCLAP::ValueArg<std::string> argParam1(
    "p",
    "param",
    "Explanation",
    false,
    "",
    "",
    cmd
    );

static void do_my_app();

int main(
    int argc,
    char** argv
    );

void gui_handler_images(
    const mrpt::rtti::CObject::Ptr& o,
    nanogui::Window* w,
    MolaViz::window_name_t parentWin,
    MolaViz* instance
    );

void gui_handler_point_cloud(
    const mrpt::rtti::CObject::Ptr& o,
    nanogui::Window* w,
    MolaViz::window_name_t parentWin,
    MolaViz* instance
    );

MRPT_INITIALIZER(do_register_MolaViz);

void gui_handler_show_common_sensor_info(
    const mrpt::obs::CObservation& obs,
    nanogui::Window* w
    );

static std::string::size_type findClosing(
    size_t pos,
    const std::string& s,
    const char searchEndChar,
    const char otherStartChar
    );

static std::tuple<std::string, std::string> splitVerticalBar(const std::string& s);
static std::string trimWSNL(const std::string& s);

static std::string parseEnvVars(
    const std::string& text,
    const mola::YAMLParseOptions& opts
    );

static std::string parseCmdRuns(
    const std::string& text,
    const mola::YAMLParseOptions& opts
    );

static void recursiveParseNodeForIncludes(
    yaml::node_t& n,
    const mola::YAMLParseOptions& opts
    );

static std::string parseIncludes(
    const std::string& text,
    const mola::YAMLParseOptions& opts
    );

static void test_yaml2string();
static void test_parseSimple();
static void test_parseIncludes();

int main(
    ] int argc,
    ] char** argv
    );

static TCLAP::CmdLine cmd("kitti2mm");

static TCLAP::ValueArg<std::string> argInput(
    "i",
    "input",
    "KITTI .bin pointcloud file.",
    true,
    "kitti-00.bin",
    "kitti-00.bin",
    cmd
    );

static TCLAP::ValueArg<std::string> argOutput(
    "o",
    "output",
    "Output file to write to.",
    true,
    "out.mm",
    "out.mm",
    cmd
    );

static TCLAP::ValueArg<std::string> argLayer(
    "l",
    "layer",
    "Target layer name(Default: \"raw\").",
    false,
    "raw",
    "raw",
    cmd
    );

static TCLAP::ValueArg<uint64_t> argID(
    "",
    "id",
    "Metric map numeric ID(Default: none).",
    false,
    0,
    "" [ID],
    cmd
    );

static TCLAP::ValueArg<std::string> argLabel(
    "",
    "label",
    "Metric map label string(Default: none).",
    false,
    "label",
    "" [label],
    cmd
    );

int main(
    int argc,
    char** argv
    );

static TCLAP::CmdLine cmd(APP_NAME);

static TCLAP::ValueArg<std::string> argExtension(
    "e",
    "file-extension",
    "Filename extension to look for. Default is `icplog`",
    false,
    "icplog",
    "icplog",
    cmd
    );

static TCLAP::ValueArg<std::string> argSearchDir(
    "d",
    "directory",
    "Directory in which to search for*.icplog files.",
    false,
    ".",
    ".",
    cmd
    );

static TCLAP::ValueArg<std::string> argVerbosity(
    "v",
    "verbose",
    "Verbosity level",
    false,
    "DEBUG",
    "DEBUG",
    cmd
    );

static TCLAP::ValueArg<double> argMinQuality(
    "",
    "min-quality",
    "Minimum quality(range [0,1]) for a log files to be loaded and shown in " "the list. Default = 0 so all log files are visible.", false, 0.0, "Quality[0,
    1]",
    cmd
    );

static void main_show_gui();

int main(
    int argc,
    char** argv
    );

static TCLAP::CmdLine cmd("mp2p-icp-run");

static TCLAP::ValueArg<std::string> argInputLocal(
    "",
    "input-local",
    "Local input point cloud/map." "It is interpreted as a rawlog entry if using the " "format `<RAWLOG_FILE.rawlog>:<N>` to select the N-th entry in the " "rawlog; otherwise,
    if the file extension is `.mm` it is loaded as a " "serialized metric_map_t object;if it is a `.icplog` file,
    the local map " "from that icp log is taken as input;in any other case,
    the file is " "assumed to be a 3D pointcloud stored as a Nx3 ASCII matrix file.",
    true,
    "pointcloud1.txt",
    "pointcloud1.txt",
    cmd
    );

static TCLAP::ValueArg<std::string> argInputGlobal(
    "",
    "input-global",
    "Global input point cloud/map. Same format than input-local. ",
    true,
    "pointcloud2.txt",
    "pointcloud2.txt",
    cmd
    );

static TCLAP::ValueArg<std::string> argYamlConfigFile(
    "c",
    "config",
    "YAML config file describing the ICP pipeline. See docs:\n" " https://docs.mola-slam.org/latest/" "module-mp2p-icp.html#yaml-pipeline-definition-files",
    true,
    "icp-config.yaml",
    "icp-config.yaml",
    cmd
    );

static TCLAP::ValueArg<std::string> argYamlConfigFileGenerators(
    "",
    "config-generators",
    "YAML config file describing the Generators. Can be also defined via an " "entry `generators` in the main `--config` yaml file. " "Can be used when processing a rawlog as input; if not present,
    a default " "Generator object will be used.",
    false,
    "generators-config.yaml",
    "generators-config.yaml",
    cmd
    );

static TCLAP::ValueArg<std::string> argYamlConfigFileFiltersLocal(
    "",
    "config-filters-local",
    "YAML config file describing a filtering pipeline for local map." "If not provided,
    and the main --config yaml file contains a " "`filters`  entry can be overriden with --entry-name-filters-local,
    it " "will be used instead.",
    false,
    "filters-config.yaml",
    "filters-config.yaml",
    cmd
    );

static TCLAP::ValueArg<std::string> argYamlConfigFileFiltersGlobal(
    "",
    "config-filters-global",
    "YAML config file describing a filtering pipeline for global map." "If not provided,
    and the main --config yaml file contains a" "`filters`  entry can be overriden with --entry-name-filters-global,
    it " "will be used instead.",
    false,
    "filters-config.yaml",
    "filters-config.yaml",
    cmd
    );

static TCLAP::ValueArg<std::string> argCfgNameFiltersGlobal(
    "",
    "entry-name-filters-global",
    "Overrides the map name in the YAML configuration file for global map " "filter.",
    false,
    "filters",
    "filters",
    cmd
    );

static TCLAP::ValueArg<std::string> argCfgNameFiltersLocal(
    "",
    "entry-name-filters-local",
    "Overrides the map name in the YAML configuration file for local map " "filter.",
    false,
    "filters",
    "filters",
    cmd
    );

static TCLAP::ValueArg<std::string> argInitialGuess(
    "",
    "guess",
    "SE(3) transformation of local wrt global,
    to use as initial guess for the " "ICP algorithm. " "Format:\"\"" [x y z yaw_deg pitch_deg roll_deg],
    false,
    "" [0 0 0 0 0 0],
    "" [0 0 0 0 0 0],
    cmd
    );

static TCLAP::SwitchArg argGenerateDebugFiles(
    "d",
    "generate-debug-log",
    "Enforces generation of the .icplog debug log files for posterior " "visualization with mp2p-icp-log- viewer,
    overriding the " "`generateDebugFiles` value in the configuration YAML file.",
    cmd
    );

static mrpt::obs::CRawlog::Ptr load_rawlog(const std::string& filename);

static mp2p_icp::metric_map_t::Ptr pc_from_rawlog(
    const mrpt::obs::CRawlog& r,
    const size_t index
    );

static mp2p_icp::metric_map_t::Ptr load_input_pc(
    const std::string& filename,
    bool local
    );

void runIcp();

int main(
    int argc,
    char** argv
    );

MRPT_FILL_ENUM(IterTermReason::Undefined);
MRPT_FILL_ENUM(IterTermReason::NoPairings);
MRPT_FILL_ENUM(IterTermReason::SolverError);
MRPT_FILL_ENUM(IterTermReason::MaxIterations);
MRPT_FILL_ENUM(IterTermReason::Stalled);

MRPT_FILL_ENUM_MEMBER(
    mp2p_icp::Coordinate,
    X
    );

MRPT_FILL_ENUM_MEMBER(
    mp2p_icp::Coordinate,
    Y
    );

MRPT_FILL_ENUM_MEMBER(
    mp2p_icp::Coordinate,
    Z
    );

static void filterListByMaxDist(
    std::vector<size_t>& kddIdxs,
    std::vector<float>& kddSqrDist,
    const float maxDistForCorrespondenceSquared
    );

IMPLEMENTS_MRPT_OBJECT(
    metric_map_t,
    mrpt::serialization::CSerializable,
    mp2p_icp
    );

static bool se3_l2_internal(
    const mp2p_icp::Pairings& in,
    const WeightParameters& wp,
    const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global,
    mrpt::math::CQuaternionDouble& out_attitude,
    OutlierIndices& in_out_outliers
    );

static mrpt::poses::CPose3D gibbs2pose(const Eigen::Vector3d& v);

static OLAE_LinearSystems olae_build_linear_system(
    const Pairings& in,
    const WeightParameters& wp,
    const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global,
    OutlierIndices& in_out_outliers
    );

template <typename T>
static void push_back_copy(
    const T& o,
    T& me
    );

template <typename T>
static void push_back_move(
    T&& o,
    T& me
    );

template <typename CONTAINER>
void append_container_size(
    const CONTAINER& c,
    const std::string& name,
    std::string& ret
    );

static void append_from_sorted(
    const std::multimap<double, mrpt::tfest::TMatchingPair>& sorted,
    Pairings& out,
    const double ratio
    );

IMPLEMENTS_MRPT_OBJECT(
    QualityEvaluator_RangeImageSimilarity,
    QualityEvaluator,
    mp2p_icp
    );

static void projectPoint(
    const mrpt::math::TPoint3D& P,
    const mrpt::img::TCamera& params,
    double& pixel_x,
    double& pixel_y
    );

static double phi(double x);
static double errorForMismatch(const double x);

static double errorForMismatch(
    const double DeltaRange,
    const double sigma
    );

static double loss(
    double x,
    double y
    );

IMPLEMENTS_MRPT_OBJECT(
    WeightParameters,
    mrpt::serialization::CSerializable,
    mp2p_icp
    );

IMPLEMENTS_MRPT_OBJECT(
    FilterBoundingBox,
    mp2p_icp_filters::FilterBase,
    mp2p_icp_filters
    );

IMPLEMENTS_MRPT_OBJECT(
    FilterDecimateVoxels,
    mp2p_icp_filters::FilterBase,
    mp2p_icp_filters
    );

IMPLEMENTS_MRPT_OBJECT(
    FilterDeleteLayer,
    mp2p_icp_filters::FilterBase,
    mp2p_icp_filters
    );

static double normald(const double sigma);
static float normalf(const float sigma);
static void test_Jacob_error_point2point();
static void test_Jacob_error_point2line();
static void test_Jacob_error_point2plane();
static void test_Jacob_error_line2line();
static void test_Jacob_error_plane2plane();
static void test_error_line2line();
static void test_against_ground_truth_error_point2line();

int main(
    ] int argc,
    ] char** argv
    );

static void test_icp(
    const std::string& inFile,
    const std::string& icpClassName,
    const std::string& solverName,
    const std::string& matcherName,
    int pointDecimation
    );

int main(
    ] int argc,
    ] char** argv
    );

static mrpt::maps::CSimplePointsMap::Ptr generateGlobalPoints();
static mrpt::maps::CSimplePointsMap::Ptr generateLocalPoints();

int main(
    ] int argc,
    ] char** argv
    );

static mrpt::maps::CSimplePointsMap::Ptr generateGlobalPoints();
static mrpt::maps::CSimplePointsMap::Ptr generateLocalPoints();

int main(
    ] int argc,
    ] char** argv
    );

TPoints generate_points(const size_t nPts);
TPlanes generate_planes(const size_t nPlanes);

std::tuple<mrpt::poses::CPose3D, std::vector<std::size_t>> transform_points_planes(
    const TPoints& pA,
    TPoints& pB,
    mrpt::tfest::TMatchingPairList& pointsPairs,
    const TPlanes& plA,
    TPlanes& plB,
    std::vector<mp2p_icp::matched_plane_t>& planePairs,
    mp2p_icp::MatchedPointPlaneList& pt2plPairs,
    const double xyz_noise_std,
    const double n_err_std,
    const double outliers_ratio
    );

bool test_icp_algos(
    const size_t numPts,
    const size_t numLines,
    const size_t numPlanes,
    const double xyz_noise_std = .0,
    const double n_err_std = .0,
    bool use_robust = false,
    const double outliers_ratio = .0
    );

int main(
    ] int argc,
    ] char** argv
    );

static void test_opt_pt2ln(const mrpt::poses::CPose3D& groundTruth);

int main(
    ] int argc,
    ] char** argv
    );

static void test_opt_pt2pl(
    const mrpt::poses::CPose3D& groundTruth,
    const mp2p_icp::Solver& solver
    );

static void test_mp2p_optimize_pt2pl();

int main(
    ] int argc,
    ] char** argv
    );

int main(
    ] int argc,
    ] char** argv
    );

// macros

#define DLL_EXT

#define ENSURE_YAML_ENTRY_EXISTS( \
    _c, \
    _name \
    )

#define MOLA_ARG_N( \
    _1, \
    _2, \
    _3, \
    _4, \
    _5, \
    _6, \
    _7, \
    _8, \
    _9, \
    _10, \
    N, \
    ... \
    )

#define MOLA_NARG(...)
#define MOLA_NARG_I_(...)
#define MOLA_REGISTER_MODULE(_classname)
#define MOLA_RSEQ_N()

#define MOLA_VFUNC( \
    name, \
    n \
    )

#define MOLA_VFUNC_( \
    name, \
    n \
    )

#define VFUNC( \
    func, \
    ... \
    )

#define YAML_LOAD_MEMBER_OPT( \
    _varname, \
    _type \
    )

#define YAML_LOAD_MEMBER_REQ( \
    _varname, \
    _type \
    )

#define YAML_LOAD_OPT(...)

#define YAML_LOAD_OPT2( \
    _varname, \
    _type \
    )

#define YAML_LOAD_OPT3( \
    _param_str, \
    _varname, \
    _type \
    )

#define YAML_LOAD_OPT_DEG(...)

#define YAML_LOAD_OPT_DEG3( \
    _param_str, \
    _varname, \
    _type \
    )

#define YAML_LOAD_REQ(...)

#define YAML_LOAD_REQ2( \
    _varname, \
    _type \
    )

#define YAML_LOAD_REQ3( \
    _param_str, \
    _varname, \
    _type \
    )

#define YAML_LOAD_REQ_DEG(...)

#define YAML_LOAD_REQ_DEG3( \
    _param_str, \
    _varname, \
    _type \
    )

Detailed Documentation

Global Variables

static std::map<std::string, LoadedModules> loaded_lib_handled

From internal_load_lib_modules()

Global Functions

const std::map<std::string, LoadedModules>& get_loaded_modules()

Returns the current list of loaded module dyanmic libraries.

void internal_load_lib_modules(
    mrpt::system::COutputLogger& app,
    const std::vector<std::string>& lib_search_paths
    )

Loads all libs under lib_search_paths_.

See also:

setup()

void internal_load_lib_modules(
    mrpt::system::COutputLogger& app,
    const std::vector<std::string>& lib_search_paths
    )

Loads all libs under lib_search_paths_.

See also:

setup()

const std::map<std::string, LoadedModules>& get_loaded_modules()

Returns the current list of loaded module dyanmic libraries.

static OLAE_LinearSystems olae_build_linear_system(
    const Pairings& in,
    const WeightParameters& wp,
    const mrpt::math::TPoint3D& ct_local,
    const mrpt::math::TPoint3D& ct_global,
    OutlierIndices& in_out_outliers
    )

Core of the OLAE algorithm