Miscellaneous.h

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2018, 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 |
   +------------------------------------------------------------------------+ */
 
/*  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>
 */
 
#ifndef __MISCELLANEOUS_H
#define __MISCELLANEOUS_H
 
#include <mrpt/config.h>
#if MRPT_HAS_PCL
 
#include <mrpt/math/types_math.h>  // Eigen
#include <map>
#include <string>
#include <iostream>
#include <iterator>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <mrpt/math/ops_containers.h>
 
namespace mrpt
{
namespace 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.block(0, 0, 3, 3) * point + pose.block(0, 3, 3, 1);
        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.block(0, 0, 3, 3) =
                pose1.block(0, 0, 3, 3) * pose2.block(0, 0, 3, 3);
        transformedPose.block(0, 3, 3, 1) =
                pose1.block(0, 3, 3, 1) +
                pose1.block(0, 0, 3, 3) * 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.block(0, 0, 3, 3) = pose.block(0, 0, 3, 3).transpose();
        inverse.block(0, 3, 3, 1) =
                -(inverse.block(0, 0, 3, 3) * 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 (std::map<int, int>::iterator 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 (typename std::vector<Eigen::Vector4f>::iterator 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 (typename std::vector<Eigen::Vector4f>::iterator 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 (typename std::vector<dataType>::iterator 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 pbmap
}  // namespace mrpt
 
#endif
 
#endif