CPointCloudFilterByDistance.cpp

Go to the documentation of this file.

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

#include "maps-precomp.h" // Precomp header

#include <mrpt/maps/CPointCloudFilterByDistance.h>
#include <mrpt/utils/CConfigFileBase.h>
#include <mrpt/utils/aligned_containers.h>

using namespace mrpt::maps;
using namespace mrpt::utils;

void CPointCloudFilterByDistance::filter(
        mrpt::maps::CPointsMap * pc,       //!< [in,out] The input pointcloud, which will be modified upon return after filtering.
        const mrpt::system::TTimeStamp pc_timestamp,     //!< [in] The timestamp of the input pointcloud
        const mrpt::poses::CPose3D & cur_pc_pose,  //!< [in] If NULL, the PC is assumed to be given in global coordinates. Otherwise, it will be transformed from local coordinates to global using this transformation.
        TExtraFilterParams * params                      //!< [in,out] additional in/out parameters
)
{
        using namespace mrpt::poses;
        using namespace mrpt::math;
        using mrpt::math::square;

        MRPT_START;
        ASSERT_(pc_timestamp!=INVALID_TIMESTAMP);
        ASSERT_(pc != nullptr);

        CSimplePointsMapPtr original_pc = CSimplePointsMap::Create();
        original_pc->copyFrom(*pc);

        // 1) Filter:
        // ---------------------
        const size_t N = pc->size();
        std::vector<bool> deletion_mask;
        deletion_mask.assign(N, false);
        size_t del_count = 0;

        // get reference, previous PC:
        ASSERT_(options.previous_keyframes >= 1);
        bool can_do_filter = true;

        std::vector<FrameInfo*> prev_pc; // (options.previous_keyframes, nullptr);
        {
                auto it = m_last_frames.rbegin();
                for (int i = 0; i < options.previous_keyframes && it!=m_last_frames.rend(); ++i, ++it)
                {
                        prev_pc.push_back(&it->second);
                }
        }

        if (prev_pc.size() < static_cast<size_t>(options.previous_keyframes) ) {
                can_do_filter = false;
        }
        else {
                for (int i = 0; can_do_filter && i < options.previous_keyframes; ++i)
                {
                        if (mrpt::system::timeDifference(m_last_frames.rbegin()->first, pc_timestamp) > options.too_old_seconds)
                        {
                                can_do_filter = false; // A required keyframe is too old
                                break;
                        }
                }
        }

        if (can_do_filter)
        {
                // Reference poses of each PC:
                // Previous: prev_pc.pose
                mrpt::aligned_containers<CPose3D>::vector_t rel_poses;
                for (int k = 0; k < options.previous_keyframes; ++k) {
                        const CPose3D rel_pose = cur_pc_pose - prev_pc[k]->pose;
                        rel_poses.push_back(rel_pose);
                }

                // The idea is that we can now find matches between pt{i} in time_{k}, composed with rel_pose
                // with the local points in time_{k-1}.

                std::vector<TPoint3D> pt_km1(options.previous_keyframes);

                for (size_t i = 0; i < N; i++)
                {
                        // get i-th point in time=k:
                        TPoint3Df ptf_k;
                        pc->getPointFast(i, ptf_k.x, ptf_k.y, ptf_k.z);
                        const TPoint3D pt_k = TPoint3D(ptf_k);

                        // Point, referred to time=k-1 frame of reference
                        for (int k = 0; k < options.previous_keyframes; ++k)
                                rel_poses[k].composePoint(pt_k, pt_km1[k]);

                        // Look for neighbors in "time=k"
                        std::vector<TPoint3D> neig_k;
                        std::vector<float>    neig_sq_dist_k;
                        pc->kdTreeNClosestPoint3D(pt_k, 2 /*num queries*/, neig_k, neig_sq_dist_k);

                        // Look for neighbors in "time=k-i"
                        std::vector<TPoint3D> neig_kmi(options.previous_keyframes);
                        std::vector<float>    neig_sq_dist_kmi(options.previous_keyframes, std::numeric_limits<float>::max() );

                        for (int k = 0; k < options.previous_keyframes; ++k) {
                                for (int prev_tim_idx = 0; prev_tim_idx < options.previous_keyframes; prev_tim_idx++) {
                                        if (prev_pc[prev_tim_idx]->pc->size() > 0) {
                                                prev_pc[prev_tim_idx]->pc->kdTreeClosestPoint3D(pt_km1[prev_tim_idx], neig_kmi[prev_tim_idx], neig_sq_dist_kmi[prev_tim_idx]);
                                        }
                                }
                        }

                        // Rule:
                        // we must have at least 1 neighbor in t=k, and 1 neighbor in t=k-i
                        const double max_allowed_dist_sq = square(options.min_dist + options.angle_tolerance * pt_k.norm());

                        bool ok_total = true;
                        const bool ok_t   = neig_k.size() > 1 && neig_sq_dist_k[1] < max_allowed_dist_sq;
                        ok_total = ok_total && ok_t;

                        for (int k = 0; k < options.previous_keyframes; ++k)
                        {
                                const bool ok_tm1 = neig_sq_dist_kmi[k] < max_allowed_dist_sq;
                                ok_total = ok_total && ok_tm1;
                        }

                        // Delete?
                        const bool do_delete = !(ok_total);
                        deletion_mask[i] = do_delete;

                        if (do_delete) del_count++;
                }

                // Remove points:
                if ( (params == nullptr || params->do_not_delete == false) &&
                        N>0 &&
                        del_count/double(N) < options.max_deletion_ratio  // If we are deleting too many points, it may be that the filter is plainly wrong
                        )
                {
                        pc->applyDeletionMask(deletion_mask);
                }
        } // we can do filter

        if (params != nullptr && params->out_deletion_mask != nullptr) {
                *params->out_deletion_mask = deletion_mask;
        }

        // 2) Add PC to list
        // ---------------------
        {
                FrameInfo fi;
                fi.pc = original_pc;
                fi.pose = cur_pc_pose;

                m_last_frames[pc_timestamp] = fi;
        }

        // 3) Remove too-old PCs.
        // ---------------------
        for (auto it = m_last_frames.begin(); it != m_last_frames.end(); )
        {
                if (mrpt::system::timeDifference(it->first, pc_timestamp) > options.too_old_seconds) {
                        it = m_last_frames.erase(it);
                }
                else {
                        ++it;
                }
        }

        MRPT_END;
}

