HandLocalisationThread.cpp

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
 */
/*
 * @file    HandLocalisationThread.cpp
 *
 * @author  David Schiebener
 * @date    21.02.2010
 */
 
/* includes */
#include "HandLocalisationThread.h"
 
#include <Image/ImageProcessor.h>
 
namespace visionx
{
    HandLocalisationThread::HandLocalisationThread(int nNumParticles,
                                                   int nNumAnnealingRuns,
                                                   int nPredictionMode,
                                                   CStereoCalibration* pCalibration,
                                                   std::string sHandModelFileName)
    {
        m_pHandLocalisation = new CHandLocalisation(
            nNumParticles, nNumAnnealingRuns, nPredictionMode, pCalibration, sHandModelFileName);
        m_bIsProcessing = false;
        m_bHasResult = false;
        m_bReadyForLocalising = false;
 
        m_pSensorConfig = new double[DSHT_NUM_PARAMETERS];
        m_pEstimatedConfig = new double[DSHT_NUM_PARAMETERS];
 
        m_bStopped = false;
    }
 
    HandLocalisationThread::~HandLocalisationThread()
    {
        Stop();
        delete m_pHandLocalisation;
        delete[] m_pSensorConfig;
        delete[] m_pEstimatedConfig;
    }
 
    void
    HandLocalisationThread::Stop()
    {
        m_bStopped = true;
        CThread::Stop();
    }
 
    bool
    HandLocalisationThread::IsProcessing()
    {
        return m_bIsProcessing;
    }
 
    bool
    HandLocalisationThread::HasResult()
    {
        return m_bHasResult;
    }
 
    void
    HandLocalisationThread::GetResult(double* pEstimatedConfig, double& dConfidenceRating)
    {
        printf("HandLocalisationThread::GetResult\n");
        m_mMutex.Lock();
        if (m_bHasResult)
        {
            for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
            {
                pEstimatedConfig[i] = m_pEstimatedConfig[i];
            }
            dConfidenceRating = m_dConfidenceRating;
            m_bHasResult = false;
        }
        m_mMutex.UnLock();
    }
 
    void
    HandLocalisationThread::Localise(const CByteImage* pNewCamImageLeft,
                                     const CByteImage* pNewCamImageRight,
                                     const double* pSensorConfig)
    {
        printf("HandLocalisationThread::Localise\n");
        m_mMutex.Lock();
        if (!m_bIsProcessing)
        {
            ImageProcessor::CopyImage(pNewCamImageLeft, m_pNewCamImageLeft);
            ImageProcessor::CopyImage(pNewCamImageRight, m_pNewCamImageRight);
            for (int i = 0; i < DSHT_NUM_PARAMETERS; i++)
            {
                m_pSensorConfig[i] = pSensorConfig[i];
            }
            m_bIsProcessing = true;
            m_bReadyForLocalising = true;
        }
        m_mMutex.UnLock();
    }
 
    int
    HandLocalisationThread::ThreadMethod()
    {
        while (!m_bStopped)
        {
            if (m_bReadyForLocalising)
            {
                printf("HandLocalisationThread: Starting localisation\n");
                m_pHandLocalisation->LocaliseHand(m_pNewCamImageLeft,
                                                  m_pNewCamImageRight,
                                                  m_pSensorConfig,
                                                  m_pEstimatedConfig,
                                                  m_dConfidenceRating);
                m_bHasResult = true;
                m_bIsProcessing = false;
                m_bReadyForLocalising = false;
                printf("HandLocThread: Confidence %f\n", m_dConfidenceRating);
                printf("HandLocalisationThread: Localisation finished\n");
            }
            else
            {
                Threading::SleepThread(10);
            }
        }
 
        return 0;
    }
} // namespace visionx