36 #define BOOST_PARAMETER_MAX_ARITY 7 39 #include <sensor_msgs/PointCloud2.h> 46 DiagnosticNodelet::onInit();
47 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
48 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
50 srv_->setCallback (f);
52 pub_ = advertise<sensor_msgs::PointCloud2>(*
pnh_,
"output", 1);
68 const Eigen::Vector2f& mean,
69 const Eigen::Matrix2f& S,
70 const Eigen::Matrix2f& S_inv)
72 Eigen::Vector2f
diff = input - mean;
85 pcl::PointCloud<pcl::PointXYZ>::Ptr
cloud(
new pcl::PointCloud<pcl::PointXYZ>);
89 mean = Eigen::Vector2f(msg->pose.pose.position.x, msg->pose.pose.position.y);
90 S << msg->pose.covariance[0], msg->pose.covariance[1],
91 msg->pose.covariance[6], msg->pose.covariance[7];
94 mean = Eigen::Vector2f(msg->pose.pose.position.y, msg->pose.pose.position.z);
95 S << msg->pose.covariance[7], msg->pose.covariance[8],
96 msg->pose.covariance[13], msg->pose.covariance[14];
99 mean = Eigen::Vector2f(msg->pose.pose.position.z, msg->pose.pose.position.x);
100 S << msg->pose.covariance[14], msg->pose.covariance[12],
101 msg->pose.covariance[0], msg->pose.covariance[2];
103 double max_sigma = 0;
104 for (
size_t i = 0; i < 2; i++) {
105 for (
size_t j = 0; j < 2; j++) {
106 double sigma =
sqrt(S(i, j));
107 if (max_sigma < sigma) {
112 Eigen::Matrix2f S_inv = S.inverse();
115 for (
double x = - 3.0 * max_sigma;
x <= 3.0 * max_sigma;
x += dx) {
116 for (
double y = - 3.0 * max_sigma;
y <= 3.0 * max_sigma;
y += dy) {
117 Eigen::Vector2f
diff(
x,
y);
118 Eigen::Vector2f input = diff + mean;
119 float z =
gaussian(input, mean, S, S_inv);
124 p.z = z + msg->pose.pose.position.z;
129 p.x = z + msg->pose.pose.position.x;
134 p.y = z + msg->pose.pose.position.y;
139 p.z = -z + msg->pose.pose.position.z;
144 p.x = -z + msg->pose.pose.position.x;
149 p.y = -z + msg->pose.pose.position.y;
151 cloud->points.push_back(p);
154 sensor_msgs::PointCloud2 ros_cloud;
156 ros_cloud.header = msg->header;
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
virtual void configCallback(Config &config, uint32_t level)
virtual float gaussian(const Eigen::Vector2f &input, const Eigen::Vector2f &mean, const Eigen::Matrix2f &S, const Eigen::Matrix2f &S_inv)
void publish(const boost::shared_ptr< M > &message) const
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
virtual void convert(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
PoseWithCovarianceStampedToGaussianPointCloudConfig Config
virtual void unsubscribe()
std::string normalize_method_
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PoseWithCovarianceStampedToGaussianPointCloud, nodelet::Nodelet)