DenseCRFFeatureTerms.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    ROBDEKON::ArmarXObjects::DenseCRFSegmentationProcessor
 * @author     Christoph Pohl ( christoph dot pohl at kit dot edu )
 * @date       2019
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
 
#ifndef ROBDEKON_DENSECRFFEATURETERMS_H
#define ROBDEKON_DENSECRFFEATURETERMS_H
 
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <pcl/pcl_macros.h>
#include "Common.h"
 
namespace armarx
{
 
 
    template<class GraphT>
    struct XYZFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        const long unsigned int _num_features = 3;
    public:
        explicit XYZFeature(GraphT& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                Eigen::Vector3f xyz = _graph[i].getVector3fMap();
                features(0, i) = xyz[0] / exp(sigma[0]);
                features(1, i) = xyz[1] / exp(sigma[1]);
                features(2, i) = xyz[2] / exp(sigma[2]);
            }
            return features;
        }
    };
 
    template<class GraphT>
    struct NormalFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        const long unsigned int _num_features = 3;
    public:
        explicit NormalFeature(GraphT& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                Eigen::Vector3f normal = _graph[i].getNormalVector3fMap();
                features(0, i) = isfinite(normal[0]) ? normal[0] / exp(sigma[0]) : 0;
                features(1, i) = isfinite(normal[1]) ? normal[1] / exp(sigma[1]) : 0;
                features(2, i) = isfinite(normal[2]) ? normal[2] / exp(sigma[2]) : 0;
            }
            return features;
        }
    };
 
    template<class GraphT>
    struct CurvatureFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        const long unsigned int _num_features = 1;
    public:
        explicit CurvatureFeature(GraphT& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                float curv = std::fabs(_graph[i].curvature);;
                features(0, i) = curv / exp(sigma[0]);
            }
            return features;
        }
    };
 
    template<class GraphT>
    struct RGBFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        const long unsigned int _num_features = 3;
    public:
        explicit RGBFeature(GraphT& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                Eigen::Vector3f rgb = _graph[i].getBGRVector3cMap().template cast<float>() / 255.0f;
                features(0, i) = rgb[0] / exp(sigma[0]);
                features(1, i) = rgb[1] / exp(sigma[1]);
                features(2, i) = rgb[2] / exp(sigma[2]);
            }
            return features;
        }
    };
 
    template<class GraphT>
    struct CombinedFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        const long unsigned int _num_features = 11;
    public:
        explicit CombinedFeature(GraphT& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            TimestampMap time = boost::get(boost::vertex_timestamp_t(), _graph.m_graph);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                Eigen::Vector3f rgb = _graph[i].getBGRVector3cMap().template cast<float>() / 255.0f;
                features(0, i) = rgb[0] / exp(sigma[0]);
                features(1, i) = rgb[1] / exp(sigma[1]);
                features(2, i) = rgb[2] / exp(sigma[2]);
 
                Eigen::Vector3f normal = _graph[i].getNormalVector3fMap();
                features(3, i) = std::isfinite(normal[0]) ? normal[0] / exp(sigma[3]) : 0.0;
                features(4, i) = std::isfinite(normal[1]) ? normal[1] / exp(sigma[4]) : 0.0;
                features(5, i) = std::isfinite(normal[2]) ? normal[2] / exp(sigma[5]) : 0.0;
 
                Eigen::Vector3f xyz = _graph[i].getVector3fMap();
                features(6, i) = xyz[0] / exp(sigma[6]);
                features(7, i) = xyz[1] / exp(sigma[7]);
                features(8, i) = xyz[2] / exp(sigma[8]);
 
                //                float curv = std::fabs(_graph[i].curvature);
                float curv = _graph[i].curvature;
                features(9, i) = std::isfinite(curv) ? curv / exp(sigma[9]) : 0.0;
 
                features(10, i) = time[i] / exp(sigma[10]);
            }
            return features;
        }
    };
 
    template<class GraphT>
    struct VariableCombinedFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        bool useRGB, useNorm, useXYZ, useCurv, useTime;
        const long unsigned int _num_features = calcNumFeatues();
    public:
        explicit VariableCombinedFeature(GraphT& graph, bool useRGB, bool useNormals, bool useXYZ, bool useCurvature,
                                         bool useTime) : _graph(graph), useRGB(useRGB), useNorm(useNormals),
            useXYZ(useXYZ), useCurv(useCurvature), useTime(useTime) {};
 
        Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            TimestampMap time = boost::get(boost::vertex_timestamp_t(), _graph.m_graph);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                int j = 0;
                if (useRGB)
                {
                    Eigen::Vector3f rgb = _graph[i].getBGRVector3cMap().template cast<float>() / 255.0f;
                    features(j, i) = rgb[0] / exp(sigma[j]);
                    features(j + 1, i) = rgb[1] / exp(sigma[j + 1]);
                    features(j + 2, i) = rgb[2] / exp(sigma[j + 2]);
                    j += 3;
                }
 
                if (useNorm)
                {
                    Eigen::Vector3f normal = _graph[i].getNormalVector3fMap();
                    features(j, i) = std::isfinite(normal[0]) ? normal[0] / exp(sigma[j]) : 0.0;
                    features(j + 1, i) = std::isfinite(normal[1]) ? normal[1] / exp(sigma[j + 1]) : 0.0;
                    features(j + 2, i) = std::isfinite(normal[2]) ? normal[2] / exp(sigma[j + 2]) : 0.0;
                    j += 3;
                }
 
                if (useXYZ)
                {
                    Eigen::Vector3f xyz = _graph[i].getVector3fMap();
                    features(j, i) = xyz[0] / exp(sigma[j]);
                    features(j + 1, i) = xyz[1] / exp(sigma[j + 1]);
                    features(j + 2, i) = xyz[2] / exp(sigma[j + 2]);
                    j += 3;
                }
 
                if (useCurv)
                {
                    //                    float curv = std::fabs(_graph[i].curvature);
                    float curv = _graph[i].curvature;
                    features(j, i) = std::isfinite(curv) ? curv / exp(sigma[j]) : 0.0;
                    j++;
                }
 
                if (useTime)
                {
                    features(j, i) = time[i] / exp(sigma[j]);
                }
 
 
            }
            return features;
        }
 
    private:
        long unsigned int calcNumFeatues()
        {
            long unsigned int _num_feat = 0;
            _num_feat += useRGB ? 3 : 0;
            _num_feat += useNorm ? 3 : 0;
            _num_feat += useXYZ ? 3 : 0;
            _num_feat += useTime ? 1 : 0;
            _num_feat += useCurv ? 1 : 0;
            return _num_feat;
        }
    };
 
    template<class GraphT>
    struct VariableCombinedNormalizedFeature
    {
        typedef typename GraphT::vertex_descriptor VertexId;
    protected:
        GraphT& _graph;
        bool useRGB, useNorm, useXYZ, useCurv, useTime;
        const long unsigned int _num_features = calcNumFeatues();
    public:
        explicit VariableCombinedNormalizedFeature(GraphT& graph, bool useRGB, bool useNormals, bool useXYZ,
                bool useCurvature,
                bool useTime) : _graph(graph), useRGB(useRGB), useNorm(useNormals),
            useXYZ(useXYZ), useCurv(useCurvature),
            useTime(useTime) {};
 
        Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            TimestampMap time = boost::get(boost::vertex_timestamp_t(), _graph.m_graph);
            Eigen::MatrixXd meanAndStd(_num_features, 2);
            meanAndStd = calculateMeanAndStd(_graph);
            ARMARX_DEBUG << "Mean of Input Values: " << meanAndStd.col(0) << "\n Standard Deviation of Input Values: "
                         << meanAndStd.col(1);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                int j = -1;
                if (useRGB)
                {
                    Eigen::Vector3f rgb = _graph[i].getBGRVector3cMap().template cast<float>() / 255.0f;
                    ++j;
                    features(j, i) = ((rgb[0] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                    ++j;
                    features(j, i) = ((rgb[1] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                    ++j;
                    features(j, i) = ((rgb[2] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                }
 
                if (useNorm)
                {
                    Eigen::Vector3f normal = _graph[i].getNormalVector3fMap();
                    ++j;
                    features(j, i) = std::isfinite(normal[0]) ? ((normal[0] - meanAndStd(j, 0)) / meanAndStd(j, 1)) /
                                     exp(sigma[j]) : 0.0;
                    ++j;
                    features(j, i) = std::isfinite(normal[1]) ? ((normal[1] - meanAndStd(j, 0)) / meanAndStd(j, 1)) /
                                     exp(sigma[j]) : 0.0;
                    ++j;
                    features(j, i) = std::isfinite(normal[2]) ? ((normal[2] - meanAndStd(j, 0)) / meanAndStd(j, 1)) /
                                     exp(sigma[j]) : 0.0;
                }
 
                if (useXYZ)
                {
                    Eigen::Vector3f xyz = _graph[i].getVector3fMap();
                    ++j;
                    features(j, i) = ((xyz[0] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                    ++j;
                    features(j, i) = ((xyz[1] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                    ++j;
                    features(j, i) = ((xyz[2] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                }
 
                if (useCurv)
                {
                    //                    float curv = std::fabs(_graph[i].curvature);
                    float curv = _graph[i].curvature;
                    ++j;
                    features(j, i) = std::isfinite(curv) ? ((curv - meanAndStd(j, 0)) / meanAndStd(j, 1)) /
                                     exp(sigma[j]) : 0.0;
                }
 
                if (useTime)
                {
                    ++j;
                    features(j, i) = ((time[i] - meanAndStd(j, 0)) / meanAndStd(j, 1)) / exp(sigma[j]);
                }
 
 
            }
            return features;
        }
 
    private:
 
        Eigen::MatrixXd calculateMeanAndStd(GraphT& graph)
        {
            // this uses Welford's algorithm
            //    k = 0
            //    M = 0
            //    S = 0
            //    for x in x_array:
            //        k += 1
            //        Mnext = M + (x - M) / k
            //        S = S + (x - M)*(x - Mnext)
            //        M = Mnext
            //    return (M, S/(k-1))
            Eigen::MatrixXd meanAndStd(_num_features, 2);
            meanAndStd.setZero();
            TimestampMap time = boost::get(boost::vertex_timestamp_t(), _graph.m_graph);
            int k = 0;
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                // TODO: k might not be needed
                k++;
                Eigen::VectorXd x(_num_features);
 
                int j = 0;
                if (useRGB)
                {
                    Eigen::Vector3f rgb = _graph[i].getBGRVector3cMap().template cast<float>() / 255.0f;
                    x(j) = rgb[0];
                    x(j + 1) = rgb[1];
                    x(j + 2) = rgb[2];
                    j += 3;
                }
 
                if (useNorm)
                {
                    Eigen::Vector3f normal = _graph[i].getNormalVector3fMap();
                    x(j) = std::isfinite(normal[0]) ? normal[0] : 0.0;
                    x(j + 1) = std::isfinite(normal[1]) ? normal[1] : 0.0;
                    x(j + 2) = std::isfinite(normal[2]) ? normal[2] : 0.0;
                    j += 3;
                }
 
                if (useXYZ)
                {
                    Eigen::Vector3f xyz = _graph[i].getVector3fMap();
                    x(j) = xyz[0];
                    x(j + 1) = xyz[1];
                    x(j + 2) = xyz[2];
                    j += 3;
                }
 
                if (useCurv)
                {
                    //                    float curv = std::fabs(_graph[i].curvature);
                    float curv = _graph[i].curvature;
                    x(j) = std::isfinite(curv) ? curv : 0.0;
                    j++;
                }
 
                if (useTime)
                {
                    x(j) = time[i];
                }
                Eigen::VectorXd meanNext = meanAndStd.col(0) + (x - meanAndStd.col(0)) / double(k);
                meanAndStd.col(1) += (x - meanAndStd.col(0)).cwiseProduct(x - meanNext);
                meanAndStd.col(0) = meanNext;
 
            }
            // for the first image, all the timestamps are the same, resulting in variance of 0 --> breaks things
            meanAndStd.col(1) = (meanAndStd.col(1) / double(k - 1)).cwiseSqrt().array() + 1e-8;
            return meanAndStd;
        }
 
        long unsigned int calcNumFeatues()
        {
            long unsigned int _num_feat = 0;
            _num_feat += useRGB ? 3 : 0;
            _num_feat += useNorm ? 3 : 0;
            _num_feat += useXYZ ? 3 : 0;
            _num_feat += useTime ? 1 : 0;
            _num_feat += useCurv ? 1 : 0;
            return _num_feat;
        }
    };
 
    struct TimeFeature
    {
        typedef typename GraphWithTimestamp::vertex_descriptor VertexId;
    protected:
        GraphWithTimestamp& _graph;
        const long unsigned int _num_features = 1;
    public:
        explicit TimeFeature(GraphWithTimestamp& graph) : _graph(graph) {};
 
        const Eigen::MatrixXf computeFeatures(std::vector<float> sigma)
        {
            ARMARX_CHECK_EXPRESSION(_num_features == sigma.size());
            const int N = boost::num_vertices(_graph);
            Eigen::MatrixXf features(_num_features, N);
            TimestampMap time = boost::get(boost::vertex_timestamp_t(), _graph.m_graph);
            for (VertexId i = 0; i < boost::num_vertices(_graph); i++)
            {
                features(0, i) = (time[i]) / exp(sigma[0]);
            }
            return features;
        }
    };
}
#endif //ROBDEKON_DENSECRFFEATURETERMS_H