ImageProviderDynamicSimulation.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2013-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     Nikolaus Vahrenkamp
 * @date       2014
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include <exception>
 
#include <Ice/Properties.h>
 
#include "ArmarXCore/core/exceptions/local/ExpressionException.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/core/system/ArmarXDataPath.h>
 
// VisionXCore
#include <VisionX/core/exceptions/user/StartingCaptureFailedException.h>
#include <VisionX/core/exceptions/user/FrameRateNotSupportedException.h>
 
// VisionXTools
#include <VisionX/tools/TypeMapping.h>
#include <VisionX/tools/ImageUtil.h>
#include <VisionX/tools/exceptions/local/InvalidFrameRateException.h>
 
#include "ImageProviderDynamicSimulation.h"
 
#include <Calibration/Calibration.h>
#include <Inventor/SoInteraction.h>
#include <Inventor/nodes/SoDirectionalLight.h>
#include <Inventor/nodes/SoUnits.h>
#include <Math/Math3d.h>
 
 
namespace armarx
{
    void ImageProviderDynamicSimulation::onInitCapturingImageProvider()
    {
        // init SoDB / Coin3D
        //SoDB::init();
 
        ARMARX_INFO << "1";
 
        VirtualRobot::init(this->getName());
 
        // needed for SoSelection
        SoInteraction::init();
 
        // image format
        setImageSyncMode(visionx::eFpsSynchronization);
        frameRate = getProperty<float>("FrameRate").getValue();
 
        std::string calibrationFileName = getProperty<std::string>("CalibrationFile").getValue();
        ARMARX_VERBOSE << "Camera calibration file: " << calibrationFileName;
 
        if (!calibrationFileName.empty())
        {
            std::string fullCalibrationFileName;
 
            if (!ArmarXDataPath::getAbsolutePath(calibrationFileName, fullCalibrationFileName))
            {
                ARMARX_ERROR << "Could not find camera calibration file in ArmarXDataPath: " << calibrationFileName;
            }
 
            setlocale(LC_NUMERIC, "C");
 
            CStereoCalibration ivtStereoCalibration;
 
            if (!ivtStereoCalibration.LoadCameraParameters(fullCalibrationFileName.c_str(), true))
            {
                ARMARX_ERROR << "Error loading camera calibration file: " << fullCalibrationFileName;
            }
            stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
 
            renderImg_width = stereoCalibration->calibrationLeft.cameraParam.width;
            renderImg_height = stereoCalibration->calibrationLeft.cameraParam.height;
 
            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.height, renderImg_height);
            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.height, renderImg_height);
            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationLeft.cameraParam.width, renderImg_width);
            ARMARX_CHECK_EQUAL(stereoCalibration->calibrationRight.cameraParam.width, renderImg_width);
        }
        else{
            const visionx::ImageDimension dimensions = getProperty<visionx::ImageDimension>("ImageSize").getValue();
 
            renderImg_width =  dimensions.width;
            renderImg_height = dimensions.height;
        }
 
        setImageFormat(visionx::ImageDimension{renderImg_width, renderImg_height}, visionx::eRgb);
 
        renderImg_bpp = 3;
       
 
        setNumberImages(2);
        images = new CByteImage*[getNumberImages()];
 
        for (int i = 0; i < getNumberImages(); i++)
        {
            images[i] = new CByteImage(renderImg_width, renderImg_height, CByteImage::eRGB24);
        }
 
        resizedImages = new CByteImage*[getNumberImages()];
 
        for (int i = 0 ; i < getNumberImages() ; i++)
        {
            resizedImages[i] = visionx::tools::createByteImage(getImageFormat(), getImageFormat().type);
        }
 
 
 
 
       
        std::stringstream svName;
        svName << getName() << "_PhysicsWorldVisualization";
        simVisu = Component::create<ArmarXPhysicsWorldVisualization>(getIceProperties(), svName.str());
        getArmarXManager()->addObject(simVisu);
    }
 
 
    void ImageProviderDynamicSimulation::onExitCapturingImageProvider()
    {
        if (images != NULL)
        {
            for (int i = 0; i < getNumberImages(); i++)
            {
                delete images[i];
                delete resizedImages[i];
            }
 
            delete [] images;
            delete [] resizedImages;
 
            images = NULL;
        }
 
        rendererLeft.reset();
        rendererRight.reset();
 
        if (simVisu)
        {
            simVisu->releaseResources();
            getArmarXManager()->removeObjectBlocking(simVisu);
        }
 
        simVisu = NULL;
 
        SoDB::finish();
    }
 
 
    void ImageProviderDynamicSimulation::onStartCapture(float frameRate)
    {
        ARMARX_VERBOSE << "start capture";
 
        // check nodes
        robotName = getProperty<std::string>("RobotName").getValue();
        leftNodeName = getProperty<std::string>("RobotNodeLeftCamera").getValue();
        rightNodeName = getProperty<std::string>("RobotNodeRightCamera").getValue();
        focalLength = getProperty<float>("focalLength").getValue();
 
        // ensure that all data is loaded
        if (simVisu)
        {
            simVisu->synchronizeVisualizationData();
        }
        while (!simVisu->getRobot(robotName))
        {
            usleep(100000);
            simVisu->synchronizeVisualizationData();
            ARMARX_INFO << deactivateSpam(3) << "Waiting for visu";
        }
        setupCameraRendering(robotName, leftNodeName, rightNodeName);
 
    }
 
 
    bool ImageProviderDynamicSimulation::setupCameraRendering(const std::string& robotName, const std::string& cameraSensorNameLeft, const std::string& cameraSensorNameRight)
    {
        std::unique_lock lock(captureMutex);
        auto l = simVisu->getScopedLock();
 
        rendererLeft.reset(VirtualRobot::CoinVisualizationFactory::createOffscreenRenderer(renderImg_width, renderImg_height));
        rendererRight.reset(VirtualRobot::CoinVisualizationFactory::createOffscreenRenderer(renderImg_width, renderImg_height));
 
        // some checks...
        if (!simVisu || !simVisu->getVisualization())
        {
            ARMARX_ERROR << "No physics visualization scene";
            return false;
        }
 
        return true;
    }
 
    static void copyRenderBufferToByteImage(CByteImage* image, std::uint8_t* renderBuffer)
    {
        int height = image->height;
        int width = image->width;
        std::uint8_t* pixelsRow = image->pixels;
        std::uint8_t* renderBufferEndOfRow = renderBuffer + 3 * (width - 1);
        for (int y = 0; y < height; y++)
        {
            for (int x = 0; x < width; x++)
            {
                pixelsRow[x * 3 + 0] = renderBufferEndOfRow[-x * 3 + 0];
                pixelsRow[x * 3 + 1] = renderBufferEndOfRow[-x * 3 + 1];
                pixelsRow[x * 3 + 2] = renderBufferEndOfRow[-x * 3 + 2];
            }
            pixelsRow += width * 3;
            renderBufferEndOfRow += width * 3;
        }
    }
 
 
    bool ImageProviderDynamicSimulation::updateCameraRendering()
    {
        auto l = simVisu->getScopedLock();
 
        if (!simVisu || !simVisu->getRobot(robotName))
        {
            ARMARX_WARNING << deactivateSpam() << " no visu or no robot";
            usleep(100000);
            return false;
        }
 
        unsigned char* renderBuffer = NULL;
        bool renderOK = false;
 
 
        cameraNodeL = simVisu->getRobot(robotName)->getRobotNode(leftNodeName);
        cameraNodeR = simVisu->getRobot(robotName)->getRobotNode(rightNodeName);
 
        if (!rendererLeft || !cameraNodeL)
        {
            ARMARX_ERROR << deactivateSpam() << "No renderer Left, node:" << leftNodeName;
            return false;
        }
 
        if (!rendererRight || !cameraNodeR)
        {
            ARMARX_ERROR << deactivateSpam() << "No renderer Right, node: " << rightNodeName;
            return false;
        }
 
 
        if(not stereoCalibration.has_value())
        {
            initStereoCalib();
        }
 
        float fovLeft = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationLeft.cameraParam.focalLength[1]));
        float fovRight = 2.0 * std::atan(renderImg_height / (2.0 * stereoCalibration->calibrationRight.cameraParam.focalLength[1]));
        // ARMARX_LOG << "fov left: " << (fovLeft / M_PI * 180.0) << " fov right: " << (fovRight / M_PI * 180.0);
 
 
 
        // render Left Camera
