0
$\begingroup$

Rosanswers logo

Hi guys,

Although, i am able to find the depth pointer and find the depth value. Now i need to publish the data and want to visualize the point cloud in Rviz. I have written a code and would like to have your opinions.

    /* OpenCV Includes */
#include "opencv2\opencv.hpp"
#include "opencv2\highgui\highgui.hpp"
#include "opencv2\imgproc\imgproc.hpp"

/* OpenNI Includes */
#include <XnCppWrapper.h>
#include <XnOpenNI.h>
#include <XnLog.h>
#include <XnOS.h>

/* PCL Includes */
#include "pcl/ModelCoefficients.h"
#include "pcl/console/parse.h"
#include "pcl/console/print.h"
#include "pcl/io/pcd_io.h"
#include "pcl/point_types.h"
#include <pcl/features/cvfh.h>
#include <pcl/features/vfh.h>
#include <pcl/features/normal_3d.h>
#include "pcl/filters/extract_indices.h"
#include "pcl/filters/passthrough.h"
#include "pcl/filters/statistical_outlier_removal.h"
#include "pcl/kdtree/kdtree.h"
#include "pcl/sample_consensus/method_types.h"
#include "pcl/sample_consensus/model_types.h"
#include "pcl/segmentation/sac_segmentation.h"
#include "pcl/segmentation/extract_clusters.h"
#include <boost/filesystem.hpp>

/* ROS Includes */
#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>

#define SAMPLE_XML_PATH "C:/Program Files (x86)/OpenNI/Data/SamplesConfig.xml"
#define X_RES 640
#define Y_RES 480
#define MAX_DEPTH 10000

using namespace std;
using namespace xn;
using namespace pcl::console;

/* Globals */
Context         context;
ImageGenerator      g_image;
DepthGenerator      d_image;
ImageMetaData       g_imageMD;
EnumerationErrors   errors;
XnStatus        nRetVal = XN_STATUS_OK;
unsigned short      depth[MAX_DEPTH];
char *depth_data;

/* Typedefs */
typedef pcl::PointCloud<pcl::PointXYZRGBNormal> PointCloud;

void raw2depth(){
  for (int i=0; i<MAX_DEPTH; i++) {
  float v = (float)i/MAX_DEPTH;//for visualization purposes only
  v = powf(v, 2);
  v = v*36*256;
  depth[i] = v;
  }
}

void depth2rgb(const XnDepthPixel* Xn_disparity){
  for (int i=0; i<307200; i++) {
    int pval = depth[Xn_disparity[i]];
    int lb = pval & 0xff;
    switch (pval>>8) {
      case 0:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 255-lb;
        break;
      case 1:
        depth_data[3*i+0] = 255;
        depth_data[3*i+1] = lb;
        depth_data[3*i+2] = 0;
        break;
      case 2:
        depth_data[3*i+0] = 255-lb;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = 0;
        break;
      case 3:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255;
        depth_data[3*i+2] = lb;
        break;
      case 4:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 255-lb;
        depth_data[3*i+2] = 255;
        break;
      case 5:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 255-lb;
        break;
      default:
        depth_data[3*i+0] = 0;
        depth_data[3*i+1] = 0;
        depth_data[3*i+2] = 0;
        break;
    }
  }
}


void rgbdInit(){
  //Initialize context object
  nRetVal = context.Init();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to initialize context: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a DepthGenerator node
  nRetVal = d_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create depth generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Create a ImageGenerator node
  nRetVal = g_image.Create(context);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to create image generator: %s\n", xnGetStatusString(nRetVal));
  }

  // Make it start generating data
  nRetVal = context.StartGeneratingAll();
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed generating data: %s\n", xnGetStatusString(nRetVal));
  }

/*  //Sync the DepthGenerator with the ImageGenerator
  nRetVal = d_image.GetFrameSyncCap().FrameSyncWith(g_image);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to sync Depth and RGB streams: %s\n", xnGetStatusString(nRetVal));
  }*/

  // Set the view point of the DepthGenerator to match the ImageGenerator
  nRetVal = d_image.GetAlternativeViewPointCap().SetViewPoint(g_image);
  if (nRetVal != XN_STATUS_OK){
    print_error("Failed to match Depth and RGB points of view: %s\n", xnGetStatusString(nRetVal));
  }
}

