From 7b4f73d7d6e84f36ae16215ef8bb86f54eab071c Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 15:48:27 +0200 Subject: [PATCH 1/6] Removed OpenCV utilization for datatype conversion --- .../nodelet/include/zed_wrapper_nodelet.hpp | 10 +- .../src/nodelet/src/zed_wrapper_nodelet.cpp | 155 ++++++++++++------ zed_wrapper/src/tools/include/sl_tools.h | 5 - zed_wrapper/src/tools/src/sl_tools.cpp | 71 +++----- 4 files changed, 128 insertions(+), 113 deletions(-) diff --git a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp index e1f987ff..89a9e000 100644 --- a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp @@ -69,7 +69,7 @@ namespace zed_wrapper { * \param frameId : the id of the reference frame of the image * \param t : the ros::Time to stamp the image */ - static sensor_msgs::ImagePtr imageToROSmsg(cv::Mat img, const std::string encodingType, std::string frameId, ros::Time t); + static sensor_msgs::ImagePtr imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t); private: /* \brief Initialization function called by the Nodelet base class @@ -122,19 +122,19 @@ namespace zed_wrapper { * \param img_frame_id : the id of the reference frame of the image (different image frames exist) * \param t : the ros::Time to stamp the image */ - void publishImage(cv::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t); + void publishImage(sl::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t); /* \brief Publish a cv::Mat depth image with a ros Publisher * \param depth : the depth image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDepth(cv::Mat depth, ros::Time t); + void publishDepth(sl::Mat depth, ros::Time t); /* \brief Publish a cv::Mat confidence image with a ros Publisher * \param conf : the confidence image to publish * \param t : the ros::Time to stamp the depth image */ - void publishConf(cv::Mat conf, ros::Time t); + void publishConf(sl::Mat conf, ros::Time t); /* \brief Publish a pointCloud with a ros Publisher * \param width : the width of the point cloud @@ -153,7 +153,7 @@ namespace zed_wrapper { * \param disparity : the disparity image to publish * \param t : the ros::Time to stamp the depth image */ - void publishDisparity(cv::Mat disparity, ros::Time t); + void publishDisparity(sl::Mat disparity, ros::Time t); /* \brief Get the information of the ZED cameras and store them in an information message * \param zed : the sl::zed::Camera* pointer to an instance diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index cf025b06..48a4af6b 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -625,35 +625,63 @@ namespace zed_wrapper { } } - sensor_msgs::ImagePtr - ZEDWrapperNodelet::imageToROSmsg(cv::Mat img, const std::string encodingType, - std::string frameId, ros::Time t) { + sensor_msgs::ImagePtr ZEDWrapperNodelet::imageToROSmsg(sl::Mat img, std::string frameId, ros::Time t) { + sensor_msgs::ImagePtr ptr = boost::make_shared(); sensor_msgs::Image& imgMessage = *ptr; + imgMessage.header.stamp = t; imgMessage.header.frame_id = frameId; - imgMessage.height = img.rows; - imgMessage.width = img.cols; - imgMessage.encoding = encodingType; + imgMessage.height = img.getHeight(); + imgMessage.width = img.getWidth(); + int num = 1; // for endianness detection imgMessage.is_bigendian = !(*(char*)&num == 1); - imgMessage.step = img.cols * img.elemSize(); - size_t size = imgMessage.step * img.rows; + + imgMessage.step = img.getStepBytes(); // TODO CHECK + + size_t size = imgMessage.step * imgMessage.height; imgMessage.data.resize(size); - if (img.isContinuous()) { - memcpy((char*)(&imgMessage.data[0]), img.data, size); - } else { - uchar* opencvData = img.data; - uchar* rosData = (uchar*)(&imgMessage.data[0]); - - #pragma omp parallel for - for (unsigned int i = 0; i < img.rows; i++) { - memcpy(rosData, opencvData, imgMessage.step); - rosData += imgMessage.step; - opencvData += img.step; - } + sl::MAT_TYPE dataType = img.getDataType(); + + switch (dataType) { + case sl::MAT_TYPE_32F_C1: /**< float 1 channel.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C2: /**< float 2 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC2; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C3: /**< float 3 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC3; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_32F_C4: /**< float 4 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC4; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C1: /**< unsigned char 1 channel.*/ + imgMessage.encoding = sensor_msgs::image_encodings::MONO8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C2: /**< unsigned char 2 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::TYPE_8UC2; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C3: /**< unsigned char 3 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::BGR8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; + case sl::MAT_TYPE_8U_C4: /**< unsigned char 4 channels.*/ + imgMessage.encoding = sensor_msgs::image_encodings::BGRA8; + memcpy((char*)(&imgMessage.data[0]), img.getPtr(), size); + break; } + + + return ptr; } @@ -861,31 +889,34 @@ namespace zed_wrapper { transformImuBroadcaster.sendTransform(transformStamped); } - void ZEDWrapperNodelet::publishImage(cv::Mat img, + void ZEDWrapperNodelet::publishImage(sl::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t) { - pubImg.publish( - imageToROSmsg(img, sensor_msgs::image_encodings::BGR8, imgFrameId, t)); + pubImg.publish(imageToROSmsg(img, imgFrameId, t)); } - void ZEDWrapperNodelet::publishDepth(cv::Mat depth, ros::Time t) { - string encoding; + void ZEDWrapperNodelet::publishDepth(sl::Mat depth, ros::Time t) { + + // TODO OPENNI CONVERSION + + /*string encoding; if (openniDepthMode) { depth *= 1000.0f; depth.convertTo(depth, CV_16UC1); // in mm, rounded encoding = sensor_msgs::image_encodings::TYPE_16UC1; } else { encoding = sensor_msgs::image_encodings::TYPE_32FC1; - } + }*/ - pubDepth.publish(imageToROSmsg(depth, encoding, depthOptFrameId, t)); + pubDepth.publish(imageToROSmsg(depth, depthOptFrameId, t)); } - void ZEDWrapperNodelet::publishDisparity(cv::Mat disparity, ros::Time t) { + void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { + sl::CameraInformation zedParam = zed.getCameraInformation(sl::Resolution(matWidth, matHeight)); - sensor_msgs::ImagePtr disparity_image = imageToROSmsg( - disparity, sensor_msgs::image_encodings::TYPE_32FC1, disparityFrameId, t); + + sensor_msgs::ImagePtr disparity_image = imageToROSmsg(disparity, disparityFrameId, t); stereo_msgs::DisparityImage msg; msg.image = *disparity_image; @@ -1393,8 +1424,7 @@ namespace zed_wrapper { } // Time update - old_t = - sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); + old_t = sl_tools::slTime2Ros(zed.getTimestamp(sl::TIME_REFERENCE_CURRENT)); if (autoExposure) { // getCameraSettings() can't check status of auto exposure @@ -1423,14 +1453,16 @@ namespace zed_wrapper { // Retrieve RGBA Left image zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT, sl::MEM_CPU, matWidth, matHeight); - cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); + + //cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); + if (left_SubNumber > 0) { publishCamInfo(leftCamInfoMsg, pubLeftCamInfo, t); - publishImage(leftImRGB, pubLeft, leftCamOptFrameId, t); + publishImage(leftZEDMat, pubLeft, leftCamOptFrameId, t); } if (rgb_SubNumber > 0) { publishCamInfo(rgbCamInfoMsg, pubRgbCamInfo, t); - publishImage(leftImRGB, pubRgb, depthOptFrameId, + publishImage(leftZEDMat, pubRgb, depthOptFrameId, t); // rgb is the left image } } @@ -1440,14 +1472,16 @@ namespace zed_wrapper { // Retrieve RGBA Left image zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED, sl::MEM_CPU, matWidth, matHeight); - cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); + + //cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); + if (left_raw_SubNumber > 0) { publishCamInfo(leftCamInfoRawMsg, pubLeftCamInfoRaw, t); - publishImage(leftImRGB, pubRawLeft, leftCamOptFrameId, t); + publishImage(leftZEDMat, pubRawLeft, leftCamOptFrameId, t); } if (rgb_raw_SubNumber > 0) { publishCamInfo(rgbCamInfoRawMsg, pubRgbCamInfoRaw, t); - publishImage(leftImRGB, pubRawRgb, depthOptFrameId, t); + publishImage(leftZEDMat, pubRawRgb, depthOptFrameId, t); } } @@ -1456,9 +1490,11 @@ namespace zed_wrapper { // Retrieve RGBA Right image zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT, sl::MEM_CPU, matWidth, matHeight); - cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); + + //cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); + publishCamInfo(rightCamInfoMsg, pubRightCamInfo, t); - publishImage(rightImRGB, pubRight, rightCamOptFrameId, t); + publishImage(rightZEDMat, pubRight, rightCamOptFrameId, t); } // Publish the right image if someone has subscribed to @@ -1466,9 +1502,11 @@ namespace zed_wrapper { // Retrieve RGBA Right image zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED, sl::MEM_CPU, matWidth, matHeight); - cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); + + //cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); + publishCamInfo(rightCamInfoRawMsg, pubRightCamInfoRaw, t); - publishImage(rightImRGB, pubRawRight, rightCamOptFrameId, t); + publishImage(rightZEDMat, pubRawRight, rightCamOptFrameId, t); } // Publish the depth image if someone has subscribed to @@ -1476,34 +1514,49 @@ namespace zed_wrapper { zed.retrieveMeasure(depthZEDMat, sl::MEASURE_DEPTH, sl::MEM_CPU, matWidth, matHeight); publishCamInfo(depthCamInfoMsg, pubDepthCamInfo, t); - publishDepth(sl_tools::toCVMat(depthZEDMat), t); // in meters + publishDepth(depthZEDMat, t); } // Publish the disparity image if someone has subscribed to if (disparity_SubNumber > 0) { zed.retrieveMeasure(disparityZEDMat, sl::MEASURE_DISPARITY, sl::MEM_CPU, matWidth, matHeight); + // Need to flip sign, but cause of this is not sure - cv::Mat disparity = sl_tools::toCVMat(disparityZEDMat) * -1.0; - publishDisparity(disparity, t); + //cv::Mat disparity = sl_tools::toCVMat(disparityZEDMat) * -1.0; + +#pragma parallel for + for (int y = 0; y < disparityZEDMat.getHeight(); y++) { +#pragma parallel for + for (int x = 0; x < disparityZEDMat.getWidth(); x++) { + sl::float1 value; + disparityZEDMat.getValue(x, y, &value); + value *= -1.0f; + disparityZEDMat.setValue(x, y, value); + } + } + + publishDisparity(disparityZEDMat, t); } // Publish the confidence image if someone has subscribed to if (conf_img_SubNumber > 0) { zed.retrieveImage(confImgZEDMat, sl::VIEW_CONFIDENCE, sl::MEM_CPU, matWidth, matHeight); - cv::cvtColor(sl_tools::toCVMat(confImgZEDMat), confImRGB, CV_RGBA2RGB); - publishImage(confImRGB, pubConfImg, confidenceOptFrameId, t); + + //cv::cvtColor(sl_tools::toCVMat(confImgZEDMat), confImRGB, CV_RGBA2RGB); + + publishImage(confImgZEDMat, pubConfImg, confidenceOptFrameId, t); } // Publish the confidence map if someone has subscribed to if (conf_map_SubNumber > 0) { zed.retrieveMeasure(confMapZEDMat, sl::MEASURE_CONFIDENCE, sl::MEM_CPU, matWidth, matHeight); - confMapFloat = sl_tools::toCVMat(confMapZEDMat); - pubConfMap.publish(imageToROSmsg( - confMapFloat, sensor_msgs::image_encodings::TYPE_32FC1, - confidenceOptFrameId, t)); + + //confMapFloat = sl_tools::toCVMat(confMapZEDMat); + + pubConfMap.publish(imageToROSmsg(confMapZEDMat, confidenceOptFrameId, t)); } // Publish the point cloud if someone has subscribed to diff --git a/zed_wrapper/src/tools/include/sl_tools.h b/zed_wrapper/src/tools/include/sl_tools.h index ba6470ce..41583d39 100644 --- a/zed_wrapper/src/tools/include/sl_tools.h +++ b/zed_wrapper/src/tools/include/sl_tools.h @@ -38,11 +38,6 @@ namespace sl_tools { */ sl::DeviceProperties getZEDFromSN(unsigned int serial_number); - /* \brief Convert an sl:Mat to a cv::Mat - * \param mat : the sl::Mat to convert - */ - cv::Mat toCVMat(sl::Mat& mat); - cv::Mat convertRodrigues(sl::float3 r); /* \brief Test if a file exist diff --git a/zed_wrapper/src/tools/src/sl_tools.cpp b/zed_wrapper/src/tools/src/sl_tools.cpp index 488511d6..59f2c89e 100644 --- a/zed_wrapper/src/tools/src/sl_tools.cpp +++ b/zed_wrapper/src/tools/src/sl_tools.cpp @@ -29,56 +29,24 @@ namespace sl_tools { int checkCameraReady(unsigned int serial_number) { int id = -1; auto f = sl::Camera::getDeviceList(); - for (auto &it : f) - if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) + for (auto& it : f) + if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { id = it.id; + } return id; } sl::DeviceProperties getZEDFromSN(unsigned int serial_number) { sl::DeviceProperties prop; auto f = sl::Camera::getDeviceList(); - for (auto &it : f) { - if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) + for (auto& it : f) { + if (it.serial_number == serial_number && it.camera_state == sl::CAMERA_STATE::CAMERA_STATE_AVAILABLE) { prop = it; + } } return prop; } - cv::Mat toCVMat(sl::Mat &mat) { - if (mat.getMemoryType() == sl::MEM_GPU) - mat.updateCPUfromGPU(); - - int cvType; - switch (mat.getDataType()) { - case sl::MAT_TYPE_32F_C1: - cvType = CV_32FC1; - break; - case sl::MAT_TYPE_32F_C2: - cvType = CV_32FC2; - break; - case sl::MAT_TYPE_32F_C3: - cvType = CV_32FC3; - break; - case sl::MAT_TYPE_32F_C4: - cvType = CV_32FC4; - break; - case sl::MAT_TYPE_8U_C1: - cvType = CV_8UC1; - break; - case sl::MAT_TYPE_8U_C2: - cvType = CV_8UC2; - break; - case sl::MAT_TYPE_8U_C3: - cvType = CV_8UC3; - break; - case sl::MAT_TYPE_8U_C4: - cvType = CV_8UC4; - break; - } - return cv::Mat((int) mat.getHeight(), (int) mat.getWidth(), cvType, mat.getPtr(sl::MEM_CPU), mat.getStepBytes(sl::MEM_CPU)); - } - cv::Mat convertRodrigues(sl::float3 r) { double theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); cv::Mat R = cv::Mat::eye(3, 3, CV_32F); @@ -118,7 +86,7 @@ namespace sl_tools { p[8] = 0; // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s*r_x; + R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x; } return R; } @@ -128,39 +96,38 @@ namespace sl_tools { return (stat(name.c_str(), &buffer) == 0); } - std::string getSDKVersion( int& major, int& minor, int& sub_minor) { + std::string getSDKVersion(int& major, int& minor, int& sub_minor) { std::string ver = sl::Camera::getSDKVersion().c_str(); std::vector strings; std::istringstream f(ver); - std::string s; + std::string s; while (getline(f, s, '.')) { strings.push_back(s); - } + } major = 0; minor = 0; sub_minor = 0; - switch( strings.size() ) - { - case 3: - sub_minor = std::stoi(strings[2]); + switch (strings.size()) { + case 3: + sub_minor = std::stoi(strings[2]); - case 2: - minor = std::stoi(strings[1]); + case 2: + minor = std::stoi(strings[1]); - case 1: - major = std::stoi(strings[0]); + case 1: + major = std::stoi(strings[0]); } return ver; } ros::Time slTime2Ros(sl::timeStamp t) { - uint32_t sec = static_cast(t/1000000000); - uint32_t nsec = static_cast(t%1000000000); + uint32_t sec = static_cast(t / 1000000000); + uint32_t nsec = static_cast(t % 1000000000); return ros::Time(sec, nsec); } From 83c7aea1d618ce2994e5a315c8e22985bf6678a2 Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 16:25:57 +0200 Subject: [PATCH 2/6] Fully removed OpenCV dependency --- zed_wrapper/CMakeLists.txt | 9 --- zed_wrapper/README.md | 2 - zed_wrapper/package.xml | 1 - .../nodelet/include/zed_wrapper_nodelet.hpp | 6 -- .../src/nodelet/src/zed_wrapper_nodelet.cpp | 18 +----- zed_wrapper/src/tools/include/sl_tools.h | 3 +- zed_wrapper/src/tools/src/sl_tools.cpp | 58 +++++++++++++++---- 7 files changed, 50 insertions(+), 47 deletions(-) diff --git a/zed_wrapper/CMakeLists.txt b/zed_wrapper/CMakeLists.txt index cb569fcf..cf8a81a7 100644 --- a/zed_wrapper/CMakeLists.txt +++ b/zed_wrapper/CMakeLists.txt @@ -27,11 +27,6 @@ if ( CMAKE_SYSTEM_NAME2 MATCHES "aarch64" ) # Jetson TX SET(CUDA_USE_STATIC_CUDA_RUNTIME OFF) endif() -find_package(OpenCV 3 COMPONENTS core highgui imgproc) -checkPackage("OPENCV_CORE" "OpenCV core not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html") -checkPackage("OPENCV_HIGHGUI" "OpenCV highgui not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html") -checkPackage("OPENCV_IMGPROC" "OpenCV imgproc not found, install it from the tutorial at:\n https://www.stereolabs.com/documentation/overview/getting-started/getting-started.html") - find_package(CUDA) checkPackage("CUDA" "CUDA not found, install it from:\n https://developer.nvidia.com/cuda-downloads") @@ -85,7 +80,6 @@ catkin_package( rosconsole sensor_msgs stereo_msgs - opencv3 image_transport dynamic_reconfigure tf2_ros @@ -111,14 +105,12 @@ include_directories( ${catkin_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS} ${ZED_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} ${CMAKE_CURRENT_SOURCE_DIR}/src/tools/include ${CMAKE_CURRENT_SOURCE_DIR}/src/nodelet/include ) link_directories(${ZED_LIBRARY_DIR}) link_directories(${CUDA_LIBRARY_DIRS}) -link_directories(${OpenCV_LIBRARY_DIRS}) ############################################################################### @@ -136,7 +128,6 @@ set(LINK_LIBRARIES ${catkin_LIBRARIES} ${ZED_LIBRARIES} ${CUDA_LIBRARIES} ${CUDA_NPP_LIBRARIES_ZED} - ${OpenCV_LIBS} ) add_library(ZEDWrapper ${TOOLS_SRC} ${NODELET_SRC}) diff --git a/zed_wrapper/README.md b/zed_wrapper/README.md index 046e86de..e5718c0a 100644 --- a/zed_wrapper/README.md +++ b/zed_wrapper/README.md @@ -25,12 +25,10 @@ The zed_ros_wrapper is a catkin package. It depends on the following ROS package - rosconsole - sensor_msgs - stereo_msgs - - opencv3 - image_transport - dynamic_reconfigure - urdf - Open a terminal and build the package: cd ~/catkin_ws/src diff --git a/zed_wrapper/package.xml b/zed_wrapper/package.xml index 9ab9bace..19e34df7 100644 --- a/zed_wrapper/package.xml +++ b/zed_wrapper/package.xml @@ -17,7 +17,6 @@ rosconsole sensor_msgs stereo_msgs - opencv3 image_transport dynamic_reconfigure nodelet diff --git a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp index 89a9e000..9987b949 100644 --- a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp @@ -42,8 +42,6 @@ #include #include -#include - #include #include #include @@ -344,10 +342,6 @@ namespace zed_wrapper { int camHeight; int matWidth; int matHeight; - cv::Mat leftImRGB; - cv::Mat rightImRGB; - cv::Mat confImRGB; - cv::Mat confMapFloat; // Mutex std::mutex dataMutex; diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index 48a4af6b..f65a2d4f 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -20,8 +20,6 @@ #include "zed_wrapper_nodelet.hpp" #include "sl_tools.h" -#include - #ifndef NDEBUG #include #endif @@ -1053,8 +1051,8 @@ namespace zed_wrapper { } if (rawParam) { - cv::Mat R_ = sl_tools::convertRodrigues(zedParam.R); - float* p = (float*)R_.data; + std::vector R_ = sl_tools::convertRodrigues(zedParam.R); + float* p = R_.data(); for (int i = 0; i < 9; i++) { right_cam_info_msg->R[i] = p[i]; } @@ -1291,20 +1289,12 @@ namespace zed_wrapper { matHeight = static_cast(camHeight * matResizeFactor); NODELET_DEBUG_STREAM("Data Mat size : " << matWidth << "x" << matHeight); - cv::Size cvSize(matWidth, matWidth); - leftImRGB = cv::Mat(cvSize, CV_8UC3); - rightImRGB = cv::Mat(cvSize, CV_8UC3); - confImRGB = cv::Mat(cvSize, CV_8UC3); - confMapFloat = cv::Mat(cvSize, CV_32FC1); - // Create and fill the camera information messages fillCamInfo(zed, leftCamInfoMsg, rightCamInfoMsg, leftCamOptFrameId, rightCamOptFrameId); fillCamInfo(zed, leftCamInfoRawMsg, rightCamInfoRawMsg, leftCamOptFrameId, rightCamOptFrameId, true); - rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg; // the reference camera is - // the Left one (next to the - // ZED logo) + rgbCamInfoMsg = depthCamInfoMsg = leftCamInfoMsg; rgbCamInfoRawMsg = leftCamInfoRawMsg; sl::RuntimeParameters runParams; @@ -1523,8 +1513,6 @@ namespace zed_wrapper { matWidth, matHeight); // Need to flip sign, but cause of this is not sure - //cv::Mat disparity = sl_tools::toCVMat(disparityZEDMat) * -1.0; - #pragma parallel for for (int y = 0; y < disparityZEDMat.getHeight(); y++) { #pragma parallel for diff --git a/zed_wrapper/src/tools/include/sl_tools.h b/zed_wrapper/src/tools/include/sl_tools.h index 41583d39..568743d8 100644 --- a/zed_wrapper/src/tools/include/sl_tools.h +++ b/zed_wrapper/src/tools/include/sl_tools.h @@ -23,7 +23,6 @@ #include #include -#include #include namespace sl_tools { @@ -38,7 +37,7 @@ namespace sl_tools { */ sl::DeviceProperties getZEDFromSN(unsigned int serial_number); - cv::Mat convertRodrigues(sl::float3 r); + std::vector convertRodrigues(sl::float3 r); /* \brief Test if a file exist * \param name : the path to the file diff --git a/zed_wrapper/src/tools/src/sl_tools.cpp b/zed_wrapper/src/tools/src/sl_tools.cpp index 59f2c89e..5f952b48 100644 --- a/zed_wrapper/src/tools/src/sl_tools.cpp +++ b/zed_wrapper/src/tools/src/sl_tools.cpp @@ -47,22 +47,32 @@ namespace sl_tools { return prop; } - cv::Mat convertRodrigues(sl::float3 r) { - double theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); - cv::Mat R = cv::Mat::eye(3, 3, CV_32F); + std::vector convertRodrigues(sl::float3 r) { + float theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); + + //cv::Mat R = cv::Mat::eye(3, 3, CV_32F); + std::vector R = {1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 1.0f + }; if (theta < DBL_EPSILON) { return R; } else { - double c = cos(theta); - double s = sin(theta); - double c1 = 1. - c; - double itheta = theta ? 1. / theta : 0.; + float c = cos(theta); + float s = sin(theta); + float c1 = 1.f - c; + float itheta = theta ? 1.f / theta : 0.f; r *= itheta; - cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F); - float* p = (float*) rrt.data; + //cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F); + std::vector rrt = {1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 1.0f + }; + + float* p = rrt.data(); p[0] = r.x * r.x; p[1] = r.x * r.y; p[2] = r.x * r.z; @@ -73,8 +83,12 @@ namespace sl_tools { p[7] = r.y * r.z; p[8] = r.z * r.z; - cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F); - p = (float*) r_x.data; + //cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F); + std::vector r_x = {1.0f, 0.0f, 0.0f, + 0.0f, 1.0f, 0.0f, + 0.0f, 0.0f, 1.0f + }; + p = r_x.data(); p[0] = 0; p[1] = -r.z; p[2] = r.y; @@ -86,8 +100,28 @@ namespace sl_tools { p[8] = 0; // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] - R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x; + + sl::Matrix3f eye; + eye.setIdentity(); + + sl::Matrix3f sl_R(R.data()); + sl::Matrix3f sl_rrt(rrt.data()); + sl::Matrix3f sl_r_x(r_x.data()); + + //R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x; + sl_R = eye * c + sl_rrt * c1 + sl_r_x * s; + + R[0] = sl_R.r00; + R[1] = sl_R.r01; + R[2] = sl_R.r02; + R[3] = sl_R.r10; + R[4] = sl_R.r11; + R[5] = sl_R.r12; + R[6] = sl_R.r20; + R[7] = sl_R.r21; + R[8] = sl_R.r22; } + return R; } From 3b7201dfb6a5d6ea5a812cbcb5511322f9c1c590 Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 17:05:16 +0200 Subject: [PATCH 3/6] Added missing OPENNI conversion if the parameter `openni_depth_mode` is set. Removed unuseful code comments --- .../nodelet/include/zed_wrapper_nodelet.hpp | 8 +-- .../src/nodelet/src/zed_wrapper_nodelet.cpp | 58 ++++++++++++------- zed_wrapper/src/tools/src/sl_tools.cpp | 4 -- 3 files changed, 41 insertions(+), 29 deletions(-) diff --git a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp index 9987b949..7c98ae1d 100644 --- a/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp +++ b/zed_wrapper/src/nodelet/include/zed_wrapper_nodelet.hpp @@ -114,7 +114,7 @@ namespace zed_wrapper { */ void publishImuFrame(tf2::Transform baseTransform); - /* \brief Publish a cv::Mat image with a ros Publisher + /* \brief Publish a sl::Mat image with a ros Publisher * \param img : the image to publish * \param pub_img : the publisher object to use (different image publishers exist) * \param img_frame_id : the id of the reference frame of the image (different image frames exist) @@ -122,13 +122,13 @@ namespace zed_wrapper { */ void publishImage(sl::Mat img, image_transport::Publisher& pubImg, string imgFrameId, ros::Time t); - /* \brief Publish a cv::Mat depth image with a ros Publisher + /* \brief Publish a sl::Mat depth image with a ros Publisher * \param depth : the depth image to publish * \param t : the ros::Time to stamp the depth image */ void publishDepth(sl::Mat depth, ros::Time t); - /* \brief Publish a cv::Mat confidence image with a ros Publisher + /* \brief Publish a sl::Mat confidence image with a ros Publisher * \param conf : the confidence image to publish * \param t : the ros::Time to stamp the depth image */ @@ -147,7 +147,7 @@ namespace zed_wrapper { */ void publishCamInfo(sensor_msgs::CameraInfoPtr camInfoMsg, ros::Publisher pubCamInfo, ros::Time t); - /* \brief Publish a cv::Mat disparity image with a ros Publisher + /* \brief Publish a sl::Mat disparity image with a ros Publisher * \param disparity : the disparity image to publish * \param t : the ros::Time to stamp the depth image */ diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index f65a2d4f..1ccf3483 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -636,7 +636,7 @@ namespace zed_wrapper { int num = 1; // for endianness detection imgMessage.is_bigendian = !(*(char*)&num == 1); - imgMessage.step = img.getStepBytes(); // TODO CHECK + imgMessage.step = img.getStepBytes(); size_t size = imgMessage.step * imgMessage.height; imgMessage.data.resize(size); @@ -895,18 +895,44 @@ namespace zed_wrapper { void ZEDWrapperNodelet::publishDepth(sl::Mat depth, ros::Time t) { - // TODO OPENNI CONVERSION + if (!openniDepthMode) { + pubDepth.publish(imageToROSmsg(depth, depthOptFrameId, t)); + return; + } - /*string encoding; - if (openniDepthMode) { - depth *= 1000.0f; - depth.convertTo(depth, CV_16UC1); // in mm, rounded - encoding = sensor_msgs::image_encodings::TYPE_16UC1; - } else { - encoding = sensor_msgs::image_encodings::TYPE_32FC1; - }*/ + // OPENNI CONVERSION + sensor_msgs::ImagePtr depthMessage = boost::make_shared(); + + depthMessage->header.stamp = t; + depthMessage->header.frame_id = depthOptFrameId; + depthMessage->height = depth.getHeight(); + depthMessage->width = depth.getWidth(); - pubDepth.publish(imageToROSmsg(depth, depthOptFrameId, t)); + int num = 1; // for endianness detection + depthMessage->is_bigendian = !(*(char*)&num == 1); + + depthMessage->step = depthMessage->width * sizeof(uint16_t); + depthMessage->encoding = sensor_msgs::image_encodings::MONO16; + + size_t size = depthMessage->step * depthMessage->height; + depthMessage->data.resize(size); + + uint16_t* data = (uint16_t*)(&depthMessage->data[0]); + +#pragma parallel for + for (int y = 0; y < depth.getHeight(); y++) { +#pragma parallel for + for (int x = 0; x < depth.getWidth(); x++) { + sl::float1 value; + depth.getValue(x, y, &value); + value *= 1000.0f; + + int idx = x + y * depthMessage->width; + data[idx] = static_cast(std::round(value)); // in mm, rounded + } + } + + pubDepth.publish(depthMessage); } void ZEDWrapperNodelet::publishDisparity(sl::Mat disparity, ros::Time t) { @@ -1444,8 +1470,6 @@ namespace zed_wrapper { zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT, sl::MEM_CPU, matWidth, matHeight); - //cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); - if (left_SubNumber > 0) { publishCamInfo(leftCamInfoMsg, pubLeftCamInfo, t); publishImage(leftZEDMat, pubLeft, leftCamOptFrameId, t); @@ -1463,8 +1487,6 @@ namespace zed_wrapper { zed.retrieveImage(leftZEDMat, sl::VIEW_LEFT_UNRECTIFIED, sl::MEM_CPU, matWidth, matHeight); - //cv::cvtColor(sl_tools::toCVMat(leftZEDMat), leftImRGB, CV_RGBA2RGB); - if (left_raw_SubNumber > 0) { publishCamInfo(leftCamInfoRawMsg, pubLeftCamInfoRaw, t); publishImage(leftZEDMat, pubRawLeft, leftCamOptFrameId, t); @@ -1481,8 +1503,6 @@ namespace zed_wrapper { zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT, sl::MEM_CPU, matWidth, matHeight); - //cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); - publishCamInfo(rightCamInfoMsg, pubRightCamInfo, t); publishImage(rightZEDMat, pubRight, rightCamOptFrameId, t); } @@ -1493,8 +1513,6 @@ namespace zed_wrapper { zed.retrieveImage(rightZEDMat, sl::VIEW_RIGHT_UNRECTIFIED, sl::MEM_CPU, matWidth, matHeight); - //cv::cvtColor(sl_tools::toCVMat(rightZEDMat), rightImRGB, CV_RGBA2RGB); - publishCamInfo(rightCamInfoRawMsg, pubRightCamInfoRaw, t); publishImage(rightZEDMat, pubRawRight, rightCamOptFrameId, t); } @@ -1532,8 +1550,6 @@ namespace zed_wrapper { zed.retrieveImage(confImgZEDMat, sl::VIEW_CONFIDENCE, sl::MEM_CPU, matWidth, matHeight); - //cv::cvtColor(sl_tools::toCVMat(confImgZEDMat), confImRGB, CV_RGBA2RGB); - publishImage(confImgZEDMat, pubConfImg, confidenceOptFrameId, t); } diff --git a/zed_wrapper/src/tools/src/sl_tools.cpp b/zed_wrapper/src/tools/src/sl_tools.cpp index 5f952b48..694fbd3c 100644 --- a/zed_wrapper/src/tools/src/sl_tools.cpp +++ b/zed_wrapper/src/tools/src/sl_tools.cpp @@ -50,7 +50,6 @@ namespace sl_tools { std::vector convertRodrigues(sl::float3 r) { float theta = sqrt(r.x * r.x + r.y * r.y + r.z * r.z); - //cv::Mat R = cv::Mat::eye(3, 3, CV_32F); std::vector R = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f @@ -66,7 +65,6 @@ namespace sl_tools { r *= itheta; - //cv::Mat rrt = cv::Mat::eye(3, 3, CV_32F); std::vector rrt = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f @@ -83,7 +81,6 @@ namespace sl_tools { p[7] = r.y * r.z; p[8] = r.z * r.z; - //cv::Mat r_x = cv::Mat::eye(3, 3, CV_32F); std::vector r_x = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f @@ -108,7 +105,6 @@ namespace sl_tools { sl::Matrix3f sl_rrt(rrt.data()); sl::Matrix3f sl_r_x(r_x.data()); - //R = c * cv::Mat::eye(3, 3, CV_32F) + c1 * rrt + s * r_x; sl_R = eye * c + sl_rrt * c1 + sl_r_x * s; R[0] = sl_R.r00; From 3962a1a28d3b224ff7b3598f81b8924bab6097ff Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 17:35:54 +0200 Subject: [PATCH 4/6] Improved OPENNI conversion Improved Disparity conversion --- .../src/nodelet/src/zed_wrapper_nodelet.cpp | 32 ++++++++----------- 1 file changed, 13 insertions(+), 19 deletions(-) diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index 1ccf3483..48d056bc 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -900,7 +900,7 @@ namespace zed_wrapper { return; } - // OPENNI CONVERSION + // OPENNI CONVERSION (meter -> millimeters - float32 -> uint16) sensor_msgs::ImagePtr depthMessage = boost::make_shared(); depthMessage->header.stamp = t; @@ -919,17 +919,14 @@ namespace zed_wrapper { uint16_t* data = (uint16_t*)(&depthMessage->data[0]); -#pragma parallel for - for (int y = 0; y < depth.getHeight(); y++) { -#pragma parallel for - for (int x = 0; x < depth.getWidth(); x++) { - sl::float1 value; - depth.getValue(x, y, &value); - value *= 1000.0f; + int dataSize = depthMessage->width * depthMessage->height; + sl::float1* depthDataPtr = depth.getPtr(); - int idx = x + y * depthMessage->width; - data[idx] = static_cast(std::round(value)); // in mm, rounded - } +#pragma parallel for + for (int i = 0; i < dataSize; i++) { + sl::float1 value = depthDataPtr[i]; + value *= 1000.0f; + data[i] = static_cast(std::round(value)); // in mm, rounded } pubDepth.publish(depthMessage); @@ -1531,15 +1528,12 @@ namespace zed_wrapper { matWidth, matHeight); // Need to flip sign, but cause of this is not sure + int dataSize = disparityZEDMat.getWidth() * disparityZEDMat.getHeight(); + sl::float1* dispDataPtr = disparityZEDMat.getPtr(); + #pragma parallel for - for (int y = 0; y < disparityZEDMat.getHeight(); y++) { -#pragma parallel for - for (int x = 0; x < disparityZEDMat.getWidth(); x++) { - sl::float1 value; - disparityZEDMat.getValue(x, y, &value); - value *= -1.0f; - disparityZEDMat.setValue(x, y, value); - } + for (int i = 0; i < dataSize; i++) { + dispDataPtr[i] *= -1.0f; } publishDisparity(disparityZEDMat, t); From fdf169e9bf7c72c0bab0e525fad986657883d30d Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 18:13:22 +0200 Subject: [PATCH 5/6] OPENNI and Disparity conversions even improved --- zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index 48d056bc..19cd0c1f 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -922,11 +922,8 @@ namespace zed_wrapper { int dataSize = depthMessage->width * depthMessage->height; sl::float1* depthDataPtr = depth.getPtr(); -#pragma parallel for for (int i = 0; i < dataSize; i++) { - sl::float1 value = depthDataPtr[i]; - value *= 1000.0f; - data[i] = static_cast(std::round(value)); // in mm, rounded + *(data++) = static_cast(std::round(*(depthDataPtr++) * 1000)); // in mm, rounded } pubDepth.publish(depthMessage); @@ -1531,9 +1528,8 @@ namespace zed_wrapper { int dataSize = disparityZEDMat.getWidth() * disparityZEDMat.getHeight(); sl::float1* dispDataPtr = disparityZEDMat.getPtr(); -#pragma parallel for for (int i = 0; i < dataSize; i++) { - dispDataPtr[i] *= -1.0f; + *(dispDataPtr++) *= -1.0f; } publishDisparity(disparityZEDMat, t); From 6dea32e8566d04d7274d42e5d9f48ce425a49b7a Mon Sep 17 00:00:00 2001 From: Myzhar Date: Thu, 13 Sep 2018 18:21:35 +0200 Subject: [PATCH 6/6] Code formatting and comments removal --- .../src/nodelet/src/zed_wrapper_nodelet.cpp | 53 ++++++------------- 1 file changed, 16 insertions(+), 37 deletions(-) diff --git a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp index 19cd0c1f..bde6163d 100644 --- a/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp +++ b/zed_wrapper/src/nodelet/src/zed_wrapper_nodelet.cpp @@ -168,10 +168,6 @@ namespace zed_wrapper { << (publishTf ? "TRUE" : "FALSE") << "]"); NODELET_INFO_STREAM("Camera Flip [" << (cameraFlip ? "TRUE" : "FALSE") << "]"); - // Status of odometry TF - // NODELET_INFO_STREAM("Publish " << odometry_frame_id << " [" << (publish_tf - // ? "TRUE" : "FALSE") << "]"); - std::string img_topic = "image_rect_color"; std::string img_raw_topic = "image_raw_color"; @@ -454,27 +450,20 @@ namespace zed_wrapper { NODELET_INFO_STREAM("Advertized on topic " << point_cloud_topic); // Camera info publishers - pubRgbCamInfo = - nh.advertise(rgb_cam_info_topic, 1); // rgb + pubRgbCamInfo = nh.advertise(rgb_cam_info_topic, 1); // rgb NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_topic); - pubLeftCamInfo = - nh.advertise(left_cam_info_topic, 1); // left + pubLeftCamInfo = nh.advertise(left_cam_info_topic, 1); // left NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_topic); - pubRightCamInfo = - nh.advertise(right_cam_info_topic, 1); // right + pubRightCamInfo = nh.advertise(right_cam_info_topic, 1); // right NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_topic); - pubDepthCamInfo = - nh.advertise(depth_cam_info_topic, 1); // depth + pubDepthCamInfo = nh.advertise(depth_cam_info_topic, 1); // depth NODELET_INFO_STREAM("Advertized on topic " << depth_cam_info_topic); // Raw - pubRgbCamInfoRaw = nh.advertise( - rgb_cam_info_raw_topic, 1); // raw rgb + pubRgbCamInfoRaw = nh.advertise(rgb_cam_info_raw_topic, 1); // raw rgb NODELET_INFO_STREAM("Advertized on topic " << rgb_cam_info_raw_topic); - pubLeftCamInfoRaw = nh.advertise( - left_cam_info_raw_topic, 1); // raw left + pubLeftCamInfoRaw = nh.advertise(left_cam_info_raw_topic, 1); // raw left NODELET_INFO_STREAM("Advertized on topic " << left_cam_info_raw_topic); - pubRightCamInfoRaw = nh.advertise( - right_cam_info_raw_topic, 1); // raw right + pubRightCamInfoRaw = nh.advertise(right_cam_info_raw_topic, 1); // raw right NODELET_INFO_STREAM("Advertized on topic " << right_cam_info_raw_topic); // Odometry and Map publisher @@ -486,12 +475,10 @@ namespace zed_wrapper { // Imu publisher if (imuPubRate > 0 && realCamModel == sl::MODEL_ZED_M) { pubImu = nh.advertise(imu_topic, 500); - NODELET_INFO_STREAM("Advertized on topic " << imu_topic << " @ " - << imuPubRate << " Hz"); + NODELET_INFO_STREAM("Advertized on topic " << imu_topic << " @ " << imuPubRate << " Hz"); pubImuRaw = nh.advertise(imu_topic_raw, 500); - NODELET_INFO_STREAM("Advertized on topic " << imu_topic_raw << " @ " - << imuPubRate << " Hz"); + NODELET_INFO_STREAM("Advertized on topic " << imu_topic_raw << " @ " << imuPubRate << " Hz"); imuTime = ros::Time::now(); pubImuTimer = nhNs.createTimer(ros::Duration(1.0 / imuPubRate), @@ -504,12 +491,9 @@ namespace zed_wrapper { } // Service - srvSetInitPose = nh.advertiseService("set_initial_pose", - &ZEDWrapperNodelet::on_set_pose, this); - srvResetOdometry = nh.advertiseService( - "reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); - srvResetTracking = nh.advertiseService( - "reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); + srvSetInitPose = nh.advertiseService("set_initial_pose", &ZEDWrapperNodelet::on_set_pose, this); + srvResetOdometry = nh.advertiseService("reset_odometry", &ZEDWrapperNodelet::on_reset_odometry, this); + srvResetTracking = nh.advertiseService("reset_tracking", &ZEDWrapperNodelet::on_reset_tracking, this); // Start Pointcloud thread mPcThread = std::thread(&ZEDWrapperNodelet::pointcloud_thread, this); @@ -1548,8 +1532,6 @@ namespace zed_wrapper { zed.retrieveMeasure(confMapZEDMat, sl::MEASURE_CONFIDENCE, sl::MEM_CPU, matWidth, matHeight); - //confMapFloat = sl_tools::toCVMat(confMapZEDMat); - pubConfMap.publish(imageToROSmsg(confMapZEDMat, confidenceOptFrameId, t)); } @@ -1594,11 +1576,8 @@ namespace zed_wrapper { } // Publish the odometry if someone has subscribed to - if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || - depth_SubNumber > - - - 0 || imu_SubNumber > 0 || imu_RawSubNumber > 0) { + if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || depth_SubNumber > 0 || + imu_SubNumber > 0 || imu_RawSubNumber > 0) { if (!initOdomWithPose) { sl::Pose deltaOdom; zed.getPosition(deltaOdom, sl::REFERENCE_FRAME_CAMERA); @@ -1635,8 +1614,8 @@ namespace zed_wrapper { } // Publish the zed camera pose if someone has subscribed to - if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || - depth_SubNumber > 0 || imu_SubNumber > 0 || imu_RawSubNumber > 0) { + if (pose_SubNumber > 0 || odom_SubNumber > 0 || cloud_SubNumber > 0 || depth_SubNumber > 0 || + imu_SubNumber > 0 || imu_RawSubNumber > 0) { sl::Pose zed_pose; // Sensor to Map transform zed.getPosition(zed_pose, sl::REFERENCE_FRAME_WORLD);