HandModelVisualizer.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
 */
#include "HandModelVisualizer.h"
 
#include <ArmarXCore/core/logging/Logging.h>
 
#include <Image/ImageProcessor.h>
 
namespace visionx
{
    //**********************************************************************************************
    //      Constructor / destructor
    //**********************************************************************************************
 
 
    // constructor
    CHandModelVisualizer::CHandModelVisualizer(CStereoCalibration* pCalibration,
                                               bool bUseLeftCamera)
    {
#ifdef DSHT_USE_ICUB
        m_pHandModelV2 = new HandModeliCub(DSHT_HAND_MODEL_PATH, pCalibration);
#else
        m_pHandModelV2 = new CHandModelV2(DSHT_HAND_MODEL_PATH, pCalibration);
#endif
 
        if (bUseLeftCamera)
        {
            m_fFocalLengthX =
                pCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.x;
            m_fFocalLengthY =
                pCalibration->GetLeftCalibration()->GetCameraParameters().focalLength.y;
            m_fPrincipalPointX =
                pCalibration->GetLeftCalibration()->GetCameraParameters().principalPoint.x;
            m_fPrincipalPointY =
                pCalibration->GetLeftCalibration()->GetCameraParameters().principalPoint.y;
        }
        else
        {
            m_fFocalLengthX =
                pCalibration->GetRightCalibration()->GetCameraParameters().focalLength.x;
            m_fFocalLengthY =
                pCalibration->GetRightCalibration()->GetCameraParameters().focalLength.y;
            m_fPrincipalPointX =
                pCalibration->GetRightCalibration()->GetCameraParameters().principalPoint.x;
            m_fPrincipalPointY =
                pCalibration->GetRightCalibration()->GetCameraParameters().principalPoint.y;
        }
 
        m_pStereoCalibration = pCalibration;
 
        m_pMoveMaster = new CMoveMasterModel();
        if (!m_pMoveMaster->init(DSHT_OI_FILE_PATH, m_fFocalLengthY))
        {
            ARMARX_WARNING_S
                << "CHandModelVisualizer constructor: Could not load OpenInventor-file from "
                << DSHT_OI_FILE_PATH;
            delete m_pMoveMaster;
            return;
        }
    }
 
    // destructor
    CHandModelVisualizer::~CHandModelVisualizer()
    {
        delete m_pMoveMaster;
    }
 
    //**********************************************************************************************
    //      Update hand configuration
    //**********************************************************************************************
 
    void
    CHandModelVisualizer::UpdateHandModel(double* pConfig,
                                          bool bUpdateOpenInventorModel,
                                          bool bDrawCylinderInHand)
    {
        //m_pHandModel->UpdateHand(pConfig);
 
        m_pHandModelV2->UpdateHand(pConfig);
 
        if (bUpdateOpenInventorModel)
        {
            ConfigureMoveMasterModel(pConfig, bDrawCylinderInHand);
        }
    }
 
