$search

SceneDrawer.cpp

Go to the documentation of this file.

/*****************************************************************************
*                                                                            *
*  OpenNI 1.0 Alpha                                                          *
*  Copyright (C) 2010 PrimeSense Ltd.                                        *
*                                                                            *
*  This file is part of OpenNI.                                              *
*                                                                            *
*  OpenNI is free software: you can redistribute it and/or modify            *
*  it under the terms of the GNU Lesser General Public License as published  *
*  by the Free Software Foundation, either version 3 of the License, or      *
*  (at your option) any later version.                                       *
*                                                                            *
*  OpenNI 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 Lesser General Public License for more details.                       *
*                                                                            *
*  You should have received a copy of the GNU Lesser General Public License  *
*  along with OpenNI. If not, see <http://www.gnu.org/licenses/>.            *
*                                                                            *
*****************************************************************************/




//---------------------------------------------------------------------------
// Includes
//---------------------------------------------------------------------------
#include "cob_people_detection/SceneDrawer.h"

extern xn::UserGenerator g_UserGenerator;
extern xn::DepthGenerator g_DepthGenerator;

#define MAX_DEPTH 10000
float g_pDepthHist[MAX_DEPTH];
unsigned int getClosestPowerOfTwo(unsigned int n)
{
        unsigned int m = 2;
        while(m < n) m<<=1;

        return m;
}

XnFloat Colors[][3] =
{
        {0,1,1},
        {0,0,1},
        {0,1,0},
        {1,1,0},
        {1,0,0},
        {1,.5,0},
        {.5,1,0},
        {0,.5,1},
        {.5,0,1},
        {1,1,.5},
        {1,1,1}
};
XnUInt32 nColors = 10;


void DrawLimb(XnUserID player, XnSkeletonJoint eJoint1, XnSkeletonJoint eJoint2)
{
        if (!g_UserGenerator.GetSkeletonCap().IsTracking(player))
        {
                printf("not tracked!\n");
                return;
        }

        XnSkeletonJointPosition joint1, joint2;
        g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint1, joint1);
        g_UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(player, eJoint2, joint2);

        if (joint1.fConfidence < 0.5 || joint2.fConfidence < 0.5)
        {
                return;
        }

        XnPoint3D pt[2];
        pt[0] = joint1.position;
        pt[1] = joint2.position;

        g_DepthGenerator.ConvertRealWorldToProjective(2, pt, pt);
}


void PublishPeopleImage(const xn::DepthMetaData& dmd, const xn::SceneMetaData& smd, image_transport::Publisher& pub)
{
        const XnDepthPixel* pDepth = dmd.Data();
        const XnLabel* pLabels = smd.Data();
        unsigned int nValue = 0;
        XnUInt16 g_nXRes = dmd.XRes();
        XnUInt16 g_nYRes = dmd.YRes();
        cv::Mat peopleSegmentation(g_nYRes, g_nXRes, CV_8UC3);

        // Prepare the texture map
        for (unsigned int nY=0; nY<g_nYRes; nY++)
        {
                uchar* pDestImage = peopleSegmentation.ptr(nY);
                for (unsigned int nX=0; nX < g_nXRes; nX++)
                {

                        pDestImage[0] = 0;
                        pDestImage[1] = 0;
                        pDestImage[2] = 0;
                        if (*pLabels != 0)
                        {
                                nValue = *pDepth;
                                XnLabel label = *pLabels;
                                XnUInt32 nColorID = label % nColors;

                                if (nValue != 0)
                                {
                                        pDestImage[0] = 255 * Colors[nColorID][0]; 
                                        pDestImage[1] = 255 * Colors[nColorID][1];
                                        pDestImage[2] = 255 * Colors[nColorID][2];
                                }
                        }

                        pDepth++;
                        pLabels++;
                        pDestImage+=3;
                }
        }
        
        // todo: stop and start with respect to odometry: segmentation works best if robot is standing still
        
        // publish
        try
        {
                IplImage img = (IplImage)peopleSegmentation;
                sensor_msgs::ImagePtr msg = (sensor_msgs::CvBridge::cvToImgMsg(&img, "bgr8"));
                msg->header.stamp = ros::Time::now();
                pub.publish(msg);
        }
        catch (sensor_msgs::CvBridgeException error)
        {
                ROS_ERROR("[openni_tracker] Could not convert IplImage to ROS message");
        }
        
//      cv_bridge::CvImage bridgeImage;         did not work
//      bridgeImage.image = peopleSegmentation.clone();
//      sensor_msgs::ImagePtr msg = bridgeImage.toImageMsg();
//      pub.publish(msg);


        // display for checking the output
//      cv::namedWindow("Test");
//      imshow("Test", peopleSegmentation);
//      uchar key = cv::waitKey(10);
//      if (key == 'r')
//      {
//        g_UserGenerator.StopGenerating();
//        std::cout << "stop\n";
//      }
//      if (key == 'c')
//      {
//        g_UserGenerator.StartGenerating();
//        std::cout << "start\n";
//      }
}

---

cob_people_detection
Author(s): Richard Bormann
autogenerated on Tue Mar 5 11:50:09 2013