HypothesisGeneration.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"
#include "ObjectHypothesis.h"
#include "LCCPSegmentation.h"
 
// IVT
#include <Math/Math3d.h>
#include <Math/Math2d.h>
 
// OpenCV
#include <opencv2/opencv.hpp>
 
// system
#include <ctime>
 
 
// forward declarations
class CSIFTFeatureCalculator;
class CCalibration;
class CByteImage;
class CStereoMatcher;
class CFloatMatrix;
 
 
class CHypothesisGeneration
{
public:
    CHypothesisGeneration(CCalibration* calibration);
    ~CHypothesisGeneration(void);
 
    void FindObjectHypotheses(CByteImage* pImageLeftColor, CByteImage* pImageRightColor, CByteImage* pImageLeftGrey, CByteImage* pImageRightGrey,
                              CSIFTFeatureArray& aAllSIFTPoints, std::vector<CMSERDescriptor3D*>& aAllMSERs, std::vector<CHypothesisPoint*>& aPointsFromDisparity,
                              CObjectHypothesisArray& aObjectHypotheses);
 
    CObjectHypothesis* CreatePlaneHypothesisDescriptor(const CVec3dArray& avPlanePoints, std::vector<CMSERDescriptor3D*>& aMSERs,
            std::vector<Vec3d>& aSigmaPoints, const CByteImage* pLeftColorImage, const CByteImage* pLeftGreyImage);
 
    CObjectHypothesis* CreateCylinderHypothesisDescriptor(const CVec3dArray& avCylinderPoints, const Vec3d vCylinderAxis, const Vec3d vCylinderCenter, const float fCylinderRadius,
            std::vector<CMSERDescriptor3D*>& aMSERs, std::vector<Vec3d>& aSigmaPoints, const CByteImage* pLeftColorImage, const CByteImage* pLeftGreyImage);
 
    CObjectHypothesis* CreateSphereHypothesisDescriptor(const CVec3dArray& avSpherePoints, const Vec3d vSphereCenter, const float fSphereRadius, std::vector<CMSERDescriptor3D*>& aMSERs,
            std::vector<Vec3d>& aSigmaPoints, const CByteImage* pLeftColorImage, const CByteImage* pLeftGreyImage);
 
    inline float Determinant4x4(const float* pMatrix);
 
private:
 
    float GetVariance(const CVec3dArray& avPoints, Vec3d& vMean);
 
    void RemoveOutliers(CVec3dArray& avPoints, float fStdDevFactor = 2.0f, std::vector<int>* paIndices = NULL);
 
    bool FindCylinder(const CVec3dArray& avSamplePoints, const CVec3dArray& avAllPoints, const float fToleranceThreshold, const float fMaxRadius,
                      CVec3dArray& avResultPoints, Vec3d& vCylinderAxis, Vec3d& vCylinderCenter, float& fCylinderRadius);
 
    void QuickSort2DPointsX(CFloatMatrix& mPoints, int nLeft, int nRight);
 
    void ClusterPointsRegularGrid2D(const CVec3dArray& avPoints, const CCalibration* calibration,
                                    const int nNumSectionsX, const int nNumSectionsY, CVec3dArray*& pClusters, int& nNumClusters);
 
    bool RANSACCylinders(const CVec3dArray& avPoints, const CCalibration* calibration,
                         const float fToleranceThreshold, const float fMaxRadius, CVec3dArray& avCylinder);
 
    bool RANSACCylinders2(const CVec3dArray& avPoints, const CCalibration* calibration,
                          const float fToleranceThreshold, const float fMaxRadius, CVec3dArray& avCylinder,
                          Vec3d& vCylinderAxis, Vec3d& vCylinderCenter, float& fCylinderRadius);
 
    bool RANSACPlane(const CVec3dArray& pointCandidates, const float fRANSACThreshold,
                     const int nIterations, CVec3dArray& resultPoints, Vec3d& vAxis, float& d);
 
    bool RANSACSphere(const CVec3dArray& aPointCandidates, const float fRANSACThreshold, const float fMaxSphereRadius,
                      const int nMaxIterations, CVec3dArray& aResultPoints, Vec3d& vCenter, float& fRadius);
 
    bool CreateFeatureDescriptorForPoint(CHypothesisPoint* pPoint, Vec3d vPosition, std::vector<CMSERDescriptor3D*>& aMSERs,
                                         std::vector<Vec3d>& aSigmaPoints, const CByteImage* pLeftColorImage, const CByteImage* pLeftGreyImage);
    void AddSIFTFeatureDescriptors(CSIFTFeatureArray* pFeatures, Vec2d vPoint2d, const CByteImage* pLeftGreyImage);
 
    void ClusterXMeans(const CVec3dArray& aPoints, const int nMaxNumClusters, const float fBICFactor, std::vector<CVec3dArray*>& aaPointClusters);
 
    inline void SortMSERsBySize(std::vector<CMSERDescriptor3D*>& aRegions3D);
    inline void SortMSERsByX(std::vector<CMSERDescriptor3D*>& aRegions3D);
    inline int FindRegionForPoint(Vec3d vPoint, std::vector<CMSERDescriptor3D*>& aRegions3D);
 
    inline void SortVec3dsByX(std::vector<Vec3d>& aPoints);
    inline bool PointIsInList(Vec3d vPoint, std::vector<Vec3d>& aPoints);
 
    void CalculateSphere(Vec3d vPoint1, Vec3d vPoint2, Vec3d vPoint3, Vec3d vPoint4, Vec3d& vCenter, float& fRadius);
 
 
    CVec3dArray m_vFeaturePoints3d;
 
    CCalibration* calibration;
    CSIFTFeatureCalculator* m_pSIFTFeatureCalculator;
 
 
    CByteImage* m_pSaliencyImage, *m_pHypothesesCoveringImage, *m_pSaliencyHypothesisRegionsImage, *m_pTempImageGray;
 
    // debug only
    long m_nTimeSum;
 
    int m_nIterations;
 
#ifdef OLP_USE_LCCP
    LCCPSegmentationWrapper* lccpSegmentation1, *lccpSegmentation2;
#endif
 
    //FILE* m_pOutFile;
    //FILE* m_pOutFile2;
    //FILE* m_pOutFile3;
 
    //bool m_bTest;
    //int m_nNumHypotheses;
};