35 #define BOOST_PARAMETER_MAX_ARITY 7 42 #include <visualization_msgs/Marker.h> 44 #include <pcl/filters/extract_indices.h> 49 DiagnosticNodelet::onInit();
58 "output/direction", 1);
61 pub_roi_ =
pnh_->advertise<jsk_recognition_msgs::PosedCameraInfo>(
104 const geometry_msgs::PolygonStamped::ConstPtr& rect)
107 int x0 = rect->polygon.points[0].x;
108 int x1 = rect->polygon.points[2].x;
109 int y0 = rect->polygon.points[0].y;
110 int y1 = rect->polygon.points[2].y;
119 int x0_index = x0 / width;
120 int x1_index = x1 / width;
121 if (x0_index != x1_index) {
130 int image_index = x0_index;
134 int width_offset = width * image_index;
135 int x0_wo_offset = x0 - width_offset;
136 int x1_wo_offset = x1 - width_offset;
137 cv::Point2d mid((x0_wo_offset + x1_wo_offset) / 2.0,
139 Eigen::Affine3d
pose(info->camera_pose_);
142 Eigen::Vector3d ray(mid_3d.x, mid_3d.y, mid_3d.z);
143 ray = ray / ray.norm();
144 Eigen::Vector3d ray_global = pose.rotation() * ray;
145 NODELET_INFO(
"ray: [%f, %f, %f]", ray_global[0], ray_global[1], ray_global[2]);
147 Eigen::Vector3d
z = pose.rotation() * Eigen::Vector3d::UnitZ();
149 Eigen::Vector3d original_pos = pose.translation();
150 Eigen::Quaterniond
q;
151 q.setFromTwoVectors(z, ray_global);
152 NODELET_INFO(
"q: [%f, %f, %f, %f]", q.x(), q.y(), q.z(), q.w());
153 Eigen::Affine3d output_pose = pose.rotate(q);
154 output_pose.translation() = original_pos;
155 geometry_msgs::PoseStamped ros_pose;
162 jsk_recognition_msgs::PosedCameraInfo camera_info;
165 camera_info.camera_info
166 = sensor_msgs::CameraInfo(info->camera_.cameraInfo());
167 camera_info.camera_info.roi.x_offset = x0_wo_offset;
168 camera_info.camera_info.roi.y_offset = y0;
169 camera_info.camera_info.roi.width = x1_wo_offset - x0_wo_offset;
170 camera_info.camera_info.roi.height = y1 - y0;
176 cv::Point2d
A(x0_wo_offset, y0);
177 cv::Point2d
B(x0_wo_offset, y1);
178 cv::Point2d
C(x1_wo_offset, y1);
179 cv::Point2d
D(x1_wo_offset, y0);
181 cv::Point3d A_3d = info->camera_.projectPixelTo3dRay(A) * 3;
182 cv::Point3d B_3d = info->camera_.projectPixelTo3dRay(B) * 3;
183 cv::Point3d C_3d = info->camera_.projectPixelTo3dRay(C) * 3;
184 cv::Point3d D_3d = info->camera_.projectPixelTo3dRay(D) * 3;
187 geometry_msgs::Point A_ros, B_ros, C_ros, D_ros, O_ros;
188 jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(A_3d, A_ros);
189 jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(B_3d, B_ros);
190 jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(C_3d, C_ros);
191 jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(D_3d, D_ros);
192 jsk_recognition_utils::pointFromXYZToXYZ<cv::Point3d, geometry_msgs::Point>(O_3d, O_ros);
194 visualization_msgs::Marker
marker;
198 marker.type = visualization_msgs::Marker::LINE_LIST;
199 marker.points.push_back(O_ros); marker.points.push_back(A_ros);
200 marker.points.push_back(O_ros); marker.points.push_back(B_ros);
201 marker.points.push_back(O_ros); marker.points.push_back(C_ros);
202 marker.points.push_back(O_ros); marker.points.push_back(D_ros);
203 marker.points.push_back(A_ros); marker.points.push_back(B_ros);
204 marker.points.push_back(B_ros); marker.points.push_back(C_ros);
205 marker.points.push_back(C_ros); marker.points.push_back(D_ros);
206 marker.points.push_back(D_ros); marker.points.push_back(A_ros);
207 marker.scale.x = 0.01;
208 marker.scale.y = 0.01;
209 marker.scale.z = 0.01;
210 marker.color.a = 1.0;
211 marker.color.r = 1.0;
217 A_3d, B_3d, C_3d, D_3d,
225 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud,
226 const cv::Point3d&
A,
const cv::Point3d& B,
227 const cv::Point3d& C,
const cv::Point3d& D,
228 const Eigen::Affine3d& pose)
230 Eigen::Vector3f A_eigen, B_eigen, C_eigen, D_eigen;
231 pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
233 pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
235 pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
237 pointFromXYZToVector<cv::Point3d, Eigen::Vector3f>(
239 Eigen::Affine3f posef;
241 Eigen::Vector3f A_global = posef * A_eigen;
242 Eigen::Vector3f B_global = posef * B_eigen;
243 Eigen::Vector3f C_global = posef * C_eigen;
244 Eigen::Vector3f D_global = posef * D_eigen;
245 Eigen::Vector3f O_global = posef.translation();
247 vertices0.push_back(O_global); vertices0.push_back(A_global); vertices0.push_back(D_global);
248 vertices1.push_back(O_global); vertices1.push_back(B_global); vertices1.push_back(A_global);
249 vertices2.push_back(O_global); vertices2.push_back(C_global); vertices2.push_back(B_global);
250 vertices3.push_back(O_global); vertices3.push_back(D_global); vertices3.push_back(C_global);
255 pcl::PointIndices::Ptr indices(
new pcl::PointIndices);
256 for (
size_t i = 0; i < cloud->points.size(); i++) {
257 pcl::PointXYZRGB
p = cloud->points[i];
258 Eigen::Vector3f pf = p.getVector3fMap();
259 if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
260 if (plane0->signedDistanceToPoint(pf) > 0 &&
261 plane1->signedDistanceToPoint(pf) > 0 &&
262 plane2->signedDistanceToPoint(pf) > 0 &&
263 plane3->signedDistanceToPoint(pf) > 0) {
264 indices->indices.push_back(i);
268 pcl::ExtractIndices<pcl::PointXYZRGB>
ex;
269 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
270 ex.setInputCloud(cloud);
272 ex.setIndices(indices);
273 ex.filter(*output_cloud);
274 sensor_msgs::PointCloud2 ros_cloud;
282 const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
289 const sensor_msgs::Image::ConstPtr& image_msg,
290 const sensor_msgs::CameraInfo::ConstPtr& info_msg)
300 cv::Mat concatenated_image;
301 std::vector<cv::Mat> images;
306 cv::hconcat(images, concatenated_image);
333 std_srvs::Empty::Request&
req,
334 std_srvs::Empty::Response&
res)
358 Eigen::Affine3d eigen_transform;
362 info->camera_pose_ = eigen_transform;
363 info->camera_ = camera;
364 info->image_ = cv_ptr->image;
372 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
373 nontransformed_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
374 pcl::PointCloud<pcl::PointXYZRGB>::Ptr
375 transformed_cloud (
new pcl::PointCloud<pcl::PointXYZRGB>);
378 *nontransformed_cloud,
381 info->cloud_ = transformed_cloud;
400 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
405 NODELET_ERROR(
"[%s] Transform error: %s", __PRETTY_FUNCTION__, e.what());
417 std_srvs::Empty::Request&
req,
418 std_srvs::Empty::Response&
res)
426 std_srvs::Empty::Request&
req,
427 std_srvs::Empty::Response&
res)
436 cv::Mat concatenated_image;
437 std::vector<cv::Mat> images;
442 cv::hconcat(images, concatenated_image);
virtual void unsubscribe()
virtual void cloudCallback(const sensor_msgs::PointCloud2::ConstPtr &cloud_msg)
virtual void rectCallback(const geometry_msgs::PolygonStamped::ConstPtr &rect)
#define NODELET_ERROR(...)
#define NODELET_WARN(...)
void publish(const boost::shared_ptr< M > &message) const
void transformTFToEigen(const tf::Transform &t, Eigen::Affine3d &e)
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
ros::Publisher pub_marker_
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
ros::Subscriber rect_sub_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
boost::circular_buffer< SnapshotInformation::Ptr > snapshot_buffer_
ros::ServiceServer clear_service_
ros::ServiceServer shutter_service_
ros::Subscriber cloud_sub_
sensor_msgs::PointCloud2::ConstPtr latest_cloud_msg_
ros::Time last_publish_time_
void publish(const sensor_msgs::Image &message) const
std::string fixed_frame_id_
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::IntermittentImageAnnotator, nodelet::Nodelet)
void convertEigenAffine3(const Eigen::Affine3d &from, Eigen::Affine3f &to)
image_transport::CameraSubscriber image_sub_
void toROSMsg(const pcl::PointCloud< T > &pcl_cloud, sensor_msgs::PointCloud2 &cloud)
tf::TransformListener * listener_
std::vector< Eigen::Vector3f, Eigen::aligned_allocator< Eigen::Vector3f > > Vertices
virtual bool shutterCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
#define NODELET_INFO(...)
virtual void cameraCallback(const sensor_msgs::Image::ConstPtr &image_msg, const sensor_msgs::CameraInfo::ConstPtr &info_msg)
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const tf::Transform &transform)
ros::Publisher pub_cloud_
virtual void publishCroppedPointCloud(pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud, const cv::Point3d &A, const cv::Point3d &B, const cv::Point3d &C, const cv::Point3d &D, const Eigen::Affine3d &pose)
virtual void waitForNextImage()
static tf::TransformListener * getInstance()
ros::ServiceServer request_service_
sensor_msgs::Image::ConstPtr latest_image_msg_
sensor_msgs::CameraInfo::ConstPtr latest_camera_info_msg_
image_transport::Publisher image_pub_
virtual bool requestCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
sensor_msgs::ImagePtr toImageMsg() const
virtual bool clearCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)