SemanticClustering.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          |
   +------------------------------------------------------------------------+ */

/*  Plane-based Map (PbMap) library
 *  Construction of plane-based maps and localization in it from RGBD Images.
 *  Writen by Eduardo Fernandez-Moral. See docs for <a
 * href="group__mrpt__pbmap__grp.html" >mrpt-pbmap</a>
 */

#pragma once

#include <mrpt/config.h>
#if MRPT_HAS_PCL

#include <mrpt/graphs/CGraphPartitioner.h>
#include <mrpt/math/CMatrixF.h>
#include <mrpt/pbmap/PbMap.h>
#include <mrpt/pbmap/Plane.h>
#include <map>
#include <set>
#include <vector>

static std::vector<unsigned> DEFAULT_VECTOR_U;

namespace mrpt::pbmap
{
/*! This class arranges the planes of a PbMap into groups according to a
 * co-visibility measure
 * The groups are divided using graph minimum normaized-cut (minNcut). The
 * resulting groups can
 * be used to represent semantic groups corresponding human identifiable
 * structures such as rooms,
 * or environments as an office. These groups of closely related planes can be
 * used also for
 * relocalization.
 *
 * \ingroup mrpt_pbmap_grp
 */
class SemanticClustering
{
   private:
    unsigned currentSemanticGroup;

    PbMap& mPbMap;

    std::vector<unsigned> neighborGroups;

    std::map<unsigned, std::vector<unsigned>> groups;  //[group][planeID]

    std::map<unsigned, std::vector<unsigned>>
        vicinity;  //[group][neighborGroup]

    mrpt::math::CMatrixF connectivity_matrix;

    std::vector<unsigned> planesVicinity_order;

    /*!
     * Build the proximity matrix
     */
    void buildProximityMatrix()
    {
        size_t neigSize = 0;
        planesVicinity_order.clear();

        // Set the Vicinity (planes of the current group and its neighbors)
        neighborGroups = vicinity[currentSemanticGroup];
        neighborGroups.push_back(currentSemanticGroup);
        //    cout << "neighborGroups: ";
        for (unsigned i = 0; i < neighborGroups.size(); i++)
        {
            //      cout << neighborGroups[i] << " ";
            neigSize += groups[neighborGroups[i]].size();
            planesVicinity_order.insert(
                planesVicinity_order.end(), groups[neighborGroups[i]].begin(),
                groups[neighborGroups[i]].end());
        }
        //    cout << endl;

        // Fill the matrix
        assert(neigSize <= mPbMap.vPlanes.size());
        connectivity_matrix.resize(neigSize, neigSize);
        connectivity_matrix.setZero();
        for (unsigned i = 0; i < planesVicinity_order.size(); i++)
        {
            unsigned plane_i = planesVicinity_order[i];
            for (unsigned j = i + 1; j < planesVicinity_order.size(); j++)
            {
                unsigned plane_j = planesVicinity_order[j];
                if (mPbMap.vPlanes[plane_i].nearbyPlanes.count(plane_j))
                    connectivity_matrix(i, j) = connectivity_matrix(j, i) = 1;
            }
        }
        // cout << "Planes in the vicinity: ";
        // for(unsigned i=0; i < planesVicinity_order.size(); i++)
        //  cout << planesVicinity_order[i] << " ";
        // cout << endl;

        // for(unsigned i=0; i < planesVicinity_order.size(); i++)
        //{
        //  cout << "\nNeighbors of " << planesVicinity_order[i] << " obs " <<
        //  mPbMap.vPlanes[planesVicinity_order[i]].numObservations << ":\n";
        //  for(map<unsigned,unsigned>::iterator
        //  it=mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.begin();
        //        it !=
        //        mPbMap.vPlanes[planesVicinity_order[i]].neighborPlanes.end();
        //        it++)
        //    cout << it->first << " " << it->second << endl;
        //}
        // cout << endl;
        // std::cout << "connectivity_matrix matrix\n" << connectivity_matrix <<
        // endl;
    }

    /*!
     * Build the co-visibility matrix
     */
    void buildCoVisibilityMatrix()
    {
        size_t neigSize = 0;
        planesVicinity_order.clear();

        // Set the Vicinity (planes of the current group and its neighbors)
        neighborGroups = vicinity[currentSemanticGroup];
        neighborGroups.push_back(currentSemanticGroup);
        for (unsigned i = 0; i < neighborGroups.size(); i++)
        {
            neigSize += groups[neighborGroups[i]].size();
            planesVicinity_order.insert(
                planesVicinity_order.end(), groups[neighborGroups[i]].begin(),
                groups[neighborGroups[i]].end());
        }

        // Fill the matrix
        assert(neigSize <= mPbMap.vPlanes.size());
        connectivity_matrix.resize(neigSize, neigSize);
        connectivity_matrix.setZero();
        for (unsigned i = 0; i < planesVicinity_order.size(); i++)
        {
            unsigned plane_i = planesVicinity_order[i];
            for (unsigned j = i + 1; j < planesVicinity_order.size(); j++)
            {
                unsigned plane_j = planesVicinity_order[j];
                if (mPbMap.vPlanes[plane_i].neighborPlanes.count(plane_j))
                {
                    // Calculate the co-visibility index
                    float sso =
                        (float)mPbMap.vPlanes[plane_i].neighborPlanes[plane_j] /
                        std::min(
                            mPbMap.vPlanes[plane_i].numObservations,
                            mPbMap.vPlanes[plane_j].numObservations);
                    connectivity_matrix(i, j) = connectivity_matrix(j, i) = sso;
                }
            }
        }
    }

