KinectAndCameraCalibration.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 "KinectAndCameraCalibration.h"
 
// VisionX
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
 
// IVT
#include "Image/ByteImage.h"
#include "Image/ImageProcessor.h"
#include "Image/IplImageAdaptor.h"
#include "Calibration/StereoCalibration.h"
#include "Calibration/Calibration.h"
 
#include <RobotAPI/libraries/core/Pose.h>
#include <opencv2/opencv.hpp>
#include <sys/time.h>
 
 
namespace visionx
{
 
    void KinectAndCameraCalibration::onInitImageProcessor()
    {
        // set desired image provider
        cameraImageProviderName = getProperty<std::string>("CameraImageProviderAdapterName").getValue();
        usingImageProvider(cameraImageProviderName);
 
        kinectImageProviderName = getProperty<std::string>("KinectImageProviderAdapterName").getValue();
        usingImageProvider(kinectImageProviderName);
 
        const int cornersPerRow = getProperty<int>("NumberOfRows").getValue() - 1;
        const int cornersPerColumn = getProperty<int>("NumberOfColumns").getValue() - 1;
        const double squareSize = getProperty<double>("PatternSquareSize").getValue();
 
        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;
        etalonParams[0] = double(cornersPerColumn + 1);
        etalonParams[1] = double(cornersPerRow + 1);
        etalonParams[2] = squareSize;
        m_pCalibFilter->SetCameraCount(2);
        m_pCalibFilter->SetFrames(desiredNumberOfImages);
        m_pCalibFilter->SetEtalon(CV_CALIB_ETALON_CHESSBOARD, etalonParams);
 
        offeringTopic("KinectAndCameraCalibration");
    }
 
    void KinectAndCameraCalibration::onConnectImageProcessor()
    {
        // connect to image provider
        ARMARX_INFO << getName() << " connecting to " << cameraImageProviderName;
        visionx::ImageProviderInfo imageProviderInfo = getImageProvider(cameraImageProviderName);
        cameraImageProviderPrx = getProxy<ImageProviderInterfacePrx>(cameraImageProviderName);
        listenerPrx = getTopic<KinectAndCameraCalibrationListenerPrx>("KinectAndCameraCalibration");
 
        cameraImages = new CByteImage*[2];
        cameraImages[0] = tools::createByteImage(imageProviderInfo);
        cameraImages[1] = tools::createByteImage(imageProviderInfo);
 
        ARMARX_INFO << getName() << " connecting to " << kinectImageProviderName;
        getImageProvider(kinectImageProviderName);
        kinectImageProviderPrx = getProxy<ImageProviderInterfacePrx>(kinectImageProviderName);
        kinectImages = new CByteImage*[kinectImageProviderPrx->getNumberImages()];
 
        for (int i = 0; i < kinectImageProviderPrx->getNumberImages(); i++)
        {
            kinectImages[i] = tools::createByteImage(kinectImageProviderPrx->getImageFormat(), kinectImageProviderPrx->getImageFormat().type);
        }
 
        startingTime = IceUtil::Time::now();
        timeOfLastCapture = IceUtil::Time::now();
 
        finished = false;
        requested = false;
        result.result = false;
    }
 
    void KinectAndCameraCalibration::onExitImageProcessor()
    {
        finished = true;
        requested = false;
        result.result = false;
 
        usleep(100000);
        delete cameraImages[0];
        delete cameraImages[1];
        delete[] cameraImages;
        delete m_pCalibFilter;
    }
 
