CalibrationCreator.cpp

Go to the documentation of this file.

/*
* This file is part of ArmarX.
*
* 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    VisionX::Component
* @author     David Schiebener <schiebener at kit dot edu>
* @copyright  2014 Humanoids Group, HIS, KIT
* @license    http://www.gnu.org/licenses/gpl-2.0.txt
*             GNU General Public License
*/
 
#include "CalibrationCreator.h"
 
// VisionX
//#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
 
// IVT
#include <sys/time.h>
 
#include <opencv2/opencv.hpp>
 
#include "Image/ByteImage.h"
#include "Image/ImageProcessor.h"
#include "Image/IplImageAdaptor.h"
 
namespace visionx
{
 
    void
    CalibrationCreator::onInitImageProcessor()
    {
        // set desired image provider
        providerName = getProperty<std::string>("ImageProviderAdapterName").getValue();
        usingImageProvider(providerName);
 
        const int cornersPerRow = getProperty<int>("NumberOfRows").getValue() - 1;
        const int cornersPerColumn = getProperty<int>("NumberOfColumns").getValue() - 1;
        const int cornersPerImage = cornersPerColumn * cornersPerRow;
        const double squareSize = getProperty<double>("PatternSquareSize").getValue();
 
        m_pCorners2DFloat = new CvPoint2D32f[cornersPerImage];
        m_pCorners2D = new CvPoint2D64f[cornersPerImage];
        m_pCorners3D = new CvPoint3D64f[cornersPerImage];
 
        waitingIntervalBetweenImages = getProperty<int>("WaitingIntervalBetweenImages").getValue();
        m_sCameraParameterFileName = getProperty<std::string>("OutputFileName").getValue();
        desiredNumberOfImages = getProperty<int>("NumberOfImages").getValue();
        numberOfCapturedImages = 0;
 
        // initialize calibration filter
        m_pCalibFilter = new CvCalibFilter();
        ARMARX_INFO << "Calibration pattern with " << cornersPerColumn + 1 << " x "
                    << cornersPerRow + 1 << " squares of size " << squareSize;
        double etalonParams[3] = {
            double(cornersPerColumn + 1), double(cornersPerRow + 1), squareSize};
        m_pCalibFilter->SetCameraCount(2);
        m_pCalibFilter->SetFrames(desiredNumberOfImages);
        m_pCalibFilter->SetEtalon(CV_CALIB_ETALON_CHESSBOARD, etalonParams);
    }
 
    void
    CalibrationCreator::onConnectImageProcessor()
    {
        // connect to image provider
        ARMARX_INFO << getName() << " connecting to " << providerName;
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(providerName);
        imageProviderPrx = getProxy<ImageProviderInterfacePrx>(providerName);
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        startingTime = IceUtil::Time::now();
        timeOfLastCapture = IceUtil::Time::now();
        finished = false;
 
 
        enableResultImages(
            2, imageProviderInfo.imageFormat.dimension, imageProviderInfo.imageFormat.type);
    }
 
    void
    CalibrationCreator::onExitImageProcessor()
    {
        finished = true;
        usleep(100000);
        delete cameraImages[0];
        delete cameraImages[1];
        delete[] cameraImages;
        delete m_pCalibFilter;
        delete[] m_pCorners3D;
        delete[] m_pCorners2D;
        delete[] m_pCorners2DFloat;
    }
 
    void
    CalibrationCreator::process()
    {
        long timeSinceStart = (IceUtil::Time::now() - startingTime).toMilliSeconds();
 
        if (timeSinceStart < 10000)
        {
            ARMARX_VERBOSE << "Time until start of capture: " << timeSinceStart - 10000 << " ms";
            usleep(100000);
        }
        else if (!finished)
        {
            if (!waitForImages(8000))
            {
                ARMARX_WARNING << "Timeout or error in wait for images";
            }
            else
            {
                // get images
                int nNumberImages = getImages(cameraImages);
                ARMARX_VERBOSE << getName() << " got " << nNumberImages << " images";
 
                if ((IceUtil::Time::now() - timeOfLastCapture).toMilliSeconds() >
                    waitingIntervalBetweenImages)
                {
                    ARMARX_INFO << "Capturing image " << numberOfCapturedImages + 1 << " of "
                                << desiredNumberOfImages;
                    IplImage* ppIplImages[2] = {IplImageAdaptor::Adapt(cameraImages[0]),
                                                IplImageAdaptor::Adapt(cameraImages[1])};
 
                    if (m_pCalibFilter->FindEtalon(ppIplImages))
                    {
                        ARMARX_INFO << "Found calibration pattern";
                        numberOfCapturedImages++;
                        m_pCalibFilter->DrawPoints(ppIplImages);
                        m_pCalibFilter->Push();
                        timeOfLastCapture = IceUtil::Time::now();
 
                        if (numberOfCapturedImages == desiredNumberOfImages)
                        {
                            ARMARX_IMPORTANT << "Calculating calibration";
 
                            if (m_pCalibFilter->IsCalibrated())
                            {
                                ARMARX_INFO << "Saving camera calibration to file "
                                            << m_sCameraParameterFileName;
                                m_pCalibFilter->SaveCameraParams(
                                    m_sCameraParameterFileName.c_str());
                                finished = true;
                                ARMARX_IMPORTANT << "Calibration finished";
                            }
                        }
                    }
                    provideResultImages(cameraImages);
                }
                else
                {
                    usleep(10000);
                }
            }
        }
    }
} // namespace visionx