ObjectHypothesis.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
 *
 * 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
 * @author
 * @date
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
#pragma once
 
#include "ObjectLearningByPushingDefinitions.h"
 
// IVT
#include "Math/Math3d.h"
#include "Features/SIFTFeatures/SIFTFeatureEntry.h"
#include "Structs/Structs.h"
#include "DataStructures/DynamicArrayTemplate.h"
 
 
class CMSERDescriptor
{
public:
    CMSERDescriptor()
    {
        pPoints2D = NULL;
    }
 
    CMSERDescriptor* GetCopy()
    {
        CMSERDescriptor* pRet = new CMSERDescriptor();
 
        if (pPoints2D)
        {
            pRet->pPoints2D = new std::vector<Vec2d>();
 
            for (int i = 0; i < (int)pPoints2D->size(); i++)
            {
                pRet->pPoints2D->push_back(pPoints2D->at(i));
            }
        }
 
        pRet->nSize = nSize;
 
        pRet->vMean = vMean;
 
        pRet->fEigenvalue1 = fEigenvalue1;
        pRet->fEigenvalue2 = fEigenvalue2;
 
        Math2d::SetVec(pRet->vEigenvector1, vEigenvector1);
        Math2d::SetVec(pRet->vEigenvector2, vEigenvector2);
 
        for (int i = 0; i < OLP_SIZE_MSER_HISTOGRAM; i++)
        {
            pRet->pHueHistogramInnerPoints[i] = pHueHistogramInnerPoints[i];
            pRet->pHueHistogramSurroundingRegion[i] = pHueHistogramSurroundingRegion[i];
        }
 
        pRet->fAverageSaturationInnerPoints = fAverageSaturationInnerPoints;
 
        return pRet;
    }
 
 
    std::vector<Vec2d>* pPoints2D;
 
    int nSize;
 
    Vec2d vMean;
 
    float fEigenvalue1, fEigenvalue2;
 
    Vec2d vEigenvector1, vEigenvector2;
 
    float pHueHistogramInnerPoints[OLP_SIZE_MSER_HISTOGRAM];
    float fAverageSaturationInnerPoints;
    float pHueHistogramSurroundingRegion[OLP_SIZE_MSER_HISTOGRAM];
    float fAverageSaturationSurroundingRegion;
};
 
 
 
class CMSERDescriptor3D
{
public:
    CMSERDescriptor3D()
    {
        pRegionLeft = NULL;
        pRegionRight = NULL;
    }
 
    ~CMSERDescriptor3D()
    {
        if (pRegionLeft)
        {
            if (pRegionLeft->pPoints2D)
            {
                //pRegionLeft->pPoints2D->clear();
                delete pRegionLeft->pPoints2D;
            }
 
            delete pRegionLeft;
        }
 
        if (pRegionRight)
        {
            if (pRegionRight->pPoints2D)
            {
                //pRegionRight->pPoints2D->clear();
                delete pRegionRight->pPoints2D;
            }
 
            delete pRegionRight;
        }
    }
 
    CMSERDescriptor3D* GetCopy()
    {
        CMSERDescriptor3D* pRet = new CMSERDescriptor3D();
 
        if (pRegionLeft)
        {
            pRet->pRegionLeft = pRegionLeft->GetCopy();
        }
 
        if (pRegionRight)
        {
            pRet->pRegionRight = pRegionRight->GetCopy();
        }
 
        pRet->vPosition = vPosition;
 
        pRet->vSigmaPoint1a = vSigmaPoint1a;
        pRet->vSigmaPoint1b = vSigmaPoint1b;
        pRet->vSigmaPoint2a = vSigmaPoint2a;
        pRet->vSigmaPoint2b = vSigmaPoint2b;
 
        return pRet;
    }
 
 
    CMSERDescriptor* pRegionLeft, * pRegionRight;
 
    Vec3d vPosition;        // calculated from stereo correspondence of the central points (i.e. means) of the two 2D regions
 
    Vec3d vSigmaPoint1a, vSigmaPoint1b, vSigmaPoint2a, vSigmaPoint2b;   // calculated from stereo correspondences of
    // central points -+ 0.5*sqrt(Eigenvalue)*Eigenvector
    // of the 2D regions
};
 
 
 
 
using CSIFTFeatureArray = CDynamicArrayTemplate<CSIFTFeatureEntry*>;
 
 
class CHypothesisPoint
{
public:
    CHypothesisPoint()
    {
        pFeatureDescriptors = NULL;
        pMSERDescriptor = NULL;
        ePointType = eDepthMapPoint;
    }
 
    ~CHypothesisPoint()
    {
        if (pFeatureDescriptors)
        {
            for (int j = 0; j < pFeatureDescriptors->GetSize(); j++)
            {
                delete (*pFeatureDescriptors)[j];
            }
 
            //pFeatureDescriptors->Clear();
            delete pFeatureDescriptors;
        }
 
        if (pMSERDescriptor)
        {
            delete pMSERDescriptor;
        }
    }
 
