diff --git a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h index 4c3b60bd..137f15d1 100644 --- a/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h +++ b/rtabmap_conversions/include/rtabmap_conversions/MsgConversion.h @@ -59,6 +59,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include namespace rtabmap_conversions { @@ -75,8 +76,17 @@ rtabmap::Transform transformFromPoseMsg(const geometry_msgs::Pose & msg, bool ig void toCvCopy(const rtabmap_msgs::RGBDImage & image, cv_bridge::CvImagePtr & rgb, cv_bridge::CvImagePtr & depth); void toCvShare(const rtabmap_msgs::RGBDImageConstPtr & image, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); void toCvShare(const rtabmap_msgs::RGBDImage & image, const boost::shared_ptr& trackedObject, cv_bridge::CvImageConstPtr & rgb, cv_bridge::CvImageConstPtr & depth); -void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage & msg, const std::string & sensorFrameId); +void rgbdImageToROS(const rtabmap::SensorData & data, + rtabmap_msgs::RGBDImage & msg, + const std_msgs::Header & header, + bool compressImages, + const std::string & compressionFormat = "*.png"); rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::RGBDImageConstPtr & image); +void rgbdImagesToROS(const rtabmap::SensorData & data, + rtabmap_msgs::RGBDImages & msg, + const std::vector & headers, + bool compressImages, + const std::string & compressionFormat = "*.png"); // copy data void compressedMatToBytes(const cv::Mat & compressed, std::vector & bytes); diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index ff155b4f..7f76ac0e 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -209,11 +209,13 @@ void toCvShare(const rtabmap_msgs::RGBDImage & image, const boost::shared_ptr1) { @@ -237,18 +239,26 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage & localTransform = data.stereoCameraModels()[0].localTransform(); } + if(compressImages && (!data.imageCompressed().empty() || !data.depthOrRightRaw().empty())) + { + ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + } + if(!data.imageRaw().empty()) { cv_bridge::CvImage cvImg; cvImg.header = header; cvImg.image = data.imageRaw(); - UASSERT(data.imageRaw().type()==CV_8UC1 || data.imageRaw().type()==CV_8UC3); - cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:sensor_msgs::image_encodings::BGR8; - cvImg.toImageMsg(msg.rgb); - } - else if(!data.imageCompressed().empty()) - { - ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + UASSERT(data.imageRaw().type()==CV_8UC1 || + data.imageRaw().type()==CV_8UC3); + cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8: + sensor_msgs::image_encodings::BGR8; + if(compressImages) { + cvImg.toCompressedImageMsg(msg.rgb_compressed, compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG); + } + else { + cvImg.toImageMsg(msg.rgb); + } } if(!data.depthOrRightRaw().empty()) @@ -256,13 +266,23 @@ void rgbdImageToROS(const rtabmap::SensorData & data, rtabmap_msgs::RGBDImage & cv_bridge::CvImage cvDepth; cvDepth.header = header; cvDepth.image = data.depthOrRightRaw(); - UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || data.depthOrRightRaw().type()==CV_16UC1 || data.depthOrRightRaw().type()==CV_32FC1); - cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8:data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1:sensor_msgs::image_encodings::TYPE_32FC1; - cvDepth.toImageMsg(msg.depth); - } - else if(!data.depthOrRightCompressed().empty()) - { - ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || + data.depthOrRightRaw().type()==CV_8UC3 || + data.depthOrRightRaw().type()==CV_16UC1 || + data.depthOrRightRaw().type()==CV_32FC1); + cvDepth.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8: + data.depthOrRightRaw().type()==CV_8UC3?sensor_msgs::image_encodings::BGR8: + data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1: + sensor_msgs::image_encodings::TYPE_32FC1; + if(compressImages) { + cvDepth.toCompressedImageMsg(msg.depth_compressed, + data.depthOrRightRaw().type()!=CV_16UC1 && + data.depthOrRightRaw().type()!=CV_32FC1 && + compressionFormat == ".jpg"?cv_bridge::JPG:cv_bridge::PNG); + } + else { + cvDepth.toImageMsg(msg.depth); + } } //convert features @@ -430,6 +450,136 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_msgs::RGBDImageConstPtr & ima return data; } +void rgbdImagesToROS(const rtabmap::SensorData & data, + rtabmap_msgs::RGBDImages & msg, + const std::vector & headers, + bool compressImages, + const std::string & compressionFormat) +{ + UASSERT(!headers.empty()); + msg.header = headers[0]; + + if(compressImages && + ((data.imageRaw().empty() && !data.imageCompressed().empty()) || + (data.depthOrRightRaw().empty() && !data.depthOrRightCompressed().empty()))) + { + ROS_ERROR("Conversion of compressed SensorData to RGBDImage is not implemented..."); + } + + cv_bridge::CvImage cvImg; + if(!data.imageRaw().empty()) + { + UASSERT(data.imageRaw().type()==CV_8UC1 || + data.imageRaw().type()==CV_8UC3); + cvImg.encoding = data.imageRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8: + sensor_msgs::image_encodings::BGR8; + } + cv_bridge::CvImage cvDepthORRight; + if(!data.depthOrRightRaw().empty()) + { + UASSERT(data.depthOrRightRaw().type()==CV_8UC1 || + data.depthOrRightRaw().type()==CV_8UC3 || + data.depthOrRightRaw().type()==CV_16UC1 || + data.depthOrRightRaw().type()==CV_32FC1); + cvDepthORRight.encoding = data.depthOrRightRaw().type()==CV_8UC1?sensor_msgs::image_encodings::MONO8: + data.depthOrRightRaw().type()==CV_8UC3?sensor_msgs::image_encodings::BGR8: + data.depthOrRightRaw().type()==CV_16UC1?sensor_msgs::image_encodings::TYPE_16UC1: + sensor_msgs::image_encodings::TYPE_32FC1; + } + + if(!data.cameraModels().empty()) + { + //rgb+depth + if(data.cameraModels().size() != headers.size()) + { + UERROR("Cannot convert multi-camera data to rgbd images if number of sensor frames are not equal to number of cameras."); + return; + } + msg.rgbd_images.resize(data.cameraModels().size()); + int subImageWidth = data.imageRaw().cols / data.cameraModels().size(); + int subDepthWidth = data.depthOrRightRaw().cols / data.cameraModels().size(); + for(size_t i=0; i & bytes) { UASSERT(compressed.empty() || compressed.type() == CV_8UC1); diff --git a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h index 24d20160..7883a569 100644 --- a/rtabmap_odom/include/rtabmap_odom/OdometryROS.h +++ b/rtabmap_odom/include/rtabmap_odom/OdometryROS.h @@ -63,7 +63,7 @@ public: OdometryROS(bool stereoParams, bool visParams, bool icpParams); virtual ~OdometryROS(); - void processData(rtabmap::SensorData & data, const std_msgs::Header & header); + void processData(rtabmap::SensorData & data, const std_msgs::Header & header, const std::vector & multiCamHeaders = std::vector()); bool reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&); bool resetToPose(rtabmap_msgs::ResetPose::Request&, rtabmap_msgs::ResetPose::Response&); @@ -127,6 +127,9 @@ private: ros::Publisher odomLocalScanMap_; ros::Publisher odomLastFrame_; ros::Publisher odomRgbdImagePub_; + ros::Publisher odomRgbdImageCompressedPub_; + ros::Publisher odomRgbdImagesPub_; + ros::Publisher odomRgbdImagesCompressedPub_; ros::Publisher odomSensorDataPub_; ros::Publisher odomSensorDataFeaturesPub_; ros::Publisher odomSensorDataCompressedPub_; @@ -148,6 +151,7 @@ private: USemaphore dataReady_; rtabmap::SensorData dataToProcess_; std_msgs::Header dataHeaderToProcess_; + std::vector dataMultiCamHeadersToProcess_; bool bufferedDataToProcess_; bool paused_; diff --git a/rtabmap_odom/src/OdometryROS.cpp b/rtabmap_odom/src/OdometryROS.cpp index 81d08076..25ab2467 100644 --- a/rtabmap_odom/src/OdometryROS.cpp +++ b/rtabmap_odom/src/OdometryROS.cpp @@ -108,6 +108,9 @@ void OdometryROS::onInit() odomLocalScanMap_ = nh.advertise("odom_local_scan_map", 1); odomLastFrame_ = nh.advertise("odom_last_frame", 1); odomRgbdImagePub_ = nh.advertise("odom_rgbd_image", 1); + odomRgbdImageCompressedPub_ = nh.advertise("odom_rgbd_image/compressed", 1); + odomRgbdImagesPub_ = nh.advertise("odom_rgbd_images", 1); + odomRgbdImagesCompressedPub_ = nh.advertise("odom_rgbd_images/compressed", 1); odomSensorDataPub_ = nh.advertise("odom_sensor_data/raw", 1); odomSensorDataFeaturesPub_ = nh.advertise("odom_sensor_data/features", 1); odomSensorDataCompressedPub_ = nh.advertise("odom_sensor_data/compressed", 1); @@ -457,7 +460,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg) } } -void OdometryROS::processData(SensorData & data, const std_msgs::Header & header) +void OdometryROS::processData(SensorData & data, const std_msgs::Header & header, const std::vector & multiCamHeaders) { //NODELET_WARN("Received image: %f delay=%f", data.stamp(), (ros::Time::now() - header.stamp).toSec()); if(dataMutex_.lockTry() == 0) @@ -468,6 +471,7 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header } dataToProcess_ = data; dataHeaderToProcess_ = header; + dataMultiCamHeadersToProcess_ = multiCamHeaders; bufferedDataToProcess_ = false; dataReady_.release(); dataMutex_.unlock(); @@ -551,12 +555,14 @@ void OdometryROS::mainLoop() (previousClockTime_ - clockNow).toSec()); SensorData dataCpy = dataToProcess_; std_msgs::Header headerCpy = dataHeaderToProcess_; + std::vector multiCamHeadersCpy = dataMultiCamHeadersToProcess_; ros::Time previousCpy = previousClockTime_; this->reset(odometry_->getPose()); if(previousCpy > headerCpy.stamp) { // new frame is using new clock, process it now dataToProcess_ = dataCpy; dataHeaderToProcess_ = headerCpy; + dataMultiCamHeadersToProcess_ = multiCamHeadersCpy; dataReady_.release(); NODELET_WARN("Odometry: Restarting with frame: %f (clock previous=%f, new=%f)", headerCpy.stamp.toSec(), previousCpy.toSec(), clockNow.toSec()); @@ -1037,18 +1043,50 @@ void OdometryROS::mainLoop() postProcessData(data, header); - if(!data.imageRaw().empty() && odomRgbdImagePub_.getNumSubscribers()) - { - if(!header.frame_id.empty()) + if(!data.imageRaw().empty()) { + if(odomRgbdImagePub_.getNumSubscribers() || odomRgbdImageCompressedPub_.getNumSubscribers()) { - rtabmap_msgs::RGBDImage msg; - rtabmap_conversions::rgbdImageToROS(data, msg, header.frame_id); - msg.header = header; // use corresponding time stamp to image - odomRgbdImagePub_.publish(msg); + if(data.cameraModels().size()<=1 && data.stereoCameraModels().size()<=1) + { + if(odomRgbdImagePub_.getNumSubscribers()) { + rtabmap_msgs::RGBDImage msg; + rtabmap_conversions::rgbdImageToROS(data, msg, header, false); + odomRgbdImagePub_.publish(msg); + } + + if(odomRgbdImageCompressedPub_.getNumSubscribers()) + { + rtabmap_msgs::RGBDImage msg; + rtabmap_conversions::rgbdImageToROS(data, msg, header, true, compressionImgFormat_); + odomRgbdImageCompressedPub_.publish(msg); + } + + } + else + { + ROS_WARN("Cannot convert SensorData for %s topic because it has more than one camera (%ld). " + "Subscribe to %s topic instead to get all cameras.", + odomRgbdImagePub_.getTopic().c_str(), + std::max(data.cameraModels().size(), data.stereoCameraModels().size()), + odomRgbdImagesPub_.getTopic().c_str()); + } } - else + + if(!dataMultiCamHeadersToProcess_.empty()) { - ROS_WARN("Sensor frame not set, cannot convert SensorData to RGBDImage"); + if(odomRgbdImagesPub_.getNumSubscribers()) { + rtabmap_msgs::RGBDImages msg; + rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, false); + msg.header = header; + odomRgbdImagesPub_.publish(msg); + } + if(odomRgbdImagesCompressedPub_.getNumSubscribers()) + { + rtabmap_msgs::RGBDImages msg; + rtabmap_conversions::rgbdImagesToROS(data, msg, dataMultiCamHeadersToProcess_, true, compressionImgFormat_); + msg.header = header; + odomRgbdImagesCompressedPub_.publish(msg); + } } } @@ -1197,6 +1235,7 @@ void OdometryROS::reset(const Transform & pose) imuProcessed_ = false; dataToProcess_ = SensorData(); dataHeaderToProcess_ = std_msgs::Header(); + dataMultiCamHeadersToProcess_.clear(); bufferedDataToProcess_ = false; imuMutex_.lock(); imus_.clear(); diff --git a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp index bda1e1a9..4fd1517f 100644 --- a/rtabmap_odom/src/nodelets/rgbd_odometry.cpp +++ b/rtabmap_odom/src/nodelets/rgbd_odometry.cpp @@ -443,6 +443,7 @@ private: cv::Mat rgb; cv::Mat depth; std::vector cameraModels; + std::vector mutiCamHeaders; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || @@ -539,7 +540,7 @@ private: } if(depth.empty()) { - depth = cv::Mat(depthHeight, depthWidth*cameraCount, ptrDepth->image.type()); + depth = cv::Mat::zeros(depthHeight, depthWidth*cameraCount, ptrDepth->image.type()); } if(ptrImage->image.type() == rgb.type()) @@ -554,7 +555,8 @@ private: if(ptrDepth->image.type() == depth.type()) { - ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); + //if(i < 4) + ptrDepth->image.copyTo(cv::Mat(depth, cv::Rect(i*depthWidth, 0, depthWidth, depthHeight))); } else { @@ -563,6 +565,7 @@ private: } cameraModels.push_back(rtabmap_conversions::cameraModelFromROS(cameraInfos[i], localTransform)); + mutiCamHeaders.push_back(rgbImages[i]->header); } rtabmap::SensorData data( @@ -575,7 +578,7 @@ private: std_msgs::Header header; header.stamp = higherStamp; header.frame_id = rgbImages.size()==1?rgbImages[0]->header.frame_id:""; - this->processData(data, header); + this->processData(data, header, mutiCamHeaders); } void callback( diff --git a/rtabmap_odom/src/nodelets/stereo_odometry.cpp b/rtabmap_odom/src/nodelets/stereo_odometry.cpp index e698c38d..6890b8f3 100644 --- a/rtabmap_odom/src/nodelets/stereo_odometry.cpp +++ b/rtabmap_odom/src/nodelets/stereo_odometry.cpp @@ -432,6 +432,7 @@ private: cv::Mat left; cv::Mat right; std::vector cameraModels; + std::vector mutiCamHeaders; for(unsigned int i=0; iencoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 || @@ -555,6 +556,8 @@ private: rtabmap::StereoCameraModel stereoModel = rtabmap_conversions::stereoCameraModelFromROS(leftCameraInfos[i], rightCameraInfos[i], localTransform, stereoTransform); + std::cout << i << " " << stereoModel << std::endl; + if( stereoModel.baseline() == 0 && alreadyRectified && !rightCameraInfos[i].header.frame_id.empty() && @@ -587,6 +590,8 @@ private: stereoTransform.x(), stereoModel.localTransform(), stereoModel.left().imageSize()); + + std::cout << i << " B " << stereoModel << std::endl; } } @@ -662,6 +667,7 @@ private: } cameraModels.push_back(stereoModel); + mutiCamHeaders.push_back(leftImages[i]->header); } else { @@ -681,7 +687,7 @@ private: std_msgs::Header header; header.stamp = higherStamp; header.frame_id = leftImages.size()==1?leftImages[0]->header.frame_id:""; - this->processData(data, header); + this->processData(data, header, mutiCamHeaders); } void callback(