Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037
00038 #include "jsk_pcl_ros/pose_with_covariance_stamped_to_gaussian_pointcloud.h"
00039 #include <sensor_msgs/PointCloud2.h>
00040 #include "jsk_pcl_ros/pcl_conversion_util.h"
00041
00042 namespace jsk_pcl_ros
00043 {
00044 void PoseWithCovarianceStampedToGaussianPointCloud::onInit()
00045 {
00046 DiagnosticNodelet::onInit();
00047 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00048 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00049 boost::bind (&PoseWithCovarianceStampedToGaussianPointCloud::configCallback, this, _1, _2);
00050 srv_->setCallback (f);
00051
00052 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00053 }
00054
00055 void PoseWithCovarianceStampedToGaussianPointCloud::subscribe()
00056 {
00057 sub_ = pnh_->subscribe("input", 1, &PoseWithCovarianceStampedToGaussianPointCloud::convert, this);
00058 }
00059
00060 void PoseWithCovarianceStampedToGaussianPointCloud::unsubscribe()
00061 {
00062 sub_.shutdown();
00063 }
00064
00065 float PoseWithCovarianceStampedToGaussianPointCloud::gaussian(const Eigen::Vector2f& input,
00066 const Eigen::Vector2f& mean,
00067 const Eigen::Matrix2f& S,
00068 const Eigen::Matrix2f& S_inv)
00069 {
00070 Eigen::Vector2f diff = input - mean;
00071 if (normalize_method_ == "normalize_area") {
00072 return normalize_value_ * 1 / (2 * M_PI * sqrt(S.determinant())) * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
00073 }
00074 else if (normalize_method_ == "normalize_height") {
00075 return normalize_value_ * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
00076 }
00077 }
00078
00079 void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
00080 {
00081 boost::mutex::scoped_lock lock(mutex_);
00082 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00083 Eigen::Vector2f mean;
00084 Eigen::Matrix2f S;
00085 if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
00086 mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
00087 S << msg->pose.covariance[0], msg->pose.covariance[1],
00088 msg->pose.covariance[6], msg->pose.covariance[7];
00089 }
00090 else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
00091 mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
00092 S << msg->pose.covariance[7], msg->pose.covariance[8],
00093 msg->pose.covariance[13], msg->pose.covariance[14];
00094 }
00095 else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
00096 mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
00097 S << msg->pose.covariance[14], msg->pose.covariance[12],
00098 msg->pose.covariance[0], msg->pose.covariance[2];
00099 }
00100 double max_sigma = 0;
00101 for (size_t i = 0; i < 2; i++) {
00102 for (size_t j = 0; j < 2; j++) {
00103 double sigma = sqrt(S(i, j));
00104 if (max_sigma < sigma) {
00105 max_sigma = sigma;
00106 }
00107 }
00108 }
00109 Eigen::Matrix2f S_inv = S.inverse();
00110 double dx = 6.0 * max_sigma / sampling_num_;
00111 double dy = 6.0 * max_sigma / sampling_num_;
00112 for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
00113 for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
00114 Eigen::Vector2f diff(x, y);
00115 Eigen::Vector2f input = diff + mean;
00116 float z = gaussian(input, mean, S, S_inv);
00117 pcl::PointXYZ p;
00118 if (cut_plane_ == "xy") {
00119 p.x = input[0];
00120 p.y = input[1];
00121 p.z = z + msg->pose.pose.position.z;
00122 }
00123 else if (cut_plane_ == "yz") {
00124 p.y = input[0];
00125 p.z = input[1];
00126 p.x = z + msg->pose.pose.position.x;
00127 }
00128 else if (cut_plane_ == "zx") {
00129 p.z = input[0];
00130 p.x = input[1];
00131 p.y = z + msg->pose.pose.position.y;
00132 }
00133 else if (cut_plane_ == "flipped_xy") {
00134 p.x = input[0];
00135 p.y = input[1];
00136 p.z = -z + msg->pose.pose.position.z;
00137 }
00138 else if (cut_plane_ == "flipped_yz") {
00139 p.y = input[0];
00140 p.z = input[1];
00141 p.x = -z + msg->pose.pose.position.x;
00142 }
00143 else if (cut_plane_ == "flipped_zx") {
00144 p.z = input[0];
00145 p.x = input[1];
00146 p.y = -z + msg->pose.pose.position.y;
00147 }
00148 cloud->points.push_back(p);
00149 }
00150 }
00151 sensor_msgs::PointCloud2 ros_cloud;
00152 pcl::toROSMsg(*cloud, ros_cloud);
00153 ros_cloud.header = msg->header;
00154 pub_.publish(ros_cloud);
00155 }
00156
00157 void PoseWithCovarianceStampedToGaussianPointCloud::configCallback(Config& config, uint32_t level)
00158 {
00159 boost::mutex::scoped_lock lock(mutex_);
00160 cut_plane_ = config.cut_plane;
00161 sampling_num_ = config.sampling_num;
00162 normalize_value_ = config.normalize_value;
00163 normalize_method_ = config.normalize_method;
00164 }
00165
00166 }
00167
00168 #include <pluginlib/class_list_macros.h>
00169 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PoseWithCovarianceStampedToGaussianPointCloud,
00170 nodelet::Nodelet);