camera_info_display.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <rviz/uniform_string_stream.h>
00037 #include "camera_info_display.h"
00038 #include <OGRE/OgreMaterialManager.h>
00039 #include <OGRE/OgreMaterial.h>
00040 #include <OGRE/OgreBlendMode.h>
00041 #include <QImage>
00042 #include <OGRE/OgreHardwarePixelBuffer.h>
00043 
00044 namespace jsk_rviz_plugin
00045 {
00046   TrianglePolygon::TrianglePolygon(
00047     Ogre::SceneManager* manager,
00048     Ogre::SceneNode* node,
00049     const cv::Point3d& O,
00050     const cv::Point3d& A,
00051     const cv::Point3d& B,
00052     const std::string& name,
00053     const Ogre::ColourValue& color)
00054   {
00055     // uniq string is requred for name
00056     
00057     manual_ = manager->createManualObject();
00058     manual_->clear();
00059     manual_->begin(name,
00060                    Ogre::RenderOperation::OT_TRIANGLE_STRIP);
00061     manual_->position(O.x, O.y, O.z);
00062     manual_->colour(color);
00063     manual_->position(A.x, A.y, A.z);
00064     manual_->colour(color);
00065     manual_->position(B.x, B.y, B.z);
00066     manual_->colour(color);
00067     manual_->end();
00068     node->attachObject(manual_);
00069   }
00070 
00071   TrianglePolygon::~TrianglePolygon()
00072   {
00073     manual_->detachFromParent();
00074     //manager_->destroyManualObject(manual_); // this crashes rviz
00075   }
00076   
00077   CameraInfoDisplay::CameraInfoDisplay()
00078   {
00080     // initialize properties
00082     far_clip_distance_property_ = new rviz::FloatProperty(
00083       "far clip",
00084       1.0,
00085       "far clip distance from the origin of camera info",
00086       this, SLOT(updateFarClipDistance()));
00087     show_polygons_property_ = new rviz::BoolProperty(
00088       "show polygons",
00089       true,
00090       "show polygons of the region of the camera info",
00091       this, SLOT(updateShowPolygons()));
00092     color_property_ = new rviz::ColorProperty(
00093       "color",
00094       QColor(85, 255, 255),
00095       "color of CameraInfo",
00096       this, SLOT(updateColor()));
00097     edge_color_property_ = new rviz::ColorProperty(
00098       "edge color",
00099       QColor(125, 125, 125),
00100       "edge color of CameraInfo",
00101       this, SLOT(updateEdgeColor()));
00102     alpha_property_ = new rviz::FloatProperty(
00103       "alpha",
00104       0.5,
00105       "alpha blending value",
00106       this, SLOT(updateAlpha()));
00107   }
00108   
00109   CameraInfoDisplay::~CameraInfoDisplay()
00110   {
00111     if (edges_) {
00112       edges_->clear();
00113     }
00114     polygons_.clear();
00115     delete far_clip_distance_property_;
00116     delete color_property_;
00117     delete alpha_property_;
00118     delete show_polygons_property_;
00119     delete edge_color_property_;
00120   }
00121 
00122   void CameraInfoDisplay::reset()
00123   {
00124     MFDClass::reset();
00125     if (edges_) {
00126       edges_->clear();
00127     }
00128     polygons_.clear();
00129     camera_info_ = sensor_msgs::CameraInfo::ConstPtr(); // reset to NULL
00130   }
00131 
00132   void CameraInfoDisplay::onInitialize()
00133   {
00134     MFDClass::onInitialize();
00135     scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00136     updateColor();
00137     updateAlpha();
00138     updateFarClipDistance();
00139     updateShowPolygons();
00140     updateEdgeColor();
00141   }
00142   
00143   void CameraInfoDisplay::processMessage(
00144     const sensor_msgs::CameraInfo::ConstPtr& msg)
00145   {
00146     if (!isSameCameraInfo(msg)) {
00147       createCameraInfoShapes(msg);
00148     }
00149     // move scene_node according to tf
00150      Ogre::Vector3 position;
00151      Ogre::Quaternion quaternion;
00152      if(!context_->getFrameManager()->getTransform(msg->header.frame_id,
00153                                                    msg->header.stamp,
00154                                                    position,
00155                                                    quaternion)) {
00156        ROS_ERROR( "Error transforming pose '%s' from frame '%s' to frame '%s'",
00157                   qPrintable( getName() ), msg->header.frame_id.c_str(),
00158                   qPrintable( fixed_frame_ ));
00159      }
00160      scene_node_->setPosition(position);
00161      scene_node_->setOrientation(quaternion);
00162      camera_info_ = msg;        // store for caching
00163   }
00164 
00165   bool CameraInfoDisplay::isSameCameraInfo(
00166     const sensor_msgs::CameraInfo::ConstPtr& msg)
00167   {
00168     if (camera_info_) {
00169       bool meta_same_p = 
00170         msg->header.frame_id == camera_info_->header.frame_id &&
00171         msg->height == camera_info_->height &&
00172         msg->width == camera_info_->width &&
00173         msg->distortion_model == camera_info_->distortion_model;
00174       if (meta_same_p) {
00175         for (size_t i = 0; i < msg->P.size(); i++) {
00176           if (msg->P[i] != camera_info_->P[i]) {
00177             return false;
00178           }
00179         }
00180         return true;
00181       }
00182       else {
00183         return false;
00184       }
00185     }
00186     else {
00187       return false;
00188     }
00189   }
00190   
00191   void CameraInfoDisplay::addPointToEdge(
00192     const cv::Point3d& point)
00193   {
00194     Ogre::Vector3 p;
00195     p[0] = point.x;
00196     p[1] = point.y;
00197     p[2] = point.z;
00198     edges_->addPoint(p);
00199   }
00200 
00201   void CameraInfoDisplay::addPolygon(
00202     const cv::Point3d& O, const cv::Point3d& A, const cv::Point3d& B)
00203   {
00204     Ogre::ColourValue color = rviz::qtToOgre(color_);
00205     color.a = alpha_;
00206     TrianglePolygon::Ptr triangle (new TrianglePolygon(
00207                                      scene_manager_,
00208                                      scene_node_,
00209                                      O, A, B, material_->getName(),
00210                                      color));
00211     polygons_.push_back(triangle);
00212   }
00213 
00214   void CameraInfoDisplay::prepareMaterial()
00215   {
00216     if (texture_.isNull()) {
00217       static uint32_t count = 0;
00218       rviz::UniformStringStream ss;
00219       ss << "CameraInfoDisplayPolygon" << count++;
00220       material_
00221         = Ogre::MaterialManager::getSingleton().create(
00222           ss.str(),
00223           Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
00224     
00225       texture_ = Ogre::TextureManager::getSingleton().createManual(
00226         material_->getName() + "Texture",        // name
00227         Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
00228         Ogre::TEX_TYPE_2D, 1, 1, 0, Ogre::PF_A8R8G8B8, Ogre::TU_DEFAULT);
00229     
00230       material_->getTechnique(0)->getPass(0)->setColourWriteEnabled(true);
00231       Ogre::ColourValue color = rviz::qtToOgre(color_);
00232       color.a = alpha_;
00233       material_->getTechnique(0)->getPass(0)->setAmbient(color);
00234       material_->setReceiveShadows(false);
00235       material_->getTechnique(0)->setLightingEnabled(true);
00236       material_->getTechnique(0)->getPass(0)->setCullingMode(Ogre::CULL_NONE);
00237       material_->getTechnique(0)->getPass(0)->setLightingEnabled(false);
00238       material_->getTechnique(0)->getPass(0)->setDepthWriteEnabled(false);
00239       material_->getTechnique(0)->getPass(0)->setDepthCheckEnabled(true);
00240       
00241       material_->getTechnique(0)->getPass(0)->setVertexColourTracking(Ogre::TVC_DIFFUSE);
00242       material_->getTechnique(0)->getPass(0)->createTextureUnitState(texture_->getName());
00243       material_->getTechnique(0)->getPass(0)->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
00244     }
00245   }
00246 
00247   
00248   void CameraInfoDisplay::createCameraInfoShapes(
00249     const sensor_msgs::CameraInfo::ConstPtr& msg)
00250   {
00251     polygons_.clear();
00252     image_geometry::PinholeCameraModel model;
00253     bool model_success_p = model.fromCameraInfo(msg);
00254     if (!model_success_p) {
00255       ROS_ERROR("failed to create camera model");
00256       return;
00257     }
00258     
00260     // initialize BillboardLine
00262     if (!edges_) {
00263       edges_.reset(new rviz::BillboardLine(context_->getSceneManager(),
00264                                            scene_node_));
00265       edges_->setLineWidth(0.01);
00266     }
00267     
00268     cv::Point2d a(0, 0), b(msg->width, 0),
00269       c(msg->width, msg->height), d(0, msg->height);
00270     // all the z = 1.0
00271     cv::Point3d A = model.projectPixelTo3dRay(a);
00272     cv::Point3d B = model.projectPixelTo3dRay(b);
00273     cv::Point3d C = model.projectPixelTo3dRay(c);
00274     cv::Point3d D = model.projectPixelTo3dRay(d);
00275 
00276     cv::Point3d scaled_A = A * far_clip_distance_;
00277     cv::Point3d scaled_B = B * far_clip_distance_;
00278     cv::Point3d scaled_C = C * far_clip_distance_;
00279     cv::Point3d scaled_D = D * far_clip_distance_;
00280 
00281     cv::Point3d O(0, 0, 0);
00282 
00284     // build polygons
00286     if (show_polygons_) {
00288       // setup color for polygons
00290       Ogre::ColourValue color = rviz::qtToOgre(color_);
00291       color.a = alpha_;
00292       prepareMaterial();
00293       material_->getTechnique(0)->getPass(0)->setAmbient(color);
00294       {      
00295         texture_->getBuffer()->lock( Ogre::HardwareBuffer::HBL_NORMAL );
00296         const Ogre::PixelBox& pixelBox = texture_->getBuffer()->getCurrentLock();
00297         Ogre::uint8* pDest = static_cast<Ogre::uint8*> (pixelBox.data);
00298         memset(pDest, 0, 1);
00299         QImage Hud(pDest, 1, 1, QImage::Format_ARGB32 );
00300         Hud.setPixel(0, 0, color_.rgba());
00301         texture_->getBuffer()->unlock();
00302       }
00303 
00304       addPolygon(O, scaled_B, scaled_A);
00305       addPolygon(O, scaled_C, scaled_B);
00306       addPolygon(O, scaled_D, scaled_C);
00307       addPolygon(O, scaled_A, scaled_D);
00308       addPolygon(scaled_A, scaled_B, scaled_D);
00309       addPolygon(scaled_B, scaled_C, scaled_D);
00310     }
00312     // build edges
00314     
00315     edges_->clear();
00316     edges_->setMaxPointsPerLine(2);
00317     edges_->setNumLines(8);
00318     edges_->setColor(edge_color_.red() / 255.0,
00319                      edge_color_.green() / 255.0,
00320                      edge_color_.blue() / 255.0,
00321                      alpha_);
00322     addPointToEdge(O); addPointToEdge(scaled_A); edges_->newLine();
00323     addPointToEdge(O); addPointToEdge(scaled_B); edges_->newLine();
00324     addPointToEdge(O); addPointToEdge(scaled_C); edges_->newLine();
00325     addPointToEdge(O); addPointToEdge(scaled_D); edges_->newLine();
00326     addPointToEdge(scaled_A); addPointToEdge(scaled_B); edges_->newLine();
00327     addPointToEdge(scaled_B); addPointToEdge(scaled_C); edges_->newLine();
00328     addPointToEdge(scaled_C); addPointToEdge(scaled_D); edges_->newLine();
00329     addPointToEdge(scaled_D); addPointToEdge(scaled_A);
00330   }
00331 
00333   // Properties updating functions
00335   void CameraInfoDisplay::updateColor()
00336   {
00337     color_ = color_property_->getColor();
00338     if (camera_info_) {
00339       createCameraInfoShapes(camera_info_);
00340     }
00341   }
00342 
00343   void CameraInfoDisplay::updateEdgeColor()
00344   {
00345     edge_color_ = edge_color_property_->getColor();
00346     if (camera_info_) {
00347       createCameraInfoShapes(camera_info_);
00348     }
00349   }
00350 
00351   void CameraInfoDisplay::updateAlpha()
00352   {
00353     alpha_ = alpha_property_->getFloat();
00354     if (camera_info_) {
00355       createCameraInfoShapes(camera_info_);
00356     }
00357   }
00358 
00359   void CameraInfoDisplay::updateFarClipDistance()
00360   {
00361     far_clip_distance_ = far_clip_distance_property_->getFloat();
00362     if (camera_info_) {
00363       createCameraInfoShapes(camera_info_);
00364     }
00365   }
00366 
00367   void CameraInfoDisplay::updateShowPolygons()
00368   {
00369     show_polygons_ = show_polygons_property_->getBool();
00370     if (camera_info_) {
00371       createCameraInfoShapes(camera_info_);
00372     }
00373   }
00374   
00375 }
00376 
00377 
00378 #include <pluginlib/class_list_macros.h>
00379 PLUGINLIB_EXPORT_CLASS( jsk_rviz_plugin::CameraInfoDisplay, rviz::Display )


jsk_rviz_plugins
Author(s): Kei Okada , Yohei Kakiuchi , Shohei Fujii , Ryohei Ueda
autogenerated on Mon Oct 6 2014 01:18:44