#if 1
        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererLeft.get(), cameraNodeL, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovLeft);
#else
        //////////////// optional: we render by our own
        SoPerspectiveCamera* cam = new SoPerspectiveCamera();
        cam->ref();
        // set camera position and orientation
        Eigen::Matrix4f camPose = cameraNodeL->getGlobalPose();
        Eigen::Vector3f camPos = MathTools::getTranslation(camPose);
        float sc = 0.001f; // to m
        cam->position.setValue(camPos[0]*sc, camPos[1]*sc, camPos[2]*sc);
 
        SbRotation align(SbVec3f(1, 0, 0), (float)(M_PI)); // first align from  default direction -z to +z by rotating with 180 degree around x axis
        SbRotation align2(SbVec3f(0, 0, 1), (float)(-M_PI / 2.0)); // align up vector by rotating with -90 degree around z axis
        SbRotation trans(CoinVisualizationFactory::getSbMatrix(camPose)); // get rotation from global pose
        cam->orientation.setValue(align2 * align * trans); // perform total transformation
 
        // 10cm to 100m
        cam->nearDistance.setValue(0.01f);
        cam->farDistance.setValue(10000.0f);
 
        // add all to a inventor scene graph
        SoSeparator* root = new SoSeparator();
        root->ref();
        SoDirectionalLight* light = new SoDirectionalLight;
        root->addChild(light);
 
        // easy light model, no shadows or something
        //SoLightModel *lightModel = new SoLightModel();
        //lightModel->model = SoLightModel::BASE_COLOR;
        //root->addChild(lightModel);
 
        root->addChild(cam);
        root->addChild(simVisu->getVisualization());
 
        renderOK = rendererLeft.get()->render(root) == TRUE ? true : false;
        root->unref();
 
        // get render buffer pointer
        renderBuffer = rendererLeft.get()->getBuffer();
        cam->unref();
 
