Primitive3DFilter.h

Go to the documentation of this file.

/********************************************************************
**                Image Component Library (ICL)                    **
**                                                                 **
** Copyright (C) 2006-2013 CITEC, University of Bielefeld          **
**                         Neuroinformatics Group                  **
** Website: www.iclcv.org and                                      **
**          http://opensource.cit-ec.de/projects/icl               **
**                                                                 **
** File   : ICLGeom/src/ICLGeom/Primitive3DFilter.h                **
** Module : ICLGeom                                                **
** Authors: Lukas Twardon                                          **
**                                                                 **
**                                                                 **
** GNU LESSER GENERAL PUBLIC LICENSE                               **
** This file may be used under the terms of the GNU Lesser General **
** Public License version 3.0 as published by the                  **
**                                                                 **
** Free Software Foundation and appearing in the file LICENSE.LGPL **
** included in the packaging of this file.  Please review the      **
** following information to ensure the license requirements will   **
** be met: http://www.gnu.org/licenses/lgpl-3.0.txt                **
**                                                                 **
** The development of this software was supported by the           **
** Excellence Cluster EXC 277 Cognitive Interaction Technology.    **
** The Excellence Cluster EXC 277 is a grant of the Deutsche       **
** Forschungsgemeinschaft (DFG) in the context of the German       **
** Excellence Initiative.                                          **
**                                                                 **
********************************************************************/

#pragma once

#include <ICLGeom/PointCloudObject.h>
#include <ICLGeom/SceneObject.h>

#ifdef ICL_HAVE_OPENCL
#include "ICLUtils/CLProgram.h"
#endif

namespace icl{
    namespace geom{

        class Primitive3DFilter {

        public:

            enum PrimitiveType {
                CUBE,
                SPHERE,
                CYLINDER
            };

            struct Quaternion {


                Quaternion(const math::Vec3 &v, const float w, bool givenInAxisAngle = false) {
                    if(givenInAxisAngle) {
                        math::Vec3 vn = v.normalized();
                        float s = sin(w/2.0);
                        this->v[0] = vn[0] * s;
                        this->v[1] = vn[1] * s;
                        this->v[2] = vn[2] * s;
                        this->w = cos(w/2.0);
                        if(vn.length() <= 0.0001)
                            this->w = 1;
                    } else {
                        this->v = v;
                        this->w = w;
                    }
                }

                const Quaternion operator*(const Quaternion &q) {
                    Quaternion res(q);
                    res.w    = w*q.w    - v[0]*q.v[0] - v[1]*q.v[1] - v[2]*q.v[2];
                    res.v[0] = w*q.v[0] + v[0]*q.w    + v[1]*q.v[2] - v[2]*q.v[1];
                    res.v[1] = w*q.v[1] - v[0]*q.v[2] + v[1]*q.w    + v[2]*q.v[0];
                    res.v[2] = w*q.v[2] + v[0]*q.v[1] - v[1]*q.v[0] + v[2]*q.w;
                    return res;
                }

                Quaternion conj() const {
                    Quaternion res(*this);
                    res.v *= -1;
                    return res;
                }

                math::Mat4 getTransformationMatrix() {
                    float x = v[0];
                    float y = v[1];
                    float z = v[2];
                    float xx = x*x;
                    float yy = y*y;
                    float zz = z*z;
                    return math::Mat4 (1-2*yy-2*zz, 2*x*y-2*w*z, 2*x*z+2*w*y, 0,
                                       2*x*y+2*w*z, 1-2*xx-2*zz, 2*y*z-2*w*x, 0,
                                       2*x*z-2*w*y, 2*y*z+2*w*x, 1-2*xx-2*yy, 0,
                                       0,0,0,1);
                }


                math::Vec3 rotateVector(const math::Vec3 &vIn) {
                    Quaternion qIn(vIn, 0);
                    Quaternion qOut = qIn * this->conj();
                    qOut = (*this) * qOut;
                    return qOut.v;
                }

                math::Vec3 v;

                float w;

            };

            struct Primitive3D {

                Primitive3D(const PrimitiveType &type, const Vec &position, const Quaternion &orientation,
                            const Vec &scale, unsigned long timestamp, const std::string &description = "") :
                    type(type), position(position), orientation(orientation), scale(scale), timestamp(timestamp), description(description) {

                }

                void toSceneObject(SceneObject *object, uint32_t slices = 15, GeomColor const &color = geom_white(100));

                PrimitiveType type;

                Vec position;

                Quaternion orientation;

                Vec scale;

                unsigned long timestamp;

                std::string description;

                unsigned char groupBit;

            };

            struct FilterAction {

                FilterAction() {}

                FilterAction(std::vector<unsigned char> formula) : formula(formula) {}

                virtual ~FilterAction() {}


                virtual void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                           std::vector<unsigned char> &groupMap, core::Img32f *depthImage) = 0;

                std::vector<unsigned char> formula;

            };

            struct RemoveAction : FilterAction {

                RemoveAction(std::vector<unsigned char> formula) : FilterAction(formula) {}

                virtual ~RemoveAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

            };

            struct SetposAction : FilterAction {

                SetposAction(std::vector<unsigned char> formula, float x, float y, float z) : FilterAction(formula), x(x), y(y), z(z) {}

                virtual ~SetposAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

                float x;
                float y;
                float z;

            };

            struct ColorAction : FilterAction {

                ColorAction(std::vector<unsigned char> formula, float r, float g, float b, float a) : FilterAction(formula), r(r), g(g), b(b), a(a) {}

                virtual ~ColorAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

                float r;
                float g;
                float b;
                float a;

            };

            struct LabelAction : FilterAction {

                LabelAction(std::vector<unsigned char> formula, icl32s value) : FilterAction(formula), value(value) {}

                virtual ~LabelAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

                icl32s value;

            };

            struct IntensityAction : FilterAction {

                IntensityAction(std::vector<unsigned char> formula, float value) : FilterAction(formula), value(value) {}

                virtual ~IntensityAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

                float value;

            };

            struct FilterDepthImgAction : FilterAction {

                FilterDepthImgAction(std::vector<unsigned char> formula, float value) : FilterAction(formula), value(value) {}

                virtual ~FilterDepthImgAction() {}

                void performAction(PointCloudObjectBase &pcObj, std::vector<unsigned char> &actionMap,
                                   std::vector<unsigned char> &groupMap, core::Img32f *depthImage);

                float value;

            };

            struct FilterConfig {


                FilterConfig(const std::string &filename);

                std::map<std::string, unsigned char> mapGroupIdToBit;

                std::map<std::string, unsigned char> mapRegexToBit;

                std::map<unsigned char, float> mapGroupBitToPadding;

                std::vector<utils::SmartPtr<FilterAction> > filterActions;

            };

            Primitive3DFilter(const FilterConfig &config);

            virtual ~Primitive3DFilter() {}

            void setConfig(const FilterConfig &config) {
                this->config = config;
            }


            void apply(const std::vector<Primitive3D> &primitives, PointCloudObjectBase &pcObj, core::Img32f *depthImage = 0);

        private:

            #ifdef ICL_HAVE_OPENCL
            utils::CLProgram program;
            utils::CLBuffer pcbuffer, groupmapbuffer, actionmapbuffer, formulabuffer;
            utils::CLKernel kernelCreateGroupMap;
            utils::CLKernel kernelCreateActionMap;
            #endif

            static const char *KERNEL_CODE;
            FilterConfig config;

        };

    } // namespace geom
}