Miscellaneous.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 <Eigen/Dense>

#include <mrpt/math/ops_containers.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <iostream>
#include <iterator>
#include <map>
#include <string>
#include <vector>

namespace mrpt::pbmap
{
using PointT = pcl::PointXYZRGBA;

/*!Transform the (x,y,z) coordinates of a PCL point into a Eigen::Vector3f.*/
template <class pointPCL>
Eigen::Vector3f getVector3fromPointXYZ(pointPCL& pt)
{
    return Eigen::Vector3f(pt.x, pt.y, pt.z);
}

template <class POINT>
inline Eigen::Vector3f diffPoints(const POINT& P1, const POINT& P2)
{
    Eigen::Vector3f diff;
    diff[0] = P1.x - P2.x;
    diff[1] = P1.y - P2.y;
    diff[2] = P1.z - P2.z;
    return diff;
}

/*!Compose a 3D-point with a pose.*/
template <class dataType>
Eigen::Matrix<dataType, 3, 1> compose(
    Eigen::Matrix<dataType, 4, 4>& pose, Eigen::Matrix<dataType, 3, 1>& point)
{
    Eigen::Matrix<dataType, 3, 1> transformedPoint =
        pose.template block<3, 3>(0, 0) * point +
        pose.template block<3, 1>(0, 3);
    return transformedPoint;
}

/*!Compose two poses.*/
template <class dataType>
Eigen::Matrix<dataType, 4, 4> compose(
    Eigen::Matrix<dataType, 4, 4>& pose1, Eigen::Matrix<dataType, 4, 4>& pose2)
{
    Eigen::Matrix<dataType, 4, 4> transformedPose;
    transformedPose.template block<3, 3>(0, 0) =
        pose1.template block<3, 3>(0, 0) * pose2.template block<3, 3>(0, 0);
    transformedPose.block(0, 3, 3, 1) =
        pose1.block(0, 3, 3, 1) +
        pose1.template block<3, 3>(0, 0) * pose2.block(0, 3, 3, 1);
    transformedPose.row(3) << 0, 0, 0, 1;
    return transformedPose;
}

/*!Get the pose's inverse.*/
template <class dataType>
Eigen::Matrix<dataType, 4, 4> inverse(Eigen::Matrix<dataType, 4, 4>& pose)
{
    Eigen::Matrix<dataType, 4, 4> inverse;
    inverse.template block<3, 3>(0, 0) =
        pose.template block<3, 3>(0, 0).transpose();
    inverse.block(0, 3, 3, 1) =
        -(inverse.template block<3, 3>(0, 0) * pose.block(0, 3, 3, 1));
    inverse.row(3) << 0, 0, 0, 1;
    return inverse;
}

struct Segment
{
    Segment(PointT p0, PointT p1) : P0(p0), P1(p1){};

