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_utils/pose_with_covariance_stamped_to_gaussian_pointcloud.h"
00039 #include <sensor_msgs/PointCloud2.h>
00040 #include "jsk_recognition_utils/pcl_conversion_util.h"
00041
00042 namespace jsk_pcl_ros_utils
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 onInitPostProcess();
00055 }
00056
00057 void PoseWithCovarianceStampedToGaussianPointCloud::subscribe()
00058 {
00059 sub_ = pnh_->subscribe("input", 1, &PoseWithCovarianceStampedToGaussianPointCloud::convert, this);
00060 }
00061
00062 void PoseWithCovarianceStampedToGaussianPointCloud::unsubscribe()
00063 {
00064 sub_.shutdown();
00065 }
00066
00067 float PoseWithCovarianceStampedToGaussianPointCloud::gaussian(const Eigen::Vector2f& input,
00068 const Eigen::Vector2f& mean,
00069 const Eigen::Matrix2f& S,
00070 const Eigen::Matrix2f& S_inv)
00071 {
00072 Eigen::Vector2f diff = input - mean;
00073 if (normalize_method_ == "normalize_area") {
00074 return normalize_value_ * 1 / (2 * M_PI * sqrt(S.determinant())) * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
00075 }
00076 else if (normalize_method_ == "normalize_height") {
00077 return normalize_value_ * exp(- 0.5 * (diff.transpose() * S_inv * diff)[0]);
00078 }
00079 }
00080
00081 void PoseWithCovarianceStampedToGaussianPointCloud::convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
00082 {
00083 boost::mutex::scoped_lock lock(mutex_);
00084 vital_checker_->poke();
00085 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
00086 Eigen::Vector2f mean;
00087 Eigen::Matrix2f S;
00088 if (cut_plane_ == "xy" || cut_plane_ == "flipped_xy") {
00089 mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
00090 S << msg->pose.covariance[0], msg->pose.covariance[1],
00091 msg->pose.covariance[6], msg->pose.covariance[7];
00092 }
00093 else if (cut_plane_ == "yz" || cut_plane_ == "flipped_yz") {
00094 mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
00095 S << msg->pose.covariance[7], msg->pose.covariance[8],
00096 msg->pose.covariance[13], msg->pose.covariance[14];
00097 }
00098 else if (cut_plane_ == "zx" || cut_plane_ == "flipped_zx") {
00099 mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
00100 S << msg->pose.covariance[14], msg->pose.covariance[12],
00101 msg->pose.covariance[0], msg->pose.covariance[2];
00102 }
00103 double max_sigma = 0;
00104 for (size_t i = 0; i < 2; i++) {
00105 for (size_t j = 0; j < 2; j++) {
00106 double sigma = sqrt(S(i, j));
00107 if (max_sigma < sigma) {
00108 max_sigma = sigma;
00109 }
00110 }
00111 }
00112 Eigen::Matrix2f S_inv = S.inverse();
00113 double dx = 6.0 * max_sigma / sampling_num_;
00114 double dy = 6.0 * max_sigma / sampling_num_;
00115 for (double x = - 3.0 * max_sigma; x <= 3.0 * max_sigma; x += dx) {
00116 for (double y = - 3.0 * max_sigma; y <= 3.0 * max_sigma; y += dy) {
00117 Eigen::Vector2f diff(x, y);
00118 Eigen::Vector2f input = diff + mean;
00119 float z = gaussian(input, mean, S, S_inv);
00120 pcl::PointXYZ p;
00121 if (cut_plane_ == "xy") {
00122 p.x = input[0];
00123 p.y = input[1];
00124 p.z = z + msg->pose.pose.position.z;
00125 }
00126 else if (cut_plane_ == "yz") {
00127 p.y = input[0];
00128 p.z = input[1];
00129 p.x = z + msg->pose.pose.position.x;
00130 }
00131 else if (cut_plane_ == "zx") {
00132 p.z = input[0];
00133 p.x = input[1];
00134 p.y = z + msg->pose.pose.position.y;
00135 }
00136 else if (cut_plane_ == "flipped_xy") {
00137 p.x = input[0];
00138 p.y = input[1];
00139 p.z = -z + msg->pose.pose.position.z;
00140 }
00141 else if (cut_plane_ == "flipped_yz") {
00142 p.y = input[0];
00143 p.z = input[1];
00144 p.x = -z + msg->pose.pose.position.x;
00145 }
00146 else if (cut_plane_ == "flipped_zx") {
00147 p.z = input[0];
00148 p.x = input[1];
00149 p.y = -z + msg->pose.pose.position.y;
00150 }
00151 cloud->points.push_back(p);
00152 }
00153 }
00154 sensor_msgs::PointCloud2 ros_cloud;
00155 pcl::toROSMsg(*cloud, ros_cloud);
00156 ros_cloud.header = msg->header;
00157 pub_.publish(ros_cloud);
00158 }
00159
00160 void PoseWithCovarianceStampedToGaussianPointCloud::configCallback(Config& config, uint32_t level)
00161 {
00162 boost::mutex::scoped_lock lock(mutex_);
00163 cut_plane_ = config.cut_plane;
00164 sampling_num_ = config.sampling_num;
00165 normalize_value_ = config.normalize_value;
00166 normalize_method_ = config.normalize_method;
00167 }
00168 }
00169
00170 #include <pluginlib/class_list_macros.h>
00171 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PoseWithCovarianceStampedToGaussianPointCloud,
00172 nodelet::Nodelet);