diff --git a/kinect2_bridge/src/kinect2_bridge.cpp b/kinect2_bridge/src/kinect2_bridge.cpp index 82c1f12c..771825e3 100644 --- a/kinect2_bridge/src/kinect2_bridge.cpp +++ b/kinect2_bridge/src/kinect2_bridge.cpp @@ -239,7 +239,7 @@ class Kinect2Bridge priv_nh.param("queue_size", queueSize, 2); priv_nh.param("bilateral_filter", bilateral_filter, true); priv_nh.param("edge_aware_filter", edge_aware_filter, true); - priv_nh.param("publish_tf", publishTF, false); + priv_nh.param("publish_tf", publishTF, true); priv_nh.param("base_name_tf", baseNameTF, base_name); priv_nh.param("worker_threads", worker_threads, 4); diff --git a/kinect2_calibration/src/kinect2_calibration.cpp b/kinect2_calibration/src/kinect2_calibration.cpp index 474e01e2..28ec2a24 100644 --- a/kinect2_calibration/src/kinect2_calibration.cpp +++ b/kinect2_calibration/src/kinect2_calibration.cpp @@ -29,6 +29,7 @@ #include #include +#include "opencv2/core/version.hpp" #include #include @@ -715,9 +716,17 @@ class CameraCalibration OUT_INFO("Distortion Coeeficients Ir:" << std::endl << distortionIr << std::endl); OUT_INFO("calibrating Color and Ir extrinsics..."); + +#if CV_VERSION_EPOCH == 2 || (!defined CV_VERSION_EPOCH && CV_VERSION_MAJOR == 2) error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor, rotation, translation, essential, fundamental, termCriteria, cv::CALIB_FIX_INTRINSIC); +#elif CV_VERSION_MAJOR == 3 + error = cv::stereoCalibrate(pointsBoard, pointsIr, pointsColor, cameraMatrixIr, distortionIr, cameraMatrixColor, distortionColor, sizeColor, + rotation, translation, essential, fundamental, + cv::CALIB_FIX_INTRINSIC,termCriteria); +#endif + OUT_INFO("re-projection error: " << error << std::endl); OUT_INFO("Rotation:" << std::endl << rotation); @@ -1047,7 +1056,12 @@ class DepthCalibration { cv::Mat rvec, rotation, translation; //cv::solvePnP(board, points[index], cameraMatrix, distortion, rvec, translation, false, cv::EPNP); + +#if CV_VERSION_EPOCH == 2 || (!defined CV_VERSION_EPOCH && CV_VERSION_MAJOR == 2) cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, board.size(), cv::noArray(), cv::ITERATIVE); +#elif CV_VERSION_MAJOR == 3 + cv::solvePnPRansac(board, points[index], cameraMatrix, distortion, rvec, translation, false, 300, 0.05, board.size(), cv::noArray(), cv::SOLVEPNP_ITERATIVE); +#endif cv::Rodrigues(rvec, rotation); normal = cv::Mat(3, 1, CV_64F); diff --git a/kinect2_registration/CMakeLists.txt b/kinect2_registration/CMakeLists.txt index ad5b9333..303cbfb3 100644 --- a/kinect2_registration/CMakeLists.txt +++ b/kinect2_registration/CMakeLists.txt @@ -27,6 +27,13 @@ find_package(cmake_modules QUIET) ## System dependencies are found with CMake's conventions find_package(OpenCV REQUIRED) +if(OpenCV_FOUND) + message(STATUS "OpenCV version: ${OpenCV_VERSION}") + if(NOT OpenCV_VERSION VERSION_LESS "3.0.0") + add_definitions(-DHAVE_OPENCV3) + message(STATUS "defined HAVE_OPENCV3") + endif() +endif() find_package(OpenMP) find_package(Eigen) find_package(OpenCL) diff --git a/kinect2_viewer/README.md b/kinect2_viewer/README.md index 13f73e10..fb0c1034 100644 --- a/kinect2_viewer/README.md +++ b/kinect2_viewer/README.md @@ -10,6 +10,9 @@ This is a simple viewer for the combined color an depth image provided by Kinect It just listens to two ROS topics and displays a the color with the overlayed colored depth image or a registered point cloud. +Update: +It can disable the visualization and just publish pointcloud2 now with publish_cloud option with changes in lock and unlock to assert that the color corresponded to the depth image in the same timeframe. Change pcl RGBA point cloud to RGB + ## Dependencies - ROS Hydro/Indigo @@ -24,8 +27,9 @@ It just listens to two ROS topics and displays a the color with the overlayed co kinect2_viewer [options] name: 'any string' equals to the kinect2_bridge topic base name mode: 'qhd', 'hd', 'sd' or 'ir' - visualization: 'image', 'cloud' or 'both' + visualization: 'none', 'image', 'cloud' or 'both' options: + 'publish_cloud' publish PointXYZRGBA cloud topic 'compressed' use compressed instead of raw topics 'approx' use approximate time synchronization ``` diff --git a/kinect2_viewer/src/viewer.cpp b/kinect2_viewer/src/viewer.cpp index e6cef92d..cbd7b9dc 100644 --- a/kinect2_viewer/src/viewer.cpp +++ b/kinect2_viewer/src/viewer.cpp @@ -38,6 +38,9 @@ #include #include +#include +#include + #include #include @@ -55,7 +58,8 @@ class Receiver public: enum Mode { - IMAGE = 0, + NONE = 0, + IMAGE, CLOUD, BOTH }; @@ -63,12 +67,13 @@ class Receiver private: std::mutex lock; - const std::string topicColor, topicDepth; + const std::string topicColor, topicDepth, topicPointCloud2; const bool useExact, useCompressed; - bool updateImage, updateCloud; + volatile bool updatedImageReadyForCloud, updatedImageReadyForViewer, updatedCloudReady; + bool publishCloud; bool save; - bool running; + volatile bool running; size_t frame; const size_t queueSize; @@ -84,22 +89,25 @@ class Receiver image_transport::ImageTransport it; image_transport::SubscriberFilter *subImageColor, *subImageDepth; message_filters::Subscriber *subCameraInfoColor, *subCameraInfoDepth; + ros::Publisher pointCloud2Publisher; message_filters::Synchronizer *syncExact; message_filters::Synchronizer *syncApproximate; - std::thread imageViewerThread; + std::thread imageViewerThread, cloudUpdaterThread; Mode mode; - pcl::PointCloud::Ptr cloud; + pcl::PointCloud::Ptr cloud; pcl::PCDWriter writer; std::ostringstream oss; std::vector params; public: - Receiver(const std::string &topicColor, const std::string &topicDepth, const bool useExact, const bool useCompressed) - : topicColor(topicColor), topicDepth(topicDepth), useExact(useExact), useCompressed(useCompressed), - updateImage(false), updateCloud(false), save(false), running(false), frame(0), queueSize(5), + Receiver(const std::string &topicColor, const std::string &topicDepth, const std::string &topicPointCloud2, + const bool useExact, const bool useCompressed, const bool publishCloud_): + topicColor(topicColor), topicDepth(topicDepth), topicPointCloud2(topicPointCloud2), useExact(useExact), useCompressed(useCompressed), + updatedImageReadyForCloud(false), updatedImageReadyForViewer(false), updatedCloudReady(false), publishCloud(publishCloud_), + save(false), running(false), frame(0), queueSize(5), nh("~"), spinner(0), it(nh), mode(CLOUD) { cameraMatrixColor = cv::Mat::zeros(3, 3, CV_64F); @@ -131,6 +139,8 @@ class Receiver std::string topicCameraInfoColor = topicColor.substr(0, topicColor.rfind('/')) + "/camera_info"; std::string topicCameraInfoDepth = topicDepth.substr(0, topicDepth.rfind('/')) + "/camera_info"; + // std::string topicCameraInfoCloud = topicPointCloud2.substr(0, topicPointCloud2.rfind('/')) + "/camera_info"; + pointCloud2Publisher = nh.advertise(topicPointCloud2,1000); image_transport::TransportHints hints(useCompressed ? "compressed" : "raw"); subImageColor = new image_transport::SubscriberFilter(it, topicColor, queueSize, hints); @@ -148,38 +158,44 @@ class Receiver syncApproximate = new message_filters::Synchronizer(ApproximateSyncPolicy(queueSize), *subImageColor, *subImageDepth, *subCameraInfoColor, *subCameraInfoDepth); syncApproximate->registerCallback(boost::bind(&Receiver::callback, this, _1, _2, _3, _4)); } - spinner.start(); - std::chrono::milliseconds duration(1); - while(!updateImage || !updateCloud) + ros::Rate rate(30); + while(!updatedImageReadyForCloud || !updatedImageReadyForViewer) { if(!ros::ok()) { return; + } + else + { + rate.sleep(); + ros::spinOnce(); } - std::this_thread::sleep_for(duration); } - cloud = pcl::PointCloud::Ptr(new pcl::PointCloud()); - cloud->height = color.rows; - cloud->width = color.cols; - cloud->is_dense = false; - cloud->points.resize(cloud->height * cloud->width); - createLookup(this->color.cols, this->color.rows); + cloud = createCloud(this->depth); + createLookup(this->color.cols, this->color.rows,cameraMatrixColor,lookupX,lookupY); + switch(mode) { case CLOUD: + cloudUpdaterThread = std::thread(&Receiver::cloudUpdater,this); cloudViewer(); break; case IMAGE: + cloudUpdaterThread = std::thread(&Receiver::cloudUpdater,this); imageViewer(); break; case BOTH: + cloudUpdaterThread = std::thread(&Receiver::cloudUpdater,this); imageViewerThread = std::thread(&Receiver::imageViewer, this); cloudViewer(); break; + default: // NONE + cloudUpdater(); } + } void stop() @@ -204,6 +220,11 @@ class Receiver if(mode == BOTH) { imageViewerThread.join(); + cloudUpdaterThread.join(); + } + else if( mode == IMAGE || mode == CLOUD) + { + cloudUpdaterThread.join(); } } @@ -228,11 +249,61 @@ class Receiver lock.lock(); this->color = color; this->depth = depth; - updateImage = true; - updateCloud = true; + updatedImageReadyForCloud = true; + updatedImageReadyForViewer = true; lock.unlock(); } + // updates and publishes point cloud + void cloudUpdater() + { + cv::Mat localcolor, localdepth, localLookupX, localLookupY; + + lock.lock(); + pcl::PointCloud::Ptr localcloud = createCloud(this->depth); + localLookupX=this->lookupX; + localLookupY=this->lookupY; + lock.unlock(); + + for(; running && ros::ok();) + { + if(updatedImageReadyForCloud) + { + lock.lock(); + //printf("ready\n"); + localcolor = this->color; + + localdepth = this->depth; + updatedImageReadyForCloud=false; + lock.unlock(); + + updateCloud(localdepth, localcolor, localLookupX, localLookupY, localcloud); + + lock.lock(); + /// @todo this can be sped up with better thread safety techniques + pcl::copyPointCloud(*localcloud,*cloud); + updatedCloudReady=true; + lock.unlock(); + + + if(publishCloud) + { + //printf("Publish cloud\n"); + sensor_msgs::PointCloud2 output_msg; + toROSMsg(*cloud,output_msg); + + output_msg.header.frame_id = "kinect2_ir_optical_frame"; + pointCloud2Publisher.publish(output_msg); + + ros::spinOnce(); + } + + } + + } + + } + void imageViewer() { cv::Mat color, depth, depthDisp, combined; @@ -252,12 +323,12 @@ class Receiver start = std::chrono::high_resolution_clock::now(); for(; running && ros::ok();) { - if(updateImage) + if(updatedImageReadyForViewer) { lock.lock(); color = this->color; depth = this->depth; - updateImage = false; + updatedImageReadyForViewer = false; lock.unlock(); ++frameCount; @@ -291,7 +362,6 @@ class Receiver case 's': if(mode == IMAGE) { - createCloud(depth, color, cloud); saveCloudAndImages(cloud, color, depth, depthDisp); } else @@ -308,18 +378,21 @@ class Receiver void cloudViewer() { cv::Mat color, depth; + pcl::visualization::PCLVisualizer::Ptr visualizer(new pcl::visualization::PCLVisualizer("Cloud Viewer")); + pcl::PointCloud::Ptr localcloud = createCloud(this->depth); const std::string cloudName = "rendered"; + while(!updatedCloudReady){ + if(!ros::ok() || !running) return; // shouldn't happen unless program is closing + } lock.lock(); color = this->color; depth = this->depth; - updateCloud = false; + pcl::copyPointCloud(*cloud,*localcloud); lock.unlock(); - createCloud(depth, color, cloud); - - visualizer->addPointCloud(cloud, cloudName); + visualizer->addPointCloud(localcloud, cloudName); visualizer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, cloudName); visualizer->initCameraParameters(); visualizer->setBackgroundColor(0, 0, 0); @@ -331,25 +404,29 @@ class Receiver for(; running && ros::ok();) { - if(updateCloud) + if(updatedCloudReady) { - lock.lock(); - color = this->color; - depth = this->depth; - updateCloud = false; - lock.unlock(); - - createCloud(depth, color, cloud); + lock.lock(); + pcl::copyPointCloud(*cloud,*localcloud); + lock.unlock(); - visualizer->updatePointCloud(cloud, cloudName); + if(mode == CLOUD || mode == BOTH) + { + visualizer->updatePointCloud(localcloud, cloudName); + } } if(save) { + lock.lock(); + color = this->color; + depth = this->depth; + lock.unlock(); save = false; cv::Mat depthDisp; dispDepth(depth, depthDisp, 12000.0f); - saveCloudAndImages(cloud, color, depth, depthDisp); + saveCloudAndImages(localcloud, color, depth, depthDisp); } + visualizer->spinOnce(10); } visualizer->close(); @@ -430,19 +507,35 @@ class Receiver } } - void createCloud(const cv::Mat &depth, const cv::Mat &color, pcl::PointCloud::Ptr &cloud) const + pcl::PointCloud::Ptr createCloud(const cv::Mat &depth) + { + + cloud = pcl::PointCloud::Ptr(new pcl::PointCloud(depth.rows,depth.cols)); + cloud->is_dense = false; + return cloud; + } + + static void updateCloud(const cv::Mat &depth, const cv::Mat &color, const cv::Mat& lookupX, const cv::Mat& lookupY, pcl::PointCloud::Ptr &cloud) { const float badPoint = std::numeric_limits::quiet_NaN(); + bool is_dense = true; + //std::cout << __LINE__ << "\n"; #pragma omp parallel for for(int r = 0; r < depth.rows; ++r) { - pcl::PointXYZRGBA *itP = &cloud->points[r * depth.cols]; + //std::cout << __LINE__ << "\n"; + //std::cout << (unsigned int)cloud << std::endl; + pcl::PointXYZRGB *itP = &cloud->points[r * depth.cols]; + + //std::cout << __LINE__ << "\n"; const uint16_t *itD = depth.ptr(r); const cv::Vec3b *itC = color.ptr(r); const float y = lookupY.at(0, r); const float *itX = lookupX.ptr(); + + //std::cout << __LINE__ << "\n"; for(size_t c = 0; c < (size_t)depth.cols; ++c, ++itP, ++itD, ++itC, ++itX) { register const float depthValue = *itD / 1000.0f; @@ -452,6 +545,7 @@ class Receiver // not valid itP->x = itP->y = itP->z = badPoint; itP->rgba = 0; + is_dense = false; continue; } itP->z = depthValue; @@ -460,12 +554,14 @@ class Receiver itP->b = itC->val[0]; itP->g = itC->val[1]; itP->r = itC->val[2]; - itP->a = 255; + //itP->a = 1; } } + + cloud->is_dense = is_dense; } - void saveCloudAndImages(const pcl::PointCloud::ConstPtr cloud, const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored) + void saveCloudAndImages(const pcl::PointCloud::ConstPtr cloud, const cv::Mat &color, const cv::Mat &depth, const cv::Mat &depthColored) { oss.str(""); oss << "./" << std::setfill('0') << std::setw(4) << frame; @@ -487,7 +583,7 @@ class Receiver ++frame; } - void createLookup(size_t width, size_t height) + static void createLookup(const size_t width,const size_t height, const cv::Mat& cameraMatrixColor, cv::Mat& lookupX, cv::Mat& lookupY) { const float fx = 1.0f / cameraMatrixColor.at(0, 0); const float fy = 1.0f / cameraMatrixColor.at(1, 1); @@ -516,8 +612,9 @@ void help(const std::string &path) std::cout << path << FG_BLUE " [options]" << std::endl << FG_GREEN " name" NO_COLOR ": " FG_YELLOW "'any string'" NO_COLOR " equals to the kinect2_bridge topic base name" << std::endl << FG_GREEN " mode" NO_COLOR ": " FG_YELLOW "'qhd'" NO_COLOR ", " FG_YELLOW "'hd'" NO_COLOR ", " FG_YELLOW "'sd'" NO_COLOR " or " FG_YELLOW "'ir'" << std::endl - << FG_GREEN " visualization" NO_COLOR ": " FG_YELLOW "'image'" NO_COLOR ", " FG_YELLOW "'cloud'" NO_COLOR " or " FG_YELLOW "'both'" << std::endl + << FG_GREEN " visualization" NO_COLOR ": " FG_YELLOW "'none'" NO_COLOR ", " FG_YELLOW "'image'" NO_COLOR ", " FG_YELLOW "'cloud'" NO_COLOR " or " FG_YELLOW "'both'" << std::endl << FG_GREEN " options" NO_COLOR ":" << std::endl + << FG_YELLOW " 'publish_cloud'" NO_COLOR " publish PointXYZRGB cloud topic" << std::endl << FG_YELLOW " 'compressed'" NO_COLOR " use compressed instead of raw topics" << std::endl << FG_YELLOW " 'approx'" NO_COLOR " use approximate time synchronization" << std::endl; } @@ -543,8 +640,10 @@ int main(int argc, char **argv) std::string ns = K2_DEFAULT_NS; std::string topicColor = K2_TOPIC_QHD K2_TOPIC_IMAGE_COLOR K2_TOPIC_IMAGE_RECT; std::string topicDepth = K2_TOPIC_QHD K2_TOPIC_IMAGE_DEPTH K2_TOPIC_IMAGE_RECT; - bool useExact = true; + + bool useExact = false; bool useCompressed = false; + bool publishCloud = false; Receiver::Mode mode = Receiver::CLOUD; for(size_t i = 1; i < (size_t)argc; ++i) @@ -586,6 +685,11 @@ int main(int argc, char **argv) { useCompressed = true; } + + else if(param == "publish_cloud") + { + publishCloud = true; + } else if(param == "image") { mode = Receiver::IMAGE; @@ -598,6 +702,10 @@ int main(int argc, char **argv) { mode = Receiver::BOTH; } + else if(param == "none") + { + mode = Receiver::NONE; + } else { ns = param; @@ -606,10 +714,13 @@ int main(int argc, char **argv) topicColor = "/" + ns + topicColor; topicDepth = "/" + ns + topicDepth; + std::string topicPointCloud2 = "/" + ns + "/depth_registered/points"; + OUT_INFO("topic color: " FG_CYAN << topicColor << NO_COLOR); OUT_INFO("topic depth: " FG_CYAN << topicDepth << NO_COLOR); + OUT_INFO("topic cloud: " FG_CYAN << topicPointCloud2 << NO_COLOR); - Receiver receiver(topicColor, topicDepth, useExact, useCompressed); + Receiver receiver(topicColor, topicDepth, topicPointCloud2, useExact, useCompressed,publishCloud); OUT_INFO("starting receiver..."); receiver.run(mode); @@ -617,3 +728,4 @@ int main(int argc, char **argv) ros::shutdown(); return 0; } +