    PointT P0, P1;
};

/*! Square of the distance between two segments */
float dist3D_Segment_to_Segment2(Segment S1, Segment S2);

/*! Check if a point lays inside a convex hull */
bool isInHull(PointT& point3D, pcl::PointCloud<PointT>::Ptr hull3D);

template <typename dataType>
dataType getMode(std::vector<dataType> data, dataType range)
{
    float normalizeConst = 255.0 / range;
    std::vector<int> data2(data.size());
    for (size_t i = 0; i < data.size(); i++)
        data2[i] = (int)(data[i] * normalizeConst);

    std::map<int, int> histogram;
    for (size_t i = 0; i < data2.size(); i++)
        if (histogram.count(data2[i]) == 0)
            histogram[data2[i]] = 1;
        else
            histogram[data2[i]]++;

    int mode = 0, count = 0;
    for (auto bin = histogram.begin(); bin != histogram.end(); bin++)
        if (bin->second > count)
        {
            count = bin->second;
            mode = bin->first;
        }

    return (dataType)mode / normalizeConst;
}

//  Eigen::Matrix4f& getMoorePenroseInverse(Eigen::Matrix4f &input)
//  {
////    Eigen::Matrix4f generalizedInverse;
////    Eigen::JacobiSVD<Eigen::Matrix3f> svd(input);
////    stdDevHist = svd.singularValues().maxCoeff() / sqrt(size);
//   void pinv( MatrixType& pinvmat) const
//   {
////     eigen_assert(m_isInitialized && "SVD is not initialized.");
//     double pinvtoler=1.e-6; // choose your tolerance wisely!
//     Eigen::SingularValuesType singularValues_inv = m_singularValues;
//     for ( long i=0; i<m_workMatrix.cols(); ++i) {
//        if ( m_singularValues(i) > pinvtoler )
//           singularValues_inv(i)=1.0/m_singularValues(i);
//       else singularValues_inv(i)=0;
//     }
//     pinvmat=
//     (m_matrixV*singularValues_inv.asDiagonal()*m_matrixU.transpose());
//   }

// Gets the center of a single-mode distribution, it performs variable mean
// shift
template <typename dataType>
Eigen::Vector4f getMultiDimMeanShift_color(
    std::vector<Eigen::Vector4f>& data, dataType& stdDevHist,
    dataType& concentration)
{
    //    cout << "Do meanShift\n";

    std::vector<Eigen::Vector4f> dataTemp = data;
    size_t size = data.size();

    //    This one is specific for normalized color
    Eigen::Vector3f sum = Eigen::Vector3f::Zero();
    for (size_t i = 0; i < data.size(); i++)
    {
        sum += data[i].head(3);
    }
    Eigen::Vector3f meanShift = sum / size;
    // cout << "First meanShift " << meanShift.transpose() << endl;

    Eigen::Matrix3f cov = Eigen::Matrix3f::Zero();
    for (size_t i = 0; i < data.size(); i++)
    {
        Eigen::Vector3f diff = data[i].head(3) - meanShift;
        cov += diff * diff.transpose();
    }
    //    cov /= size;
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(cov);
    stdDevHist = svd.singularValues().maxCoeff() / sqrt((double)size);
    //    stdDevHist = 0.05;

    double shift = 1000;  // Large limit
    int iteration_counter = 0;
    double convergence = 0.001;
    while (2 * dataTemp.size() > size && shift > convergence)
    {
        //  std::cout << "iteration " << iteration_counter << " Std " <<
        //  stdDevHist << " maxEig " << svd.singularValues().maxCoeff() <<
        //  std::endl;
        for (auto it = dataTemp.begin(); it != dataTemp.end();)
        {
            //        cout << "CHeck\n";
            Eigen::Vector3f diff = (*it).head(3) - meanShift;
            if (diff.norm() > stdDevHist)
            {
                sum -= (*it).head(3);
                cov -= diff * diff.transpose();
                dataTemp.erase(it);
            }
            else
                it++;
        }
        //    cout << "sum " << sum.transpose() << " newdatasize " <<
        //    dataTemp.size() << endl;
        Eigen::Vector3f meanUpdated = sum / dataTemp.size();
        shift = (meanUpdated - meanShift).norm();
        meanShift = meanUpdated;
        svd = Eigen::JacobiSVD<Eigen::Matrix3f>(cov);
        //      stdDevHist = svd.singularValues().maxCoeff() / dataTemp.size();
        stdDevHist =
            svd.singularValues().maxCoeff() / sqrt((double)dataTemp.size());

        iteration_counter++;
    }
    //  std::cout << "Number of iterations: " << iteration_counter << " shift "
    //  << shift
    //            << " size " << (float)dataTemp.size() / size << " in " <<
    //            clock.Tac() * 1e3 << " ms." << std::endl;

    //  stdDevHist = calcStdDev(data, meanShift);

    Eigen::Vector4f dominantColor;
    dominantColor.head(3) = meanShift;
    float averageIntensity = 0;
    for (unsigned i = 0; i < dataTemp.size(); i++)
        averageIntensity += dataTemp[i][3];
    averageIntensity /= dataTemp.size();
    dominantColor(3) = averageIntensity;

    //    concentration = float(dataTemp.size()) / size;
    int countFringe05 = 0;
    for (auto it = data.begin(); it != data.end(); it++)
        if ((it->head(3) - meanShift).norm() <
            0.05)  //&& *it(3) - averageIntensity < 0.3)
            ++countFringe05;
    concentration = static_cast<dataType>(countFringe05) / data.size();

    return dominantColor;
}

// Gets the center of a single-mode distribution, it performs variable mean
// shift
template <typename dataType>
dataType getHistogramMeanShift(
    std::vector<dataType>& data, double range,
    dataType& stdDevHist_out)  //, dataType &concentration05)
{
    //    cout << "Do meanShift\n";
    //  mrpt::system::CTicTac clock;
    //  clock.Tic();
    size_t size = data.size();
    std::vector<dataType> dataTemp = data;

    dataType sum = 0;
    for (size_t i = 0; i < data.size(); i++)
    {
        sum += data[i];
    }
    dataType meanShift = sum / size;
    dataType stdDevHist = mrpt::math::stddev(data);

    ////    dataType meanShift;
    //    double meanShift, stdDevHist;
    //    mrpt::math::meanAndStd(data,meanShift,stdDevHist);
    //    double sum = meanShift*data.size();
    // cout << "mean " << meanShift << endl;

    // dataType step = 1;
    double shift = 1000;
    int iteration_counter = 0;
    double convergence = range * 0.001;
    while (2 * dataTemp.size() > size && shift > convergence)
    {
        //  std::cout << "iteration " << iteration_counter << " Std " <<
        //  stdDevHist << std::endl;
        for (auto it = dataTemp.begin(); it != dataTemp.end();)
        {
            //        cout << "CHeck\n";
            if (fabs(*it - meanShift) > stdDevHist)
            {
                sum -= *it;
                dataTemp.erase(it);
            }
            else
                it++;
        }
        //    cout << "sum " << sum << " newdatasize " << dataTemp.size() <<
        //    endl;
        double meanUpdated = sum / dataTemp.size();
        shift = fabs(meanUpdated - meanShift);
        meanShift = meanUpdated;
        stdDevHist = mrpt::math::stddev(dataTemp);

        iteration_counter++;
    }
    //  std::cout << "Number of iterations: " << iteration_counter << " shift "
    //  << shift
    //            << " size " << (float)dataTemp.size() / size << " in " <<
    //            clock.Tac() * 1e3 << " ms." << std::endl;

    //  stdDevHist = calcStdDev(data, meanShift);

    //    // Calculate concentration05
    ////    stdDevHist_out = float(dataTemp.size()) / size;
    //    int countFringe05 = 0;
    //    for(typename std::vector<dataType>::iterator it=data.begin(); it !=
    //    data.end(); it++)
    //        if(fabs(*it - meanShift) < 0.05)
    //            ++countFringe05;
    //    concentration05 = static_cast<dataType>(countFringe05) / data.size();

    return static_cast<dataType>(meanShift);
}

//  // Bhattacharyya histogram distance function
//  double  BhattacharyyaDist(std::vector<float> &hist1,
//  std::vector<float> &hist2)
//  {
//    assert(hist1.size() == hist2.size());
//    double BhattachDist;
//    double BhattachDist_aux = 0.0;
//    for(unsigned i=0; i < hist1.size(); i++)
//      BhattachDist_aux += sqrt(hist1[i]*hist2[i]);
//
//    BhattachDist = sqrt(1 - BhattachDist_aux);
//
//    return BhattachDist;
//  }

/**
 * Output a vector as a stream that is space separated.
 * @param os Output stream
 * @param v Vector to output
 * @return the stream output
 */
template <class T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v)
{
    std::copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
    return os;
}
}  // namespace mrpt::pbmap

#endif