    /*!
     * Arrange the semantic groups
     */
    void arrangeNewGroups(std::vector<std::vector<uint32_t>>& parts)
    {
        using namespace std;
        int group_diff = parts.size() - neighborGroups.size();
        std::map<unsigned, std::vector<unsigned>> newGroups;

        if (group_diff > 0)  // Create new groups
        {
            for (unsigned i = 0; i < neighborGroups.size(); i++)
                newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
            for (int i = 0; i < group_diff; i++)
                newGroups[groups.size() + i] = DEFAULT_VECTOR_U;
        }
        else if (group_diff < 0)  // Discard old groups
        {
            for (unsigned i = 0; i < parts.size(); i++)
                newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
        }
        else  // Re-arrange previous groups
        {
            for (unsigned i = 0; i < neighborGroups.size(); i++)
                newGroups[neighborGroups[i]] = DEFAULT_VECTOR_U;
        }
        // cout << "Size new groups " << newGroups.size();
        std::vector<unsigned> group_limits(1, 0);
        for (unsigned i = 0; i < neighborGroups.size(); i++)
            group_limits.push_back(
                group_limits.back() + groups[neighborGroups[i]].size());

        // Re-assign plane's semanticGroup and groups
        for (unsigned i = 0; i < parts.size(); i++)
        {
            for (unsigned j = 0; j < parts[i].size(); j++)
            {
                if (i < neighborGroups.size())
                {
                    if (parts[i][j] < group_limits[i] ||
                        parts[i][j] >=
                            group_limits[i + 1])  // The plane has changed the
                    // group
                    {
                        for (unsigned k = 0;
                             k < neighborGroups.size() && i != k; k++)
                            if (parts[i][j] >= group_limits[k] ||
                                parts[i][j] < group_limits[k + 1])  // The plane
                            // has
                            // changed
                            // the group
                            {
                                assert(
                                    groups[neighborGroups[k]]
                                          [parts[i][j] - group_limits[k]] ==
                                    planesVicinity_order[parts[i][j]]);
                                //                cout << parts[i][j] << "
                                //                swithces to group " <<
                                //                neighborGroups[k] << endl;
                                newGroups[neighborGroups[k]].push_back(
                                    mPbMap
                                        .vPlanes[groups[neighborGroups[k]]
                                                       [parts[i][j] -
                                                        group_limits[k]]]
                                        .id);
                                mPbMap
                                    .vPlanes[groups[neighborGroups[k]]
                                                   [parts[i][j] -
                                                    group_limits[k]]]
                                    .semanticGroup = neighborGroups[k];
                            }
                    }
                    else  // The plane remains in its group
                    {
                        //            cout << parts[i][j] << " remains in group
                        //            " << neighborGroups[i] << endl;
                        newGroups[neighborGroups[i]].push_back(
                            mPbMap
                                .vPlanes[groups[neighborGroups[i]]
                                               [parts[i][j] - group_limits[i]]]
                                .id);
                        //              mPbMap.vPlanes[groups[neighborGroups[k]][parts[i][j]-group_limits[k]]].semanticGroup
                        //              = neighborGroups[k];
                    }
                }
                else
                {
                    newGroups[groups.size() + i - neighborGroups.size()]
                        .push_back(planesVicinity_order[parts[i][j]]);
                    mPbMap.vPlanes[planesVicinity_order[parts[i][j]]]
                        .semanticGroup =
                        groups.size() + i - neighborGroups.size();
                    //          cout << parts[i][j] << " swithces to NEW group "
                    //          << groups.size()+i-neighborGroups.size() <<
                    //          endl;
                }
            }
        }

        for (auto it = newGroups.begin(); it != newGroups.end(); it++)
            groups[it->first] = it->second;

        if (group_diff < 0)
        {
            int sizeVicinity = neighborGroups.size();
            for (int i = -1; i >= group_diff; i--)
            {
                if (neighborGroups[sizeVicinity + i] == groups.size() - 1)
                    groups.erase(neighborGroups[sizeVicinity + i]);
                else
                {
                    for (unsigned j = 0; j < mPbMap.vPlanes.size(); j++)
                        if (mPbMap.vPlanes[j].semanticGroup >
                            neighborGroups[sizeVicinity + i])
                            mPbMap.vPlanes[j].semanticGroup--;
                    for (unsigned j = neighborGroups[sizeVicinity + i];
                         j < groups.size() - 1; j++)
                        groups[j] = groups[j + 1];
                    groups.erase(groups.size() - 1);
                }
            }
        }

        // cout << "Updated arrangement of groups\n";
        // for(map<unsigned,vector<unsigned> >::iterator it=groups.begin(); it
        // != groups.end(); it++)
        //{
        //  cout << "group " << it->first << ": ";
        //  for(unsigned i=0; i < it->second.size(); i++)
        //    cout << it->second[i] << " ";
        //  cout << endl;
        //}

        // Re-define currentSemanticGroup and current vicinity
        //      std::vector<unsigned> newNeighborGroups;

        for (auto it1 = newGroups.begin(); it1 != newGroups.end(); it1++)
            vicinity[it1->first] = DEFAULT_VECTOR_U;

        for (auto it1 = newGroups.begin(); it1 != newGroups.end(); it1++)
        {
            auto it2 = it1;
            for (it2++; it2 != newGroups.end(); it2++)
                for (unsigned i = 0; i < it1->second.size(); i++)
                {
                    bool linked = false;
                    for (unsigned j = 0; j < it2->second.size(); j++)
                    {
                        if (mPbMap.vPlanes[it1->second[i]].neighborPlanes.count(
                                mPbMap.vPlanes[it2->second[j]].id))
                        {
                            vicinity[it1->first].push_back(it2->first);
                            vicinity[it2->first].push_back(it1->first);
                            linked = true;
                            break;
                        }
                    }
                    if (linked) break;
                }
        }

        // TODO: Re-define second order vicinity
    }

