Skip to content

fix disparity to depth conversion #743

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
KmakD wants to merge 1 commit into
base: jazzy
Choose a base branch
from
+10 −5
Open

fix disparity to depth conversion #743

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion depthai_bridge/include/depthai_bridge/ImageConverter.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -71,8 +71,9 @@ class ImageConverter {
/**
* @brief Sets converter behavior to convert from disparity to depth when converting messages from bitstream.
* @param baseline: The baseline of the stereo pair.
* @param focalLength: The focal length of the camera.
*/
void convertDispToDepth(double baseline);
void convertDispToDepth(double baseline, double focalLength);

/**
* @brief Reverses the order of the stereo sockets when creating CameraInfo to calculate Tx component of Projection matrix.
Expand Down Expand Up @@ -140,6 +141,7 @@ class ImageConverter {
dai::CameraExposureOffset expOffset;
bool reversedStereoSocketOrder = false;
double baseline;
double focalLength;
double alphaScalingFactor = 0.0;
int camHeight = -1;
int camWidth = -1;
Expand Down
5 changes: 3 additions & 2 deletions depthai_bridge/src/ImageConverter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -50,9 +50,10 @@ void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) {
this->srcType = srcType;
}

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

void ImageConverter::addExposureOffset(dai::CameraExposureOffset& offset) {
Expand Down Expand Up @@ -133,7 +134,7 @@ ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> i

// converting disparity
if(dispToDepth) {
auto factor = std::abs(baseline * 10) * info.p[0];
auto factor = std::abs(baseline * 10) * focalLength;
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
6 changes: 4 additions & 2 deletions depthai_ros_driver/src/dai_nodes/sensors/img_pub.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -92,10 +92,11 @@ void ImagePublisher::createImageConverter(std::shared_ptr<dai::Device> device) {
try {
auto calHandler = device->readCalibration();
double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false);
double focalLength = calHandler.getCameraIntrinsics(pubConfig.leftSocket).at(0).at(0);
if(convConfig.reverseSocketOrder) {
baseline = calHandler.getBaselineDistance(pubConfig.rightSocket, pubConfig.leftSocket, false);
}
converter->convertDispToDepth(baseline);
converter->convertDispToDepth(baseline, focalLength);
} catch(const std::exception& e) {
RCLCPP_DEBUG(node->get_logger(), "Failed to convert disparity to depth: %s", e.what());
}
Expand All @@ -113,10 +114,11 @@ void ImagePublisher::createImageConverter(std::shared_ptr<dai::Device> device) {
if(convConfig.isStereo && !convConfig.outputDisparity) {
auto calHandler = device->readCalibration();
double baseline = calHandler.getBaselineDistance(pubConfig.leftSocket, pubConfig.rightSocket, false);
double focalLength = calHandler.getCameraIntrinsics(pubConfig.leftSocket).at(0).at(0);
if(convConfig.reverseSocketOrder) {
baseline = calHandler.getBaselineDistance(pubConfig.rightSocket, pubConfig.leftSocket, false);
}
converter->convertDispToDepth(baseline);
converter->convertDispToDepth(baseline, focalLength);
}
converter->setFFMPEGEncoding(convConfig.ffmpegEncoder);
}
Expand Down