    void
    CHandModelVisualizer::ConfigureMoveMasterModel(double* pConfig, bool bDrawCylinderInHand)
    {
        /*
            // --- set object cylinder ---
 
            // get positions of palm center, thumb tip, index tip and middle tip
            Vec3d vPalmCenter, vThumbTip, vIndexTip, vMiddleTip, vIndexAndMiddle, tempVec1, tempVec2;
 
            // palm center
            tempVec1.x = 0;
            tempVec1.y = 20;
            tempVec1.z = 0;
            Math3d::AddVecVec(tempVec1, palm_center_translation, tempVec2);
            Math3d::MulMatVec(m_pHandModel->GetHandOrientation(), tempVec2, tempVec1);
            Math3d::AddVecVec(m_pHandModel->GetHandPosition(), tempVec1, vPalmCenter);
 
            // thumb tip
            tempVec1.x = 0;
            tempVec1.y = -20;
            tempVec1.z = 0;
            Math3d::MulMatVec(m_pHandModel->fingers[0]->joint_rot_abs[2], tempVec1, tempVec2);
            Math3d::AddVecVec(m_pHandModel->fingers[0]->joint_pos_abs[2], tempVec2, vThumbTip);
 
            // index tip
            tempVec1.x = 0;
            tempVec1.y = -20;
            tempVec1.z = 0;
            Math3d::MulMatVec(m_pHandModel->fingers[1]->joint_rot_abs[2], tempVec1, tempVec2);
            Math3d::AddVecVec(m_pHandModel->fingers[1]->joint_pos_abs[2], tempVec2, vIndexTip);
 
            // middle tip
            tempVec1.x = 0;
            tempVec1.y = -20;
            tempVec1.z = 0;
            Math3d::MulMatVec(m_pHandModel->fingers[2]->joint_rot_abs[2], tempVec1, tempVec2);
            Math3d::AddVecVec(m_pHandModel->fingers[2]->joint_pos_abs[2], tempVec2, vMiddleTip);
 
            // calc average of index and middle finger tip
            Math3d::AddVecVec(vIndexTip, vMiddleTip, tempVec1);
            Math3d::MulVecScalar(tempVec1, 0.5, vIndexAndMiddle);
 
 
            // calc circumscribed circle of the three points
            ColumnVector vPointA(3), vPointB(3), vPointC(3);
            vPointA << vPalmCenter.x << vPalmCenter.y << vPalmCenter.z;
            vPointB << vThumbTip.x << vThumbTip.y << vThumbTip.z;
            vPointC << vIndexAndMiddle.x << vIndexAndMiddle.y << vIndexAndMiddle.z;
 
            ColumnVector vDistAB(3), vDistAC(3), vDistBC(3);
            vDistAB = vPointB - vPointA;
            vDistAC = vPointC - vPointA;
            vDistBC = vPointC - vPointB;
 
            float alpha = acos( DotProduct(vDistAB, vDistAC) / vDistAB.NormFrobenius() / vDistAC.NormFrobenius() );
            float beta = acos( DotProduct(-vDistAB, vDistBC) / vDistAB.NormFrobenius() / vDistBC.NormFrobenius() );
            float gamma = acos( DotProduct(-vDistBC, -vDistAC) / vDistBC.NormFrobenius() / vDistAC.NormFrobenius() );
 
            // use the barycentric coordinates (wiki: umkreis) to calc the center of the circle
            ColumnVector vCenter(3);
            vCenter = 0.5*(1.0*sin(2*alpha)*vPointA + sin(2*beta)*vPointB + sin(2*gamma)*vPointC);
            vCenter /= 0.5 * ( 1.0*sin(2*alpha) + sin(2*beta) + sin(2*gamma) );
 
            Vec3d vCylinderCenter;
            vCylinderCenter.x = vCenter(1);
            vCylinderCenter.y = vCenter(2);
            vCylinderCenter.z = vCenter(3);
 
            float fRadius = 0.9 * (vCenter-vPointA).NormFrobenius();
            float fHeight = 120;
 
            if (!bDrawCylinderInHand)
            {
                fRadius = 0;
                fHeight = 0;
            }
 
            m_pMoveMaster->setObjCylConfig(vCylinderCenter, -pConfig[3], pConfig[4], -pConfig[5], fHeight, fRadius);
        */
        if (bDrawCylinderInHand)
        {
            printf("Warning: CHandModelVisualizer::ConfigureMoveMasterModel: Cylinder not "
                   "available with new HandmodelV2. Code needs to be changed.\n");
        }
 
        Vec3d vCylinderCenter = {0, 0, -1000};
        m_pMoveMaster->setObjCylConfig(vCylinderCenter, -pConfig[3], pConfig[4], -pConfig[5], 0, 0);
 
 
        // --- set hand configuration ---
 
        Vec3d vConfPos;
        vConfPos.x = pConfig[0];
        vConfPos.y = pConfig[1];
        vConfPos.z = pConfig[2];
 
        m_pMoveMaster->setHandPosition(vConfPos);
 
        Mat3d m0, m1, m2, m3;
        Vec3d vAngles;
        Math3d::SetRotationMat(m0, pConfig[3], pConfig[4], pConfig[5]);
        Math3d::SetRotationMatX(m1, -M_PI / 2);
        Math3d::SetRotationMatZ(m2, M_PI / 2);
        Math3d::MulMatMat(m1, m2, m3);
        Math3d::MulMatMat(m0, m3, m1);
        ExtractAnglesFromRotationMatrix(m1, vAngles);
        m_pMoveMaster->setHandRotation(vAngles.x, vAngles.y, vAngles.z);
        //m_pMoveMaster->setHandRotation(-pConfig[3], pConfig[4], -pConfig[5]);
 
        m_pMoveMaster->setJointAngle(5, pConfig[6]); // finger joints
        m_pMoveMaster->setJointAngle(6, pConfig[9]);
        m_pMoveMaster->setJointAngle(7, pConfig[9]);
        m_pMoveMaster->setJointAngle(8, pConfig[10]);
        m_pMoveMaster->setJointAngle(9, pConfig[10]);
        m_pMoveMaster->setJointAngle(10, pConfig[11]);
        m_pMoveMaster->setJointAngle(11, pConfig[11]);
        m_pMoveMaster->setJointAngle(12, pConfig[11]);
        m_pMoveMaster->setJointAngle(13, pConfig[11]);
        m_pMoveMaster->setJointAngle(14, pConfig[7]);
        m_pMoveMaster->setJointAngle(15, pConfig[8]);
 
        m_pMoveMaster->setJointAngle(16, pConfig[7] / 2); // actuators
        m_pMoveMaster->setJointAngle(17, pConfig[8] / 2);
        m_pMoveMaster->setJointAngle(18, pConfig[9] / 2);
        m_pMoveMaster->setJointAngle(19, pConfig[9] / 2);
        m_pMoveMaster->setJointAngle(20, pConfig[10] / 2);
        m_pMoveMaster->setJointAngle(21, pConfig[10] / 2);
        m_pMoveMaster->setJointAngle(22, pConfig[11] / 2);
        m_pMoveMaster->setJointAngle(23, pConfig[11] / 2);
        m_pMoveMaster->setJointAngle(24, pConfig[11] / 2);
        m_pMoveMaster->setJointAngle(25, pConfig[11] / 2);
 
 
        m_pMoveMaster->update();
    }
 
