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 "LCCPSegmentation.h"
#include "ObjectHypothesis.h"
#include "ObjectLearningByPushingDefinitions.h"
 
// IVT
#include <Math/Math2d.h>
#include <Math/Math3d.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;
};