Added RVIZ plugin "MapCloud" to show incrementally point clouds of the map in RVIZ

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/ros-pkg@1456 f169173b-cf89-36c8-b27e-44dbe73f0c83
2026-10-04 08:47:45 +08:00 · 2014-07-04 23:13:33 +00:00
parent 4384dead67
commit 1fa95fa358
2 changed files with 836 additions and 0 deletions
@@ -0,0 +1,682 @@
+
+#include <OgreSceneNode.h>
+#include <OgreSceneManager.h>
+
+#include <ros/time.h>
+
+#include <tf/transform_listener.h>
+
+#include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
+#include <rviz/ogre_helpers/point_cloud.h>
+#include <rviz/validate_floats.h>
+#include <rviz/properties/int_property.h>
+#include "rviz/properties/bool_property.h"
+#include "rviz/properties/enum_property.h"
+#include "rviz/properties/float_property.h"
+#include "rviz/properties/vector_property.h"
+
+#include <pcl_conversions/pcl_conversions.h>
+
+#include "MapCloudDisplay.h"
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/core/util3d.h>
+#include <rtabmap/MsgConversion.h>
+
+namespace rtabmap
+{
+
+
+MapCloudDisplay::CloudInfo::CloudInfo()
+: manager_(0)
+, scene_node_(0)
+{}
+
+MapCloudDisplay::CloudInfo::~CloudInfo()
+{
+  clear();
+}
+
+void MapCloudDisplay::CloudInfo::clear()
+{
+  if ( scene_node_ )
+  {
+    manager_->destroySceneNode( scene_node_ );
+    scene_node_=0;
+  }
+}
+
+MapCloudDisplay::MapCloudDisplay()
+  : spinner_(1, &cbqueue_),
+    transformer_class_loader_(NULL)
+{
+	//QIcon icon;
+	//this->setIcon(icon);
+
+    style_property_ = new rviz::EnumProperty( "Style", "Flat Squares",
+                                        "Rendering mode to use, in order of computational complexity.",
+                                        this, SLOT( updateStyle() ), this );
+    style_property_->addOption( "Points", rviz::PointCloud::RM_POINTS );
+    style_property_->addOption( "Squares", rviz::PointCloud::RM_SQUARES );
+    style_property_->addOption( "Flat Squares", rviz::PointCloud::RM_FLAT_SQUARES );
+    style_property_->addOption( "Spheres", rviz::PointCloud::RM_SPHERES );
+    style_property_->addOption( "Boxes", rviz::PointCloud::RM_BOXES );
+
+    point_world_size_property_ = new rviz::FloatProperty( "Size (m)", 0.01,
+                                                  "Point size in meters.",
+                                                  this, SLOT( updateBillboardSize() ), this );
+    point_world_size_property_->setMin( 0.0001 );
+
+    point_pixel_size_property_ = new rviz::FloatProperty( "Size (Pixels)", 3,
+                                                  "Point size in pixels.",
+                                                  this, SLOT( updateBillboardSize() ), this );
+    point_pixel_size_property_->setMin( 1 );
+
+    alpha_property_ = new rviz::FloatProperty( "Alpha", 1.0,
+                                         "Amount of transparency to apply to the points.  Note that this is experimental and does not always look correct.",
+                                         this, SLOT( updateAlpha() ), this );
+    alpha_property_->setMin( 0 );
+    alpha_property_->setMax( 1 );
+
+    xyz_transformer_property_ = new rviz::EnumProperty( "Position Transformer", "",
+                                                  "Set the transformer to use to set the position of the points.",
+                                                  this, SLOT( updateXyzTransformer() ), this );
+    connect( xyz_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
+             this, SLOT( setXyzTransformerOptions( EnumProperty* )));
+
+    color_transformer_property_ = new rviz::EnumProperty( "Color Transformer", "",
+                                                    "Set the transformer to use to set the color of the points.",
+                                                    this, SLOT( updateColorTransformer() ), this );
+    connect( color_transformer_property_, SIGNAL( requestOptions( EnumProperty* )),
+             this, SLOT( setColorTransformerOptions( EnumProperty* )));
+
+  // PointCloudCommon sets up a callback queue with a thread for each
+  // instance.  Use that for processing incoming messages.
+  update_nh_.setCallbackQueue( &cbqueue_ );
+}
+
+MapCloudDisplay::~MapCloudDisplay()
+{
+	if ( transformer_class_loader_ )
+	{
+		delete transformer_class_loader_;
+	}
+
+	spinner_.stop();
+}
+
+void MapCloudDisplay::loadTransformers()
+{
+  std::vector<std::string> classes = transformer_class_loader_->getDeclaredClasses();
+  std::vector<std::string>::iterator ci;
+
+  for( ci = classes.begin(); ci != classes.end(); ci++ )
+  {
+    const std::string& lookup_name = *ci;
+    std::string name = transformer_class_loader_->getName( lookup_name );
+
+    if( transformers_.count( name ) > 0 )
+    {
+      ROS_ERROR( "Transformer type [%s] is already loaded.", name.c_str() );
+      continue;
+    }
+
+    rviz::PointCloudTransformerPtr trans( transformer_class_loader_->createUnmanagedInstance( lookup_name ));
+    trans->init();
+    connect( trans.get(), SIGNAL( needRetransform() ), this, SLOT( causeRetransform() ));
+
+    TransformerInfo info;
+    info.transformer = trans;
+    info.readable_name = name;
+    info.lookup_name = lookup_name;
+
+    info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_XYZ, info.xyz_props );
+    setPropertiesHidden( info.xyz_props, true );
+
+    info.transformer->createProperties( this, rviz::PointCloudTransformer::Support_Color, info.color_props );
+    setPropertiesHidden( info.color_props, true );
+
+    transformers_[ name ] = info;
+  }
+}
+
+void MapCloudDisplay::onInitialize()
+{
+	MFDClass::onInitialize();
+
+	transformer_class_loader_ = new pluginlib::ClassLoader<rviz::PointCloudTransformer>( "rviz", "rviz::PointCloudTransformer" );
+	loadTransformers();
+
+	updateStyle();
+	updateBillboardSize();
+	updateAlpha();
+
+	spinner_.start();
+}
+
+void MapCloudDisplay::processMessage( const rtabmap::InfoExConstPtr& msg )
+{
+	// Add new clouds...
+	for(unsigned int i=0; i<msg->data.localTransformIDs.size() && i<msg->data.localTransforms.size(); ++i)
+	{
+		int id = msg->data.localTransformIDs[i];
+		if(cloud_infos_.find(id) == cloud_infos_.end())
+		{
+			// Cloud not added to RVIZ, add it!
+			rtabmap::Transform localTransform = transformFromGeometryMsg(msg->data.localTransforms[i]);
+			if(!localTransform.isNull())
+			{
+				cv::Mat image, depth;
+				float depthConstant = 0.0f;
+				rtabmap::Transform pose;
+
+				for(unsigned int i=0; i<msg->data.imageIDs.size() && i<msg->data.images.size(); ++i)
+				{
+					if(msg->data.imageIDs[i] == id)
+					{
+						image = util3d::uncompressImage(msg->data.images[i].bytes);
+						break;
+					}
+				}
+				for(unsigned int i=0; i<msg->data.depthIDs.size() && i<msg->data.depths.size(); ++i)
+				{
+					if(msg->data.depthIDs[i] == id)
+					{
+						depth = util3d::uncompressImage(msg->data.depths[i].bytes);
+						break;
+					}
+				}
+				for(unsigned int i=0; i<msg->data.depthConstantIDs.size() && i<msg->data.depthConstants.size(); ++i)
+				{
+					if(msg->data.depthConstantIDs[i] == id)
+					{
+						depthConstant = msg->data.depthConstants[i];
+						break;
+					}
+				}
+				for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+				{
+					if(msg->data.poseIDs[i] == id)
+					{
+						pose = transformFromPoseMsg(msg->data.poses[i]);
+						break;
+					}
+				}
+
+				if(!image.empty() && !depth.empty() && depthConstant > 0.0f && !pose.isNull())
+				{
+					int cloudDecimation_ = 4;
+					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudFromDepthRGB(image, depth, depthConstant, cloudDecimation_);
+
+					float cloudMaxDepth_ = 4.0f;
+					float cloudVoxelSize_ = 0.02f;
+					if(cloudMaxDepth_ > 0)
+					{
+						cloud = util3d::passThrough(cloud, "z", 0, cloudMaxDepth_);
+					}
+					if(cloudVoxelSize_ > 0)
+					{
+						cloud = util3d::voxelize(cloud, cloudVoxelSize_);
+					}
+
+					cloud = util3d::transformPointCloud(cloud, localTransform);
+
+					sensor_msgs::PointCloud2::Ptr cloudMsg(new sensor_msgs::PointCloud2);
+					pcl::toROSMsg(*cloud, *cloudMsg);
+					cloudMsg->header = msg->data.header;
+
+					CloudInfoPtr info(new CloudInfo);
+					info->message_ = cloudMsg;
+					info->pose_ = pose;
+					info->id_ = id;
+
+					if (transformCloud(info, true))
+					{
+						boost::mutex::scoped_lock lock(new_clouds_mutex_);
+						new_cloud_infos_.insert(std::make_pair(id, info));
+					}
+				}
+			}
+		}
+	}
+
+	// Update graph
+	std::map<int, Transform> poses;
+	for(unsigned int i=0; i<msg->data.poseIDs.size() && i<msg->data.poses.size(); ++i)
+	{
+		poses.insert(std::make_pair(msg->data.poseIDs[i], transformFromPoseMsg(msg->data.poses[i])));
+	}
+
+	{
+		boost::mutex::scoped_lock lock(current_map_mutex_);
+		current_map_ = poses;
+	}
+
+	this->emitTimeSignal(msg->header.stamp);
+}
+
+void MapCloudDisplay::setPropertiesHidden( const QList<Property*>& props, bool hide )
+{
+  for( int i = 0; i < props.size(); i++ )
+  {
+    props[ i ]->setHidden( hide );
+  }
+}
+
+void MapCloudDisplay::updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud )
+{
+  std::string xyz_name = xyz_transformer_property_->getStdString();
+  std::string color_name = color_transformer_property_->getStdString();
+
+  xyz_transformer_property_->clearOptions();
+  color_transformer_property_->clearOptions();
+
+  // Get the channels that we could potentially render
+  typedef std::set<std::pair<uint8_t, std::string> > S_string;
+  S_string valid_xyz, valid_color;
+  bool cur_xyz_valid = false;
+  bool cur_color_valid = false;
+  bool has_rgb_transformer = false;
+  M_TransformerInfo::iterator trans_it = transformers_.begin();
+  M_TransformerInfo::iterator trans_end = transformers_.end();
+  for(;trans_it != trans_end; ++trans_it)
+  {
+    const std::string& name = trans_it->first;
+    const rviz::PointCloudTransformerPtr& trans = trans_it->second.transformer;
+    uint32_t mask = trans->supports(cloud);
+    if (mask & rviz::PointCloudTransformer::Support_XYZ)
+    {
+      valid_xyz.insert(std::make_pair(trans->score(cloud), name));
+      if (name == xyz_name)
+      {
+        cur_xyz_valid = true;
+      }
+      xyz_transformer_property_->addOptionStd( name );
+    }
+
+    if (mask & rviz::PointCloudTransformer::Support_Color)
+    {
+      valid_color.insert(std::make_pair(trans->score(cloud), name));
+      if (name == color_name)
+      {
+        cur_color_valid = true;
+      }
+      if (name == "RGB8")
+      {
+        has_rgb_transformer = true;
+      }
+      color_transformer_property_->addOptionStd( name );
+    }
+  }
+
+  if( !cur_xyz_valid )
+  {
+    if( !valid_xyz.empty() )
+    {
+      xyz_transformer_property_->setStringStd( valid_xyz.rbegin()->second );
+    }
+  }
+
+  if( !cur_color_valid )
+  {
+    if( !valid_color.empty() )
+    {
+      if (has_rgb_transformer)
+      {
+        color_transformer_property_->setStringStd( "RGB8" );
+      } else
+      {
+        color_transformer_property_->setStringStd( valid_color.rbegin()->second );
+      }
+    }
+  }
+}
+
+void MapCloudDisplay::updateAlpha()
+{
+  for ( unsigned i=0; i<cloud_infos_.size(); i++ )
+  {
+    cloud_infos_[i]->cloud_->setAlpha( alpha_property_->getFloat() );
+  }
+}
+
+void MapCloudDisplay::updateStyle()
+{
+  rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+  if( mode == rviz::PointCloud::RM_POINTS )
+  {
+    point_world_size_property_->hide();
+    point_pixel_size_property_->show();
+  }
+  else
+  {
+    point_world_size_property_->show();
+    point_pixel_size_property_->hide();
+  }
+  for( unsigned int i = 0; i < cloud_infos_.size(); i++ )
+  {
+    cloud_infos_[i]->cloud_->setRenderMode( mode );
+  }
+  updateBillboardSize();
+}
+
+void MapCloudDisplay::updateBillboardSize()
+{
+  rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+  float size;
+  if( mode == rviz::PointCloud::RM_POINTS ) {
+    size = point_pixel_size_property_->getFloat();
+  } else {
+    size = point_world_size_property_->getFloat();
+  }
+  for ( unsigned i=0; i<cloud_infos_.size(); i++ )
+  {
+    cloud_infos_[i]->cloud_->setDimensions( size, size, size );
+  }
+  context_->queueRender();
+}
+
+void MapCloudDisplay::causeRetransform()
+{
+  needs_retransform_ = true;
+}
+
+void MapCloudDisplay::update( float wall_dt, float ros_dt )
+{
+	rviz::PointCloud::RenderMode mode = (rviz::PointCloud::RenderMode) style_property_->getOptionInt();
+
+	if (needs_retransform_)
+	{
+		retransform();
+		needs_retransform_ = false;
+	}
+
+	{
+		boost::mutex::scoped_lock lock(new_clouds_mutex_);
+		if( !new_cloud_infos_.empty() )
+		{
+			float size;
+			if( mode == rviz::PointCloud::RM_POINTS ) {
+				size = point_pixel_size_property_->getFloat();
+			} else {
+				size = point_world_size_property_->getFloat();
+			}
+
+			std::map<int, CloudInfoPtr>::iterator it = new_cloud_infos_.begin();
+			std::map<int, CloudInfoPtr>::iterator end = new_cloud_infos_.end();
+			for (; it != end; ++it)
+			{
+				CloudInfoPtr cloud_info = it->second;
+
+				cloud_info->cloud_.reset( new rviz::PointCloud() );
+				cloud_info->cloud_->addPoints( &(cloud_info->transformed_points_.front()), cloud_info->transformed_points_.size() );
+				cloud_info->cloud_->setRenderMode( mode );
+				cloud_info->cloud_->setAlpha( alpha_property_->getFloat() );
+				cloud_info->cloud_->setDimensions( size, size, size );
+				cloud_info->cloud_->setAutoSize(false);
+
+				cloud_info->manager_ = context_->getSceneManager();
+
+				cloud_info->scene_node_ = scene_node_->createChildSceneNode( cloud_info->position_, cloud_info->orientation_ );
+
+				cloud_info->scene_node_->attachObject( cloud_info->cloud_.get() );
+
+				cloud_infos_.insert(*it);
+			}
+
+			new_cloud_infos_.clear();
+		}
+	}
+
+	{
+		boost::recursive_mutex::scoped_try_lock lock( transformers_mutex_ );
+
+		if( lock.owns_lock() )
+		{
+			if( new_xyz_transformer_ || new_color_transformer_ )
+			{
+				M_TransformerInfo::iterator it = transformers_.begin();
+				M_TransformerInfo::iterator end = transformers_.end();
+				for (; it != end; ++it)
+				{
+					const std::string& name = it->first;
+					TransformerInfo& info = it->second;
+
+					setPropertiesHidden( info.xyz_props, name != xyz_transformer_property_->getStdString() );
+					setPropertiesHidden( info.color_props, name != color_transformer_property_->getStdString() );
+				}
+			}
+		}
+
+		new_xyz_transformer_ = false;
+		new_color_transformer_ = false;
+	}
+
+	{
+		// update poses
+		boost::mutex::scoped_lock lock(current_map_mutex_);
+		if(!current_map_.empty())
+		{
+			for (std::map<int, rtabmap::Transform>::iterator it=current_map_.begin(); it != current_map_.end(); ++it)
+			{
+				std::map<int, CloudInfoPtr>::iterator cloudInfoIt = cloud_infos_.find(it->first);
+				if(cloudInfoIt != cloud_infos_.end())
+				{
+					cloudInfoIt->second->pose_ = it->second;
+					if (context_->getFrameManager()->getTransform(cloudInfoIt->second->message_->header, cloudInfoIt->second->position_, cloudInfoIt->second->orientation_))
+					{
+						// Set pose
+						Ogre::Matrix4 frameTransform;
+						frameTransform.makeTransform( cloudInfoIt->second->position_, Ogre::Vector3(1,1,1), cloudInfoIt->second->orientation_ );
+						const rtabmap::Transform & p = cloudInfoIt->second->pose_;
+						Ogre::Matrix4 pose(p[0], p[1], p[2], p[3],
+										 p[4], p[5], p[6], p[7],
+										 p[8], p[9], p[10], p[11],
+										 0, 0, 0, 1);
+						frameTransform = frameTransform * pose;
+						cloudInfoIt->second->position_ = frameTransform.getTrans();
+						cloudInfoIt->second->orientation_ = frameTransform.extractQuaternion();
+
+						cloudInfoIt->second->scene_node_->setPosition(cloudInfoIt->second->position_);
+						cloudInfoIt->second->scene_node_->setOrientation(cloudInfoIt->second->orientation_);
+						cloudInfoIt->second->scene_node_->setVisible(true);
+					}
+					else
+					{
+						ROS_ERROR("MapCloudDisplay: Could not update pose of node %d", it->first);
+					}
+
+				}
+			}
+			//hide not used clouds
+			for(std::map<int, CloudInfoPtr>::iterator iter = cloud_infos_.begin(); iter!=cloud_infos_.end(); ++iter)
+			{
+				if(current_map_.find(iter->first) == current_map_.end())
+				{
+					iter->second->scene_node_->setVisible(false);
+				}
+			}
+		}
+	}
+
+	std::stringstream ss;
+	//ss << "Showing [" << total_point_count_ << "] points from [" << clouds_.size() << "] messages";
+	this->setStatusStd(rviz::StatusProperty::Ok, "Points", ss.str());
+}
+
+void MapCloudDisplay::reset()
+{
+	MFDClass::reset();
+	{
+		boost::mutex::scoped_lock lock(new_clouds_mutex_);
+		cloud_infos_.clear();
+		new_cloud_infos_.clear();
+	}
+	{
+		boost::mutex::scoped_lock lock(current_map_mutex_);
+		current_map_.clear();
+	}
+}
+
+void MapCloudDisplay::updateXyzTransformer()
+{
+  boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+  if( transformers_.count( xyz_transformer_property_->getStdString() ) == 0 )
+  {
+    return;
+  }
+  new_xyz_transformer_ = true;
+  causeRetransform();
+}
+
+void MapCloudDisplay::updateColorTransformer()
+{
+  boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+  if( transformers_.count( color_transformer_property_->getStdString() ) == 0 )
+  {
+    return;
+  }
+  new_color_transformer_ = true;
+  causeRetransform();
+}
+
+void MapCloudDisplay::setXyzTransformerOptions( EnumProperty* prop )
+{
+  fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_XYZ );
+}
+
+void MapCloudDisplay::setColorTransformerOptions( EnumProperty* prop )
+{
+  fillTransformerOptions( prop, rviz::PointCloudTransformer::Support_Color );
+}
+
+void MapCloudDisplay::fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask )
+{
+  prop->clearOptions();
+
+  if (cloud_infos_.empty())
+  {
+    return;
+  }
+
+  boost::recursive_mutex::scoped_lock tlock(transformers_mutex_);
+
+  const sensor_msgs::PointCloud2ConstPtr& msg = cloud_infos_.begin()->second->message_;
+
+  M_TransformerInfo::iterator it = transformers_.begin();
+  M_TransformerInfo::iterator end = transformers_.end();
+  for (; it != end; ++it)
+  {
+    const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+    if ((trans->supports(msg) & mask) == mask)
+    {
+      prop->addOption( QString::fromStdString( it->first ));
+    }
+  }
+}
+
+rviz::PointCloudTransformerPtr MapCloudDisplay::getXYZTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
+{
+  boost::recursive_mutex::scoped_lock lock( transformers_mutex_);
+  M_TransformerInfo::iterator it = transformers_.find( xyz_transformer_property_->getStdString() );
+  if( it != transformers_.end() )
+  {
+    const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+    if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_XYZ )
+    {
+      return trans;
+    }
+  }
+
+  return rviz::PointCloudTransformerPtr();
+}
+
+rviz::PointCloudTransformerPtr MapCloudDisplay::getColorTransformer( const sensor_msgs::PointCloud2ConstPtr& cloud )
+{
+  boost::recursive_mutex::scoped_lock lock( transformers_mutex_ );
+  M_TransformerInfo::iterator it = transformers_.find( color_transformer_property_->getStdString() );
+  if( it != transformers_.end() )
+  {
+    const rviz::PointCloudTransformerPtr& trans = it->second.transformer;
+    if( trans->supports( cloud ) & rviz::PointCloudTransformer::Support_Color )
+    {
+      return trans;
+    }
+  }
+
+  return rviz::PointCloudTransformerPtr();
+}
+
+
+void MapCloudDisplay::retransform()
+{
+  boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
+
+  std::map<int, CloudInfoPtr>::iterator it = cloud_infos_.begin();
+  std::map<int, CloudInfoPtr>::iterator end = cloud_infos_.end();
+  for (; it != end; ++it)
+  {
+    const CloudInfoPtr& cloud_info = it->second;
+    transformCloud(cloud_info, false);
+    cloud_info->cloud_->clear();
+    cloud_info->cloud_->addPoints(&cloud_info->transformed_points_.front(), cloud_info->transformed_points_.size());
+  }
+}
+
+bool MapCloudDisplay::transformCloud(const CloudInfoPtr& cloud_info, bool update_transformers)
+{
+  rviz::V_PointCloudPoint& cloud_points = cloud_info->transformed_points_;
+  cloud_points.clear();
+
+  size_t size = cloud_info->message_->width * cloud_info->message_->height;
+  rviz::PointCloud::Point default_pt;
+  default_pt.color = Ogre::ColourValue(1, 1, 1);
+  default_pt.position = Ogre::Vector3::ZERO;
+  cloud_points.resize(size, default_pt);
+
+  {
+    boost::recursive_mutex::scoped_lock lock(transformers_mutex_);
+    if( update_transformers )
+    {
+      updateTransformers( cloud_info->message_ );
+    }
+    rviz::PointCloudTransformerPtr xyz_trans = getXYZTransformer(cloud_info->message_);
+    rviz::PointCloudTransformerPtr color_trans = getColorTransformer(cloud_info->message_);
+
+    if (!xyz_trans)
+    {
+      std::stringstream ss;
+      ss << "No position transformer available for cloud";
+      this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
+      return false;
+    }
+
+    if (!color_trans)
+    {
+      std::stringstream ss;
+      ss << "No color transformer available for cloud";
+      this->setStatusStd(rviz::StatusProperty::Error, "Message", ss.str());
+      return false;
+    }
+
+    xyz_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_XYZ, Ogre::Matrix4::IDENTITY, cloud_points);
+    color_trans->transform(cloud_info->message_, rviz::PointCloudTransformer::Support_Color, Ogre::Matrix4::IDENTITY, cloud_points);
+  }
+
+  for (rviz::V_PointCloudPoint::iterator cloud_point = cloud_points.begin(); cloud_point != cloud_points.end(); ++cloud_point)
+  {
+    if (!rviz::validateFloats(cloud_point->position))
+    {
+      cloud_point->position.x = 999999.0f;
+      cloud_point->position.y = 999999.0f;
+      cloud_point->position.z = 999999.0f;
+    }
+  }
+
+  return true;
+}
+
+} // namespace rviz
+
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS( rtabmap::MapCloudDisplay, rviz::Display )
@@ -0,0 +1,154 @@
+
+#ifndef MAP_CLOUD_DISPLAY_H
+#define MAP_CLOUD_DISPLAY_H
+
+#include <deque>
+#include <queue>
+#include <vector>
+
+#include <rtabmap/InfoEx.h>
+#include <rtabmap/core/Transform.h>
+
+#include <pluginlib/class_loader.h>
+#include <sensor_msgs/PointCloud2.h>
+
+#include <rviz/ogre_helpers/point_cloud.h>
+#include <rviz/message_filter_display.h>
+#include <rviz/default_plugin/point_cloud_transformer.h>
+
+namespace rviz {
+class IntProperty;
+class BoolProperty;
+class EnumProperty;
+class FloatProperty;
+class PointCloudTransformer;
+typedef boost::shared_ptr<PointCloudTransformer> PointCloudTransformerPtr;
+typedef std::vector<std::string> V_string;
+}
+
+using namespace rviz;
+
+namespace rtabmap
+{
+
+class PointCloudCommon;
+
+/**
+ * \class PointCloudDisplay
+ * \brief Displays a point cloud of type sensor_msgs::PointCloud
+ *
+ * By default it will assume channel 0 of the cloud is an intensity value, and will color them by intensity.
+ * If you set the channel's name to "rgb", it will interpret the channel as an integer rgb value, with r, g and b
+ * all being 8 bits.
+ */
+class MapCloudDisplay: public rviz::MessageFilterDisplay<rtabmap::InfoEx>
+{
+Q_OBJECT
+public:
+	struct CloudInfo
+	  {
+		CloudInfo();
+		~CloudInfo();
+
+		// clear the point cloud, but keep selection handler around
+		void clear();
+
+		Ogre::SceneManager *manager_;
+
+		sensor_msgs::PointCloud2ConstPtr message_;
+		rtabmap::Transform pose_;
+		int id_;
+
+		Ogre::SceneNode *scene_node_;
+		boost::shared_ptr<rviz::PointCloud> cloud_;
+
+		std::vector<rviz::PointCloud::Point> transformed_points_;
+
+		Ogre::Quaternion orientation_;
+		Ogre::Vector3 position_;
+	};
+	typedef boost::shared_ptr<CloudInfo> CloudInfoPtr;
+
+	MapCloudDisplay();
+	virtual ~MapCloudDisplay();
+
+	virtual void reset();
+	virtual void update( float wall_dt, float ros_dt );
+
+	rviz::FloatProperty* point_world_size_property_;
+	rviz::FloatProperty* point_pixel_size_property_;
+	rviz::FloatProperty* alpha_property_;
+	rviz::EnumProperty* xyz_transformer_property_;
+	rviz::EnumProperty* color_transformer_property_;
+	rviz::EnumProperty* style_property_;
+
+public Q_SLOTS:
+	void causeRetransform();
+
+private Q_SLOTS:
+	void updateStyle();
+	void updateBillboardSize();
+	void updateAlpha();
+	void updateXyzTransformer();
+	void updateColorTransformer();
+	void setXyzTransformerOptions( EnumProperty* prop );
+	void setColorTransformerOptions( EnumProperty* prop );
+
+protected:
+	/** @brief Do initialization. Overridden from MessageFilterDisplay. */
+	virtual void onInitialize();
+
+	/** @brief Process a single message.  Overridden from MessageFilterDisplay. */
+	virtual void processMessage( const rtabmap::InfoExConstPtr& cloud );
+
+private:
+	/**
+	* \brief Transforms the cloud into the correct frame, and sets up our renderable cloud
+	*/
+	bool transformCloud(const CloudInfoPtr& cloud, bool fully_update_transformers);
+
+	rviz::PointCloudTransformerPtr getXYZTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
+	rviz::PointCloudTransformerPtr getColorTransformer(const sensor_msgs::PointCloud2ConstPtr& cloud);
+	void updateTransformers( const sensor_msgs::PointCloud2ConstPtr& cloud );
+	void retransform();
+
+	void loadTransformers();
+
+	void setPropertiesHidden( const QList<Property*>& props, bool hide );
+	void fillTransformerOptions( rviz::EnumProperty* prop, uint32_t mask );
+
+private:
+	ros::AsyncSpinner spinner_;
+	ros::CallbackQueue cbqueue_;
+
+	std::map<int, CloudInfoPtr> cloud_infos_;
+
+	std::map<int, CloudInfoPtr> new_cloud_infos_;
+	boost::mutex new_clouds_mutex_;
+
+	std::map<int, rtabmap::Transform> current_map_;
+	boost::mutex current_map_mutex_;
+
+	struct TransformerInfo
+	{
+		rviz::PointCloudTransformerPtr transformer;
+		QList<Property*> xyz_props;
+		QList<Property*> color_props;
+
+		std::string readable_name;
+		std::string lookup_name;
+	};
+	typedef std::map<std::string, TransformerInfo> M_TransformerInfo;
+
+	boost::recursive_mutex transformers_mutex_;
+	M_TransformerInfo transformers_;
+	bool new_xyz_transformer_;
+	bool new_color_transformer_;
+	bool needs_retransform_;
+
+	pluginlib::ClassLoader<rviz::PointCloudTransformer>* transformer_class_loader_;
+};
+
+} // namespace rtabmap
+
+#endif