    //**********************************************************************************************
    //      Visualisations
    //**********************************************************************************************
    void
    CHandModelVisualizer::DrawPolygon(CByteImage* pImage,
                                      ConvexPolygonCalculations::Polygon& pPolygon)
    {
        const int width = pImage->width;
        int leftBorderInt, rightBorderInt, up, down, indexLeft, indexRight, indexMain, nTempOffset;
        float leftBorder = 0, rightBorder = 0, leftOffset = 0, rightOffset = 0;
 
        indexLeft = 1;
        indexRight = 1;
        for (indexMain = 0; indexMain < pPolygon.nCorners - 1; indexMain++)
        {
            if (pPolygon.hull[indexMain].z != 2)
            {
                leftBorder = pPolygon.hullLeft[indexLeft].x;
                leftOffset = (pPolygon.hullLeft[indexLeft + 1].x - pPolygon.hullLeft[indexLeft].x) /
                             (pPolygon.hullLeft[indexLeft + 1].y - pPolygon.hullLeft[indexLeft].y);
            }
            if (pPolygon.hull[indexMain].z != 1)
            {
                rightBorder = pPolygon.hullRight[indexRight].x;
                rightOffset =
                    (pPolygon.hullRight[indexRight + 1].x - pPolygon.hullRight[indexRight].x) /
                    (pPolygon.hullRight[indexRight + 1].y - pPolygon.hullRight[indexRight].y);
            }
            up = (int)pPolygon.hull[indexMain].y;
            down = (int)pPolygon.hull[indexMain + 1].y;
 
            if (pPolygon.hull[indexMain + 1].z != 2)
            {
                indexLeft++;
            }
            if (pPolygon.hull[indexMain + 1].z != 1)
            {
                indexRight++;
            }
 
            if ((down > DSHT_IMAGE_HEIGHT - 1) || (up < 0))
            {
                goto skip_updown; // not in the image
            }
 
            // stay inside image
            if (down < 0)
            {
                down = 0;
            }
            if (up > DSHT_IMAGE_HEIGHT - 1)
            {
                up = DSHT_IMAGE_HEIGHT - 1;
            }
 
            for (int j = up; j > down; j--, leftBorder -= leftOffset, rightBorder -= rightOffset)
            {
                // optim: width/2 sofort dazuaddieren
                leftBorderInt = (int)leftBorder;
                rightBorderInt = (int)rightBorder;
 
                // stay inside image
                if ((leftBorderInt > DSHT_IMAGE_WIDTH - 1) || (rightBorderInt < 0))
                {
                    goto skip_leftright;
                }
 
                if (leftBorderInt < 0)
                {
                    leftBorderInt = 0;
                }
                if (rightBorderInt > DSHT_IMAGE_WIDTH - 1)
                {
                    rightBorderInt = DSHT_IMAGE_WIDTH - 1;
                }
 
                nTempOffset = j * width;
                for (int k = leftBorderInt; k < rightBorderInt; k++)
                {
                    pImage->pixels[3 * (nTempOffset + k) + 0] = 0;
                    pImage->pixels[3 * (nTempOffset + k) + 1] = 0;
                    pImage->pixels[3 * (nTempOffset + k) + 2] = 255;
                }
            skip_leftright:;
            }
        skip_updown:;
        }
    }
 