   public:
    friend class PbMapMaker;

    // Constructor
    SemanticClustering(PbMap& mPbM) : currentSemanticGroup(0), mPbMap(mPbM)
    {
        vicinity[0] = DEFAULT_VECTOR_U;
    };

    /*!
     * Evaluate the partition of the current semantic groups into new ones with
     * minNcut
     */
    int evalPartition(std::set<unsigned>& observedPlanes)
    {
        using namespace std;
        //      mrpt::system::CTicTac time;
        //      time.Tic();
        //

        // Select current group
        unsigned current_group_votes = 0;
        map<unsigned, unsigned> observed_group;
        for (auto it = observedPlanes.begin(); it != observedPlanes.end(); it++)
            if (observed_group.count(mPbMap.vPlanes[*it].semanticGroup))
                observed_group[mPbMap.vPlanes[*it].semanticGroup]++;
            else
                observed_group[mPbMap.vPlanes[*it].semanticGroup] = 1;
        for (auto it = observed_group.begin(); it != observed_group.end(); it++)
            if (it->second > current_group_votes)
            {
                currentSemanticGroup = it->first;
                current_group_votes = it->second;
            }
        //      cout << "currentSemanticGroup " << currentSemanticGroup << endl;

        //      buildCoVisibilityMatrix();
        buildProximityMatrix();
        std::vector<std::vector<uint32_t>>
            parts;  // Vector of vectors to keep the
        // KFs index of the different
        // partitions (submaps)
        mrpt::graphs::CGraphPartitioner<mrpt::math::CMatrixF>::
			RecursiveSpectralPartition(
                connectivity_matrix, parts, 0.8, false, true, true, 1);

        //    cout << "Time RecursiveSpectralPartition " << time.Tac()*1000 <<
        //    "ms" << endl;

        if (neighborGroups.size() == 1 && parts.size() == 1) return 1;

        // Check if this partition produces any change over the previous group
        // structure
        bool different_partition = false;
        if (neighborGroups.size() != parts.size())
            different_partition = true;
        else
        {
            unsigned prev_size = 0;
            for (unsigned i = 0; i < neighborGroups.size(); i++)
            {
                if (groups[neighborGroups[i]].size() != parts[i].size())
                {
                    different_partition = true;
                    break;
                }

                for (unsigned j = 0; j < parts[i].size(); j++)
                {
                    if (parts[i][j] != j + prev_size)
                    {
                        different_partition = true;
                        break;
                    }
                }
                prev_size += parts[i].size();
            }
        }

        if (!different_partition) return -1;

        arrangeNewGroups(parts);

        // Update currentSemanticGroup
        current_group_votes = 0;
        observed_group.clear();
        for (auto it = observedPlanes.begin(); it != observedPlanes.end(); it++)
            if (observed_group.count(mPbMap.vPlanes[*it].semanticGroup))
                observed_group[mPbMap.vPlanes[*it].semanticGroup]++;
            else
                observed_group[mPbMap.vPlanes[*it].semanticGroup] = 1;
        for (auto it = observed_group.begin(); it != observed_group.end(); it++)
            if (it->second > current_group_votes)
            {
                currentSemanticGroup = it->first;
                current_group_votes = it->second;
            }
        return parts.size();
    }
};
}  // namespace mrpt::pbmap
#endif