[Jderobot-admin] jderobot-r889 - trunk/src/components/pluginsGazebo/kinect
ahcorde en jderobot.org
ahcorde en jderobot.org
Mie Mar 20 09:22:36 CET 2013
Author: ahcorde
Date: 2013-03-20 09:21:35 +0100 (Wed, 20 Mar 2013)
New Revision: 889
Modified:
trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
Log:
[ahcorde]
Actualacion del plugin de kinect para Gazebo
- Se a?\195?\177ade color a la nube de puntos
- Imagen de profundidad mejorada(Juanjo Garc?\195?\173a)
Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc 2013-03-19 09:45:37 UTC (rev 888)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.cc 2013-03-20 08:21:35 UTC (rev 889)
@@ -35,8 +35,8 @@
count = 0;
pthread_mutex_init (&mutex, NULL);
- pthread_mutex_init (&mutexRGB, NULL);
- gzerr << "Kinect Construtor\n";
+ pthread_mutex_init (&mutexRGB, NULL);
+ pthread_mutex_init (&mutexDepth, NULL);
}
@@ -78,72 +78,40 @@
this->parentSensor->SetActive(true);
}
- void raw2depth(){
- int i;
- for ( i=0; i<MAX_DEPTH; i++) {
- float v = (float)i/MAX_DEPTH;//for visualization purposes only
- v = v*v;
- v = v*36*256;
- depth[i] = v;
- //std::cout << "depth[]:" << depth[i] << std::endl;
- }
- }
-
- void depth2rgb( const float* disparity , cv::Mat image){
- int i;
-
- for (i=0; i<image.rows* image.cols; i++) {
+ void DepthCameraPlugin::depth2rgb(cv::Mat image){
+ //const unsigned short *disparity = Xn_disparity;
+ imageDepth.create(image.rows,image.cols,CV_8UC3);
+ float * data= (float*)image.data;
+ for (int i=0; i<image.rows* image.cols; i++) {
//std::cout << i << std::endl;
- int pval = depth[(int)(disparity[i]*10000)];
- int lb = pval & 0xff;
- switch (pval>>8) {
- case 0:
- image.data[3*i+0] = 255;
- image.data[3*i+1] = 255-lb;
- image.data[3*i+2] = 255-lb;
- break;
- case 1:
- image.data[3*i+0] = 255;
- image.data[3*i+1] = lb;
- image.data[3*i+2] = 0;
- break;
- case 2:
- image.data[3*i+0] = 255-lb;
- image.data[3*i+1] = 255;
- image.data[3*i+2] = 0;
- break;
- case 5:
- image.data[3*i+0] = 0;
- image.data[3*i+1] = 0;
- image.data[3*i+2] = 255-lb;
- break;
- default:
- image.data[3*i+0] = 0;
- image.data[3*i+1] = 0;
- image.data[3*i+2] = 0;
- break;
- }
+ int val = (int)(data[i]*1000);
+ imageDepth.data[3*i+0] = (float)val/10000*255;;
+ imageDepth.data[3*i+1] = val>>8;
+ imageDepth.data[3*i+2] = val&0xff;
+
+ if(imageDepth.data[i*3]!=0)
+ imageDepth.data[i*3]=255-imageDepth.data[i*3];
+ imageDepth.data[i*3+1]=imageDepth.data[i*3];
+ imageDepth.data[i*3+2]=imageDepth.data[i*3];
+
}
-}
+ }
+
/////////////////////////////////////////////////
void DepthCameraPlugin::OnNewDepthFrame(const float *_image,
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &_format)
{
- cycle = 100;
- gettimeofday(&a, NULL);
- totala = a.tv_sec * 1000000 + a.tv_usec;
-
if(count == 0){
count++;
std::string name = this->parentSensor->GetName();
nameKinect = std::string("--Ice.Config=" + name + ".cfg");
pthread_t thr_gui;
pthread_create(&thr_gui, NULL, &mainKinect, (void*)this);
- gzerr << "Creating Kinect\n";
cloud->points.resize(_width*_height);
+ imageDepth.create(_height, _width, CV_8UC3);
}
double hfov = 1.04719755;
@@ -151,20 +119,16 @@
double pointCloudCutoff = 0.001;
//std::cout << "depth: " << (int)(_image[0]*1000) << std::endl;
-/*
+
cv::Mat image;
image.create(_height, _width, CV_32FC1);
- imageDepth.create(_height, _width, CV_8UC1);
-
- pthread_mutex_lock (&mutex);
+
memcpy( (float *)image.data,(float *) _image, _width*_height*4 );
- image.convertTo(imageDepth, CV_8UC1, 255, 0);
- cv::cvtColor(imageDepth, imageDepth, CV_GRAY2RGB);
- //cv::imshow("profundidad", imageDepth);
- //cv::waitKey(10);
-*/
+
+
+
double pAngle;
double yAngle;
@@ -178,7 +142,6 @@
point.g = 0;
point.b = 0;
-
for(int x = 0 ; x < _width ; x++){
for(int y = 0; y < _height; y++){
int indice = y*_width + x;
@@ -198,7 +161,16 @@
point.x = d * tan(yAngle);
point.y = d * tan(pAngle);
-
+ if(!imageRGB.data){
+ point.r = 255;
+ point.g = 0;
+ point.b = 0;
+ }else{
+ int indice = y*imageRGB.step + x*imageRGB.channels();
+ point.r = imageRGB.data[indice];
+ point.g = imageRGB.data[indice+1];
+ point.b = imageRGB.data[indice+2];
+ }
if(d > pointCloudCutoff){
point.z = _image[indice];
}else{ //point in the unseeable range
@@ -211,11 +183,10 @@
}
}
-
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
sor.setInputCloud (cloud);
- sor.setLeafSize (0.05, 0.05, 0.05);
+ sor.setLeafSize (0.01, 0.01, 0.01);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud2(new pcl::PointCloud<pcl::PointXYZRGBA>);
@@ -225,22 +196,16 @@
pthread_mutex_unlock (&kinect->mutex);
+ pthread_mutex_lock(&mutexDepth);
+ depth2rgb(image);
+ pthread_mutex_unlock(&mutexDepth);
+
//pthread_mutex_unlock (&mutex);
//std::cout << "cloud: " << cloud->points.size() << std::endl;
//if (!viewer.wasStopped())
// viewer.showCloud(cloud);
-
- gettimeofday(&b, NULL);
- totalb = b.tv_sec * 1000000 + b.tv_usec;
- diff = (totalb - totala) / 1000;
- diff = cycle - diff;
-
- if (diff < 10)
- diff = 10;
-
- sleep(diff/1000);
}
@@ -249,22 +214,8 @@
unsigned int _width, unsigned int _height,
unsigned int _depth, const std::string &/*_format*/)
{
- cycle3 = 100;
- gettimeofday(&a3, NULL);
- totala3 = a3.tv_sec * 1000000 + a3.tv_usec;
+
- gettimeofday(&b3, NULL);
- totalb3 = b3.tv_sec * 1000000 + b3.tv_usec;
-
- diff3 = (totalb3 - totala3) / 1000;
- diff3 = cycle3 - diff3;
-
- if (diff3 < 10)
- diff3 = 10;
-
- sleep(diff3/1000);
-
-
}
/////////////////////////////////////////////////
@@ -275,29 +226,13 @@
const std::string &_format)
{
- cycle2 = 100;
- gettimeofday(&a2, NULL);
- totala2 = a2.tv_sec * 1000000 + a2.tv_usec;
//std::cout << "OnNewImageFrame Format: " << format << " Depth: " << _depth << std::endl;
pthread_mutex_lock (&mutexRGB);
imageRGB.create(_height, _width, CV_8UC3);
memcpy((unsigned char *) imageRGB.data, &(_image[0]), _width*_height * 3);
- //cv::imshow("Color", imageRGB);
- //cv::waitKey(10);
pthread_mutex_unlock (&mutexRGB);
-
- gettimeofday(&b2, NULL);
- totalb2 = b2.tv_sec * 1000000 + b2.tv_usec;
- diff2 = (totalb2 - totala2) / 1000;
- diff2 = cycle2 - diff2;
-
- if (diff2 < 10)
- diff2 = 10;
-
- sleep(diff2/1000);
-
}
class KinectI: virtual public jderobot::pointCloud{
@@ -567,11 +502,11 @@
continue;
}
if(count==0){
- pthread_mutex_lock (&mycamera->cameraI->mutex);
+ pthread_mutex_lock (&mycamera->cameraI->mutexDepth);
mycamera->imageDescription->width = mycamera->cameraI->imageDepth.cols;
mycamera->imageDescription->height = mycamera->cameraI->imageDepth.rows;
mycamera->imageDescription->size = mycamera->cameraI->imageDepth.cols*mycamera->cameraI->imageDepth.rows*3;
- pthread_mutex_unlock (&mycamera->cameraI->mutex);
+ pthread_mutex_unlock (&mycamera->cameraI->mutexDepth);
mycamera->imageDescription->format = "RGB8";
@@ -589,11 +524,11 @@
reply->timeStamp.seconds = (long)t.toSeconds();
reply->timeStamp.useconds = (long)t.toMicroSeconds() - reply->timeStamp.seconds*1000000;
- pthread_mutex_lock (&mycamera->cameraI->mutex);
+ pthread_mutex_lock (&mycamera->cameraI->mutexDepth);
reply->pixelData.resize(mycamera->cameraI->imageDepth.rows*mycamera->cameraI->imageDepth.cols*3);
memcpy( &(reply->pixelData[0]), (unsigned char *) mycamera->cameraI->imageDepth.data, mycamera->cameraI->imageDepth.rows*mycamera->cameraI->imageDepth.cols*3);
- pthread_mutex_unlock (&mycamera->cameraI->mutex);
+ pthread_mutex_unlock (&mycamera->cameraI->mutexDepth);
{ //critical region start
IceUtil::Mutex::Lock sync(requestsMutex);
@@ -664,11 +599,11 @@
Ice::ObjectPtr object = new KinectI(std::string("KinectGazebo"), context, kinect);
Ice::ObjectPtr object2 = new CameraI(std::string("KinectGazebo"), context, kinect);
- //Ice::ObjectPtr object3 = new CameraII(std::string("KinectGazebo"), context, kinect);
+ Ice::ObjectPtr object3 = new CameraII(std::string("KinectGazebo"), context, kinect);
adapter->add(object, ic->stringToIdentity("Kinect"));
adapter->add(object2, ic->stringToIdentity("KinectRGB"));
- //adapter->add(object3, ic->stringToIdentity("KinectDepth"));
+ adapter->add(object3, ic->stringToIdentity("KinectDepth"));
std::cout << " adapter->add(object, ic->stringToIdentity(Kinect)); " << std::endl;
adapter->activate();
ic->waitForShutdown();
Modified: trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h
===================================================================
--- trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h 2013-03-19 09:45:37 UTC (rev 888)
+++ trunk/src/components/pluginsGazebo/kinect/kinectPlugin.h 2013-03-20 08:21:35 UTC (rev 889)
@@ -20,11 +20,11 @@
#include <string>
-#include "common/Plugin.hh"
-#include "sensors/DepthCameraSensor.hh"
-#include "sensors/CameraSensor.hh"
-#include "rendering/DepthCamera.hh"
-#include "gazebo.hh"
+#include <common/Plugin.hh>
+#include <sensors/DepthCameraSensor.hh>
+#include <sensors/CameraSensor.hh>
+#include <rendering/DepthCamera.hh>
+#include <gazebo.hh>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/point_types.h>
@@ -88,11 +88,14 @@
private: event::ConnectionPtr newRGBPointCloudConnection;
private: event::ConnectionPtr newImageFrameConnection;
//private: pcl::visualization::CloudViewer viewer;
+
+ void depth2rgb(cv::Mat image);
private: int count;
public:
std::string nameKinect;
pthread_mutex_t mutex;
pthread_mutex_t mutexRGB;
+ pthread_mutex_t mutexDepth;
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud;
cv::Mat imageRGB;
cv::Mat imageDepth;
More information about the Jderobot-admin
mailing list