#endif
 
 
        if (renderOK && renderBuffer != NULL)
        {
            copyRenderBufferToByteImage(images[0], renderBuffer);
        }
 
 
        // render Right Camera
        renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(rendererRight.get(), cameraNodeR, simVisu->getVisualization(), &renderBuffer, 100.0f, 100000.0f, fovRight);
 
        if (renderOK && renderBuffer != NULL)
        {
            copyRenderBufferToByteImage(images[1], renderBuffer);
        }
 
        return true;
    }
 
    void ImageProviderDynamicSimulation::onStopCapture()
    {
        ARMARX_VERBOSE << "stop capture";
    }
 
    bool ImageProviderDynamicSimulation::capture(void** ppImageBuffers)
    {
        bool succeeded = true;
 
        try
        {
            simVisu->synchronizeVisualizationData();
 
            {
                std::unique_lock lock(captureMutex);
                succeeded = updateCameraRendering();
            }
 
        }
        catch (...)
        {
            handleExceptions();
        }
 
        if (succeeded)
        {
            try
            {
                if (!sharedMemoryProvider)
                {
                    ARMARX_WARNING << "Shared memory provider is null (possibly shutting down)";
                    return false;
                }
                armarx::SharedMemoryScopedWriteLockPtr lock = getScopedWriteLock();
 
                for (int i = 0; i < getNumberImages(); i++)
                {
                    ::ImageProcessor::Resize(images[i], resizedImages[i]);
 
                    memcpy(ppImageBuffers[i],
                           resizedImages[i]->pixels,
                           getImageFormat().dimension.width * getImageFormat().dimension.height * getImageFormat().bytesPerPixel);
                }
            }
            catch (...)
            {
                handleExceptions();
            }
        }
 
 
        return succeeded;
    }
 
 
    void ImageProviderDynamicSimulation::initStereoCalib()
    {
        
 
        CCalibration leftCalibration;
        leftCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, Math3d::zero_vec, renderImg_width, renderImg_height);
        
        ARMARX_CHECK_NOT_NULL(simVisu);
        while (!simVisu->getRobot(robotName))
        {
            usleep(100000);
            simVisu->synchronizeVisualizationData();
            ARMARX_INFO << deactivateSpam(3) << "Waiting for visu";
        }
 
        ARMARX_CHECK(simVisu->synchronizeVisualizationData());
        auto robot = simVisu->getRobot(robotName);
        ARMARX_CHECK_NOT_NULL(robot);
 
        cameraNodeL = robot->getRobotNode(leftNodeName);
        cameraNodeR = robot->getRobotNode(rightNodeName);
 
        ARMARX_CHECK_NOT_NULL(cameraNodeL);
        ARMARX_CHECK_NOT_NULL(cameraNodeR);
 
        const Eigen::Vector3f left_P_right = cameraNodeR->getPositionInFrame(cameraNodeL);
        ARMARX_IMPORTANT << VAROUT(left_P_right);
 
        CCalibration rightCalibration;
        Vec3d transRight{-left_P_right.x(), 0, 0};
        rightCalibration.SetCameraParameters(focalLength,focalLength, renderImg_width / 2, renderImg_height / 2, 0,0,0,0, Math3d::unit_mat, transRight, renderImg_width, renderImg_height);
 
        CStereoCalibration ivtStereoCalibration;
        ivtStereoCalibration.SetSingleCalibrations(leftCalibration, rightCalibration);
        stereoCalibration = visionx::tools::convert(ivtStereoCalibration);
    }
 
    visionx::StereoCalibration ImageProviderDynamicSimulation::getStereoCalibration(const Ice::Current& c)
    {
        if(not stereoCalibration.has_value())
        {
            initStereoCalib();
        }
 
        return stereoCalibration.value();
    }
 
    bool ImageProviderDynamicSimulation::getImagesAreUndistorted(const Ice::Current& c)
    {
        return true;
    }
 
    std::string ImageProviderDynamicSimulation::getReferenceFrame(const Ice::Current&)
    {
        return getProperty<std::string>("ReferenceFrame").getValue();
    }
 
}