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 #include <jsk_topic_tools/log_utils.h>
00036 #include "jsk_pcl_ros/particle_filter_tracking.h"
00037 #include <pcl/tracking/impl/distance_coherence.hpp>
00038 #include <pcl/tracking/impl/approx_nearest_pair_point_cloud_coherence.hpp>
00039 #include <pluginlib/class_list_macros.h>
00040 #include <jsk_topic_tools/rosparam_utils.h>
00041
00042 using namespace pcl::tracking;
00043
00044 namespace jsk_pcl_ros
00045 {
00046
00047 void ParticleFilterTracking::onInit(void)
00048 {
00049 pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00050
00051 PCLNodelet::onInit();
00052 track_target_set_ = false;
00053 new_cloud_ = false;
00054
00055 default_step_covariance_.resize(6);
00056
00057 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> >(*pnh_);
00058 dynamic_reconfigure::Server<Config>::CallbackType f =
00059 boost::bind(&ParticleFilterTracking::config_callback, this, _1, _2);
00060 srv_->setCallback(f);
00061
00062 int particle_num;
00063 pnh_->param("particle_num", particle_num, max_particle_num_ - 1);
00064 bool use_normal;
00065 pnh_->param("use_normal", use_normal, false);
00066 bool use_hsv;
00067 pnh_->param("use_hsv", use_hsv, true);
00068 pnh_->param("track_target_name", track_target_name_,
00069 std::string("track_result"));
00070 std::vector<double> initial_noise_covariance(6, 0.00001);
00071 jsk_topic_tools::readVectorParameter(
00072 *pnh_, "initial_noise_covariance",
00073 initial_noise_covariance);
00074 std::vector<double> default_initial_mean(6, 0.0);
00075 jsk_topic_tools::readVectorParameter(
00076 *pnh_, "default_initial_mean", default_initial_mean);
00077
00078
00079 double octree_resolution = 0.01;
00080 pnh_->getParam("octree_resolution", octree_resolution);
00081 pnh_->param("align_box", align_box_, false);
00082 pnh_->param("BASE_FRAME_ID", base_frame_id_, std::string("NONE"));
00083
00084 target_cloud_.reset(new pcl::PointCloud<PointT>());
00085 pnh_->param("not_use_reference_centroid", not_use_reference_centroid_,
00086 false);
00087 pnh_->param("not_publish_tf", not_publish_tf_, false);
00088 pnh_->param("reversed", reversed_, false);
00089
00090 int thread_nr;
00091 pnh_->param("thread_nr", thread_nr, omp_get_num_procs());
00092
00093 if (!reversed_) {
00094 boost::shared_ptr<KLDAdaptiveParticleFilterOMPTracker<PointT, ParticleXYZRPY> > tracker
00095 (new KLDAdaptiveParticleFilterOMPTracker<PointT, ParticleXYZRPY>(thread_nr));
00096 tracker->setMaximumParticleNum(max_particle_num_);
00097 tracker->setDelta(delta_);
00098 tracker->setEpsilon(epsilon_);
00099 tracker->setBinSize(bin_size_);
00100 tracker_ = tracker;
00101 }
00102 else {
00103 boost::shared_ptr<ReversedParticleFilterOMPTracker<PointT, ParticleXYZRPY> > tracker
00104 (new ReversedParticleFilterOMPTracker<PointT, ParticleXYZRPY>(thread_nr));
00105
00106
00107 reversed_tracker_ = tracker;
00108 }
00109 tracker_set_trans(Eigen::Affine3f::Identity());
00110 tracker_set_step_noise_covariance(default_step_covariance_);
00111 tracker_set_initial_noise_covariance(initial_noise_covariance);
00112 tracker_set_initial_noise_mean(default_initial_mean);
00113 tracker_set_iteration_num(iteration_num_);
00114 tracker_set_particle_num(particle_num);
00115 tracker_set_resample_likelihood_thr(resample_likelihood_thr_);
00116 tracker_set_use_normal(use_normal);
00117
00118
00119 bool enable_cache;
00120 bool enable_organized;
00121 pnh_->param("enable_cache", enable_cache, false);
00122 pnh_->param("enable_organized", enable_organized, false);
00123 ApproxNearestPairPointCloudCoherence<PointT>::Ptr coherence;
00124 if (enable_cache) {
00125 double cache_bin_size_x, cache_bin_size_y, cache_bin_size_z;
00126 pnh_->param("cache_size_x", cache_bin_size_x, 0.01);
00127 pnh_->param("cache_size_y", cache_bin_size_y, 0.01);
00128 pnh_->param("cache_size_z", cache_bin_size_z, 0.01);
00129 coherence.reset(new CachedApproxNearestPairPointCloudCoherence<PointT>(
00130 cache_bin_size_x, cache_bin_size_y, cache_bin_size_z));
00131 }else if(enable_organized){
00132 coherence.reset(new OrganizedNearestPairPointCloudCoherence<PointT>());
00133 }
00134 else {
00135 coherence.reset(new ApproxNearestPairPointCloudCoherence<PointT>());
00136 }
00137
00138
00139 boost::shared_ptr<DistanceCoherence<PointT> >
00140 distance_coherence(new DistanceCoherence<PointT>);
00141 coherence->addPointCoherence(distance_coherence);
00142
00143
00144 if (use_hsv) {
00145 boost::shared_ptr<HSVColorCoherence<PointT> > hsv_color_coherence
00146 = boost::shared_ptr<HSVColorCoherence<PointT> >(new HSVColorCoherence<PointT>());
00147 coherence->addPointCoherence(hsv_color_coherence);
00148 }
00149
00150 boost::shared_ptr<pcl::search::Octree<PointT> > search
00151 (new pcl::search::Octree<PointT>(octree_resolution));
00152
00153 coherence->setSearchMethod(search);
00154 double max_distance;
00155 pnh_->param("max_distance", max_distance, 0.01);
00156 coherence->setMaximumDistance(max_distance);
00157
00158 tracker_set_cloud_coherence(coherence);
00159
00160
00161 particle_publisher_ = pnh_->advertise<sensor_msgs::PointCloud2>(
00162 "particle", 1);
00163 track_result_publisher_ = pnh_->advertise<sensor_msgs::PointCloud2>(
00164 "track_result", 1);
00165 pose_stamped_publisher_ = pnh_->advertise<geometry_msgs::PoseStamped>(
00166 "track_result_pose", 1);
00167
00168 sub_ = pnh_->subscribe("input", 1, &ParticleFilterTracking::cloud_cb,this);
00169 if (align_box_) {
00170 sub_input_.subscribe(*pnh_, "renew_model", 1);
00171 sub_box_.subscribe(*pnh_, "renew_box", 1);
00172 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00173 sync_->connectInput(sub_input_, sub_box_);
00174 sync_->registerCallback(
00175 boost::bind(
00176 &ParticleFilterTracking::renew_model_with_box_topic_cb,
00177 this, _1, _2));
00178 }
00179 else {
00180 sub_update_model_ = pnh_->subscribe(
00181 "renew_model", 1, &ParticleFilterTracking::renew_model_topic_cb,this);
00182 sub_update_with_marker_model_
00183 = pnh_->subscribe("renew_model_with_marker", 1, &ParticleFilterTracking::renew_model_with_marker_topic_cb, this);
00184 }
00185
00186 pnh_->param("marker_to_pointcloud_sampling_nums", marker_to_pointcloud_sampling_nums_, 10000);
00187 renew_model_srv_
00188 = pnh_->advertiseService(
00189 "renew_model", &ParticleFilterTracking::renew_model_cb, this);
00190 }
00191
00192 void ParticleFilterTracking::config_callback(Config &config, uint32_t level)
00193 {
00194 boost::mutex::scoped_lock lock(mtx_);
00195 max_particle_num_ = config.max_particle_num;
00196 iteration_num_ = config.iteration_num;
00197 resample_likelihood_thr_ = config.resample_likelihood_thr;
00198 delta_ = config.delta;
00199 epsilon_ = config.epsilon;
00200 bin_size_.x = config.bin_size_x;
00201 bin_size_.y = config.bin_size_y;
00202 bin_size_.z = config.bin_size_z;
00203 bin_size_.roll = config.bin_size_roll;
00204 bin_size_.pitch = config.bin_size_pitch;
00205 bin_size_.yaw = config.bin_size_yaw;
00206 default_step_covariance_[0] = config.default_step_covariance_x;
00207 default_step_covariance_[1] = config.default_step_covariance_y;
00208 default_step_covariance_[2] = config.default_step_covariance_z;
00209 default_step_covariance_[3] = config.default_step_covariance_roll;
00210 default_step_covariance_[4] = config.default_step_covariance_pitch;
00211 default_step_covariance_[5] = config.default_step_covariance_yaw;
00212 if (tracker_ || reversed_tracker_)
00213 {
00214 JSK_NODELET_INFO("update tracker parameter");
00215 tracker_set_step_noise_covariance(default_step_covariance_);
00216 tracker_set_iteration_num(iteration_num_);
00217 tracker_set_resample_likelihood_thr(resample_likelihood_thr_);
00218 tracker_set_maximum_particle_num(max_particle_num_);
00219 tracker_set_delta(delta_);
00220 tracker_set_epsilon(epsilon_);
00221 tracker_set_bin_size(bin_size_);
00222 }
00223 }
00224
00225
00226 void ParticleFilterTracking::publish_particles()
00227 {
00228 PointCloudStatePtr particles = tracker_get_particles();
00229 if (particles && new_cloud_ && particle_publisher_.getNumSubscribers()) {
00230
00231 pcl::PointCloud<pcl::PointXYZ>::Ptr particle_cloud
00232 (new pcl::PointCloud<pcl::PointXYZ>());
00233 for (size_t i = 0; i < particles->points.size(); i++)
00234 {
00235 pcl::PointXYZ point;
00236 point.x = particles->points[i].x;
00237 point.y = particles->points[i].y;
00238 point.z = particles->points[i].z;
00239 particle_cloud->points.push_back(point);
00240 }
00241
00242 sensor_msgs::PointCloud2 particle_pointcloud2;
00243 pcl::toROSMsg(*particle_cloud, particle_pointcloud2);
00244 particle_pointcloud2.header.frame_id = reference_frame_id();
00245 particle_pointcloud2.header.stamp = stamp_;
00246 particle_publisher_.publish(particle_pointcloud2);
00247 }
00248 }
00249
00250 void ParticleFilterTracking::publish_result()
00251 {
00252 ParticleXYZRPY result = tracker_get_result();
00253 Eigen::Affine3f transformation = tracker_to_eigen_matrix(result);
00254
00255
00256 tf::Transform tfTransformation;
00257 tf::transformEigenToTF((Eigen::Affine3d) transformation, tfTransformation);
00258
00259 if (!not_publish_tf_) {
00260 static tf::TransformBroadcaster tfBroadcaster;
00261 tfBroadcaster.sendTransform(tf::StampedTransform(
00262 tfTransformation, stamp_,
00263 reference_frame_id(), track_target_name_));
00264 }
00265
00266 geometry_msgs::PoseStamped result_pose_stamped;
00267 result_pose_stamped.header.frame_id = reference_frame_id();
00268 result_pose_stamped.header.stamp = stamp_;
00269 tf::Quaternion q;
00270 tf::poseTFToMsg(tfTransformation, result_pose_stamped.pose);
00271 pose_stamped_publisher_.publish(result_pose_stamped);
00272
00273 pcl::PointCloud<PointT>::Ptr result_cloud
00274 (new pcl::PointCloud<PointT>());
00275 pcl::transformPointCloud<PointT>(
00276 *(tracker_get_reference_cloud()), *result_cloud, transformation);
00277 sensor_msgs::PointCloud2 result_pointcloud2;
00278 pcl::toROSMsg(*result_cloud, result_pointcloud2);
00279 result_pointcloud2.header.frame_id = reference_frame_id();
00280 result_pointcloud2.header.stamp = stamp_;
00281 track_result_publisher_.publish(result_pointcloud2);
00282 }
00283
00284 std::string ParticleFilterTracking::reference_frame_id()
00285 {
00286 if (base_frame_id_.compare("NONE") == 0) {
00287 return frame_id_;
00288 }
00289 else {
00290 return base_frame_id_;
00291 }
00292 }
00293
00294 void ParticleFilterTracking::reset_tracking_target_model(
00295 const pcl::PointCloud<PointT>::ConstPtr &recieved_target_cloud)
00296 {
00297 pcl::PointCloud<PointT>::Ptr new_target_cloud(new pcl::PointCloud<PointT>);
00298 std::vector<int> indices;
00299 new_target_cloud->is_dense = false;
00300 pcl::removeNaNFromPointCloud(
00301 *recieved_target_cloud, *new_target_cloud, indices);
00302
00303 if (base_frame_id_.compare("NONE") != 0) {
00304 tf::Transform transform_result
00305 = change_pointcloud_frame(new_target_cloud);
00306 reference_transform_ = transform_result * reference_transform_;;
00307 }
00308
00309 if (!recieved_target_cloud->points.empty()) {
00310
00311 Eigen::Affine3f trans = Eigen::Affine3f::Identity();
00312 pcl::PointCloud<PointT>::Ptr transed_ref(new pcl::PointCloud<PointT>);
00313 if (!align_box_) {
00314 if (!not_use_reference_centroid_) {
00315 Eigen::Vector4f c;
00316 pcl::compute3DCentroid(*new_target_cloud, c);
00317 trans.translation().matrix() = Eigen::Vector3f(c[0], c[1], c[2]);
00318 }
00319 }
00320 else {
00321 Eigen::Affine3d trans_3d = Eigen::Affine3d::Identity();
00322 tf::transformTFToEigen(reference_transform_, trans_3d);
00323 trans = (Eigen::Affine3f) trans_3d;
00324 }
00325 pcl::transformPointCloud(*new_target_cloud, *transed_ref, trans.inverse());
00326
00327 {
00328 boost::mutex::scoped_lock lock(mtx_);
00329 tracker_set_reference_cloud(transed_ref);
00330 tracker_set_trans(trans);
00331 tracker_reset_tracking();
00332 }
00333 track_target_set_ = true;
00334 JSK_NODELET_INFO("RESET TARGET MODEL");
00335 }
00336 else {
00337 track_target_set_ = false;
00338 JSK_NODELET_ERROR("TARGET MODEL POINTS SIZE IS 0 !! Stop TRACKING");
00339 }
00340 }
00341
00342 tf::Transform ParticleFilterTracking::change_pointcloud_frame(
00343 pcl::PointCloud<PointT>::Ptr cloud)
00344 {
00345 tf::Transform tfTransformation;
00346 tf::StampedTransform tfTransformationStamped;
00347 ros::Time now = ros::Time::now();
00348 try {
00349 listener_.waitForTransform(base_frame_id_, frame_id_, now,
00350 ros::Duration(2.0));
00351 listener_.lookupTransform(base_frame_id_, frame_id_, now,
00352 tfTransformationStamped);
00353
00354 }
00355 catch(tf::TransformException ex) {
00356 JSK_NODELET_ERROR("%s",ex.what());
00357 tfTransformation = tf::Transform(tf::Quaternion(0, 0, 0, 1));
00358 }
00359 tfTransformation = tf::Transform(tfTransformationStamped.getBasis(),
00360 tfTransformationStamped.getOrigin());
00361 Eigen::Affine3f trans; Eigen::Affine3d trans_3d;
00362 tf::transformTFToEigen(tfTransformation, trans_3d);
00363 trans = (Eigen::Affine3f) trans_3d;
00364 pcl::transformPointCloud(*cloud, *cloud, trans);
00365 return tfTransformation;
00366 }
00367
00368
00369
00370 void ParticleFilterTracking::cloud_cb(const sensor_msgs::PointCloud2 &pc)
00371 {
00372 if (track_target_set_) {
00373 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
00374 frame_id_ = pc.header.frame_id;
00375 stamp_ = pc.header.stamp;
00376 std::vector<int> indices;
00377 pcl::fromROSMsg(pc, *cloud);
00378 cloud->is_dense = false;
00379 pcl::removeNaNFromPointCloud(*cloud, *cloud, indices);
00380 if (base_frame_id_.compare("NONE")!=0) {
00381 change_pointcloud_frame(cloud);
00382 }
00383 cloud_pass_downsampled_.reset(new pcl::PointCloud<PointT>);
00384 pcl::copyPointCloud(*cloud, *cloud_pass_downsampled_);
00385 if (!cloud_pass_downsampled_->points.empty()) {
00386 boost::mutex::scoped_lock lock(mtx_);
00387 tracker_set_input_cloud(cloud_pass_downsampled_);
00388 tracker_compute();
00389 publish_particles();
00390 publish_result();
00391 }
00392 new_cloud_ = true;
00393 }
00394 }
00395
00396 void ParticleFilterTracking::renew_model_topic_cb(
00397 const sensor_msgs::PointCloud2 &pc)
00398 {
00399 pcl::PointCloud<PointT>::Ptr new_target_cloud
00400 (new pcl::PointCloud<PointT>());
00401 pcl::fromROSMsg(pc, *new_target_cloud);
00402 frame_id_ = pc.header.frame_id;
00403 reset_tracking_target_model(new_target_cloud);
00404 }
00405
00406 void ParticleFilterTracking::renew_model_with_marker_topic_cb(const visualization_msgs::Marker &marker)
00407 {
00408 if(marker.type == visualization_msgs::Marker::TRIANGLE_LIST && !marker.points.empty()){
00409 ROS_INFO("Reset Tracker Model with renew_model_with_marker_topic_cb");
00410 pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
00411 markerMsgToPointCloud(marker,
00412 marker_to_pointcloud_sampling_nums_,
00413 *cloud
00414 );
00415
00416 Eigen::Affine3f trans;
00417 tf::poseMsgToEigen(marker.pose, trans);
00418 pcl::transformPointCloud(*cloud, *cloud, trans);
00419
00420 frame_id_ = marker.header.frame_id;
00421 reset_tracking_target_model(cloud);
00422 }else{
00423 JSK_ROS_ERROR(" Marker Models type is not TRIANGLE ");
00424 JSK_ROS_ERROR(" OR ");
00425 JSK_ROS_ERROR(" Marker Points is empty ");
00426 }
00427 }
00428
00429 void ParticleFilterTracking::renew_model_with_box_topic_cb(
00430 const sensor_msgs::PointCloud2::ConstPtr &pc_ptr,
00431 const jsk_recognition_msgs::BoundingBox::ConstPtr &bb_ptr)
00432 {
00433 pcl::PointCloud<PointT>::Ptr new_target_cloud
00434 (new pcl::PointCloud<PointT>());
00435 pcl::fromROSMsg(*pc_ptr, *new_target_cloud);
00436 frame_id_ = pc_ptr->header.frame_id;
00437 tf::poseMsgToTF(bb_ptr->pose, reference_transform_);
00438 reset_tracking_target_model(new_target_cloud);
00439 }
00440
00441 bool ParticleFilterTracking::renew_model_cb(
00442 jsk_pcl_ros::SetPointCloud2::Request &req,
00443 jsk_pcl_ros::SetPointCloud2::Response &res)
00444 {
00445 pcl::PointCloud<PointT>::Ptr new_target_cloud(new pcl::PointCloud<PointT>());
00446 pcl::fromROSMsg(req.cloud, *new_target_cloud);
00447 frame_id_ = req.cloud.header.frame_id;
00448 reset_tracking_target_model(new_target_cloud);
00449 return true;
00450 }
00451
00452 void ParticleFilterTracking::tracker_set_trans(
00453 const Eigen::Affine3f& trans)
00454 {
00455 Eigen::Vector3f pos = trans.translation();
00456 JSK_NODELET_INFO("trans: [%f, %f, %f]", pos[0], pos[1], pos[2]);
00457 if (reversed_) {
00458 reversed_tracker_->setTrans(trans);
00459 }
00460 else {
00461 tracker_->setTrans(trans);
00462 }
00463 }
00464
00465 void ParticleFilterTracking::tracker_set_step_noise_covariance(
00466 const std::vector<double>& covariance)
00467 {
00468 if (reversed_) {
00469 reversed_tracker_->setStepNoiseCovariance(covariance);
00470 }
00471 else {
00472 tracker_->setStepNoiseCovariance(covariance);
00473 }
00474 }
00475
00476 void ParticleFilterTracking::tracker_set_initial_noise_covariance(
00477 const std::vector<double>& covariance)
00478 {
00479 if (reversed_) {
00480 reversed_tracker_->setInitialNoiseCovariance(covariance);
00481 }
00482 else {
00483 tracker_->setInitialNoiseCovariance(covariance);
00484 }
00485 }
00486
00487 void ParticleFilterTracking::tracker_set_initial_noise_mean(
00488 const std::vector<double>& mean)
00489 {
00490 if (reversed_) {
00491 reversed_tracker_->setInitialNoiseMean(mean);
00492 }
00493 else {
00494 tracker_->setInitialNoiseMean(mean);
00495 }
00496 }
00497
00498 void ParticleFilterTracking::tracker_set_iteration_num(const int num)
00499 {
00500 if (reversed_) {
00501 reversed_tracker_->setIterationNum(num);
00502 }
00503 else {
00504 tracker_->setIterationNum(num);
00505 }
00506 }
00507
00508 void ParticleFilterTracking::tracker_set_particle_num(const int num)
00509 {
00510 if (reversed_) {
00511 reversed_tracker_->setParticleNum(num);
00512 }
00513 else {
00514 tracker_->setParticleNum(num);
00515 }
00516 }
00517
00518 void ParticleFilterTracking::tracker_set_resample_likelihood_thr(double thr)
00519 {
00520 if (reversed_) {
00521 reversed_tracker_->setResampleLikelihoodThr(thr);
00522 }
00523 else {
00524 tracker_->setResampleLikelihoodThr(thr);
00525 }
00526 }
00527
00528 void ParticleFilterTracking::tracker_set_use_normal(bool use_normal)
00529 {
00530 if (reversed_) {
00531 reversed_tracker_->setUseNormal(use_normal);
00532 }
00533 else {
00534 tracker_->setUseNormal(use_normal);
00535 }
00536 }
00537
00538 void ParticleFilterTracking::tracker_set_cloud_coherence(
00539 ApproxNearestPairPointCloudCoherence<PointT>::Ptr coherence)
00540 {
00541 if (reversed_) {
00542 reversed_tracker_->setCloudCoherence(coherence);
00543 }
00544 else {
00545 tracker_->setCloudCoherence(coherence);
00546 }
00547 }
00548
00549 void ParticleFilterTracking::tracker_set_maximum_particle_num(int num)
00550 {
00551 if (!reversed_) {
00552 tracker_->setMaximumParticleNum(num);
00553 }
00554 }
00555
00556 void ParticleFilterTracking::tracker_set_delta(double delta)
00557 {
00558 if (!reversed_) {
00559 tracker_->setDelta(delta);
00560 }
00561 }
00562
00563 void ParticleFilterTracking::tracker_set_epsilon(double epsilon)
00564 {
00565 if (!reversed_) {
00566 tracker_->setEpsilon(epsilon);
00567 }
00568 }
00569
00570 void ParticleFilterTracking::tracker_set_bin_size(
00571 const ParticleXYZRPY bin_size)
00572 {
00573 if (!reversed_) {
00574 tracker_->setBinSize(bin_size);
00575 }
00576 }
00577
00578 ParticleFilterTracking::PointCloudStatePtr
00579 ParticleFilterTracking::tracker_get_particles()
00580 {
00581 if (!reversed_) {
00582 return tracker_->getParticles();
00583 }
00584 else {
00585 return reversed_tracker_->getParticles();
00586 }
00587 }
00588
00589 ParticleXYZRPY ParticleFilterTracking::tracker_get_result()
00590 {
00591 if (!reversed_) {
00592 return tracker_->getResult();
00593 }
00594 else {
00595 return reversed_tracker_->getResult();
00596 }
00597 }
00598
00599 Eigen::Affine3f ParticleFilterTracking::tracker_to_eigen_matrix(
00600 const ParticleXYZRPY& result)
00601 {
00602 if (!reversed_) {
00603 return tracker_->toEigenMatrix(result);
00604 }
00605 else {
00606 return reversed_tracker_->toEigenMatrix(result);
00607 }
00608 }
00609
00610 pcl::PointCloud<ParticleFilterTracking::PointT>::ConstPtr
00611 ParticleFilterTracking::tracker_get_reference_cloud()
00612 {
00613 if (!reversed_) {
00614 return tracker_->getReferenceCloud();
00615 }
00616 else {
00617 return reversed_tracker_->getReferenceCloud();
00618 }
00619 }
00620
00621 void ParticleFilterTracking::tracker_set_reference_cloud(
00622 pcl::PointCloud<PointT>::Ptr ref)
00623 {
00624 if (!reversed_) {
00625 tracker_->setReferenceCloud(ref);
00626 }
00627 else {
00628 reversed_tracker_->setReferenceCloud(ref);
00629 }
00630 }
00631
00632 void ParticleFilterTracking::tracker_reset_tracking()
00633 {
00634 if (!reversed_) {
00635 tracker_->resetTracking();
00636 }
00637 else {
00638 reversed_tracker_->resetTracking();
00639 }
00640 }
00641
00642 void ParticleFilterTracking::tracker_set_input_cloud(
00643 pcl::PointCloud<PointT>::Ptr input)
00644 {
00645 if (!reversed_) {
00646 tracker_->setInputCloud(input);
00647 }
00648 else {
00649 reversed_tracker_->setInputCloud(input);
00650 }
00651 }
00652 void ParticleFilterTracking::tracker_compute()
00653 {
00654 if (!reversed_) {
00655 tracker_->compute();
00656 }
00657 else {
00658 reversed_tracker_->compute();
00659 }
00660 }
00661 }
00662
00663 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ParticleFilterTracking, nodelet::Nodelet);