    void KinectAndCameraCalibration::process()
    {
        if (requested)
        {
            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)
            {
                // get images
                armarx::MetaInfoSizeBasePtr info;
                int nNumberCameraImages = getImages(cameraImageProviderName, cameraImages, info);
                ARMARX_VERBOSE << getName() << " got " << nNumberCameraImages << " camera images";
 
                armarx::MetaInfoSizeBasePtr info2;
                int nNumberKinectImages = getImages(kinectImageProviderName, kinectImages, info2);
                ARMARX_VERBOSE << getName() << " got " << nNumberKinectImages << " kinect images";
 
                if (nNumberCameraImages > 0 && nNumberKinectImages > 0)
                {
                    if ((IceUtil::Time::now() - timeOfLastCapture).toMilliSeconds() > waitingIntervalBetweenImages)
                    {
                        ARMARX_INFO << "Capturing image " << numberOfCapturedImages + 1 << " of " << desiredNumberOfImages;
                        IplImage* ppIplImages[2] = { IplImageAdaptor::Adapt(cameraImages[0]), IplImageAdaptor::Adapt(kinectImages[0]) };
 
                        if (m_pCalibFilter->FindEtalon(ppIplImages))
                        {
                            ARMARX_INFO << "Found calibration pattern";
                            numberOfCapturedImages++;
                            timeOfLastCapture = IceUtil::Time::now();
                            m_pCalibFilter->DrawPoints(ppIplImages);
                            m_pCalibFilter->Push();
 
                            if (numberOfCapturedImages == desiredNumberOfImages)
                            {
                                ARMARX_IMPORTANT << "Calculating calibration";
                                usleep(10000);
 
                                if (m_pCalibFilter->IsCalibrated())
                                {
                                    ARMARX_INFO << "Saving camera calibration to file " << m_sCameraParameterFileName;
                                    m_pCalibFilter->SaveCameraParams(m_sCameraParameterFileName.c_str());
                                    CStereoCalibration calibration;
                                    calibration.LoadCameraParameters(m_sCameraParameterFileName.c_str());
                                    CCalibration cRight = *calibration.GetRightCalibration();
                                    Vec3d translation = cRight.GetCameraParameters().translation;
                                    ARMARX_INFO << "RightCalibration traslation; " << translation.x << " " << translation.y << " " << translation.z;
                                    Mat3d rotation = cRight.GetCameraParameters().rotation;
                                    ARMARX_INFO << "RightCalibration rotation; "
                                                << rotation.r1 << " " << rotation.r2 << " " << rotation.r3 << " \n"
                                                << rotation.r4 << " " << rotation.r5 << " " << rotation.r6 << " \n"
                                                << rotation.r7 << " " << rotation.r8 << " " << rotation.r9 << " \n";
 
 
                                    CCalibration cLeft = *calibration.GetLeftCalibration();
                                    Vec3d translationLeft = cLeft.GetCameraParameters().translation;
                                    ARMARX_INFO << "LeftCalibration traslation; " << translationLeft.x << " " << translationLeft.y << " " << translationLeft.z;
                                    Mat3d rotationLeft = cLeft.GetCameraParameters().rotation;
                                    ARMARX_INFO << "LeftCalibration rotation; "
                                                << rotationLeft.r1 << " " << rotationLeft.r2 << " " << rotationLeft.r3 << " \n"
                                                << rotationLeft.r4 << " " << rotationLeft.r5 << " " << rotationLeft.r6 << " \n"
                                                << rotationLeft.r7 << " " << rotationLeft.r8 << " " << rotationLeft.r9 << " \n";
 
                                    finished = true;
                                    result.result = finished;
                                    result.transformation = new armarx::Pose(tools::convertIVTtoEigen(rotation), tools::convertIVTtoEigen(translation));
 
                                    listenerPrx->reportCalibrationFinished(result.result);
                                    ARMARX_IMPORTANT << "Calibration finished";
                                }
                            }
                        }
                    }
                    else
                    {
                        usleep(10000);
                    }
                }
                else
                {
                    usleep(100000);
                }
            }
        }
        else
        {
            usleep(100000);
            startingTime = IceUtil::Time::now();
        }
    }
 
 
    void KinectAndCameraCalibration::startCalibration(const ::Ice::Current& c)
    {
        requested = true;
        startingTime = IceUtil::Time::now();
    }
 
    void KinectAndCameraCalibration::stopCalibration(const ::Ice::Current& c)
    {
        requested = false;
        usleep(10000);
 
        numberOfCapturedImages = 0;
        delete m_pCalibFilter;
        m_pCalibFilter = new CvCalibFilter();
        m_pCalibFilter->SetCameraCount(2);
        m_pCalibFilter->SetFrames(desiredNumberOfImages);
        m_pCalibFilter->SetEtalon(CV_CALIB_ETALON_CHESSBOARD, etalonParams);
 
        finished = false;
        result.result = false;
 
        startingTime = IceUtil::Time::now();
    }
 
    KinectPoseCalibration KinectAndCameraCalibration::getCalibrationParameters(const ::Ice::Current& c)
    {
        return result;
    }
}