46 ConnectionBasedNodelet::onInit();
52 polygons_pub_ = advertise<jsk_recognition_msgs::PolygonArray>(*
pnh_,
"output_polygons", 1);
54 *
pnh_,
"output_coefficients", 1);
60 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
76 Eigen::Vector3d
A, B, C;
77 A[0] = polygon.polygon.points[0].x;
78 A[1] = polygon.polygon.points[0].y;
79 A[2] = polygon.polygon.points[0].z;
80 B[0] = polygon.polygon.points[1].x;
81 B[1] = polygon.polygon.points[1].y;
82 B[2] = polygon.polygon.points[1].z;
83 C[0] = polygon.polygon.points[2].x;
84 C[1] = polygon.polygon.points[2].y;
85 C[2] = polygon.polygon.points[2].z;
86 Eigen::Vector3d n = (B - A).
cross(C - A).normalized();
90 double d = -(a * A[0] + b * A[1] + c * A[2]);
91 coefficient.header = polygon.header;
92 coefficient.values.push_back(a);
93 coefficient.values.push_back(b);
94 coefficient.values.push_back(c);
95 coefficient.values.push_back(d);
105 result.header.stamp = coefficient.header.stamp;
109 coefficient.values[0], coefficient.values[1], coefficient.values[2], coefficient.values[3],
110 result.values[0], result.values[1], result.values[2], result.values[3]);
114 const geometry_msgs::PolygonStamped& polygon,
115 geometry_msgs::PolygonStamped& result)
117 result.header = polygon.header;
119 for (
size_t i = 0; i < polygon.polygon.points.size(); i++) {
120 Eigen::Vector4d
point;
121 point[0] = polygon.polygon.points[i].
x;
122 point[1] = polygon.polygon.points[i].y;
123 point[2] = polygon.polygon.points[i].z;
125 Eigen::Vector4d transformed_point_eigen = transform.inverse() * point;
126 geometry_msgs::Point32 transformed_point;
127 transformed_point.x = transformed_point_eigen[0];
128 transformed_point.y = transformed_point_eigen[1];
129 transformed_point.z = transformed_point_eigen[2];
130 result.polygon.points.push_back(transformed_point);
135 const jsk_recognition_msgs::ModelCoefficientsArray::ConstPtr& coefficients)
137 if (polygons->polygons.size() != coefficients->coefficients.size()) {
138 NODELET_ERROR(
"the size of polygons(%lu) does not match with the size of coefficients(%lu)",
139 polygons->polygons.size(),
140 coefficients->coefficients.size());
144 jsk_recognition_msgs::PolygonArray transformed_polygon_array;
145 jsk_recognition_msgs::ModelCoefficientsArray transformed_model_coefficients_array;
146 transformed_polygon_array.header = polygons->header;
147 transformed_model_coefficients_array.header = coefficients->header;
148 transformed_polygon_array.header.frame_id =
frame_id_;
149 transformed_model_coefficients_array.header.frame_id =
frame_id_;
150 for (
size_t i = 0; i < polygons->polygons.size(); i++) {
151 geometry_msgs::PolygonStamped polygon = polygons->polygons[i];
154 if (polygon.header.frame_id != coefficient.header.frame_id) {
155 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",
156 i, polygon.header.frame_id.c_str(),
157 i, coefficient.header.frame_id.c_str());
162 coefficient.header.stamp,
166 coefficient.header.stamp)) {
170 coefficient.header.stamp, transform);
171 Eigen::Affine3d eigen_transform;
176 geometry_msgs::PolygonStamped transformed_polygon;
177 if (polygon.polygon.points.size() == 0) {
178 transformed_polygon.header = polygon.header;
179 transformed_polygon.header.frame_id =
frame_id_;
180 transformed_polygon_array.polygons.push_back(transformed_polygon);
181 transformed_coefficient.values = coefficient.values;
182 transformed_coefficient.header = polygon.header;
183 transformed_coefficient.header.frame_id =
frame_id_;
184 transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
190 transformed_coefficient);
191 if (isnan(transformed_coefficient.values[0]) ||
192 isnan(transformed_coefficient.values[1]) ||
193 isnan(transformed_coefficient.values[2]) ||
194 isnan(transformed_coefficient.values[3])) {
197 transformed_polygon_array.polygons.push_back(transformed_polygon);
198 transformed_model_coefficients_array.coefficients.push_back(transformed_coefficient);
203 frame_id_.c_str(), coefficient.header.frame_id.c_str(),
204 coefficient.header.stamp.toSec());
#define NODELET_ERROR(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
TFSIMD_FORCE_INLINE Vector3 cross(const Vector3 &v) const
virtual Plane transform(const Eigen::Affine3d &transform)
virtual void toCoefficients(std::vector< float > &output)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static tf::TransformListener * getInstance()
#define NODELET_FATAL(...)
#define NODELET_DEBUG(...)
pcl::ModelCoefficients PCLModelCoefficientMsg