    CHypothesisPoint* GetCopy()
    {
        CHypothesisPoint* pRet = new CHypothesisPoint();
        Math3d::SetVec(pRet->vPosition, vPosition);
        Math3d::SetVec(pRet->vOldPosition, vOldPosition);
        pRet->ePointType = ePointType;
        pRet->fColorR = fColorR;
        pRet->fColorG = fColorG;
        pRet->fColorB = fColorB;
        pRet->fIntensity = fIntensity;
        pRet->fMembershipProbability = fMembershipProbability;
 
        if (pMSERDescriptor)
        {
            pRet->pMSERDescriptor = pMSERDescriptor->GetCopy();
        }
 
        if (pFeatureDescriptors)
        {
            pRet->pFeatureDescriptors = new CSIFTFeatureArray();
 
            for (int i = 0; i < pFeatureDescriptors->GetSize(); i++)
            {
                //                //pRet->pFeatureDescriptors->AddElement(new CSIFTFeatureEntry(*(*pFeatureDescriptors)[i]));
                pRet->pFeatureDescriptors->AddElement((CSIFTFeatureEntry*)((*pFeatureDescriptors)[i])->Clone());
            }
        }
 
        return pRet;
    }
 
    enum EPointType
    {
        eHarrisCornerPoint,
        eMSER,
        eDepthMapPoint
    };
 
    Vec3d vPosition;
    Vec3d vOldPosition;
    EPointType ePointType;
    float fColorR, fColorG, fColorB, fIntensity;
    float fMembershipProbability;
    CSIFTFeatureArray* pFeatureDescriptors;
    CMSERDescriptor3D* pMSERDescriptor;
};
 
 
 
class CObjectHypothesis
{
public:
    CObjectHypothesis()
    {
        nHypothesisNumber = -1;
        fMaxExtent = 0;
        Math3d::SetVec(vCenter, Math3d::zero_vec);
        Math3d::SetVec(vLastTranslation, Math3d::zero_vec);
        Math3d::SetMat(mLastRotation, Math3d::unit_mat);
    }
 
    ~CObjectHypothesis()
    {
        for (size_t i = 0; i < aNewPoints.size(); i++)
        {
            delete aNewPoints.at(i);
        }
 
        aNewPoints.clear();
 
        for (size_t i = 0; i < aConfirmedPoints.size(); i++)
        {
            delete aConfirmedPoints.at(i);
        }
 
        aConfirmedPoints.clear();
 
        for (size_t i = 0; i < aDoubtablePoints.size(); i++)
        {
            delete aDoubtablePoints.at(i);
        }
 
        aDoubtablePoints.clear();
    }
 
    enum eObjectType {ePlane, eCylinder, eSphere, eUnstructured, eSingleColored, eRGBD};
 
    int nHypothesisNumber;
 
    eObjectType eType;  // the type of the hypothetical object: plane or cylinder or ...
 
    std::vector<CHypothesisPoint*> aNewPoints;
    std::vector<CHypothesisPoint*> aConfirmedPoints;
    std::vector<CHypothesisPoint*> aDoubtablePoints;
    std::vector<CHypothesisPoint*> aVisibleConfirmedPoints;
 
    // indicates for each iteration if the hypothesis has moved
    std::vector<bool> aHypothesisHasMoved;
 
    Vec3d vNormal;  // if hypothesis is a plane, this is the plane normal.
    // if it is a cylinder, it is the normal of the cylinder surface pointing
    // approximately towards the camera.
 
    Vec3d vCylinderAxis;    // only valid if the object is a cylinder
 
    Vec3d vSecondAxis, vThirdAxis;  // axes that span an orthonormal coordinate system together
    // with the plane normal or the cylinder axis
 
    Vec3d vCenter;  // a central point on the plane or on the cylinder axis or the sphere center
 
    float fStdDev1, fStdDev2;   // standard deviations along vSecondAxis and vThirdAxis in the case of a plane,
    // along cylinder axis and vSecondAxis in the case of a cylinder
 
    float fRadius;  // only valid if the object is a cylinder or a sphere
 
    float fMaxExtent;   // maximal distance of a point from the center
 
    Mat3d mLastRotation;
    Vec3d vLastTranslation;
};
 
 
/*
struct CObjectHypothesis2
{
    enum eObjectType {ePlane, eCylinder};
 
 
    eObjectType eType;  // the type of the hypothetical object: plane or cylinder
 
    CVec3dArray* paPoints3d;        // the points that belong to the hypothetical object
    int* pnCorrespondingPointIndices;       // contains the index of the corresponding 3D point
                                            // amongst all points that were in the image that
                                            // was used to create the hypothesis
 
    CVec3dArray* paPointsThatMovedConsistently; // points that on validation moved consistently with
                                                // the transformation of the object
 
    CSIFTFeatureArray* paFeatureDescriptors;    // contains the SIFT descriptors of the points
 
    Vec3d vNormal;  // if hypothesis is a plane, this is the plane normal.
                    // if it is a cylinder, it is the normal of the cylinder surface pointing
                    // approximately towards the camera.
 
    Vec3d vCylinderAxis;    // only valid if the object is a cylinder
 
    Vec3d vSecondAxis, vThirdAxis;  // axes that span an orthonormal coordinate system together
                                    // with the plane normal or the cylinder axis
 
    Vec3d vCenter;  // a point on the plane or on the cylinder axis
 
    float fStdDev1, fStdDev2;   // standard deviations along vSecondAxis and vThirdAxis in the case of a plane,
                                // along cylinder axis and vSecondAxis in the case of a cylinder
 
    float fCylinderRadius;  // only valid if the object is a cylinder
 
};
*/
 
using CObjectHypothesisArray = CDynamicArrayTemplate<CObjectHypothesis*>;