    void
    CHandModelVisualizer::DrawPolygonGrayscale(CByteImage* pImage,
                                               ConvexPolygonCalculations::Polygon& pPolygon)
    {
        const int width = pImage->width;
        int leftBorderInt, rightBorderInt, up, down, indexLeft, indexRight, indexMain, nTempOffset;
        float leftBorder = 0, rightBorder = 0, leftOffset = 0, rightOffset = 0;
 
        indexLeft = 1;
        indexRight = 1;
        for (indexMain = 0; indexMain < pPolygon.nCorners - 1; indexMain++)
        {
            if (pPolygon.hull[indexMain].z != 2)
            {
                leftBorder = pPolygon.hullLeft[indexLeft].x;
                leftOffset = (pPolygon.hullLeft[indexLeft + 1].x - pPolygon.hullLeft[indexLeft].x) /
                             (pPolygon.hullLeft[indexLeft + 1].y - pPolygon.hullLeft[indexLeft].y);
            }
            if (pPolygon.hull[indexMain].z != 1)
            {
                rightBorder = pPolygon.hullRight[indexRight].x;
                rightOffset =
                    (pPolygon.hullRight[indexRight + 1].x - pPolygon.hullRight[indexRight].x) /
                    (pPolygon.hullRight[indexRight + 1].y - pPolygon.hullRight[indexRight].y);
            }
            up = (int)pPolygon.hull[indexMain].y;
            down = (int)pPolygon.hull[indexMain + 1].y;
 
            if (pPolygon.hull[indexMain + 1].z != 2)
            {
                indexLeft++;
            }
            if (pPolygon.hull[indexMain + 1].z != 1)
            {
                indexRight++;
            }
 
            if ((down > DSHT_IMAGE_HEIGHT - 1) || (up < 0))
            {
                goto skip_updown; // not in the image
            }
 
            // stay inside image
            if (down < 0)
            {
                down = 0;
            }
            if (up > DSHT_IMAGE_HEIGHT - 1)
            {
                up = DSHT_IMAGE_HEIGHT - 1;
            }
 
            for (int j = up; j > down; j--, leftBorder -= leftOffset, rightBorder -= rightOffset)
            {
                // optim: width/2 sofort dazuaddieren
                leftBorderInt = (int)leftBorder;
                rightBorderInt = (int)rightBorder;
 
                // stay inside image
                if ((leftBorderInt > DSHT_IMAGE_WIDTH - 1) || (rightBorderInt < 0))
                {
                    goto skip_leftright;
                }
 
                if (leftBorderInt < 0)
                {
                    leftBorderInt = 0;
                }
                if (rightBorderInt > DSHT_IMAGE_WIDTH - 1)
                {
                    rightBorderInt = DSHT_IMAGE_WIDTH - 1;
                }
 
                nTempOffset = j * width;
                for (int k = leftBorderInt; k < rightBorderInt; k++)
                {
                    pImage->pixels[nTempOffset + k] = 255;
                }
            skip_leftright:;
            }
        skip_updown:;
        }
    }
 
    /*
    void CHandModelVisualizer::DrawFingertips(CByteImage* pImage)
    {
        // draw fingertips
        const int width = pImage->width;
        int leftBorderInt, rightBorderInt, up, down;
 
        int num_polygons = m_pHandModel->GetOverallNumberOfPolygons();
        Polygon* polygons = m_pHandModel->GetProjectedPolygonsLeftCam();
 
        for (int i=0; i<num_polygons; i++)
        {
            DrawPolygon(pImage, polygons[i]);
        }
 
 
 
        // draw circle for tracking ball
        double dCenterX, dCenterY, dRadius;
        m_pHandModel->GetTrackingBallPositionLeftCam(dCenterX, dCenterY, dRadius);
 
        up = (int)(dCenterY-dRadius);
        down = (int)(dCenterY+dRadius);
        for (int i=up; i<=down; i++)
        {
            leftBorderInt = (int)( dCenterX - sqrt(dRadius*dRadius-(i-dCenterY)*(i-dCenterY)) );
            rightBorderInt = (int)( dCenterX + sqrt(dRadius*dRadius-(i-dCenterY)*(i-dCenterY)) );
            for (int j=leftBorderInt; j<=rightBorderInt; j++)
            {
                if (i>=0 && i<DSHT_IMAGE_HEIGHT && j>=0 && j<DSHT_IMAGE_WIDTH)
                {
                    pImage->pixels[3 * (i*width + j) + 0] = 0;
                    pImage->pixels[3 * (i*width + j) + 1] = 255;
                    pImage->pixels[3 * (i*width + j) + 2] = 0;
                }
            }
        }
 
 
        // draw center point of the hand
        Vec3d vHandCenter3D = m_pHandModel->GetHandPosition();
        Vec2d vHandCenter2D;
        m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenter3D, vHandCenter2D);
        //DrawCross(pImage, vHandCenter2D.x, vHandCenter2D.y, 255, 255, 255);
 
 
        // draw coordinate system
        Vec3d vOffset, vHandCenterPlusOffset;
        Vec2d vProjectedPoint;
        Mat3d mHandOrientation = m_pHandModel->GetHandOrientation();
 
        Math3d::SetVec(vOffset, 100, 0, 0);
        Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
        //Math3d::AddVecVec(vHandCenter3D, vOffset, vHandCenterPlusOffset);
        m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 255, 0, 0);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 255, 0, 0);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 255, 0, 0);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 255, 0, 0);
 
        Math3d::SetVec(vOffset, 0, 100, 0);
        Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
        m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 0, 255, 0);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 0, 0, 255);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 0, 255, 0);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 0, 255, 0);
 
        Math3d::SetVec(vOffset, 0, 0, 100);
        Math3d::MulMatVec(mHandOrientation, vOffset, vHandCenter3D, vHandCenterPlusOffset);
        m_pStereoCalibration->GetLeftCalibration()->CameraToImageCoordinates(vHandCenterPlusOffset, vProjectedPoint);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y, vProjectedPoint.x, vProjectedPoint.y, 0, 0, 255);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y, vProjectedPoint.x+1, vProjectedPoint.y, 0, 0, 255);
        DrawLineIntoImage(pImage, vHandCenter2D.x, vHandCenter2D.y+1, vProjectedPoint.x, vProjectedPoint.y+1, 0, 0, 255);
        DrawLineIntoImage(pImage, vHandCenter2D.x+1, vHandCenter2D.y+1, vProjectedPoint.x+1, vProjectedPoint.y+1, 0, 0, 255);
    }
    */
 
 
    void
    CHandModelVisualizer::DrawHandModelV2(CByteImage* pImage, bool bLeftCameraImage)
    {
        const CCalibration* pCalibration = bLeftCameraImage
                                               ? m_pStereoCalibration->GetLeftCalibration()
                                               : m_pStereoCalibration->GetRightCalibration();
 
        // fingertips
        if (bLeftCameraImage)
        {
            for (int i = 0; i < 5; i++)
            {
                DrawPolygon(pImage, m_pHandModelV2->m_aFingerTipPolygonsLeftCam.at(i));
            }
        }
        else
        {
            for (int i = 0; i < 5; i++)
            {
                DrawPolygon(pImage, m_pHandModelV2->m_aFingerTipPolygonsRightCam.at(i));
            }
        }
 
 
        // finger kinematics
 
        Vec2d vProjectedPoint1, vProjectedPoint2;
 
        pCalibration->WorldToImageCoordinates(
            m_pHandModelV2->m_vHandPosition, vProjectedPoint1, false);
 
        // lines from center to finger bases
        for (int i = 0; i < 5; i++)
        {
            pCalibration->WorldToImageCoordinates(
                m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0), vProjectedPoint2, false);
            DrawLineIntoImage(pImage,
                              vProjectedPoint1.x,
                              vProjectedPoint1.y,
                              vProjectedPoint2.x,
                              vProjectedPoint2.y,
                              50 * i,
                              50 * i,
                              255 - 50 * i);
        }
 
