HandLocalisation.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
 */
// *****************************************************************
// -----------------------------------------------------------------
// HandLocalisation.h
// Declaration of class CHandLocalisation
// -----------------------------------------------------------------
// *****************************************************************
 
// *****************************************************************
// Author:  David Schiebener
// Date:    11.12.2009
// *****************************************************************
 
 
// *****************************************************************
// double-include protection
// *****************************************************************
 
#pragma once
 
 
// *****************************************************************
// includes
// *****************************************************************
 
// Hand localisation
#include "HandLocalisationConstants.h"
#include "ParticleFilter/ParticleFilterRobotHandLocalisation.h"
 
// IVT
#include "Calibration/StereoCalibration.h"
#include "Image/ByteImage.h"
#include "Math/Math3d.h"
#include "Image/ImageProcessor.h"
#include "ObjectFinder/ObjectFinder.h"
 
// Eigen
#include <Eigen/Eigen>
 
#include <mutex>
 
// **************************************************************
// Class definition
// **************************************************************
 
 
 
namespace visionx
{
    class CHandLocalisation
    {
        // public methods
    public:
        CHandLocalisation(int nNumParticles, int nNumAnnealingRuns, int nPredictionMode, CStereoCalibration* pCalibration, std::string sHandModelFileName);
        ~CHandLocalisation();
 
        void LocaliseHand(const CByteImage* pNewCamImageLeft, const CByteImage* pNewCamImageRight, const double* pSensorConfig, double* pEstimatedConfig, double& dConfidenceRating);
        void LocaliseHand(const CByteImage* pNewCamImageLeft, const CByteImage* pNewCamImageRight, const Vec3d vPositionFromSensors, const Mat3d mOrientationFromSensors,
                          const double* pSensorConfigFingers, double* pEstimatedConfig, double& dConfidenceRating);
 
        Eigen::Matrix4f GetHandPose();
        std::vector<Vec3d> GetFingertipPositions(); // ordering: thumb, index, middle, ring, pinky
 
        // modify the variance for the random noise during particle re-drawing
        void SetParticleVarianceFactor(double dNewFactor = 1.0)
        {
            m_dVarianceFactor = dNewFactor;
        }
 
        CByteImage* m_pColorFilteredImageLeft;
        CByteImage* m_pColorFilteredImageRight;
        CByteImage* m_pSobelImageLeft;
        CByteImage* m_pSobelImageRight;
        CByteImage* m_pTrackingBallImageLeft;
        CByteImage* m_pTrackingBallImageRight;
 
        double* GetResultConfig();
        void SetResultConfig(const Vec3d vPosition, const Mat3d mOrientation, const double* pConfigFingers);
        double* GetSensorConfig();
        void SetSensorConfig(const Vec3d vPosition, const Mat3d mOrientation, const double* pConfigFingers);
 
    private:
 
        bool m_bFirstRun;
 
        // images
        CByteImage* m_pCamImageLeft, *m_pCamImageRight;
 
        CByteImage* m_pHSVImageLeft, *m_pHSVImageRight;
        CByteImage* m_pSobelXImageLeft, *m_pSobelXImageRight;
        CByteImage* m_pSobelYImageLeft, *m_pSobelYImageRight;
 
        CByteImage* m_pTempGrayImage1, *m_pTempGrayImage2, *m_pTempGrayImage3, *m_pTempGrayImage4, *m_pTempGrayImage5, *m_pTempGrayImage6;
 
 
        // configs & ratings
        double* m_pFinalConf;
        double* m_pOldFinalConf;
        double* m_pSensorConf;
        double* m_pOldSensorConf;
        double* m_pPFConf;
        double* m_pOldPFConf;
        double* m_pPredictedConf;
        std::mutex configMutex;
 
        double m_dRating;
        double m_dOldRating;
        DetailedPFRating m_PFRating;
 
 
        // settings
        int m_nNumParticles;
        int m_nNumAnnealingRuns;
        int m_nPredictionMode;
        float m_dVarianceFactor;
 
 
        // particle filter
        CParticleFilterRobotHandLocalisation* m_pParticleFilter;
 
 
        // ObjectFinder for the tracking ball
        CObjectFinder* m_pObjectFinderLeft, *m_pObjectFinderRight;
        static const int m_nMaxTrBallRegions = 30;
        double* m_pdTrBallPosXLeft, *m_pdTrBallPosXRight;
        double* m_pdTrBallPosYLeft, *m_pdTrBallPosYRight;
        double* m_pdTrBallRadiusLeft, *m_pdTrBallRadiusRight;
 
        // for returning the fingertip poses
        CHandModelV2* m_pHandModel;
    };
}