reverse3D

Untitled

May 1st, 2018
150
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // ROS includes
  2. #include <ros/ros.h>
  3. #include <ros/console.h>
  4. #include <std_msgs/String.h>
  5. #include "image_geometry/pinhole_camera_model.h"
  6. #include <sensor_msgs/image_encodings.h>
  7. #include <image_transport/image_transport.h>
  8. #include <cv_bridge/cv_bridge.h>
  9. #include <tf/transform_listener.h>
  10. #include <boost/foreach.hpp>
  11. #include <message_filters/subscriber.h>
  12. #include <message_filters/time_synchronizer.h>
  13. #include <sensor_msgs/Image.h>
  14. #include <sensor_msgs/CameraInfo.h>
  15.  
  16.  
  17. // OpenCV includes
  18. #include "opencv2/core/core.hpp"
  19. #include "opencv2/imgproc/imgproc.hpp"
  20. #include "opencv2/calib3d/calib3d.hpp"
  21. #include "opencv2/highgui/highgui.hpp"
  22.  
  23. // Include pcl
  24. #include <pcl_conversions/pcl_conversions.h>
  25. #include <pcl/point_cloud.h>
  26. #include <pcl/point_types.h>
  27. #include <pcl/filters/voxel_grid.h>
  28. #include <pcl_ros/point_cloud.h>
  29. #include <pcl/filters/passthrough.h>
  30.  
  31.  
  32.  
  33. // Include PointCloud2 ROS message
  34. #include <sensor_msgs/PointCloud2.h>
  35. #include <pcl/io/io.h>
  36. #include <pcl/point_types.h>
  37. #include "pcl_ros/transforms.h"
  38. #include "pcl_ros/impl/transforms.hpp"
  39.  
  40. #include <iostream>
  41. #include <string>
  42.  
  43. #define IMAGE_WIDTH 964
  44. #define IMAGE_HEIGHT 724
  45.  
  46. // Topics
  47. static const std::string LIDAR_TOPIC = "/sensors/velodyne_points";
  48. static const std::string IMG_TOPIC = "/sensors/camera/image_color";
  49. static const std::string CAMERA_INFO = "/sensors/camera/camera_info";
  50. static const std::string COMPOSITE_IMG_OUT = "/sensors/camera/lidar_image"; // lidar points on camera image
  51.  
  52. static const std::string OPENCV_WINDOW = "Image window";
  53. const tf::TransformListener tf_listener_;
  54. image_geometry::PinholeCameraModel cam_model_;
  55. tf::StampedTransform transform;
  56. std::vector <tf::Vector3> objectPoints;
  57. tf::Vector3 pt_cv;
  58. std::vector <cv::Point3d> pt_transformed;
  59.  
  60. void processCallback(const sensor_msgs::ImageConstPtr& image_msg,
  61. const sensor_msgs::CameraInfoConstPtr& info_msg,
  62. const sensor_msgs::PointCloud2ConstPtr& pointCloudMsg) {
  63.  
  64. // ROS_INFO_STREAM("Image: " << image_msg->header.stamp << " Camera: " << info_msg->header.stamp);
  65.  
  66. ros::Time cloudHeader = pointCloudMsg->header.stamp;
  67. //ROS_INFO_STREAM("LIDAR: " << cloudHeader);
  68.  
  69. pcl::PointCloud<pcl::PointXYZI>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZI>);
  70. pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZI>);
  71.  
  72. pcl::fromROSMsg (*pointCloudMsg, *cloud);
  73.  
  74. // Create the filtering object
  75. pcl::PassThrough<pcl::PointXYZI> pass_x;
  76. pass_x.setInputCloud (cloud);
  77. pass_x.setFilterFieldName ("x");
  78. pass_x.setFilterLimits (0.0, 4.5); // 0 to 4.5 mts limitation
  79. pass_x.filter (*cloud_filtered);
  80.  
  81. objectPoints.clear();
  82. for(size_t i=0; i < cloud_filtered->size(); ++i) {
  83. objectPoints.push_back(tf::Vector3(cloud_filtered->points[i].x,
  84. cloud_filtered->points[i].y,
  85. cloud_filtered->points[i].z));
  86. //ROS_INFO_STREAM("X: " << objectPoints[i].x() << " Y: "<< objectPoints[i].y() << " Z: "<< objectPoints[i].z());
  87.  
  88. }
  89.  
  90.  
  91. cv_bridge::CvImagePtr cv_ptr (new cv_bridge::CvImage);
  92.  
  93. try
  94. {
  95. cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
  96. }
  97. catch (cv_bridge::Exception& e)
  98. {
  99. ROS_ERROR("cv_bridge exception: %s", e.what());
  100. return;
  101. }
  102.  
  103. cam_model_.fromCameraInfo(info_msg);
  104.  
  105. /* try
  106. {
  107. tf_listener_.lookupTransform("/camera_optical", "/velodyne", ros::Time(0), transform);
  108. }
  109. catch (tf::TransformException& ex) {
  110. ROS_ERROR("%s", ex.what());
  111. ros::Duration(1.0).sleep();
  112. //continue;
  113. }
  114. */
  115. try {
  116. ros::Time acquisition_time = cloudHeader;
  117. ros::Duration timeout(1.0 / 30);
  118. tf_listener_.waitForTransform("/camera_optical", "/velodyne",
  119. acquisition_time, timeout);
  120. tf_listener_.lookupTransform("/camera_optical", "/velodyne",
  121. acquisition_time, transform);
  122. }
  123. catch (tf::TransformException& ex) {
  124. ROS_ERROR("%s", ex.what());
  125. ros::Duration(1.0).sleep();
  126. }
  127.  
  128.  
  129. // tranform the xyz point from pointcoud
  130. for(size_t i = 0; i < objectPoints.size(); ++i) {
  131.  
  132. pt_cv = transform(objectPoints[i]);
  133.  
  134. pt_transformed.push_back(cv::Point3d(pt_cv.x(), pt_cv.y(), pt_cv.z()));
  135. cv::Point2d uv;
  136. uv = cam_model_.project3dToPixel(pt_transformed[i]);
  137.  
  138. if(uv.x >= 0 && uv.x <= IMAGE_WIDTH && uv.y >= 0 && uv.y <= IMAGE_HEIGHT)
  139. {
  140. static const int RADIUS = 3;
  141. cv::circle(cv_ptr->image, uv, RADIUS, CV_RGB(255,0,0));
  142. }
  143.  
  144. }
  145. cv::imshow(OPENCV_WINDOW,cv_ptr->image);
  146. cv::waitKey(1);
  147.  
  148.  
  149. }
  150.  
  151. int main(int argc, char** argv) {
  152.  
  153. ros::init(argc, argv, "lidar_calibration");
  154.  
  155. ros::NodeHandle nh;
  156. // ROS_INFO("Starting LiDAR node");
  157.  
  158. message_filters::Subscriber<sensor_msgs::Image> image_sub(nh, IMG_TOPIC, 1);
  159. message_filters::Subscriber<sensor_msgs::CameraInfo> info_sub(nh, CAMERA_INFO, 1);
  160. message_filters::Subscriber<sensor_msgs::PointCloud2> lidar_sub(nh, LIDAR_TOPIC, 1);
  161. message_filters::TimeSynchronizer<sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> sync(image_sub, info_sub, lidar_sub, 5);
  162. sync.registerCallback(boost::bind(&processCallback, _1, _2, _3));
  163.  
  164. ros::spin();
  165. // cv::destroyWindow(OPENCV_WINDOW);
  166.  
  167. return 0;
  168.  
  169. }
RAW Paste Data