CPointCloudFilterByDistance::TOptions::TOptions() :
        min_dist(0.10),
        angle_tolerance( mrpt::utils::DEG2RAD(5)),
        too_old_seconds(1.0),
        previous_keyframes(1),
        max_deletion_ratio(.4)
{
}

void CPointCloudFilterByDistance::TOptions::loadFromConfigFile(const mrpt::utils::CConfigFileBase &c, const std::string &s)
{
        MRPT_LOAD_CONFIG_VAR(min_dist, double, c, s);
        MRPT_LOAD_CONFIG_VAR_DEGREES(angle_tolerance, c, s);
        MRPT_LOAD_CONFIG_VAR(too_old_seconds, double, c, s);
        MRPT_LOAD_CONFIG_VAR(previous_keyframes, int, c, s);
        MRPT_LOAD_CONFIG_VAR(max_deletion_ratio, double, c, s);
}

void CPointCloudFilterByDistance::TOptions::saveToConfigFile(mrpt::utils::CConfigFileBase &c, const std::string &s) const
{
        MRPT_SAVE_CONFIG_VAR_COMMENT(min_dist, "");
        MRPT_SAVE_CONFIG_VAR_DEGREES_COMMENT("angle_tolerance",angle_tolerance, "");
        MRPT_SAVE_CONFIG_VAR_COMMENT(too_old_seconds, "");
        MRPT_SAVE_CONFIG_VAR_COMMENT(previous_keyframes, "(Default: 1) How many previous keyframes will be compared with the latest pointcloud.");
        MRPT_SAVE_CONFIG_VAR_COMMENT(max_deletion_ratio, "(Default: 0.4) If the ratio [0,1] of points considered invalid (`deletion` ) is larger than this ratio, no point will be deleted since it'd be too suspicious and may indicate a failure of this filter.");
}