disparity_renderer.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "rviz_interaction_tools/disparity_renderer.h"
00031 #include "rviz_interaction_tools/image_tools.h"
00032 
00033 #include <sensor_msgs/image_encodings.h>
00034 
00035 #include <OGRE/OgreSceneNode.h>
00036 
00037 namespace rviz_interaction_tools
00038 {
00039 
00040 DisparityRenderer::DisparityRenderer( Ogre::SceneNode* scene_root, unsigned char render_queue_group ) :
00041   scene_root_(scene_root),
00042   new_point_cloud_(0)
00043 {
00044   image_point_cloud_.setRenderQueueGroup(render_queue_group);
00045   scene_root_->attachObject(&image_point_cloud_);
00046 }
00047 
00048 DisparityRenderer::~DisparityRenderer()
00049 {
00050   scene_root_->detachObject(&image_point_cloud_);
00051 }
00052 
00053 bool DisparityRenderer::setDisparityImage(
00054     const stereo_msgs::DisparityImage &disparity_image,
00055     const sensor_msgs::CameraInfo &camera_info, const sensor_msgs::Image *image )
00056 {
00057   boost::mutex::scoped_lock(mutex_);
00058 
00059   //-------------------- prepare point cloud from image
00060 
00061   raw_points_.clear();
00062   raw_points_.reserve(disparity_image.image.width
00063       * disparity_image.image.height);
00064 
00065   rviz_interaction_tools::PointCloud::Point point;
00066   point.setColor(1, 1, 1);
00067 
00068   for (unsigned int i = 0; i < disparity_image.image.height; i++)
00069   {
00070     for (unsigned int j = 0; j < disparity_image.image.width; j++)
00071     {
00072       if (rviz_interaction_tools::hasDisparityValue(disparity_image, i, j))
00073       {
00074 
00075         rviz_interaction_tools::getPoint(disparity_image, camera_info, i, j,
00076             point.x, point.y, point.z);
00077         if (std::isnan(point.x) || std::isinf(point.x))
00078           continue;
00079         if (std::isnan(point.y) || std::isinf(point.y))
00080           continue;
00081         if (std::isnan(point.z) || std::isinf(point.z))
00082           continue;
00083 
00084         if (point.x < -50 || point.x > 50)
00085           ROS_INFO("Point x: %f", point.x);
00086         if (point.y < -50 || point.y > 50)
00087           ROS_INFO("Point x: %f", point.y);
00088         if (point.z < -50 || point.z > 50)
00089           ROS_INFO("Point x: %f", point.z);
00090 
00091         if (image)
00092         {
00093           if (image->encoding == sensor_msgs::image_encodings::MONO8)
00094           {
00095             float l = float(image->data[i * image->step + j]) / 255.0;
00096             point.setColor(l, l, l);
00097           }
00098           else if (image->encoding == sensor_msgs::image_encodings::BGR8)
00099           {
00100             float b = float(image->data[i * image->step + j*3]) / 255.0;
00101             float g = float(image->data[i * image->step + j*3 + 1]) / 255.0;
00102             float r = float(image->data[i * image->step + j*3 + 2]) / 255.0;
00103             point.setColor(r, g, b);
00104           }
00105           else if (image->encoding == sensor_msgs::image_encodings::RGB8)
00106           {
00107             float r = float(image->data[i * image->step + j*3]) / 255.0;
00108             float g = float(image->data[i * image->step + j*3 + 1]) / 255.0;
00109             float b = float(image->data[i * image->step + j*3 + 2]) / 255.0;
00110             point.setColor(r, g, b);
00111           }
00112           else
00113           {
00114             point.setColor(1,1,1);
00115           }
00116         }
00117 
00118         raw_points_.push_back(point);
00119       }
00120     }
00121   }
00122 
00123   new_point_cloud_ = true;
00124   return true;
00125 }
00126 
00127 bool DisparityRenderer::update()
00128 {
00129   boost::mutex::scoped_lock(mutex_);
00130 
00131   if (!new_point_cloud_)
00132   {
00133     return false;
00134   }
00135 
00136   image_point_cloud_.clear();
00137   image_point_cloud_.addPoints(&raw_points_[0], raw_points_.size());
00138 
00139   Ogre::Vector3 min = image_point_cloud_.getBoundingBox().getMinimum();
00140   Ogre::Vector3 max = image_point_cloud_.getBoundingBox().getMaximum();
00141   ROS_DEBUG("Bbox min: %f %f %f max: %f %f %f  # of points: %d",
00142       min.x, min.y, min.z, max.x, max.y, max.z, (int)raw_points_.size() );
00143 
00144   return true;
00145 }
00146 
00147 void DisparityRenderer::clear()
00148 {
00149   boost::mutex::scoped_lock(mutex_);
00150   image_point_cloud_.clear();
00151 }
00152 
00153 }


rviz_interaction_tools
Author(s): David Gossow
autogenerated on Mon Oct 6 2014 03:03:25