        // lines along the finger segments
        for (int i = 0; i < 5; i++)
        {
            pCalibration->WorldToImageCoordinates(
                m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0), vProjectedPoint1, false);
            for (size_t j = 1; j < m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).size(); j++)
            {
                pCalibration->WorldToImageCoordinates(
                    m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j), vProjectedPoint2, false);
                DrawLineIntoImage(pImage,
                                  vProjectedPoint1.x,
                                  vProjectedPoint1.y,
                                  vProjectedPoint2.x,
                                  vProjectedPoint2.y,
                                  50 * j,
                                  255 - 50 * i,
                                  50 * i);
                Math2d::SetVec(vProjectedPoint1, vProjectedPoint2);
            }
        }
 
 
        // draw circle for tracking ball
        double dCenterX, dCenterY, dRadius;
        if (bLeftCameraImage)
        {
            dCenterX = m_pHandModelV2->m_vTrackingBallPosLeftCam.x;
            dCenterY = m_pHandModelV2->m_vTrackingBallPosLeftCam.y;
            dRadius = m_pHandModelV2->m_fTrackingBallRadiusLeftCam;
        }
        else
        {
            dCenterX = m_pHandModelV2->m_vTrackingBallPosRightCam.x;
            dCenterY = m_pHandModelV2->m_vTrackingBallPosRightCam.y;
            dRadius = m_pHandModelV2->m_fTrackingBallRadiusRightCam;
        }
 
        int up = (int)(dCenterY - dRadius);
        int down = (int)(dCenterY + dRadius);
        for (int i = up; i <= down; i++)
        {
            int leftBorderInt =
                (int)(dCenterX - sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
            int rightBorderInt =
                (int)(dCenterX + sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
            for (int j = leftBorderInt; j <= rightBorderInt; j++)
            {
                if (i >= 0 && i < DSHT_IMAGE_HEIGHT && j >= 0 && j < DSHT_IMAGE_WIDTH)
                {
                    pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 0] = 0;
                    pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 1] = 255;
                    pImage->pixels[3 * (i * DSHT_IMAGE_WIDTH + j) + 2] = 0;
                }
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawHand(CByteImage* pImage)
    {
        int principal_point_x = (int)(m_fPrincipalPointX + 0.5);
        int principal_point_y = (int)(m_fPrincipalPointY + 0.5);
        unsigned char* pRenderedImage =
            m_pMoveMaster->m_pOIFwdKinematicsInterface->m_pOffscreenRenderer->getBuffer();
        int tempIndex, tempIndexX, tempIndexY;
        int renderSizeX = DSHT_OI_RENDERSIZE_X;
        int renderSizeY = DSHT_OI_RENDERSIZE_Y;
 
        if (!pRenderedImage)
        {
            ARMARX_WARNING_S << "pRenderedImage is NULL" << armarx::flush;
            return;
        }
 
        for (int i = 0; i < renderSizeY; i++)
        {
            for (int j = 0; j < renderSizeX; j++)
            {
                // if pixel is not red (red => background), draw it into the image
                if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] < 255 ||
                    pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] > 0 ||
                    pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] > 0)
                {
                    tempIndexX = j - (renderSizeX / 2) + principal_point_x;
                    tempIndexY = i - (renderSizeY / 2) + principal_point_y;
                    if (0 <= tempIndexX && tempIndexX < DSHT_IMAGE_WIDTH && 0 <= tempIndexY &&
                        tempIndexY < DSHT_IMAGE_HEIGHT)
                    {
                        tempIndex = 3 * (DSHT_IMAGE_WIDTH * tempIndexY + tempIndexX);
 
                        //if (tempIndex >= 0 && tempIndex<3*DSHT_IMAGE_WIDTH*DSHT_IMAGE_HEIGHT)
                        {
                            pImage->pixels[tempIndex + 0] =
                                0.4 *
                                    pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 0] +
                                0.6 * pImage->pixels[tempIndex + 0];
                            pImage->pixels[tempIndex + 1] =
                                0.4 *
                                    pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] +
                                0.6 * pImage->pixels[tempIndex + 1];
                            pImage->pixels[tempIndex + 2] =
                                0.4 *
                                    pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] +
                                0.6 * pImage->pixels[tempIndex + 2];
                        }
                    }
                }
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawSegmentedImage(CByteImage* pImage, bool bUseOpenInventorModel)
    {
        if (!bUseOpenInventorModel)
        {
            DrawSegmentedImageWithoutOpenInventor(pImage);
        }
        else
        {
            unsigned char* pRenderedImage =
                m_pMoveMaster->m_pOIFwdKinematicsInterface->m_pOffscreenRenderer->getBuffer();
 
            if (!pRenderedImage)
            {
                ARMARX_WARNING_S << "pRenderedImage is NULL" << armarx::flush;
                DrawSegmentedImageWithoutOpenInventor(pImage);
            }
            else
            {
                // reset image
                ::ImageProcessor::Zero(pImage);
 
                int principal_point_x = (int)(m_fPrincipalPointX + 0.5);
                int principal_point_y = (int)(m_fPrincipalPointY + 0.5);
                int tempIndex, tempIndexX, tempIndexY;
                int renderSizeX = DSHT_OI_RENDERSIZE_X;
                int renderSizeY = DSHT_OI_RENDERSIZE_Y;
 
                for (int i = 0; i < renderSizeY; i++)
                {
                    for (int j = 0; j < renderSizeX; j++)
                    {
                        // check if pixel is not background
                        if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] < 250 ||
                            pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 1] > 10 ||
                            pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) + 2] > 10)
                        {
                            tempIndexX = j - (renderSizeX / 2) + principal_point_x;
                            tempIndexY = i - (renderSizeY / 2) + principal_point_y;
                            // check if pixel is inside the image
                            if ((tempIndexX >= 0) && (tempIndexX < DSHT_IMAGE_WIDTH) &&
                                (tempIndexY >= 0) && (tempIndexY < DSHT_IMAGE_HEIGHT))
                            {
                                tempIndex = DSHT_IMAGE_WIDTH * tempIndexY + tempIndexX;
                                if (tempIndex >= 0 &&
                                    tempIndex < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT)
                                {
                                    // check for object color
                                    if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j)] >
                                            250 &&
                                        pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) +
                                                       1] > 250 &&
                                        pRenderedImage[3 * (renderSizeX * (renderSizeY - i) + j) +
                                                       2] < 10)
                                    {
                                        pImage->pixels[tempIndex] = 42;
                                    }
                                    // check for arm color
                                    else if (pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
                                                                 j)] < 10 &&
                                             pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
                                                                 j) +
                                                            1] > 250 &&
                                             pRenderedImage[3 * (renderSizeX * (renderSizeY - i) +
                                                                 j) +
                                                            2] > 250)
                                    {
                                        pImage->pixels[tempIndex] = 128;
                                    }
                                    // otherwise it must be the hand
                                    else
                                    {
                                        pImage->pixels[tempIndex] = 255;
                                    }
                                }
                            }
                        }
                    }
                }
 
                if (pImage->type == CByteImage::eRGB24)
                {
                    for (int i = DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT - 1; i >= 0; i--)
                    {
                        pImage->pixels[3 * i] = pImage->pixels[i];
                    }
                    for (int i = 0; i < DSHT_IMAGE_WIDTH * DSHT_IMAGE_HEIGHT; i++)
                    {
                        pImage->pixels[3 * i + 1] = pImage->pixels[3 * i];
                        pImage->pixels[3 * i + 2] = pImage->pixels[3 * i];
                    }
                }
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawSegmentedImageWithoutOpenInventor(CByteImage* pSegmentedImage)
    {
        CByteImage* pImage;
        if (pSegmentedImage->type == CByteImage::eRGB24)
        {
            pImage = new CByteImage(
                pSegmentedImage->width, pSegmentedImage->height, CByteImage::eGrayScale);
        }
        else
        {
            pImage = pSegmentedImage;
        }
        ::ImageProcessor::Zero(pImage);
 
        // fingertips
        for (int i = 0; i < 5; i++)
        {
            DrawPolygonGrayscale(pImage, m_pHandModelV2->m_aFingerTipPolygonsLeftCam.at(i));
        }
 
        const float fFingerWidth = 25.0f;
 
        // lines from center to finger bases
        for (int i = 0; i < 5; i++)
        {
            DrawQuadrangleAlongA3DLine(pImage,
                                       m_pHandModelV2->m_vHandPosition,
                                       m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(0),
                                       fFingerWidth,
                                       fFingerWidth);
        }
 
        // lines along the finger segments
        for (int i = 0; i < 5; i++)
        {
            for (size_t j = 0; j < m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).size() - 1; j++)
            {
                DrawQuadrangleAlongA3DLine(pImage,
                                           m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j),
                                           m_pHandModelV2->m_aFingerJointsInWorldCS.at(i).at(j + 1),
                                           fFingerWidth,
                                           fFingerWidth);
            }
        }
 
        // draw circle for tracking ball
        double dCenterX, dCenterY, dRadius;
        dCenterX = m_pHandModelV2->m_vTrackingBallPosLeftCam.x;
        dCenterY = m_pHandModelV2->m_vTrackingBallPosLeftCam.y;
        dRadius = m_pHandModelV2->m_fTrackingBallRadiusLeftCam;
 
        int up = (int)(dCenterY - dRadius);
        int down = (int)(dCenterY + dRadius);
        for (int i = up; i <= down; i++)
        {
            int leftBorderInt =
                (int)(dCenterX - sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
            int rightBorderInt =
                (int)(dCenterX + sqrt(dRadius * dRadius - (i - dCenterY) * (i - dCenterY)));
            for (int j = leftBorderInt; j <= rightBorderInt; j++)
            {
                if (i >= 0 && i < DSHT_IMAGE_HEIGHT && j >= 0 && j < DSHT_IMAGE_WIDTH)
                {
                    pImage->pixels[i * DSHT_IMAGE_WIDTH + j] = 255;
                }
            }
        }
 
        // arm
        Vec3d vArmLength = {0, 0, -500};
        Vec3d vArmStart;
        Math3d::MulMatVec(m_pHandModelV2->m_mHandOrientation,
                          vArmLength,
                          m_pHandModelV2->m_vHandPosition,
                          vArmStart);
        DrawQuadrangleAlongA3DLine(pImage, vArmStart, m_pHandModelV2->m_vHandPosition, 200, 100);
 
        CByteImage* pTempImage = new CByteImage(pImage);
        //timeval tStart, tEnd;
        //long tTimeDiff;
        //gettimeofday(&tStart, 0);
        ::ImageProcessor::Dilate(pImage, pTempImage, 14);
        ::ImageProcessor::Dilate(pTempImage, pImage, 14);
        //gettimeofday(&tEnd, 0);
        //tTimeDiff = (1000*tEnd.tv_sec+tEnd.tv_usec/1000) - (1000*tStart.tv_sec+tStart.tv_usec/1000);
        //ARMARX_VERBOSE_S << "Time for dilate: " << tTimeDiff << " ms";
        delete pTempImage;
 
        // return result
        if (pSegmentedImage->type == CByteImage::eRGB24)
        {
            for (int i = 0; i < pSegmentedImage->width * pSegmentedImage->height; i++)
            {
                pSegmentedImage->pixels[3 * i + 0] = pImage->pixels[i];
                pSegmentedImage->pixels[3 * i + 1] = pImage->pixels[i];
                pSegmentedImage->pixels[3 * i + 2] = pImage->pixels[i];
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawCross(CByteImage* pGreyImage, int x, int y, int nBrightness)
    {
        if (x > 2 && y > 2 && x < DSHT_IMAGE_WIDTH - 3 && y < DSHT_IMAGE_HEIGHT - 3)
        {
            for (int j = -3; j <= 3; j++)
            {
                pGreyImage->pixels[(y + j) * DSHT_IMAGE_WIDTH + x] = nBrightness;
                pGreyImage->pixels[y * DSHT_IMAGE_WIDTH + x + j] = nBrightness;
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawCross(CByteImage* pColorImage, int x, int y, int r, int g, int b)
    {
        if (x > 2 && y > 2 && x < DSHT_IMAGE_WIDTH - 3 && y < DSHT_IMAGE_HEIGHT - 3)
        {
            for (int j = -3; j <= 3; j++)
            {
                pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x)] = r;
                pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x) + 1] = g;
                pColorImage->pixels[3 * ((y + j) * DSHT_IMAGE_WIDTH + x) + 2] = b;
 
                pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j)] = r;
                pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j) + 1] = g;
                pColorImage->pixels[3 * (y * DSHT_IMAGE_WIDTH + x + j) + 2] = b;
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawLineIntoImage(CByteImage* pImage,
                                            int x1,
                                            int y1,
                                            int x2,
                                            int y2,
                                            int red,
                                            int green,
                                            int blue)
    {
        if (x1 < 0 || x1 >= pImage->width || x2 < 0 || x2 >= pImage->width || y1 < 0 ||
            y1 >= pImage->height || y2 < 0 || y2 >= pImage->height)
        {
            return;
        }
 
        if (x1 == x2 && y1 == y2)
        {
            pImage->pixels[3 * (y1 * pImage->width + x1)] = red;
            pImage->pixels[3 * (y1 * pImage->width + x1) + 1] = green;
            pImage->pixels[3 * (y1 * pImage->width + x1) + 2] = blue;
        }
        else
        {
            int temp;
 
            if (x1 > x2)
            {
                temp = x1;
                x1 = x2;
                x2 = temp;
                temp = y1;
                y1 = y2;
                y2 = temp;
            }
 
            float divided_diff = (float)(y2 - y1) / (float)(x2 - x1);
 
            for (int i = x1; i < x2; i++)
            {
                pImage->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i)] =
                    red;
                pImage
                    ->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i) + 1] =
                    green;
                pImage
                    ->pixels[3 * (((int)((i - x1) * divided_diff) + y1) * pImage->width + i) + 2] =
                    blue;
            }
 
 
            if (y1 > y2)
            {
                temp = x1;
                x1 = x2;
                x2 = temp;
                temp = y1;
                y1 = y2;
                y2 = temp;
            }
 
 
            divided_diff = (float)(x2 - x1) / (float)(y2 - y1);
 
            for (int i = y1; i < y2; i++)
            {
                pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff))] = red;
                pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff)) + 1] =
                    green;
                pImage->pixels[3 * (i * pImage->width + x1 + (int)((i - y1) * divided_diff)) + 2] =
                    blue;
            }
        }
    }
 
    void
    CHandModelVisualizer::DrawQuadrangleAlongA3DLine(CByteImage* pImage,
                                                     Vec3d vStart,
                                                     Vec3d vEnd,
                                                     float fWidthAtStart,
                                                     float fWidthAtEnd)
    {
        Vec3d vDirection;
        Math3d::SubtractVecVec(vEnd, vStart, vDirection);
        Vec3d vOrthogonalXY = {-vDirection.y, vDirection.x, 0};
        if (Math3d::Length(vOrthogonalXY) == 0)
        {
            return;
        }
        Math3d::NormalizeVec(vOrthogonalXY);
        Vec3d vOrthogonalStart, vOrthogonalEnd;
        Math3d::MulVecScalar(vOrthogonalXY, 0.5f * fWidthAtStart, vOrthogonalStart);
        Math3d::MulVecScalar(vOrthogonalXY, 0.5f * fWidthAtEnd, vOrthogonalEnd);
 
        Vec3d p1, p2, p3, p4;
        Math3d::AddVecVec(vStart, vOrthogonalStart, p1);
        Math3d::SubtractVecVec(vStart, vOrthogonalStart, p2);
        Math3d::AddVecVec(vEnd, vOrthogonalEnd, p3);
        Math3d::SubtractVecVec(vEnd, vOrthogonalEnd, p4);
 
        Vec2d vProjPoint1, vProjPoint2, vProjPoint3, vProjPoint4;
        m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p1, vProjPoint1, false);
        m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p2, vProjPoint2, false);
        m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p3, vProjPoint3, false);
        m_pStereoCalibration->GetLeftCalibration()->WorldToImageCoordinates(p4, vProjPoint4, false);
 
        ConvexPolygonCalculations::Polygon pProjectedQuadrangle, pImageCorners,
            pIntersectingPolygon;
        Vec3d pPoints[4];
        pPoints[0].x = vProjPoint1.x;
        pPoints[0].y = vProjPoint1.y;
        pPoints[1].x = vProjPoint2.x;
        pPoints[1].y = vProjPoint2.y;
        pPoints[2].x = vProjPoint3.x;
        pPoints[2].y = vProjPoint3.y;
        pPoints[3].x = vProjPoint4.x;
        pPoints[3].y = vProjPoint4.y;
        ConvexPolygonCalculations::CreateConvexPolygonFromHullPoints(
            pPoints, 4, &pProjectedQuadrangle);
 
        pPoints[0].x = 0;
        pPoints[0].y = 0;
        pPoints[1].x = DSHT_IMAGE_WIDTH - 1;
        pPoints[1].y = 1;
        pPoints[2].x = DSHT_IMAGE_WIDTH - 2;
        pPoints[2].y = DSHT_IMAGE_HEIGHT - 1;
        pPoints[3].x = 1;
        pPoints[3].y = DSHT_IMAGE_HEIGHT - 2;
        ConvexPolygonCalculations::CreateConvexPolygonFromHullPoints(pPoints, 4, &pImageCorners);
 
        ConvexPolygonCalculations::GetPolygonIntersection(
            &pProjectedQuadrangle, &pImageCorners, &pIntersectingPolygon);
 
        if (pImage->type == CByteImage::eGrayScale)
        {
            DrawPolygonGrayscale(pImage, pIntersectingPolygon);
        }
        else
        {
            DrawPolygon(pImage, pIntersectingPolygon);
        }
    }
 
    void
    CHandModelVisualizer::ExtractAnglesFromRotationMatrix(const Mat3d& mat, Vec3d& angles)
    {
        angles.y = asin(mat.r3);
        angles.x = atan2((-mat.r6), (mat.r9));
        angles.z = atan2((-mat.r2), (mat.r1));
    }
} // namespace visionx