Skip to content

Commit

Permalink
Compressed image publishers (#580)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Serafadam
Serafadam authored Aug 22, 2024
1 parent 24ce893 commit b897bff
Show file tree
Hide file tree
Showing 28 changed files with 724 additions and 404 deletions.
2 changes: 2 additions & 0 deletions depthai_bridge/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ find_package(tf2_ros REQUIRED)
find_package(tf2 REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(composition_interfaces REQUIRED)
find_package(ffmpeg_image_transport_msgs REQUIRED)

set(dependencies
camera_info_manager
Expand All @@ -56,6 +57,7 @@ set(dependencies
tf2_geometry_msgs
tf2
composition_interfaces
ffmpeg_image_transport_msgs
)

include_directories(
Expand Down
59 changes: 38 additions & 21 deletions depthai_bridge/include/depthai_bridge/ImageConverter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,12 @@
#include "depthai-shared/common/CameraBoardSocket.hpp"
#include "depthai-shared/common/Point2f.hpp"
#include "depthai/device/CalibrationHandler.hpp"
#include "depthai/pipeline/datatype/EncodedFrame.hpp"
#include "depthai/pipeline/datatype/ImgFrame.hpp"
#include "ffmpeg_image_transport_msgs/msg/ffmpeg_packet.hpp"
#include "rclcpp/time.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/msg/compressed_image.hpp"
#include "sensor_msgs/msg/image.hpp"
#include "std_msgs/msg/header.hpp"

Expand All @@ -22,7 +25,10 @@ namespace ros {

namespace StdMsgs = std_msgs::msg;
namespace ImageMsgs = sensor_msgs::msg;
namespace FFMPEGMsgs = ffmpeg_image_transport_msgs::msg;
using ImagePtr = ImageMsgs::Image::SharedPtr;
using FFMPEGImagePtr = FFMPEGMsgs::FFMPEGPacket::SharedPtr;
using CompImagePtr = ImageMsgs::CompressedImage::SharedPtr;

using TimePoint = std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration>;

Expand All @@ -47,7 +53,7 @@ class ImageConverter {
* @param update: bool whether to automatically update the ROS base time on message conversion
*/
void setUpdateRosBaseTimeOnToRosMsg(bool update = true) {
_updateRosBaseTimeOnToRosMsg = update;
updateRosBaseTimeOnToRosMsg = update;
}

/**
Expand Down Expand Up @@ -80,10 +86,20 @@ class ImageConverter {
*/
void setAlphaScaling(double alphaScalingFactor = 0.0);

/**
* @brief Sets the encoding of the image when converting to FFMPEG message. Default is libx264.
* @param encoding: The encoding to be used.
*/
void setFFMPEGEncoding(const std::string& encoding);

void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
ImageMsgs::Image toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info = sensor_msgs::msg::CameraInfo());
ImagePtr toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData);

FFMPEGMsgs::FFMPEGPacket toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData);

ImageMsgs::CompressedImage toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData);

void toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData);

/** TODO(sachin): Add support for ros msg to cv mat since we have some
Expand All @@ -99,36 +115,37 @@ class ImageConverter {
Point2f bottomRightPixelId = Point2f());

private:
void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
static std::unordered_map<dai::RawImgFrame::Type, std::string> encodingEnumMap;
static std::unordered_map<dai::RawImgFrame::Type, std::string> planarEncodingEnumMap;

// dai::RawImgFrame::Type _srcType;
bool _daiInterleaved;
bool daiInterleaved;
// bool c
const std::string _frameName = "";
void planarToInterleaved(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
void interleavedToPlanar(const std::vector<uint8_t>& srcData, std::vector<uint8_t>& destData, int w, int h, int numPlanes, int bpp);
std::chrono::time_point<std::chrono::steady_clock> _steadyBaseTime;
const std::string frameName = "";
std::chrono::time_point<std::chrono::steady_clock> steadyBaseTime;

rclcpp::Time _rosBaseTime;
bool _getBaseDeviceTimestamp;
rclcpp::Time rosBaseTime;
bool getBaseDeviceTimestamp;
// For handling ROS time shifts and debugging
int64_t _totalNsChange{0};
int64_t totalNsChange{0};
// Whether to update the ROS base time on each message conversion
bool _updateRosBaseTimeOnToRosMsg{false};
dai::RawImgFrame::Type _srcType;
bool _fromBitstream = false;
bool _convertDispToDepth = false;
bool _addExpOffset = false;
bool _alphaScalingEnabled = false;
dai::CameraExposureOffset _expOffset;
bool _reverseStereoSocketOrder = false;
double _baseline;
double _alphaScalingFactor = 0.0;
bool updateRosBaseTimeOnToRosMsg{false};
dai::RawImgFrame::Type srcType;
bool fromBitstream = false;
bool dispToDepth = false;
bool addExpOffset = false;
bool alphaScalingEnabled = false;
dai::CameraExposureOffset expOffset;
bool reversedStereoSocketOrder = false;
double baseline;
double alphaScalingFactor = 0.0;
int camHeight = -1;
int camWidth = -1;
std::string ffmpegEncoding = "libx264";
};

} // namespace ros

namespace rosBridge = ros;

} // namespace dai
1 change: 1 addition & 0 deletions depthai_bridge/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@
<depend>std_msgs</depend>
<depend>stereo_msgs</depend>
<depend>vision_msgs</depend>
<depend>ffmpeg_image_transport_msgs</depend>
<depend>tf2_ros</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
Expand Down
146 changes: 114 additions & 32 deletions depthai_bridge/src/ImageConverter.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,10 @@

#include "depthai_bridge/ImageConverter.hpp"

#include <sensor_msgs/msg/detail/compressed_image__struct.hpp>

#include "depthai-shared/datatype/RawEncodedFrame.hpp"
#include "depthai/pipeline/datatype/EncodedFrame.hpp"
#include "depthai_bridge/depthaiUtility.hpp"
#include "opencv2/calib3d.hpp"
#include "opencv2/imgcodecs.hpp"
Expand All @@ -26,71 +30,75 @@ std::unordered_map<dai::RawImgFrame::Type, std::string> ImageConverter::planarEn
{dai::RawImgFrame::Type::YUV420p, "rgb8"}};

ImageConverter::ImageConverter(bool interleaved, bool getBaseDeviceTimestamp)
: _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
_rosBaseTime = rclcpp::Clock().now();
: daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
rosBaseTime = rclcpp::Clock().now();
}

ImageConverter::ImageConverter(const std::string frameName, bool interleaved, bool getBaseDeviceTimestamp)
: _frameName(frameName), _daiInterleaved(interleaved), _steadyBaseTime(std::chrono::steady_clock::now()), _getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
_rosBaseTime = rclcpp::Clock().now();
: frameName(frameName), daiInterleaved(interleaved), steadyBaseTime(std::chrono::steady_clock::now()), getBaseDeviceTimestamp(getBaseDeviceTimestamp) {
rosBaseTime = rclcpp::Clock().now();
}

ImageConverter::~ImageConverter() = default;

void ImageConverter::updateRosBaseTime() {
updateBaseTime(_steadyBaseTime, _rosBaseTime, _totalNsChange);
updateBaseTime(steadyBaseTime, rosBaseTime, totalNsChange);
}

void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) {
_fromBitstream = true;
_srcType = srcType;
fromBitstream = true;
this->srcType = srcType;
}

void ImageConverter::convertDispToDepth(double baseline) {
_convertDispToDepth = true;
_baseline = baseline;
dispToDepth = true;
this->baseline = baseline;
}

void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) {
_expOffset = offset;
_addExpOffset = true;
expOffset = offset;
addExpOffset = true;
}

void ImageConverter::reverseStereoSocketOrder() {
_reverseStereoSocketOrder = true;
reversedStereoSocketOrder = true;
}

void ImageConverter::setAlphaScaling(double alphaScalingFactor) {
_alphaScalingEnabled = true;
_alphaScalingFactor = alphaScalingFactor;
alphaScalingEnabled = true;
this->alphaScalingFactor = alphaScalingFactor;
}

void ImageConverter::setFFMPEGEncoding(const std::string& encoding) {
ffmpegEncoding = encoding;
}

ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info) {
if(_updateRosBaseTimeOnToRosMsg) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
if(_addExpOffset)
tstamp = inData->getTimestampDevice(_expOffset);
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = inData->getTimestampDevice(expOffset);
else
tstamp = inData->getTimestampDevice();
else if(_addExpOffset)
tstamp = inData->getTimestamp(_expOffset);
else if(addExpOffset)
tstamp = inData->getTimestamp(expOffset);
else
tstamp = inData->getTimestamp();
ImageMsgs::Image outImageMsg;
StdMsgs::Header header;
header.frame_id = _frameName;
header.frame_id = frameName;

header.stamp = getFrameTime(_rosBaseTime, _steadyBaseTime, tstamp);
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);

if(_fromBitstream) {
if(fromBitstream) {
std::string encoding;
int decodeFlags;
int channels;
cv::Mat output;
switch(_srcType) {
switch(srcType) {
case dai::RawImgFrame::Type::BGR888i: {
encoding = sensor_msgs::image_encodings::BGR8;
decodeFlags = cv::IMREAD_COLOR;
Expand All @@ -110,16 +118,16 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i
break;
}
default: {
std::cout << _frameName << static_cast<int>(_srcType) << std::endl;
std::cout << frameName << static_cast<int>(srcType) << std::endl;
throw(std::runtime_error("Converted type not supported!"));
}
}

output = cv::imdecode(cv::Mat(inData->getData()), decodeFlags);

// converting disparity
if(_convertDispToDepth) {
auto factor = std::abs(_baseline * 10) * info.p[0];
if(dispToDepth) {
auto factor = std::abs(baseline * 10) * info.p[0];
cv::Mat depthOut = cv::Mat(cv::Size(output.cols, output.rows), CV_16UC1);
depthOut.forEach<uint16_t>([&output, &factor](uint16_t& pixel, const int* position) -> void {
auto disp = output.at<uint8_t>(position);
Expand Down Expand Up @@ -209,7 +217,79 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i
}
return outImageMsg;
}
std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> getOffsetTimestamp(
std::chrono::time_point<std::chrono::steady_clock, std::chrono::steady_clock::duration> ts,
CameraExposureOffset offset,
std::chrono::microseconds expTime) {
switch(offset) {
case CameraExposureOffset::START:
return ts - expTime;
case CameraExposureOffset::MIDDLE:
return ts - expTime / 2;
case CameraExposureOffset::END:
default:
return ts;
}
}

ImageMsgs::CompressedImage ImageConverter::toRosCompressedMsg(std::shared_ptr<dai::ImgFrame> inData) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestampDevice();
else if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestamp();

ImageMsgs::CompressedImage outImageMsg;
StdMsgs::Header header;
header.frame_id = frameName;
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);

outImageMsg.header = header;
outImageMsg.format = "jpeg";
outImageMsg.data = inData->getData();
return outImageMsg;
}

FFMPEGMsgs::FFMPEGPacket ImageConverter::toRosFFMPEGPacket(std::shared_ptr<dai::EncodedFrame> inData) {
if(updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(getBaseDeviceTimestamp)
if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestampDevice(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestampDevice();
else if(addExpOffset)
tstamp = getOffsetTimestamp(inData->getTimestamp(), expOffset, inData->getExposureTime());
else
tstamp = inData->getTimestamp();

FFMPEGMsgs::FFMPEGPacket outFrameMsg;
StdMsgs::Header header;
header.frame_id = frameName;
header.stamp = getFrameTime(rosBaseTime, steadyBaseTime, tstamp);
outFrameMsg.header = header;
auto ft = inData->getFrameType();

outFrameMsg.width = camWidth;
outFrameMsg.height = camHeight;
outFrameMsg.encoding = ffmpegEncoding;
outFrameMsg.pts = header.stamp.sec * 1000000000 + header.stamp.nanosec; // in nanoseconds
outFrameMsg.flags = (int)(ft == RawEncodedFrame::FrameType::I);
outFrameMsg.is_bigendian = false;
outFrameMsg.data = inData->getData();

return outFrameMsg;
}
void ImageConverter::toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs) {
auto outImageMsg = toRosMsgRawPtr(inData);
outImageMsgs.push_back(outImageMsg);
Expand All @@ -225,7 +305,7 @@ ImagePtr ImageConverter::toRosMsgPtr(std::shared_ptr<dai::ImgFrame> inData) {

void ImageConverter::toDaiMsg(const ImageMsgs::Image& inMsg, dai::ImgFrame& outData) {
std::unordered_map<dai::RawImgFrame::Type, std::string>::iterator revEncodingIter;
if(_daiInterleaved) {
if(daiInterleaved) {
revEncodingIter = std::find_if(encodingEnumMap.begin(), encodingEnumMap.end(), [&](const std::pair<dai::RawImgFrame::Type, std::string>& pair) {
return pair.second == inMsg.encoding;
});
Expand Down Expand Up @@ -352,6 +432,8 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
cameraData.height = static_cast<uint32_t>(height);
}

camWidth = cameraData.width;
camHeight = cameraData.height;
camIntrinsics = calibHandler.getCameraIntrinsics(cameraId, cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);

flatIntrinsics.resize(9);
Expand Down Expand Up @@ -382,7 +464,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
std::vector<std::vector<float>> stereoIntrinsics = calibHandler.getCameraIntrinsics(
calibHandler.getStereoRightCameraId(), cameraData.width, cameraData.height, topLeftPixelId, bottomRightPixelId);

if(_alphaScalingEnabled) {
if(alphaScalingEnabled) {
cv::Mat cameraMatrix = cv::Mat(3, 3, CV_64F);
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
Expand All @@ -391,7 +473,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
}
cv::Mat distCoefficients(distCoeffs);

cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), _alphaScalingFactor);
cv::Mat newCameraMatrix = cv::getOptimalNewCameraMatrix(cameraMatrix, distCoefficients, cv::Size(width, height), alphaScalingFactor);
// Copying the contents of newCameraMatrix to stereoIntrinsics
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 3; j++) {
Expand All @@ -411,7 +493,7 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
dai::CameraBoardSocket stereoSocketFirst = calibHandler.getStereoLeftCameraId();
dai::CameraBoardSocket stereoSocketSecond = calibHandler.getStereoRightCameraId();
double factor = 1.0;
if(_reverseStereoSocketOrder) {
if(reversedStereoSocketOrder) {
stereoSocketFirst = calibHandler.getStereoRightCameraId();
stereoSocketSecond = calibHandler.getStereoLeftCameraId();
factor = -1.0;
Expand Down Expand Up @@ -440,4 +522,4 @@ ImageMsgs::CameraInfo ImageConverter::calibrationToCameraInfo(
return cameraData;
}
} // namespace ros
} // namespace dai
} // namespace dai
Loading

0 comments on commit b897bff

Please sign in to comment.