void TfTransformCloud::transform(const sensor_msgs::PointCloud2ConstPtr &input) { vital_checker_->poke(); sensor_msgs::PointCloud2 output; try { if (use_latest_tf_) { sensor_msgs::PointCloud2 latest_pointcloud(*input); latest_pointcloud.header.stamp = ros::Time(0); if (pcl_ros::transformPointCloud(target_frame_id_, latest_pointcloud, output, *tf_listener_)) { output.header.stamp = input->header.stamp; pub_cloud_.publish(output); } } else { if (pcl_ros::transformPointCloud(target_frame_id_, *input, output, *tf_listener_)) { pub_cloud_.publish(output); } } } catch (tf2::ConnectivityException &e) { JSK_NODELET_ERROR("Transform error: %s", e.what()); } catch (tf2::InvalidArgumentException &e) { JSK_NODELET_ERROR("Transform error: %s", e.what()); } catch (...) { NODELET_ERROR("Unknown transform error"); } }
void SnapIt::convexAlignPolygonCallback( const geometry_msgs::PolygonStamped::ConstPtr& poly_msg) { boost::mutex::scoped_lock lock(mutex_); geometry_msgs::PoseArray pose_array; pose_array.header = poly_msg->header; if (!polygons_) { JSK_NODELET_ERROR("no polygon is ready"); return; } std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes = createConvexes(poly_msg->header.frame_id, poly_msg->header.stamp, polygons_); for (size_t i = 0; i < poly_msg->polygon.points.size(); i++) { geometry_msgs::Point32 p = poly_msg->polygon.points[i]; Eigen::Vector3f pose_point(p.x, p.y, p.z); int min_index = findNearestConvex(pose_point, convexes); if (min_index == -1) { JSK_NODELET_ERROR("cannot project onto convex"); return; } else { jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index]; Eigen::Affine3f pose_eigen = Eigen::Affine3f::Identity(); pose_eigen.translate(pose_point); geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex); aligned_pose.header = poly_msg->header; pose_array.poses.push_back(aligned_pose.pose); } } convex_aligned_pose_array_pub_.publish(pose_array); }
void TiltLaserListener::onInit() { DiagnosticNodelet::onInit(); skip_counter_ = 0; if (pnh_->hasParam("joint_name")) { pnh_->getParam("joint_name", joint_name_); } else { JSK_NODELET_ERROR("no ~joint_state is specified"); return; } pnh_->param("overwrap_angle", overwrap_angle_, 0.0); std::string laser_type; pnh_->param("skip_number", skip_number_, 1); pnh_->param("laser_type", laser_type, std::string("tilt_half_down")); if (laser_type == "tilt_half_up") { laser_type_ = TILT_HALF_UP; } else if (laser_type == "tilt_half_down") { laser_type_ = TILT_HALF_DOWN; } else if (laser_type == "tilt") { laser_type_ = TILT; } else if (laser_type == "infinite_spindle") { laser_type_ = INFINITE_SPINDLE; } else if (laser_type == "infinite_spindle_half") { laser_type_ = INFINITE_SPINDLE_HALF; } else { JSK_NODELET_ERROR("unknown ~laser_type: %s", laser_type.c_str()); return; } pnh_->param("not_use_laser_assembler_service", not_use_laser_assembler_service_, false); pnh_->param("use_laser_assembler", use_laser_assembler_, false); if (use_laser_assembler_) { if (not_use_laser_assembler_service_) { sub_cloud_ = pnh_->subscribe("input/cloud", 100, &TiltLaserListener::cloudCallback, this); } else { assemble_cloud_srv_ = pnh_->serviceClient<laser_assembler::AssembleScans2>("assemble_scans2"); } cloud_pub_ = pnh_->advertise<sensor_msgs::PointCloud2>("output_cloud", 1); } prev_angle_ = 0; prev_velocity_ = 0; start_time_ = ros::Time::now(); clear_cache_service_ = pnh_->advertiseService( "clear_cache", &TiltLaserListener::clearCacheCallback, this); trigger_pub_ = advertise<jsk_recognition_msgs::TimeRange>(*pnh_, "output", 1); sub_ = pnh_->subscribe("input", 1, &TiltLaserListener::jointCallback, this); }
void TiltLaserListener::publishTimeRange( const ros::Time& stamp, const ros::Time& start, const ros::Time& end) { jsk_recognition_msgs::TimeRange range; range.header.stamp = stamp; range.start = start; range.end = end; trigger_pub_.publish(range); if (use_laser_assembler_) { if (skip_counter_++ % skip_number_ == 0) { laser_assembler::AssembleScans2 srv; srv.request.begin = start; srv.request.end = end; try { if (!not_use_laser_assembler_service_) { if (assemble_cloud_srv_.call(srv)) { sensor_msgs::PointCloud2 output_cloud = srv.response.cloud; output_cloud.header.stamp = stamp; cloud_pub_.publish(output_cloud); } else { JSK_NODELET_ERROR("Failed to call assemble cloud service"); } } else { // Assemble cloud from local buffer std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds; { boost::mutex::scoped_lock lock(cloud_mutex_); for (size_t i = 0; i < cloud_buffer_.size(); i++) { ros::Time the_stamp = cloud_buffer_[i]->header.stamp; if (the_stamp > start && the_stamp < end) { target_clouds.push_back(cloud_buffer_[i]); } } cloud_buffer_.removeBefore(start); } sensor_msgs::PointCloud2 output_cloud; getPointCloudFromLocalBuffer(target_clouds, output_cloud); output_cloud.header.stamp = stamp; cloud_pub_.publish(output_cloud); } } catch (...) { JSK_NODELET_ERROR("Exception in calling assemble cloud service"); } } } }
std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> SnapIt::createConvexes( const std::string& frame_id, const ros::Time& stamp, jsk_recognition_msgs::PolygonArray::ConstPtr polygons) { std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> result; try { for (size_t i = 0; i < polygons->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = polygons->polygons[i]; jsk_recognition_utils::Vertices vertices; tf::StampedTransform transform = lookupTransformWithDuration( tf_listener_, polygon.header.frame_id, frame_id, stamp, ros::Duration(5.0)); for (size_t j = 0; j < polygon.polygon.points.size(); j++) { Eigen::Vector4d p; p[0] = polygon.polygon.points[j].x; p[1] = polygon.polygon.points[j].y; p[2] = polygon.polygon.points[j].z; p[3] = 1; Eigen::Affine3d eigen_transform; tf::transformTFToEigen(transform, eigen_transform); Eigen::Vector4d transformed_pointd = eigen_transform.inverse() * p; Eigen::Vector3f transformed_point; transformed_point[0] = transformed_pointd[0]; transformed_point[1] = transformed_pointd[1]; transformed_point[2] = transformed_pointd[2]; vertices.push_back(transformed_point); } std::reverse(vertices.begin(), vertices.end()); jsk_recognition_utils::ConvexPolygon::Ptr convex(new jsk_recognition_utils::ConvexPolygon(vertices)); result.push_back(convex); } } catch (tf2::ConnectivityException &e) { JSK_NODELET_ERROR("Transform error: %s", e.what()); } catch (tf2::InvalidArgumentException &e) { JSK_NODELET_ERROR("Transform error: %s", e.what()); } catch (tf2::ExtrapolationException &e) { JSK_NODELET_ERROR("Transform error: %s", e.what()); } return result; }
void SnapIt::convexAlignCallback( const geometry_msgs::PoseStamped::ConstPtr& pose_msg) { boost::mutex::scoped_lock lock(mutex_); if (!polygons_) { JSK_NODELET_ERROR("no polygon is ready"); convex_aligned_pub_.publish(pose_msg); return; } std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes = createConvexes(pose_msg->header.frame_id, pose_msg->header.stamp, polygons_); Eigen::Affine3d pose_eigend; Eigen::Affine3f pose_eigen; tf::poseMsgToEigen(pose_msg->pose, pose_eigend); convertEigenAffine3(pose_eigend, pose_eigen); Eigen::Vector3f pose_point(pose_eigen.translation()); int min_index = findNearestConvex(pose_point, convexes); if (min_index != -1) { jsk_recognition_utils::ConvexPolygon::Ptr min_convex = convexes[min_index]; geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex); aligned_pose.header = pose_msg->header; convex_aligned_pub_.publish(aligned_pose); } else { convex_aligned_pub_.publish(pose_msg); // shoud we publish this? } }
void HeightmapToPointCloud::convert(const sensor_msgs::Image::ConstPtr& msg) { boost::mutex::scoped_lock lock(mutex_); if (!config_msg_) { JSK_NODELET_ERROR("no ~input/config is yet available"); return; } cv::Mat float_image = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::TYPE_32FC1)->image; pcl::PointCloud<pcl::PointXYZ> cloud; cloud.points.reserve(float_image.rows * float_image.cols); double dx = (max_x_ - min_x_) / float_image.cols; double dy = (max_y_ - min_y_) / float_image.rows; for (size_t j = 0; j < float_image.rows; j++) { for (size_t i = 0; i < float_image.cols; i++) { float v = float_image.at<float>(j, i); pcl::PointXYZ p; if (v != -FLT_MAX) { p.y = j * dy + min_y_ + dy / 2.0; p.x = i * dx + min_x_ + dx / 2.0; p.z = v; cloud.points.push_back(p); } } } sensor_msgs::PointCloud2 ros_cloud; pcl::toROSMsg(cloud, ros_cloud); ros_cloud.header = msg->header; pub_.publish(ros_cloud); }
void ProjectImagePoint::project( const geometry_msgs::PointStamped::ConstPtr& msg) { vital_checker_->poke(); boost::mutex::scoped_lock lock(mutex_); if (!camera_info_) { JSK_NODELET_WARN( "[ProjectImagePoint::project] camera info is not yet available"); return; } image_geometry::PinholeCameraModel model; model.fromCameraInfo(camera_info_); cv::Point3d ray = model.projectPixelTo3dRay( cv::Point2d(msg->point.x, msg->point.y)); geometry_msgs::Vector3Stamped vector; vector.header.frame_id = camera_info_->header.frame_id; vector.header = msg->header; vector.vector.x = ray.x; vector.vector.y = ray.y; vector.vector.z = ray.z; pub_vector_.publish(vector); if (ray.z == 0.0) { JSK_NODELET_ERROR("Z value of projected ray is 0"); return; } double alpha = z_ / ray.z; geometry_msgs::PointStamped point; point.header = msg->header; point.header.frame_id = camera_info_->header.frame_id; point.point.x = ray.x * alpha; point.point.y = ray.y * alpha; point.point.z = ray.z * alpha; pub_.publish(point); }
void OctreeVoxelGrid::generateVoxelCloud(const sensor_msgs::PointCloud2ConstPtr& input_msg) { boost::mutex::scoped_lock lock(mutex_); if (resolution_ == 0.0) { pub_cloud_.publish(input_msg); } else { if (point_type_ == "xyz") { generateVoxelCloudImpl<pcl::PointXYZ>(input_msg); } else if (point_type_ == "xyznormal") { generateVoxelCloudImpl<pcl::PointNormal>(input_msg); } else if (point_type_ == "xyzrgb") { generateVoxelCloudImpl<pcl::PointXYZRGB>(input_msg); } else if (point_type_ == "xyzrgbnormal") { generateVoxelCloudImpl<pcl::PointXYZRGBNormal>(input_msg); } else { JSK_NODELET_ERROR("unknown point type: %s", point_type_.c_str()); } } std_msgs::Float32 resolution; resolution.data = resolution_; pub_octree_resolution_.publish(resolution); }
void PolygonArrayColorLikelihood::likelihood( const jsk_recognition_msgs::PolygonArray::ConstPtr& polygon_msg, const jsk_recognition_msgs::HistogramWithRangeArray::ConstPtr& histogram_msg) { boost::mutex::scoped_lock lock(mutex_); if (!reference_) { return; } if (polygon_msg->polygons.size() != histogram_msg->histograms.size()) { JSK_NODELET_ERROR("length of polygon and histogram are not same"); return; } cv::MatND reference_histogram = jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND( reference_->bins); cv::normalize(reference_histogram, reference_histogram, 1, reference_histogram.rows, cv::NORM_L2, -1, cv::Mat()); jsk_recognition_msgs::PolygonArray new_msg(*polygon_msg); for (size_t i = 0; i < new_msg.polygons.size(); i++) { cv::MatND hist = jsk_recognition_utils::HistogramWithRangeBinArrayTocvMatND( histogram_msg->histograms[i].bins); cv::normalize(hist, hist, 1, hist.rows, cv::NORM_L2, -1, cv::Mat()); double d = compareHist(reference_histogram, hist); if (polygon_msg->likelihood.size() == 0) { new_msg.likelihood.push_back(d); } else { new_msg.likelihood[i] *= d; } } pub_.publish(new_msg); }
void BorderEstimator::estimate( const sensor_msgs::PointCloud2::ConstPtr& msg, const sensor_msgs::CameraInfo::ConstPtr& info) { if (msg->height == 1) { JSK_NODELET_ERROR("[BorderEstimator::estimate] pointcloud must be organized"); return; } pcl::RangeImagePlanar range_image; pcl::PointCloud<pcl::PointXYZ> cloud; pcl::fromROSMsg(*msg, cloud); Eigen::Affine3f dummytrans = Eigen::Affine3f::Identity(); float fx = info->P[0]; float cx = info->P[2]; float tx = info->P[3]; float fy = info->P[5]; float cy = info->P[6]; range_image.createFromPointCloudWithFixedSize (cloud, msg->width, msg->height, cx, cy, fx, fy, dummytrans); range_image.setUnseenToMaxRange(); computeBorder(range_image, msg->header); }
void LabDecomposer::decompose( const sensor_msgs::Image::ConstPtr& image_msg) { cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy( image_msg, image_msg->encoding); cv::Mat image = cv_ptr->image; cv::Mat lab_image; std::vector<cv::Mat> lab_planes; if (image_msg->encoding == sensor_msgs::image_encodings::BGR8) { cv::cvtColor(image, lab_image, CV_BGR2Lab); } else if (image_msg->encoding == sensor_msgs::image_encodings::RGB8) { cv::cvtColor(image, lab_image, CV_RGB2Lab); } else { JSK_NODELET_ERROR("unsupported format to Lab: %s", image_msg->encoding.c_str()); return; } cv::split(lab_image, lab_planes); cv::Mat l = lab_planes[0]; cv::Mat a = lab_planes[1]; cv::Mat b = lab_planes[2]; pub_l_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, l).toImageMsg()); pub_a_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, a).toImageMsg()); pub_b_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, b).toImageMsg()); }
void DepthCalibration::calibrate( const sensor_msgs::Image::ConstPtr& msg, const sensor_msgs::CameraInfo::ConstPtr& camera_info) { boost::mutex::scoped_lock lock(mutex_); cv_bridge::CvImagePtr cv_ptr; try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); } catch (cv_bridge::Exception& e) { JSK_NODELET_ERROR("cv_bridge exception: %s", e.what()); return; } cv::Mat image = cv_ptr->image; cv::Mat output_image = image.clone(); double cu = camera_info->P[2]; double cv = camera_info->P[6]; for(int v = 0; v < image.rows; v++) { for(int u = 0; u < image.cols; u++) { float z = image.at<float>(v, u); if (isnan(z)) { output_image.at<float>(v, u) = z; } else { output_image.at<float>(v, u) = applyModel(z, u, v, cu, cv); } //JSK_NODELET_INFO("z: %f", z); } } sensor_msgs::Image::Ptr ros_image = cv_bridge::CvImage(msg->header, "32FC1", output_image).toImageMsg(); pub_.publish(ros_image); }
void HeightmapTimeAccumulation::accumulate( const sensor_msgs::Image::ConstPtr& msg) { boost::mutex::scoped_lock lock(mutex_); if (!config_) { JSK_NODELET_ERROR("no ~input/config is yet available"); return; } tf::StampedTransform tf_transform; tf_->lookupTransform(fixed_frame_id_, center_frame_id_, msg->header.stamp, tf_transform); Eigen::Affine3f from_center_to_fixed; tf::transformTFToEigen(tf_transform, from_center_to_fixed); cv::Mat new_heightmap = cv_bridge::toCvShare( msg, sensor_msgs::image_encodings::TYPE_32FC1)->image; // Transform prev_cloud_ to current frame Eigen::Affine3f from_prev_to_current = prev_from_center_to_fixed_.inverse() * from_center_to_fixed; pcl::PointCloud<pcl::PointXYZ> transformed_pointcloud; pcl::transformPointCloud(prev_cloud_, transformed_pointcloud, from_prev_to_current.inverse()); for (size_t i = 0; i < transformed_pointcloud.points.size(); i++) { pcl::PointXYZ p = transformed_pointcloud.points[i]; if (isValidPoint(p)) { cv::Point index = toIndex(p, new_heightmap); if (isValidIndex(index, new_heightmap)) { if (!isValidCell(index, new_heightmap)) { new_heightmap.at<float>(index.y, index.x) = p.z; } } } } publishHeightmap(new_heightmap, msg->header); prev_from_center_to_fixed_ = from_center_to_fixed; }
void PolygonArrayAngleLikelihood::likelihood( const jsk_recognition_msgs::PolygonArray::ConstPtr& msg) { boost::mutex::scoped_lock lock(mutex_); jsk_recognition_msgs::PolygonArray new_msg(*msg); try { // Resolve tf // ConstPtrmpute position of target_frame_id // respected from msg->header.frame_id tf::StampedTransform transform; tf_listener_->lookupTransform( target_frame_id_, msg->header.frame_id, msg->header.stamp, transform); Eigen::Affine3f pose; tf::transformTFToEigen(transform, pose); // Use x Eigen::Vector3f reference_axis = pose.rotation() * axis_; double min_distance = DBL_MAX; double max_distance = - DBL_MAX; std::vector<double> distances; for (size_t i = 0; i < msg->polygons.size(); i++) { jsk_recognition_utils::Polygon::Ptr polygon = jsk_recognition_utils::Polygon::fromROSMsgPtr(msg->polygons[i].polygon); Eigen::Vector3f n = polygon->getNormal(); double distance = std::abs(reference_axis.dot(n)); min_distance = std::min(distance, min_distance); max_distance = std::max(distance, max_distance); distances.push_back(distance); } // Normalization for (size_t i = 0; i < distances.size(); i++) { // double normalized_distance // = (distances[i] - min_distance) / (max_distance - min_distance); double likelihood = 1 / (1 + (distances[i] - 1) * (distances[i] - 1)); if (msg->likelihood.size() == 0) { new_msg.likelihood.push_back(likelihood); } else { new_msg.likelihood[i] = new_msg.likelihood[i] * likelihood; } } pub_.publish(new_msg); } catch (...) { JSK_NODELET_ERROR("Unknown transform error"); } }
void LineSegmentCollector::onInit() { DiagnosticNodelet::onInit(); srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_); dynamic_reconfigure::Server<Config>::CallbackType f = boost::bind (&LineSegmentCollector::configCallback, this, _1, _2); srv_->setCallback (f); if (!pnh_->getParam("fixed_frame_id", fixed_frame_id_)) { JSK_NODELET_ERROR("no ~fixed_frame_id is specified"); return; } std::string rotate_type_str; pnh_->param("rotate_type", rotate_type_str, std::string("tilt_two_way")); if (rotate_type_str == "tilt") { rotate_type_ = ROTATION_TILT; } else if (rotate_type_str == "tilt_two_way") { rotate_type_ = ROTATION_TILT_TWO_WAY; } else if (rotate_type_str == "spindle") { rotate_type_ = ROTATION_SPINDLE; } else { JSK_NODELET_ERROR("unknown ~rotate_type: %s", rotate_type_str.c_str()); return; } pub_point_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1); pub_inliers_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output/inliers", 1); pub_coefficients_ = advertise<jsk_recognition_msgs::ModelCoefficientsArray>(*pnh_, "output/coefficients", 1); pub_polygons_ = advertise<jsk_recognition_msgs::PolygonArray>(*pnh_, "output/polygons", 1); debug_pub_inliers_before_plane_ = advertise<jsk_recognition_msgs::ClusterPointIndices>( *pnh_, "debug/connect_segments/inliers", 1); }
void HSVDecomposer::decompose( const sensor_msgs::Image::ConstPtr& image_msg) { cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy( image_msg, image_msg->encoding); cv::Mat image = cv_ptr->image; cv::Mat hsv_image; std::vector<cv::Mat> hsv_planes; if (image_msg->encoding == sensor_msgs::image_encodings::BGR8) { cv::cvtColor(image, hsv_image, CV_BGR2HSV); } else if (image_msg->encoding == sensor_msgs::image_encodings::RGB8) { cv::cvtColor(image, hsv_image, CV_RGB2HSV); } else if (image_msg->encoding == sensor_msgs::image_encodings::BGRA8 || image_msg->encoding == sensor_msgs::image_encodings::BGRA16) { cv::Mat tmp_image; cv::cvtColor(image, tmp_image, CV_BGRA2BGR); cv::cvtColor(tmp_image, hsv_image, CV_BGR2HSV); } else if (image_msg->encoding == sensor_msgs::image_encodings::RGBA8 || image_msg->encoding == sensor_msgs::image_encodings::RGBA16) { cv::Mat tmp_image; cv::cvtColor(image, tmp_image, CV_RGBA2BGR); cv::cvtColor(tmp_image, hsv_image, CV_BGR2HSV); } else { JSK_NODELET_ERROR("unsupported format to HSV: %s", image_msg->encoding.c_str()); return; } cv::split(hsv_image, hsv_planes); cv::Mat hue = hsv_planes[0]; cv::Mat saturation = hsv_planes[1]; cv::Mat value = hsv_planes[2]; pub_h_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, hue).toImageMsg()); pub_s_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, saturation).toImageMsg()); pub_v_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::MONO8, value).toImageMsg()); }
double ColorHistogramLabelMatch::coefficients( const cv::Mat& ref_hist, const cv::Mat& target_hist) { if (coefficient_method_ == 0) { return (1.0 + cv::compareHist(ref_hist, target_hist, CV_COMP_CORREL)) / 2.0; } else if (coefficient_method_ == 1) { double x = cv::compareHist(ref_hist, target_hist, CV_COMP_CHISQR); return 1/ (x * x + 1); } else if (coefficient_method_ == 2) { return cv::compareHist(ref_hist, target_hist, CV_COMP_INTERSECT); } else if (coefficient_method_ == 3) { return 1.0 - cv::compareHist(ref_hist, target_hist, CV_COMP_BHATTACHARYYA); } else if (coefficient_method_ == 4 || coefficient_method_ == 5) { cv::Mat ref_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1); cv::Mat target_sig = cv::Mat::zeros(ref_hist.cols, 2, CV_32FC1); //JSK_NODELET_INFO("ref_hist.cols = %d", ref_hist.cols); for (size_t i = 0; i < ref_hist.cols; i++) { ref_sig.at<float>(i, 0) = ref_hist.at<float>(0, i); target_sig.at<float>(i, 0) = target_hist.at<float>(0, i); ref_sig.at<float>(i, 1) = i; target_sig.at<float>(i, 1) = i; } if (coefficient_method_ == 4) { double x = cv::EMD(ref_sig, target_sig, CV_DIST_L1); return 1.0 / (1.0 + x * x); } else { double x = cv::EMD(ref_sig, target_sig, CV_DIST_L2); return 1.0 / (1.0 + x * x); } } else { JSK_NODELET_ERROR("unknown coefficiet method: %d", coefficient_method_); return 0; } }
void SnapIt::polygonAlignCallback( const geometry_msgs::PoseStamped::ConstPtr& pose_msg) { boost::mutex::scoped_lock lock(mutex_); if (!polygons_) { JSK_NODELET_ERROR("no polygon is ready"); polygon_aligned_pub_.publish(pose_msg); return; } std::vector<jsk_recognition_utils::ConvexPolygon::Ptr> convexes = createConvexes(pose_msg->header.frame_id, pose_msg->header.stamp, polygons_); Eigen::Affine3d pose_eigend; Eigen::Affine3f pose_eigen; tf::poseMsgToEigen(pose_msg->pose, pose_eigend); convertEigenAffine3(pose_eigend, pose_eigen); Eigen::Vector3f pose_point(pose_eigen.translation()); double min_distance = DBL_MAX; jsk_recognition_utils::ConvexPolygon::Ptr min_convex; for (size_t i = 0; i < convexes.size(); i++) { jsk_recognition_utils::ConvexPolygon::Ptr convex = convexes[i]; double d = convex->distanceToPoint(pose_point); if (d < min_distance) { min_convex = convex; min_distance = d; } } if (min_convex) { geometry_msgs::PoseStamped aligned_pose = alignPose(pose_eigen, min_convex); aligned_pose.header = pose_msg->header; polygon_aligned_pub_.publish(aligned_pose); } else { polygon_aligned_pub_.publish(pose_msg); } }
void ApplyMaskImage::apply( const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::Image::ConstPtr& mask_msg) { vital_checker_->poke(); cv::Mat image; if (jsk_recognition_utils::isBGRA(image_msg->encoding)) { cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; cv::cvtColor(tmp_image, image, cv::COLOR_BGRA2BGR); } else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) { cv::Mat tmp_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; cv::cvtColor(tmp_image, image, cv::COLOR_RGBA2BGR); } else { // BGR, RGB or GRAY image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image; } cv::Mat mask = cv_bridge::toCvShare(mask_msg, "mono8")->image; if (image.cols != mask.cols || image.rows != mask.rows) { JSK_NODELET_ERROR("size of image and mask is different"); JSK_NODELET_ERROR("image: %dx%dx", image.cols, image.rows); JSK_NODELET_ERROR("mask: %dx%dx", mask.cols, mask.rows); return; } if (clip_) { cv::Rect region = jsk_recognition_utils::boundingRectOfMaskImage(mask); mask = mask(region); image = image(region); } pub_mask_.publish(cv_bridge::CvImage( mask_msg->header, "mono8", mask).toImageMsg()); cv::Mat masked_image; image.copyTo(masked_image, mask); cv::Mat output_image; if (mask_black_to_transparent_) { if (sensor_msgs::image_encodings::isMono(image_msg->encoding)) { cv::cvtColor(masked_image, output_image, CV_GRAY2BGRA); } else if (jsk_recognition_utils::isRGB(image_msg->encoding)) { cv::cvtColor(masked_image, output_image, CV_RGB2BGRA); } else { // BGR, BGRA or RGBA cv::cvtColor(masked_image, output_image, CV_BGR2BGRA); } for (size_t j=0; j < mask.rows; j++) { for (int i=0; i < mask.cols; i++) { if (mask.at<uchar>(j, i) == 0) { cv::Vec4b color = output_image.at<cv::Vec4b>(j, i); color[3] = 0; // mask black -> transparent output_image.at<cv::Vec4b>(j, i) = color; } } } // publish bgr8 image pub_image_.publish(cv_bridge::CvImage( image_msg->header, sensor_msgs::image_encodings::BGRA8, output_image).toImageMsg()); } else { if (jsk_recognition_utils::isBGRA(image_msg->encoding)) { cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2BGRA); } else if (jsk_recognition_utils::isRGBA(image_msg->encoding)) { cv::cvtColor(masked_image, output_image, cv::COLOR_BGR2RGBA); } else { // BGR, RGB or GRAY masked_image.copyTo(output_image); } pub_image_.publish(cv_bridge::CvImage( image_msg->header, image_msg->encoding, output_image).toImageMsg()); } }
void PointCloudLocalization::cloudCallback( const sensor_msgs::PointCloud2::ConstPtr& cloud_msg) { vital_checker_->poke(); boost::mutex::scoped_lock lock(mutex_); //JSK_NODELET_INFO("cloudCallback"); latest_cloud_ = cloud_msg; if (localize_requested_){ JSK_NODELET_INFO("localization is requested"); try { pcl::PointCloud<pcl::PointNormal>::Ptr local_cloud (new pcl::PointCloud<pcl::PointNormal>); pcl::fromROSMsg(*latest_cloud_, *local_cloud); JSK_NODELET_INFO("waiting for tf transformation from %s tp %s", latest_cloud_->header.frame_id.c_str(), global_frame_.c_str()); if (tf_listener_->waitForTransform( latest_cloud_->header.frame_id, global_frame_, latest_cloud_->header.stamp, ros::Duration(1.0))) { pcl::PointCloud<pcl::PointNormal>::Ptr input_cloud (new pcl::PointCloud<pcl::PointNormal>); if (use_normal_) { pcl_ros::transformPointCloudWithNormals(global_frame_, *local_cloud, *input_cloud, *tf_listener_); } else { pcl_ros::transformPointCloud(global_frame_, *local_cloud, *input_cloud, *tf_listener_); } pcl::PointCloud<pcl::PointNormal>::Ptr input_downsampled_cloud (new pcl::PointCloud<pcl::PointNormal>); applyDownsampling(input_cloud, *input_downsampled_cloud); if (isFirstTime()) { all_cloud_ = input_downsampled_cloud; first_time_ = false; } else { // run ICP ros::ServiceClient client = pnh_->serviceClient<jsk_pcl_ros::ICPAlign>("icp_align"); jsk_pcl_ros::ICPAlign icp_srv; if (clip_unseen_pointcloud_) { // Before running ICP, remove pointcloud where we cannot see // First, transform reference pointcloud, that is all_cloud_, into // sensor frame. // And after that, remove points which are x < 0. tf::StampedTransform global_sensor_tf_transform = lookupTransformWithDuration( tf_listener_, global_frame_, sensor_frame_, cloud_msg->header.stamp, ros::Duration(1.0)); Eigen::Affine3f global_sensor_transform; tf::transformTFToEigen(global_sensor_tf_transform, global_sensor_transform); pcl::PointCloud<pcl::PointNormal>::Ptr sensor_cloud (new pcl::PointCloud<pcl::PointNormal>); pcl::transformPointCloudWithNormals( *all_cloud_, *sensor_cloud, global_sensor_transform.inverse()); // Remove negative-x points pcl::PassThrough<pcl::PointNormal> pass; pass.setInputCloud(sensor_cloud); pass.setFilterFieldName("x"); pass.setFilterLimits(0.0, 100.0); pcl::PointCloud<pcl::PointNormal>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointNormal>); pass.filter(*filtered_cloud); JSK_NODELET_INFO("clipping: %lu -> %lu", sensor_cloud->points.size(), filtered_cloud->points.size()); // Convert the pointcloud to global frame again pcl::PointCloud<pcl::PointNormal>::Ptr global_filtered_cloud (new pcl::PointCloud<pcl::PointNormal>); pcl::transformPointCloudWithNormals( *filtered_cloud, *global_filtered_cloud, global_sensor_transform); pcl::toROSMsg(*global_filtered_cloud, icp_srv.request.target_cloud); } else { pcl::toROSMsg(*all_cloud_, icp_srv.request.target_cloud); } pcl::toROSMsg(*input_downsampled_cloud, icp_srv.request.reference_cloud); if (client.call(icp_srv)) { Eigen::Affine3f transform; tf::poseMsgToEigen(icp_srv.response.result.pose, transform); Eigen::Vector3f transform_pos(transform.translation()); float roll, pitch, yaw; pcl::getEulerAngles(transform, roll, pitch, yaw); JSK_NODELET_INFO("aligned parameter --"); JSK_NODELET_INFO(" - pos: [%f, %f, %f]", transform_pos[0], transform_pos[1], transform_pos[2]); JSK_NODELET_INFO(" - rot: [%f, %f, %f]", roll, pitch, yaw); pcl::PointCloud<pcl::PointNormal>::Ptr transformed_input_cloud (new pcl::PointCloud<pcl::PointNormal>); if (use_normal_) { pcl::transformPointCloudWithNormals(*input_cloud, *transformed_input_cloud, transform); } else { pcl::transformPointCloud(*input_cloud, *transformed_input_cloud, transform); } pcl::PointCloud<pcl::PointNormal>::Ptr concatenated_cloud (new pcl::PointCloud<pcl::PointNormal>); *concatenated_cloud = *all_cloud_ + *transformed_input_cloud; // update *all_cloud applyDownsampling(concatenated_cloud, *all_cloud_); // update localize_transform_ tf::Transform icp_transform; tf::transformEigenToTF(transform, icp_transform); { boost::mutex::scoped_lock tf_lock(tf_mutex_); localize_transform_ = localize_transform_ * icp_transform; } } else { JSK_NODELET_ERROR("Failed to call ~icp_align"); return; } } localize_requested_ = false; } else { JSK_NODELET_WARN("No tf transformation is available"); } } catch (tf2::ConnectivityException &e) { JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what()); return; } catch (tf2::InvalidArgumentException &e) { JSK_NODELET_ERROR("[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what()); return; } } }
void TiltLaserListener::publishTimeRange( const ros::Time& stamp, const ros::Time& start, const ros::Time& end) { jsk_recognition_msgs::TimeRange range; range.header.stamp = stamp; range.start = start; range.end = end; trigger_pub_.publish(range); // publish velocity // only publish if twist_frame_id is not empty if (!twist_frame_id_.empty()) { // simply compute from the latest velocity if (buffer_.size() >= 2) { // at least, we need two joint angles to // compute velocity StampedJointAngle::Ptr latest = buffer_[buffer_.size() - 1]; StampedJointAngle::Ptr last_second = buffer_[buffer_.size() - 2]; double value_diff = latest->getValue() - last_second->getValue(); double time_diff = (latest->header.stamp - last_second->header.stamp).toSec(); double velocity = value_diff / time_diff; geometry_msgs::TwistStamped twist; twist.header.frame_id = twist_frame_id_; twist.header.stamp = latest->header.stamp; if (laser_type_ == INFINITE_SPINDLE_HALF || // roll laser laser_type_ == INFINITE_SPINDLE) { twist.twist.angular.x = velocity; } else { // tilt laser twist.twist.angular.y = velocity; } twist_pub_.publish(twist); } } if (use_laser_assembler_) { if (skip_counter_++ % skip_number_ == 0) { laser_assembler::AssembleScans2 srv; srv.request.begin = start; srv.request.end = end; try { if (!not_use_laser_assembler_service_) { if (assemble_cloud_srv_.call(srv)) { sensor_msgs::PointCloud2 output_cloud = srv.response.cloud; output_cloud.header.stamp = stamp; cloud_pub_.publish(output_cloud); } else { JSK_NODELET_ERROR("Failed to call assemble cloud service"); } } else { // Assemble cloud from local buffer std::vector<sensor_msgs::PointCloud2::ConstPtr> target_clouds; { boost::mutex::scoped_lock lock(cloud_mutex_); if (cloud_buffer_.size() == 0) { return; } for (size_t i = 0; i < cloud_buffer_.size(); i++) { ros::Time the_stamp = cloud_buffer_[i]->header.stamp; if (the_stamp > start && the_stamp < end) { target_clouds.push_back(cloud_buffer_[i]); } } if (clear_assembled_scans_) { cloud_buffer_.removeBefore(end); } else { cloud_buffer_.removeBefore(start); } } sensor_msgs::PointCloud2 output_cloud; getPointCloudFromLocalBuffer(target_clouds, output_cloud); output_cloud.header.stamp = stamp; cloud_pub_.publish(output_cloud); } } catch (...) { JSK_NODELET_ERROR("Exception in calling assemble cloud service"); } } } }
void CollisionDetector::initSelfMask() { // genearte urdf model std::string content; urdf::Model urdf_model; if (nh_->getParam("robot_description", content)) { if (!urdf_model.initString(content)) { JSK_NODELET_ERROR("Unable to parse URDF description!"); } } std::string root_link_id; std::string world_frame_id; pnh_->param<std::string>("root_link_id", root_link_id, "BODY"); pnh_->param<std::string>("world_frame_id", world_frame_id, "map"); double default_padding, default_scale; pnh_->param("self_see_default_padding", default_padding, 0.01); pnh_->param("self_see_default_scale", default_scale, 1.0); std::vector<robot_self_filter::LinkInfo> links; std::string link_names; if (!pnh_->hasParam("self_see_links")) { JSK_NODELET_WARN("No links specified for self filtering."); } else { XmlRpc::XmlRpcValue ssl_vals;; pnh_->getParam("self_see_links", ssl_vals); if (ssl_vals.getType() != XmlRpc::XmlRpcValue::TypeArray) { JSK_NODELET_WARN("Self see links need to be an array"); } else { if (ssl_vals.size() == 0) { JSK_NODELET_WARN("No values in self see links array"); } else { for (int i = 0; i < ssl_vals.size(); i++) { robot_self_filter::LinkInfo li; if (ssl_vals[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) { JSK_NODELET_WARN("Self see links entry %d is not a structure. Stopping processing of self see links",i); break; } if (!ssl_vals[i].hasMember("name")) { JSK_NODELET_WARN("Self see links entry %d has no name. Stopping processing of self see links",i); break; } li.name = std::string(ssl_vals[i]["name"]); if (!ssl_vals[i].hasMember("padding")) { JSK_NODELET_DEBUG("Self see links entry %d has no padding. Assuming default padding of %g",i,default_padding); li.padding = default_padding; } else { li.padding = ssl_vals[i]["padding"]; } if (!ssl_vals[i].hasMember("scale")) { JSK_NODELET_DEBUG("Self see links entry %d has no scale. Assuming default scale of %g",i,default_scale); li.scale = default_scale; } else { li.scale = ssl_vals[i]["scale"]; } links.push_back(li); } } } } self_mask_ = boost::shared_ptr<robot_self_filter::SelfMaskUrdfRobot>(new robot_self_filter::SelfMaskUrdfRobot(tf_listener_, tf_broadcaster_, links, urdf_model, root_link_id, world_frame_id)); }
void FeatureRegistration::estimate( const sensor_msgs::PointCloud2::ConstPtr& cloud_msg, const sensor_msgs::PointCloud2::ConstPtr& feature_msg) { boost::mutex::scoped_lock lock(mutex_); if (!reference_cloud_ || !reference_feature_) { JSK_NODELET_ERROR("Not yet reference data is available"); return; } pcl::PointCloud<pcl::PointNormal>::Ptr cloud (new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::PointNormal>::Ptr object_aligned (new pcl::PointCloud<pcl::PointNormal>); pcl::PointCloud<pcl::FPFHSignature33>::Ptr feature (new pcl::PointCloud<pcl::FPFHSignature33>); pcl::fromROSMsg(*cloud_msg, *cloud); pcl::fromROSMsg(*feature_msg, *feature); pcl::SampleConsensusPrerejective<pcl::PointNormal, pcl::PointNormal, pcl::FPFHSignature33> align; align.setInputSource(reference_cloud_); align.setSourceFeatures(reference_feature_); align.setInputTarget(cloud); align.setTargetFeatures(feature); align.setMaximumIterations(max_iterations_); // Number of RANSAC iterations align.setNumberOfSamples(3); // Number of points to sample for generating/prerejecting a pose align.setCorrespondenceRandomness(correspondence_randomness_); // Number of nearest features to use align.setSimilarityThreshold(similarity_threshold_); // Polygonal edge length similarity threshold align.setMaxCorrespondenceDistance(max_correspondence_distance_); // Inlier threshold align.setInlierFraction(inlier_fraction_); // Required inlier fraction for accepting a pose hypothesis align.align (*object_aligned); if (align.hasConverged ()) { // Print results printf ("\n"); Eigen::Affine3f transformation(align.getFinalTransformation()); geometry_msgs::PoseStamped ros_pose; tf::poseEigenToMsg(transformation, ros_pose.pose); ros_pose.header = cloud_msg->header; pub_pose_.publish(ros_pose); pcl::PointCloud<pcl::PointNormal>::Ptr result_cloud (new pcl::PointCloud<pcl::PointNormal>); pcl::transformPointCloud( *reference_cloud_, *result_cloud, transformation); sensor_msgs::PointCloud2 ros_cloud; pcl::toROSMsg(*object_aligned, ros_cloud); ros_cloud.header = cloud_msg->header; pub_cloud_.publish(ros_cloud); //pcl::console::print_info ("Inliers: %i/%i\n", align.getInliers ().size (), object->size ()); } else { JSK_NODELET_WARN("failed to align pointcloud"); } }
void PlaneSupportedCuboidEstimator::estimate( const sensor_msgs::PointCloud2::ConstPtr& msg) { // Update polygons_ vector polygons_.clear(); for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) { Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon); polygon->decomposeToTriangles(); polygons_.push_back(polygon); } try { // viewpoint tf::StampedTransform transform = lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id, ros::Time(0.0), ros::Duration(0.0)); tf::vectorTFToEigen(transform.getOrigin(), viewpoint_); if (!tracker_) { NODELET_INFO("initTracker"); pcl::PointCloud<pcl::tracking::ParticleCuboid>::Ptr particles = initParticles(); tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>); tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1)); tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2)); tracker_->setParticles(particles); tracker_->setParticleNum(particle_num_); } else { pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); pcl::fromROSMsg(*msg, *cloud); tracker_->setInputCloud(cloud); // Before call compute() method, we prepare candidate_cloud_ for // weight phase. candidate_cloud_.reset(new pcl::PointCloud<pcl::PointXYZ>); std::set<int> all_indices; for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) { Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon); pcl::ExtractPolygonalPrismData<pcl::PointXYZ> prism_extract; pcl::PointCloud<pcl::PointXYZ>::Ptr boundaries (new pcl::PointCloud<pcl::PointXYZ>); polygon->boundariesToPointCloud<pcl::PointXYZ>(*boundaries); boundaries->points.push_back(boundaries->points[0]); prism_extract.setInputCloud(cloud); prism_extract.setViewPoint(viewpoint_[0], viewpoint_[1], viewpoint_[2]); prism_extract.setHeightLimits(init_local_position_z_min_, init_local_position_z_max_); prism_extract.setInputPlanarHull(boundaries); pcl::PointIndices output_indices; prism_extract.segment(output_indices); for (size_t i = 0; i < output_indices.indices.size(); i++) { all_indices.insert(output_indices.indices[i]); } } pcl::ExtractIndices<pcl::PointXYZ> ex; ex.setInputCloud(cloud); pcl::PointIndices::Ptr indices (new pcl::PointIndices); indices->indices = std::vector<int>(all_indices.begin(), all_indices.end()); ex.setIndices(indices); ex.filter(*candidate_cloud_); sensor_msgs::PointCloud2 ros_candidate_cloud; pcl::toROSMsg(*candidate_cloud_, ros_candidate_cloud); ros_candidate_cloud.header = msg->header; pub_candidate_cloud_.publish(ros_candidate_cloud); // check the number of candidate points if (candidate_cloud_->points.size() == 0) { JSK_NODELET_ERROR("No candidate cloud"); return; } tree_.setInputCloud(candidate_cloud_); if (support_plane_updated_) { // Compute assignment between particles and polygons NODELET_INFO("polygon updated"); updateParticlePolygonRelationship(tracker_->getParticles()); } ROS_INFO("start tracker_->compute()"); tracker_->compute(); ROS_INFO("done tracker_->compute()"); Particle result = tracker_->getResult(); jsk_recognition_msgs::BoundingBoxArray box_array; box_array.header = msg->header; box_array.boxes.push_back(result.toBoundingBox()); box_array.boxes[0].header = msg->header; pub_result_.publish(box_array); } support_plane_updated_ = false; ParticleCloud::Ptr particles = tracker_->getParticles(); // Publish histograms publishHistogram(particles, 0, pub_histogram_global_x_, msg->header); publishHistogram(particles, 1, pub_histogram_global_y_, msg->header); publishHistogram(particles, 2, pub_histogram_global_z_, msg->header); publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header); publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header); publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header); publishHistogram(particles, 6, pub_histogram_dx_, msg->header); publishHistogram(particles, 7, pub_histogram_dy_, msg->header); publishHistogram(particles, 8, pub_histogram_dz_, msg->header); // Publish particles sensor_msgs::PointCloud2 ros_particles; pcl::toROSMsg(*particles, ros_particles); ros_particles.header = msg->header; pub_particles_.publish(ros_particles); } catch (tf2::TransformException& e) { JSK_ROS_ERROR("tf exception"); } }
void PolygonArrayTransformer::transform(const jsk_recognition_msgs::PolygonArray::ConstPtr& polygons, const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients) { if (polygons->polygons.size() != coefficients->coefficients.size()) { JSK_NODELET_ERROR("the size of polygons(%lu) does not match with the size of coefficients(%lu)", polygons->polygons.size(), coefficients->coefficients.size()); return; } jsk_recognition_msgs::PolygonArray transformed_polygon_array; jsk_recognition_msgs::ModelCoefficientsArray transformed_model_coefficients_array; transformed_polygon_array.header = polygons->header; transformed_model_coefficients_array.header = coefficients->header; transformed_polygon_array.header.frame_id = frame_id_; transformed_model_coefficients_array.header.frame_id = frame_id_; for (size_t i = 0; i < polygons->polygons.size(); i++) { geometry_msgs::PolygonStamped polygon = polygons->polygons[i]; PCLModelCoefficientMsg coefficient = coefficients->coefficients[i]; if (polygon.header.frame_id != coefficient.header.frame_id) { 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", i, polygon.header.frame_id.c_str(), i, coefficient.header.frame_id.c_str()); return; } listener_->waitForTransform(coefficient.header.frame_id, frame_id_, coefficient.header.stamp, ros::Duration(1.0)); if (listener_->canTransform(coefficient.header.frame_id, frame_id_, coefficient.header.stamp)) { tf::StampedTransform transform; // header -> frame_id_ listener_->lookupTransform(coefficient.header.frame_id, frame_id_, //ros::Time(0.0), transform); coefficient.header.stamp, transform); Eigen::Affine3d eigen_transform; tf::transformTFToEigen(transform, eigen_transform); PCLModelCoefficientMsg transformed_coefficient; //transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient); geometry_msgs::PolygonStamped transformed_polygon; if (polygon.polygon.points.size() == 0) { transformed_polygon.header = polygon.header; transformed_polygon.header.frame_id = frame_id_; transformed_polygon_array.polygons.push_back(transformed_polygon); transformed_coefficient.values = coefficient.values; transformed_coefficient.header = polygon.header; transformed_coefficient.header.frame_id = frame_id_; transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient); } else { transformPolygon(eigen_transform, polygon, transformed_polygon); //computeCoefficients(transformed_polygon, transformed_coefficient); transformModelCoefficient(eigen_transform, coefficient, transformed_coefficient); if (isnan(transformed_coefficient.values[0]) || isnan(transformed_coefficient.values[1]) || isnan(transformed_coefficient.values[2]) || isnan(transformed_coefficient.values[3])) { continue; } transformed_polygon_array.polygons.push_back(transformed_polygon); transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient); } } else { JSK_NODELET_ERROR("cannot lookup transform from %s to %s at %f", frame_id_.c_str(), coefficient.header.frame_id.c_str(), coefficient.header.stamp.toSec()); return; } } polygons_pub_.publish(transformed_polygon_array); coefficients_pub_.publish(transformed_model_coefficients_array); }