void getPointCloud(const XnDepthPixel* pDepthMap, const XnUInt8 *rgb, PointCloud &cloud){
  // Fill in the cloud data
  cloud.width  = X_RES;
  cloud.height = Y_RES;
  cloud.points.resize (cloud.width * cloud.height);
  XnPoint3D *depthpx = (XnPoint3D*)malloc(X_RES*Y_RES*sizeof(XnFloat)*3); //preallocate data for speed boost
  int i=0;
  for (size_t y=0; y<cloud.height; y++){
    for (size_t x=0; x<cloud.width; x++){
      depthpx[i].X= x;
      depthpx[i].Y= y;
      depthpx[i].Z= pDepthMap[i];
      i++;
    }
  }
  d_image.ConvertProjectiveToRealWorld(i,depthpx,depthpx);
  int32_t color;
  i=0;
  for (size_t y = 0; y<cloud.height; y++){
    for (size_t x=0; x<cloud.width; x++){
      cloud(x,y).x = depthpx[i].X;
      cloud(x,y).y = depthpx[i].Y;
      cloud(x,y).z = depthpx[i].Z;
      color =  (rgb[(3*i)+2]<<16)|(rgb[(3*i)+1]<<8)|(rgb[3*i]);
      cloud.points[i].rgb = *(float *)(&color);
      i++;
    }
  }
}

int main( int argc, char** argv )
{
    //Initialize ROS
    ros::init (argc, argv, "pub_pcl");
    ros::NodeHandle nh;
    ros::Publisher pub = nh.advertise<PointCloud> ("points2", 1);

    rgbdInit();

  // Allocate Storage
  IplImage *rgbimg = cvCreateImageHeader(cvSize(X_RES,Y_RES), 8, 3);
  IplImage *depthimg = cvCreateImageHeader(cvSize(X_RES,Y_RES), 8, 3);
  IplImage *grayimg = cvCreateImage(cvSize(X_RES,Y_RES), IPL_DEPTH_8U, 1);
  raw2depth();
  depth_data = (char*)malloc(X_RES*Y_RES*3);

  while( true ) {
  // Wait for new data to be available
    nRetVal = context.WaitAndUpdateAll();
    if (nRetVal != XN_STATUS_OK)
    {
       printf("Failed updating data: %s\n", xnGetStatusString(nRetVal));
       continue;
    }

    // Take current depth map
    const XnDepthPixel* pDepthMap = d_image.GetDepthMap();
    // Take current image
    const XnRGB24Pixel* pImage = g_image.GetRGB24ImageMap();

    //process depth map
    depth2rgb(pDepthMap);
    cvSetData(depthimg,depth_data, X_RES*3);
    cvShowImage("Depth",depthimg);

    //process image data
    g_image.GetMetaData (g_imageMD);
    XnUInt8* rgb = g_imageMD.WritableData();
    cvSetData(rgbimg,rgb,rgbimg->widthStep);
    cvCvtColor(rgbimg,rgbimg,CV_RGB2BGR);
    cvCvtColor(rgbimg,grayimg,CV_BGR2GRAY);

    PointCloud cloud;
    getPointCloud(pDepthMap, rgb,cloud);
    PointCloud::Ptr cloudPtr (new PointCloud(cloud));
    
    ros::Rate loop_rate(4);
    while (nh.ok())
    {
        msg->header.stamp = ros ::Time::now();
        pub.publish(msg);
        ros::spinOnce();
        loop_rate.sleep();
    }

    getCVFHSignatures(cloudPtr);

    cvShowImage("RGB",rgbimg);

    char key =cvWaitKey(15);
    if (key==27/*ESC*/) {
      break;
    }
  }
  
  // Clean-up
  context.Shutdown();
  
}

Hi guys, i am in real need of help.. hope someone can help me. I am a newbie in this. I need to do this urgently.


Originally posted by lakshmen on ROS Answers with karma: 101 on 2011-07-07

Post score: 0


Original comments

Comment by Martin Günther on 2011-07-10:
What do you expect this to do? What doesn't work? Do you really expect us to compile and debug your code in order to find out the answer to these two questions ourselves? Also, stating that you need an answer urgently doesn't help getting an answer faster, it just makes you sound impatient.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

You have two main loops.

Only when the first one quits, the second one runs. You should merge them.


Originally posted by dornhege with karma: 31395 on 2011-07-07

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by lakshmen on 2011-07-07:
is it better now?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.