[Personalrobots-commit] SF.net SVN: personalrobots:[7014] pkg/trunk

[<jl...@us...>]

Revision: 7014
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7014&view=rev
Author:   jleibs
Date:     2008-11-19 23:26:15 +0000 (Wed, 19 Nov 2008)

Log Message:
-----------
Stereo point clouds now have color information as well.

Modified Paths:
--------------
    pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
    pkg/trunk/vision/stereo_view/stereo_view.cpp

Modified: pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp
===================================================================
--- pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp	2008-11-19 22:59:41 UTC (rev 7013)
+++ pkg/trunk/drivers/cam/dcam_node/dcam_node.cpp	2008-11-19 23:26:15 UTC (rev 7014)
@@ -293,6 +293,9 @@
         cloud_.header.stamp = ros::Time(cam_->camIm->im_time * 1000);
         cloud_.header.frame_id = "stereo";
         cloud_.pts.resize(stcam->stIm->numPts);
+        cloud_.chan.resize(1);
+        cloud_.chan[0].name = "rgb";
+        cloud_.chan[0].vals.resize(stcam->stIm->numPts);
         
         for (int i = 0; i < stcam->stIm->numPts; i++)
         {
@@ -302,6 +305,14 @@
           //          printf("(%d/%d) %f %f %f\n", i, stcam->stIm->numPts, cloud_.pts[i].x, cloud_.pts[i].y, cloud_.pts[i].z);
         }
 
+        for (int i = 0; i < stcam->stIm->numPts; i++)
+        {
+          int rgb = (stcam->stIm->imPtsColor[3*i] << 16) | (stcam->stIm->imPtsColor[3*i + 1] << 8) | stcam->stIm->imPtsColor[3*i + 2];
+          cloud_.chan[0].vals[i] = *(float*)(&rgb);
+        }
+
+        
+
         publish("~cloud", cloud_);
       }
 

Modified: pkg/trunk/vision/stereo_view/stereo_view.cpp
===================================================================
--- pkg/trunk/vision/stereo_view/stereo_view.cpp	2008-11-19 22:59:41 UTC (rev 7013)
+++ pkg/trunk/vision/stereo_view/stereo_view.cpp	2008-11-19 23:26:15 UTC (rev 7014)
@@ -64,7 +64,7 @@
 
   bool subscribe_color_;
 
-  StereoView() : ros::node("cv_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
+  StereoView() : ros::node("stereo_view"), sync(this, &StereoView::image_cb_all, ros::Duration(0.05), &StereoView::image_cb_timeout), subscribe_color_(false)
   { 
     cvNamedWindow("left", CV_WINDOW_AUTOSIZE);
     cvNamedWindow("right", CV_WINDOW_AUTOSIZE);


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.