CRangeScanOps_impl.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */

#pragma once
#include <mrpt/maps/CSimplePointsMap.h>

namespace mrpt::graphslam::deciders
{
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::getICPEdge(
    const mrpt::obs::CObservation2DRangeScan& from,
    const mrpt::obs::CObservation2DRangeScan& to, constraint_t* rel_edge,
    const mrpt::poses::CPose2D* initial_pose_in,
    mrpt::slam::CICP::TReturnInfo* icp_info)
{
    MRPT_START;

    mrpt::maps::CSimplePointsMap m1, m2;
    float running_time;
    mrpt::slam::CICP::TReturnInfo info;

    // have them initialized prior - and then just clear them
    m1.insertObservation(from);
    m2.insertObservation(to);

    // If given, use initial_pose_in as a first guess for the ICP
    mrpt::poses::CPose2D initial_pose;
    if (initial_pose_in)
    {
        initial_pose = *initial_pose_in;
    }

    mrpt::poses::CPosePDF::Ptr pdf =
        params.icp.Align(&m1, &m2, initial_pose, &running_time, (void*)&info);

    // return the edge regardless of the goodness of the alignment
    rel_edge->copyFrom(*pdf);

    // if given, fill the TReturnInfo Struct
    if (icp_info) *icp_info = info;

    MRPT_END;
}
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::getICPEdge(
    const mrpt::obs::CObservation3DRangeScan& from,
    const mrpt::obs::CObservation3DRangeScan& to, constraint_t* rel_edge,
    const mrpt::poses::CPose2D* initial_pose_in,
    mrpt::slam::CICP::TReturnInfo* icp_info)
{
    MRPT_START;

    ASSERTDEBMSG_(
        from.hasRangeImage,
        mrpt::format("Laser scan doesn't contain valid range image"));
    ASSERTDEBMSG_(
        to.hasRangeImage,
        mrpt::format("Laser scan doesn't contain valid range image"));

    // TODO - have this as a class member
    mrpt::maps::CSimplePointsMap m1, m2;
    float running_time;
    mrpt::slam::CICP::TReturnInfo info;

    m1.insertObservation(from);
    m2.insertObservation(to);

    // this->decimatePointsMap(&m1, [> keep every = */ 40, /* low_lim = <]
    // 5000);
    // this->decimatePointsMap(&m2, [> keep every = */ 40, /* low_lim = <]
    // 5000);

    // If given, use initial_pose_in as a first guess for the ICP
    mrpt::poses::CPose3D initial_pose;
    if (initial_pose_in)
    {
        initial_pose = mrpt::poses::CPose3D(*initial_pose_in);
    }

    mrpt::poses::CPose3DPDF::Ptr pdf =
        params.icp.Align3D(&m1, &m2, initial_pose, &running_time, (void*)&info);

    // return the edge regardless of the goodness of the alignment
    // copy from the 3D PDF
    // NULLIFY contraint_t if Z displacement is high (normally this should be 0)
    // since we are moving in 2D
    if (fabs(pdf->getMeanVal().z()) < 0.1)
    {
        rel_edge->copyFrom(*pdf);
    }
    else
    {
        rel_edge = nullptr;
    }

    // if given, fill the TReturnInfo Struct
    if (icp_info) *icp_info = info;

    MRPT_END;
}

template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::decimatePointsMap(
    mrpt::maps::CPointsMap* m, size_t keep_point_every, size_t low_lim)
{
    MRPT_START;

    size_t map_size = m->size();

    if (low_lim)
    {
        // check if current keep_point_every variable is too large
        size_t conservative_keep_point_every = map_size / low_lim;
        keep_point_every =
            std::min(keep_point_every, conservative_keep_point_every);
    }

    // insert a false every "keep_point_every" points
    std::vector<bool> deletion_mask(map_size, true);
    for (size_t i = 0; i != map_size; ++i)
    {
        if (i % keep_point_every == 0)
        {
            deletion_mask[i] = false;
        }
    }
    m->applyDeletionMask(deletion_mask);

    // std::cout << "Map size: " << map_size << " => " << m->size() <<
    // std::endl;

    MRPT_END;
}

template <class GRAPH_T>
bool CRangeScanOps<GRAPH_T>::convert3DTo2DRangeScan(
    mrpt::obs::CObservation3DRangeScan::Ptr& scan3D_in,
    mrpt::obs::CObservation2DRangeScan::Ptr* scan2D_out)
{
    MRPT_START;

    bool success = false;
    // if it doesn't exist, create it
    if (!*scan2D_out)
    {
        *scan2D_out = mrpt::obs::CObservation2DRangeScan::Create();
    }

    if (scan3D_in->hasRangeImage)
    {
        scan3D_in->convertTo2DScan(**scan2D_out, params.conversion_params);
        success = true;
    }
    else
    {
        std::cout << "No valid rangeImage found" << std::endl;
        success = false;
    }

    return success;
    MRPT_END;
}

template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::TParams::dumpToTextStream(std::ostream& out) const
{
    MRPT_START;

    out << mrpt::format(
        "3D=>2D LaserScan Conversion Sensor label       = %s\n",
        conversion_params.sensorLabel.c_str());
    out << mrpt::format(
        "3D=>2D LaserScan Conversion angle sup          = %.2f deg\n",
        mrpt::RAD2DEG(conversion_params.angle_sup));
    out << mrpt::format(
        "3D=>2D LaserScan Conversion angle inf          = %.2f deg\n",
        mrpt::RAD2DEG(conversion_params.angle_inf));
    out << mrpt::format(
        "3D=>2D LaserScan Conversion oversampling ratio = %.2f\n",
        conversion_params.oversampling_ratio);
    out << mrpt::format(
        "3D=>2D LaserScan Conversion Z minimum          = %.2f\n",
        conversion_params.z_min);

    icp.options.dumpToTextStream(out);

    MRPT_END;
}
template <class GRAPH_T>
void CRangeScanOps<GRAPH_T>::TParams::loadFromConfigFile(
    const mrpt::config::CConfigFileBase& source, const std::string& section)
{
    MRPT_START;

    conversion_params.sensorLabel = source.read_string(
        section, "conversion_sensor_label", "KINECT_TO_2D_SCAN", false);
    conversion_params.angle_sup = mrpt::DEG2RAD(
        source.read_double(section, "conversion_angle_sup", 10, false));
    conversion_params.angle_inf = mrpt::DEG2RAD(
        source.read_double(section, "conversion_angle_inf", 10, false));
    conversion_params.oversampling_ratio = source.read_double(
        section, "conversion_oversampling_ratio", 1.1, false);
    conversion_params.z_min = source.read_double(
        section, "conversion_z_min", 0, false);  // TODO - is this accurate?

    // load the icp parameters - from "ICP" section explicitly
    icp.options.loadFromConfigFile(source, "ICP");

    has_read_config = true;

    MRPT_END;
}
}  // namespace mrpt::graphslam::deciders