jsk_pcl_ros: polygon_array_transformer

Go to the documentation of this file.
00001 // -*- mode: C++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include "jsk_pcl_ros/polygon_array_transformer.h"
00037 #include <tf_conversions/tf_eigen.h>
00038 #include "jsk_pcl_ros/geo_util.h"
00039 #include <pluginlib/class_list_macros.h>
00040 
00041 namespace jsk_pcl_ros
00042 {
00043 
00044   void PolygonArrayTransformer::onInit()
00045   {
00046     ConnectionBasedNodelet::onInit();
00047     if (!pnh_->getParam("frame_id", frame_id_)) {
00048       JSK_NODELET_FATAL("~frame_id is not specified");
00049       return;
00050     }
00051     listener_ = TfListenerSingleton::getInstance();
00052     polygons_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output_polygons", 1);
00053     coefficients_pub_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(
00054       *pnh_, "output_coefficients", 1);
00055     
00056   }
00057 
00058   void PolygonArrayTransformer::subscribe()
00059   {
00060     sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00061     sub_polygons_.subscribe(*pnh_, "input_polygons", 1);
00062     sub_coefficients_.subscribe(*pnh_, "input_coefficients", 1);
00063     sync_->connectInput(sub_polygons_, sub_coefficients_);
00064     sync_->registerCallback(boost::bind(&PolygonArrayTransformer::transform, this, _1, _2));
00065   }
00066 
00067   void PolygonArrayTransformer::unsubscribe()
00068   {
00069     sub_polygons_.unsubscribe();
00070     sub_coefficients_.unsubscribe();
00071   }
00072   
00073   void PolygonArrayTransformer::computeCoefficients(const geometry_msgs::PolygonStamped& polygon,
00074                                                     PCLModelCoefficientMsg& coefficient)
00075   {
00076     Eigen::Vector3d A, B, C;
00077     A[0] = polygon.polygon.points[0].x;
00078     A[1] = polygon.polygon.points[0].y;
00079     A[2] = polygon.polygon.points[0].z;
00080     B[0] = polygon.polygon.points[1].x;
00081     B[1] = polygon.polygon.points[1].y;
00082     B[2] = polygon.polygon.points[1].z;
00083     C[0] = polygon.polygon.points[2].x;
00084     C[1] = polygon.polygon.points[2].y;
00085     C[2] = polygon.polygon.points[2].z;
00086     Eigen::Vector3d n = (B - A).cross(C - A).normalized();
00087     double a = n[0];
00088     double b = n[1];
00089     double c = n[2];
00090     double d = -(a * A[0] + b * A[1] + c * A[2]);
00091     coefficient.header = polygon.header;
00092     coefficient.values.push_back(a);
00093     coefficient.values.push_back(b);
00094     coefficient.values.push_back(c);
00095     coefficient.values.push_back(d);
00096     
00097   }
00098 
00099   void PolygonArrayTransformer::transformModelCoefficient(const Eigen::Affine3d& transform,
00100                                                           const PCLModelCoefficientMsg& coefficient,
00101                                                           PCLModelCoefficientMsg& result)
00102   {
00103     Plane plane(coefficient.values);
00104     Plane transformed_plane = plane.transform(transform);
00105     result.header.stamp = coefficient.header.stamp;
00106     result.header.frame_id = frame_id_;
00107     transformed_plane.toCoefficients(result.values);
00108     JSK_NODELET_DEBUG("[%f, %f, %f, %f] => [%f, %f, %f, %f]",
00109                   coefficient.values[0], coefficient.values[1], coefficient.values[2], coefficient.values[3],
00110                   result.values[0], result.values[1], result.values[2], result.values[3]);
00111   }
00112 
00113   void PolygonArrayTransformer::transformPolygon(const Eigen::Affine3d& transform,
00114                                                  const geometry_msgs::PolygonStamped& polygon,
00115                                                  geometry_msgs::PolygonStamped& result)
00116   {
00117     result.header = polygon.header;
00118     result.header.frame_id = frame_id_;
00119     for (size_t i = 0; i < polygon.polygon.points.size(); i++) {
00120       Eigen::Vector4d point;
00121       point[0] = polygon.polygon.points[i].x;
00122       point[1] = polygon.polygon.points[i].y;
00123       point[2] = polygon.polygon.points[i].z;
00124       point[3] = 1;             // homogenious
00125       Eigen::Vector4d transformed_point_eigen = transform.inverse() * point;
00126       geometry_msgs::Point32 transformed_point;
00127       transformed_point.x = transformed_point_eigen[0];
00128       transformed_point.y = transformed_point_eigen[1];
00129       transformed_point.z = transformed_point_eigen[2];
00130       result.polygon.points.push_back(transformed_point);
00131     }
00132   }
00133   
00134   void PolygonArrayTransformer::transform(const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons,
00135                                           const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
00136   {
00137     if (polygons->polygons.size() != coefficients->coefficients.size()) {
00138       JSK_NODELET_ERROR("the size of polygons(%lu) does not match with the size of coefficients(%lu)",
00139                     polygons->polygons.size(),
00140                     coefficients->coefficients.size());
00141       return;
00142     }
00143 
00144     jsk_recognition_msgs::PolygonArray transformed_polygon_array;
00145     jsk_recognition_msgs::ModelCoefficientsArray transformed_model_coefficients_array;
00146     transformed_polygon_array.header = polygons->header;
00147     transformed_model_coefficients_array.header = coefficients->header;
00148     transformed_polygon_array.header.frame_id = frame_id_;
00149     transformed_model_coefficients_array.header.frame_id = frame_id_;
00150     for (size_t i = 0; i < polygons->polygons.size(); i++) {
00151       geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
00152       PCLModelCoefficientMsg coefficient = coefficients->coefficients[i];
00153 
00154       if (polygon.header.frame_id != coefficient.header.frame_id) {
00155         JSK_NODELET_ERROR("frame_id of polygon[%lu] is %s and frame_id of coefficient[%lu] is %s, they does not point to the same frame_id",
00156                       i, polygon.header.frame_id.c_str(),
00157                       i, coefficient.header.frame_id.c_str());
00158         return;
00159       }
00160       listener_->waitForTransform(coefficient.header.frame_id,
00161                                   frame_id_,
00162                                   coefficient.header.stamp,
00163                                   ros::Duration(1.0));
00164       if (listener_->canTransform(coefficient.header.frame_id,
00165                                   frame_id_,
00166                                   coefficient.header.stamp)) {
00167         tf::StampedTransform transform; // header -> frame_id_
00168         listener_->lookupTransform(coefficient.header.frame_id, frame_id_,
00169                                    //ros::Time(0.0), transform);
00170                                    coefficient.header.stamp, transform);
00171         Eigen::Affine3d eigen_transform;
00172         tf::transformTFToEigen(transform, eigen_transform);
00173         PCLModelCoefficientMsg transformed_coefficient;
00174         //transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient);
00175         
00176         geometry_msgs::PolygonStamped transformed_polygon;
00177         if (polygon.polygon.points.size() == 0) {
00178           transformed_polygon.header = polygon.header;
00179           transformed_polygon.header.frame_id = frame_id_;
00180           transformed_polygon_array.polygons.push_back(transformed_polygon);
00181           transformed_coefficient.values = coefficient.values;
00182           transformed_coefficient.header = polygon.header;
00183           transformed_coefficient.header.frame_id = frame_id_;
00184           transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
00185         }
00186         else {
00187           transformPolygon(eigen_transform, polygon, transformed_polygon);
00188           //computeCoefficients(transformed_polygon, transformed_coefficient);
00189           transformModelCoefficient(eigen_transform, coefficient,
00190                                     transformed_coefficient);
00191           if (isnan(transformed_coefficient.values[0]) ||
00192               isnan(transformed_coefficient.values[1]) ||
00193               isnan(transformed_coefficient.values[2]) ||
00194               isnan(transformed_coefficient.values[3])) {
00195             continue;
00196           }
00197           transformed_polygon_array.polygons.push_back(transformed_polygon);
00198           transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
00199         }
00200       }
00201       else {
00202         JSK_NODELET_ERROR("cannot lookup transform from %s to %s at %f",
00203                       frame_id_.c_str(), coefficient.header.frame_id.c_str(),
00204                       coefficient.header.stamp.toSec());
00205         return;
00206       }
00207     }
00208     polygons_pub_.publish(transformed_polygon_array);
00209     coefficients_pub_.publish(transformed_model_coefficients_array);
00210   }
00211   
00212 }
00213 
00214 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PolygonArrayTransformer, nodelet::Nodelet);