MaskRobotInImage.cpp

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * Copyright (C) 2011-2018, 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    VisionX
 * @author     Julian Zimmer ( urdbu at student dot kit dot edu )
 * @date       2018
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#include "MaskRobotInImage.h"
 
#include <inttypes.h>
 
#include <VirtualRobot/CollisionDetection/CollisionModel.h>
 
#include <ArmarXCore/core/system/cmake/CMakePackageFinder.h>
 
#include <RobotAPI/libraries/core/remoterobot/RemoteRobot.h>
 
#include <Inventor/SoOffscreenRenderer.h>
 
 
using namespace armarx;
 
MaskRobotInImage::MaskRobotInImage(RobotStateComponentInterfacePrx robotStateComponent,
                                   std::string cameraFrameName,
                                   visionx::ImageFormatInfo imageFormat,
                                   float fov,
                                   SbColor backgroundColor,
                                   bool flipImages,
                                   bool useFullModel,
                                   float collisionModelInflationMargin)
{
    VirtualRobot::init("MaskRobotInImage");
    // needed for SoSelection
    SoInteraction::init();
 
    this->cameraFrameName = cameraFrameName;
    this->fov = fov;
    this->imageWidth = imageFormat.dimension.width;
    this->imageHeight = imageFormat.dimension.height;
    this->backgroundColor = backgroundColor;
    this->flipImages = flipImages;
    this->useFullModel = useFullModel;
    this->collisionModelInflationMargin = collisionModelInflationMargin;
 
 
    renderedImage = new CByteImage(imageWidth, imageHeight, CByteImage::eRGB24);
    maskImage = new CByteImage(imageWidth, imageHeight, CByteImage::eGrayScale);
 
    //get the local robot
    ARMARX_CHECK_EXPRESSION(robotStateComponent);
 
    this->robotStateComponent = robotStateComponent;
 
    localRobot = RemoteRobot::createLocalCloneFromFile(
        robotStateComponent, VirtualRobot::RobotIO::RobotDescription::eFull);
 
 
    //create the offscreen renderer
    renderer.reset(
        VirtualRobot::CoinVisualizationFactory::createOffscreenRenderer(imageWidth, imageHeight));
    renderBuffer.resize(imageWidth * imageHeight * 3);
 
 
    renderer->setBackgroundColor(backgroundColor);
 
 
    if (!useFullModel)
    {
        if (collisionModelInflationMargin > 0.f)
        {
            //inflate the collision model
            for (auto& model : localRobot->getCollisionModels())
            {
                model->setUpdateVisualization(true);
                model->inflateModel(collisionModelInflationMargin);
                ARMARX_INFO << "MARGIN AFTER INITIALIZATION: " << model->getMargin();
            }
        }
    }
    else
    {
        if (collisionModelInflationMargin > 0.f)
        {
            ARMARX_IMPORTANT << "Inflating collisionModel but not rendering collisionModel!";
        }
    }
 
 
    //ARMARX_INFO << "HASTIMESERVER: " << TimeUtil::HasTimeServer();
 
 
    ARMARX_INFO << "RENDERER BACKGROUND COLOR IS: " << renderer->getBackgroundColor().getValue()[0]
                << ";" << renderer->getBackgroundColor().getValue()[1] << ";"
                << renderer->getBackgroundColor().getValue()[2] << "]";
}
 
MaskRobotInImage::~MaskRobotInImage()
{
}
 
CByteImage*
MaskRobotInImage::getMaskImage(Ice::Long timestamp)
{
    ARMARX_CHECK_EXPRESSION(robotStateComponent);
    //synchronize the localRobot
    RemoteRobot::synchronizeLocalCloneToTimestamp(localRobot, robotStateComponent, timestamp);
    ARMARX_CHECK_EXPRESSION(localRobot);
    //get the cameraNode
    VirtualRobot::RobotNodePtr cameraNode = localRobot->getRobotNode(cameraFrameName);
    ARMARX_CHECK_EXPRESSION(cameraNode);
 
 
    //get the visualization
    VirtualRobot::SceneObject::VisualizationType visuType = VirtualRobot::SceneObject::Collision;
 
    if (useFullModel)
    {
        visuType = VirtualRobot::SceneObject::Full;
    }
 
 
    VirtualRobot::CoinVisualizationPtr visualization = localRobot->getVisualization(visuType);
    ARMARX_CHECK_EXPRESSION(visualization);
 
 
    SoNode* visualisationNode = NULL;
    ARMARX_CHECK_EXPRESSION(visualization);
 
    visualisationNode = visualization->getCoinVisualization();
 
    ARMARX_CHECK_EXPRESSION(renderer);
 
 
    //feed it into the renderer
    bool renderOK = false;
 
    auto bufferPtr = renderBuffer.data();
 
 
    //increasing the zNear drawing distance, because the DepthCameraSim camera node is inside the depth camera model
    renderOK = VirtualRobot::CoinVisualizationFactory::renderOffscreen(
        renderer.get(), cameraNode, visualisationNode, &bufferPtr, 20.0f, 100000.0f, fov);
 
    //Depending on the pose of the camera node, the image should be flipped upside down
    //If the nodes like EyeLeftCameraSim or DepthCameraSim are used, this has to be performed,
    //since the orientation of these nodes is flipped in comparison to the orientation expected by the offscreen renderer
 
    //copy rendering into image and create the image mask
    unsigned char* pRenderedImage = renderer->getBuffer();
    if (renderOK)
    {
        std::uint8_t* pixelsRow = renderedImage->pixels;
        std::uint8_t* maskPixelsRow = maskImage->pixels;
        for (int y = 0; y < imageHeight; y++)
        {
            for (int x = 0; x < imageWidth; x++)
            {
                int adjustedX = x;
                int adjustedY = y;
                if (flipImages)
                {
                    adjustedX = imageWidth - 1 - x;
                    adjustedY = imageHeight - 1 - y;
                }
 
 
                pixelsRow[x * 3 + 0] =
                    pRenderedImage[3 * (imageWidth * (imageHeight - adjustedY) + adjustedX) + 0];
                pixelsRow[x * 3 + 1] =
                    pRenderedImage[3 * (imageWidth * (imageHeight - adjustedY) + adjustedX) + 1];
                pixelsRow[x * 3 + 2] =
                    pRenderedImage[3 * (imageWidth * (imageHeight - adjustedY) + adjustedX) + 2];
 
                //set the masking pixel to 255 if the background color is in the image pixel, else set it to 0
                SbColor pixelColor(((float)pixelsRow[x * 3 + 0]) / 255.f,
                                   ((float)pixelsRow[x * 3 + 1]) / 255.f,
                                   ((float)pixelsRow[x * 3 + 2]) / 255.f);
                if (backgroundColor.equals(pixelColor, 0.004f))
                {
                    maskPixelsRow[x] = 0;
                }
                else
                {
                    maskPixelsRow[x] = 255;
                }
            }
            pixelsRow += imageWidth * 3;
            maskPixelsRow += imageWidth;
        }
    }
 
 
    return maskImage;
}
 
void
MaskRobotInImage::setBackgroundColor(SbColor newBackgroundColor)
{
    backgroundColor = newBackgroundColor;
    renderer->setBackgroundColor(newBackgroundColor);
}