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 "Image/ImageProcessor.h"
#include "Math/Math3d.h"
#include "ObjectFinder/ObjectFinder.h"
 
// Eigen
#include <mutex>
 
#include <Eigen/Eigen>
 
// **************************************************************
// 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;
    };
} // namespace visionx