Skip to content

Commit

Permalink
Add exposure offset (#382)
Browse files Browse the repository at this point in the history
  • Loading branch information
@Serafadam
Serafadam authored Sep 1, 2023
1 parent eca7bae commit e544cb9
Show file tree
Hide file tree
Showing 13 changed files with 106 additions and 106 deletions.
4 changes: 4 additions & 0 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,10 @@ When setting `stereo.i_align_depth: true`, stereo output is aligned to board soc

You can enable rectified Stereo streams by setting, for example in the case of right stream `i_publish_right_rect: true`. You can also set `i_publish_synced_rect_pair: true` to get both images with the same timestamps.

##### Custom Sensor sockets

Configuration of which sensors are used for computing stereo pair can be done either programatically, by specifying them in a constructor of a Stereo node (for example when building a custom pipeline), or via parameters - `stereo.i_left_socket_id`/`stereo.i_right_socket_id`. Please note that currently if you want to use rgb/center socket instead of one of the given pairs you will need to build a custom pipeline for that.

#### Feature Tracker

Each sensor node (and rectified streams from Stereo node) has the option to add FeatureTracker node, which publishes `depthai_ros_msgs/msg/TrackedFeatures` messages.
Expand Down
3 changes: 3 additions & 0 deletions depthai_bridge/include/depthai_bridge/ImageConverter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,6 +56,7 @@ class ImageConverter {
}

void convertFromBitstream(dai::RawImgFrame::Type srcType);
void addExposureOffset(dai::CameraExposureOffset& offset);
void convertDispToDepth();

void toRosMsg(std::shared_ptr<dai::ImgFrame> inData, std::deque<ImageMsgs::Image>& outImageMsgs);
Expand Down Expand Up @@ -97,6 +98,8 @@ class ImageConverter {
dai::RawImgFrame::Type _srcType;
bool _fromBitstream = false;
bool _convertDispToDepth = false;
bool _addExpOffset = false;
dai::CameraExposureOffset _expOffset;
};

} // namespace ros
Expand Down
13 changes: 11 additions & 2 deletions depthai_bridge/src/ImageConverter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,20 +43,29 @@ void ImageConverter::updateRosBaseTime() {
void ImageConverter::convertFromBitstream(dai::RawImgFrame::Type srcType) {
_fromBitstream = true;
_srcType = srcType;
std::cout << static_cast<int>(_srcType) << std::endl;
}

void ImageConverter::convertDispToDepth() {
_convertDispToDepth = true;
}

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

ImageMsgs::Image ImageConverter::toRosMsgRawPtr(std::shared_ptr<dai::ImgFrame> inData, const sensor_msgs::msg::CameraInfo& info) {
if(_updateRosBaseTimeOnToRosMsg) {
updateRosBaseTime();
}
std::chrono::_V2::steady_clock::time_point tstamp;
if(_getBaseDeviceTimestamp)
tstamp = inData->getTimestampDevice();
if(_addExpOffset)
tstamp = inData->getTimestampDevice(_expOffset);
else
tstamp = inData->getTimestampDevice();
else if(_addExpOffset)
tstamp = inData->getTimestamp(_expOffset);
else
tstamp = inData->getTimestamp();
ImageMsgs::Image outImageMsg;
Expand Down
2 changes: 1 addition & 1 deletion depthai_bridge/src/TFPublisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,7 @@ std::string TFPublisher::getCamSocketName(int socketNum) {
std::string name;
for(auto& cam : _camFeatures) {
if(cam.socket == static_cast<dai::CameraBoardSocket>(socketNum)) {
if(cam.name == "color") {
if(cam.name == "color" || cam.name == "center") {
name = "rgb";
} else {
name = cam.name;
Expand Down
4 changes: 3 additions & 1 deletion depthai_ros_driver/include/depthai_ros_driver/dai_nodes/sensors/sensor_helpers.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,12 +38,14 @@ enum class RGBLinkType { video, isp, preview };
namespace sensor_helpers {
struct ImageSensor {
std::string name;
std::string defaultResolution;
std::vector<std::string> allowedResolutions;
bool color;
void getSizeFromResolution(const dai::ColorCameraProperties::SensorResolution& res, int& width, int& height);
};
extern std::vector<ImageSensor> availableSensors;

extern std::unordered_map<std::string, dai::ColorCameraProperties::SensorResolution> stringToColorResMap;

void basicCameraPub(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
dai::ros::ImageConverter& converter,
Expand Down
1 change: 1 addition & 0 deletions depthai_ros_driver/include/depthai_ros_driver/param_handlers/stereo_param_handler.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ class StereoParamHandler : public BaseParamHandler {
~StereoParamHandler();
void declareParams(std::shared_ptr<dai::node::StereoDepth> stereo, const std::vector<dai::CameraFeatures>& camFeatures);
dai::CameraControl setRuntimeParams(const std::vector<rclcpp::Parameter>& params) override;
void updateSocketsFromParams(dai::CameraBoardSocket& left, dai::CameraBoardSocket& right);

private:
std::unordered_map<std::string, dai::node::StereoDepth::PresetMode> depthPresetMap;
Expand Down
4 changes: 4 additions & 0 deletions depthai_ros_driver/src/dai_nodes/sensors/mono.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,10 @@ void Mono::setupQueues(std::shared_ptr<dai::Device> device) {
if(ph->getParam<bool>("i_low_bandwidth")) {
imageConverter->convertFromBitstream(dai::RawImgFrame::Type::GRAY8);
}
if(ph->getParam<bool>("i_add_exposure_offset")) {
auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
imageConverter->addExposureOffset(offset);
}
infoManager = std::make_shared<camera_info_manager::CameraInfoManager>(
getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName());
if(ph->getParam<std::string>("i_calibration_file").empty()) {
Expand Down
8 changes: 6 additions & 2 deletions depthai_ros_driver/src/dai_nodes/sensors/rgb.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ RGB::RGB(const std::string& daiNodeName,
rclcpp::Node* node,
std::shared_ptr<dai::Pipeline> pipeline,
dai::CameraBoardSocket socket = dai::CameraBoardSocket::CAM_A,
sensor_helpers::ImageSensor sensor = {"IMX378", {"12mp", "4k"}, true},
sensor_helpers::ImageSensor sensor = {"IMX378", "4k", {"12mp", "4k"}, true},
bool publish = true)
: BaseNode(daiNodeName, node, pipeline) {
RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str());
Expand Down Expand Up @@ -77,9 +77,13 @@ void RGB::setupQueues(std::shared_ptr<dai::Device> device) {
std::make_unique<dai::ros::ImageConverter>(tfPrefix + "_camera_optical_frame", false, ph->getParam<bool>("i_get_base_device_timestamp"));
imageConverter->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
if(ph->getParam<bool>("i_low_bandwidth")) {
RCLCPP_INFO(getROSNode()->get_logger(), "Low bandwidth mode enabled");
imageConverter->convertFromBitstream(dai::RawImgFrame::Type::BGR888i);
}
if(ph->getParam<bool>("i_add_exposure_offset")) {
auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
imageConverter->addExposureOffset(offset);
}

if(ph->getParam<std::string>("i_calibration_file").empty()) {
infoManager->setCameraInfo(sensor_helpers::getCalibInfo(getROSNode()->get_logger(),
*imageConverter,
Expand Down
96 changes: 14 additions & 82 deletions depthai_ros_driver/src/dai_nodes/sensors/sensor_helpers.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,88 +9,20 @@
namespace depthai_ros_driver {
namespace dai_nodes {
namespace sensor_helpers {
void ImageSensor::getSizeFromResolution(const dai::ColorCameraProperties::SensorResolution& res, int& width, int& height) {
switch(res) {
case dai::ColorCameraProperties::SensorResolution::THE_720_P: {
width = 1280;
height = 720;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_800_P: {
width = 1280;
height = 800;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_1080_P: {
width = 1920;
height = 1080;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_4_K: {
width = 3840;
height = 2160;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_12_MP: {
width = 4056;
height = 3040;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_1200_P: {
width = 1920;
height = 1200;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_5_MP: {
width = 2592;
height = 1944;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_13_MP: {
width = 4208;
height = 3120;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_4000X3000: {
width = 4000;
height = 3000;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_5312X6000: {
width = 5312;
height = 6000;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_48_MP: {
width = 8000;
height = 6000;
break;
}
case dai::ColorCameraProperties::SensorResolution::THE_1440X1080: {
width = 1440;
height = 1080;
break;
}
default: {
throw std::runtime_error("Resolution not supported!");
}
}
}
std::vector<ImageSensor> availableSensors{
{"IMX378", {"12mp", "4k"}, true},
{"OV9282", {"800P", "720p", "400p"}, false},
{"OV9782", {"800P", "720p", "400p"}, true},
{"OV9281", {"800P", "720p", "400p"}, true},
{"IMX214", {"13mp", "12mp", "4k", "1080p"}, true},
{"IMX412", {"13mp", "12mp", "4k", "1080p"}, true},
{"OV7750", {"480P", "400p"}, false},
{"OV7251", {"480P", "400p"}, false},
{"IMX477", {"12mp", "4k", "1080p"}, true},
{"IMX577", {"12mp", "4k", "1080p"}, true},
{"AR0234", {"1200P"}, true},
{"IMX582", {"48mp", "12mp", "4k"}, true},
{"LCM48", {"48mp", "12mp", "4k"}, true},
};

std::vector<ImageSensor> availableSensors{{"IMX378", "4k", {"12mp", "4k"}, true},
{"OV9282", "800p", {"800p", "720p", "400p"}, false},
{"OV9782", "800p", {"800p", "720p", "400p"}, true},
{"OV9281", "800p", {"800p", "720p", "400p"}, true},
{"IMX214", "1080p", {"13mp", "12mp", "4k", "1080p"}, true},
{"IMX412", "1080p", {"13mp", "12mp", "4k", "1080p"}, true},
{"OV7750", "480p", {"480p", "400p"}, false},
{"OV7251", "480p", {"480p", "400p"}, false},
{"IMX477", "1080p", {"12mp", "4k", "1080p"}, true},
{"IMX577", "1080p", {"12mp", "4k", "1080p"}, true},
{"AR0234", "1200p", {"1200p"}, true},
{"IMX582", "4k", {"48mp", "12mp", "4k"}, true},
{"LCM48", "4k", {"48mp", "12mp", "4k"}, true}};

void basicCameraPub(const std::string& /*name*/,
const std::shared_ptr<dai::ADatatype>& data,
Expand Down
15 changes: 11 additions & 4 deletions depthai_ros_driver/src/dai_nodes/stereo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,8 @@ Stereo::Stereo(const std::string& daiNodeName,
: BaseNode(daiNodeName, node, pipeline) {
RCLCPP_DEBUG(node->get_logger(), "Creating node %s", daiNodeName.c_str());
setNames();
ph = std::make_unique<param_handlers::StereoParamHandler>(node, daiNodeName);
ph->updateSocketsFromParams(leftSocket, rightSocket);
auto features = device->getConnectedCameraFeatures();
for(auto f : features) {
if(f.socket == leftSocket) {
Expand All @@ -41,7 +43,6 @@ Stereo::Stereo(const std::string& daiNodeName,
stereoCamNode = pipeline->create<dai::node::StereoDepth>();
left = std::make_unique<SensorWrapper>(leftSensInfo.name, node, pipeline, device, leftSensInfo.socket, false);
right = std::make_unique<SensorWrapper>(rightSensInfo.name, node, pipeline, device, rightSensInfo.socket, false);
ph = std::make_unique<param_handlers::StereoParamHandler>(node, daiNodeName);
ph->declareParams(stereoCamNode, features);
setXinXout(pipeline);
left->link(stereoCamNode->left);
Expand Down Expand Up @@ -124,7 +125,11 @@ void Stereo::setupRectQueue(std::shared_ptr<dai::Device> device,
if(lowBandwidth) {
conv->convertFromBitstream(dai::RawImgFrame::Type::GRAY8);
}

bool addOffset = ph->getParam<bool>(isLeft ? "i_left_rect_add_exposure_offset" : "i_right_rect_add_exposure_offset");
if(addOffset) {
auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>(isLeft ? "i_left_rect_exposure_offset" : "i_right_rect_exposure_offset"));
conv->addExposureOffset(offset);
}
im = std::make_shared<camera_info_manager::CameraInfoManager>(
getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + sensorInfo.name).get(), "/rect");

Expand Down Expand Up @@ -186,11 +191,13 @@ void Stereo::setupStereoQueue(std::shared_ptr<dai::Device> device) {
stereoConv->setUpdateRosBaseTimeOnToRosMsg(ph->getParam<bool>("i_update_ros_base_time_on_ros_msg"));
if(ph->getParam<bool>("i_low_bandwidth")) {
stereoConv->convertFromBitstream(dai::RawImgFrame::Type::RAW8);
RCLCPP_INFO(getROSNode()->get_logger(), "Setting stereo low bandwidth mode");
}
if(!ph->getParam<bool>("i_output_disparity")) {
stereoConv->convertDispToDepth();
RCLCPP_INFO(getROSNode()->get_logger(), "Setting stereo output to depth");
}
if(ph->getParam<bool>("i_add_exposure_offset")) {
auto offset = static_cast<dai::CameraExposureOffset>(ph->getParam<int>("i_exposure_offset"));
stereoConv->addExposureOffset(offset);
}
stereoIM = std::make_shared<camera_info_manager::CameraInfoManager>(
getROSNode()->create_sub_node(std::string(getROSNode()->get_name()) + "/" + getName()).get(), "/" + getName());
Expand Down
37 changes: 25 additions & 12 deletions depthai_ros_driver/src/param_handlers/sensor_param_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@ void SensorParamHandler::declareCommonParams() {
declareAndLogParam<bool>("i_update_ros_base_time_on_ros_msg", false);
declareAndLogParam<bool>("i_enable_feature_tracker", false);
declareAndLogParam<bool>("i_enable_lazy_publisher", true);
declareAndLogParam<bool>("i_add_exposure_offset", false);
declareAndLogParam<int>("i_exposure_offset", 0);
fSyncModeMap = {
{"OFF", dai::CameraControl::FrameSyncMode::OFF},
{"OUTPUT", dai::CameraControl::FrameSyncMode::OUTPUT},
Expand Down Expand Up @@ -82,17 +84,17 @@ void SensorParamHandler::declareParams(std::shared_ptr<dai::node::ColorCamera> c
dai::CameraBoardSocket socket,
dai_nodes::sensor_helpers::ImageSensor sensor,
bool publish) {
rgbResolutionMap = {{"720", dai::ColorCameraProperties::SensorResolution::THE_720_P},
{"1080", dai::ColorCameraProperties::SensorResolution::THE_1080_P},
{"4K", dai::ColorCameraProperties::SensorResolution::THE_4_K},
{"12MP", dai::ColorCameraProperties::SensorResolution::THE_12_MP},
{"13MP", dai::ColorCameraProperties::SensorResolution::THE_13_MP},
{"800", dai::ColorCameraProperties::SensorResolution::THE_800_P},
{"1200", dai::ColorCameraProperties::SensorResolution::THE_1200_P},
rgbResolutionMap = {{"720p", dai::ColorCameraProperties::SensorResolution::THE_720_P},
{"1080p", dai::ColorCameraProperties::SensorResolution::THE_1080_P},
{"4k", dai::ColorCameraProperties::SensorResolution::THE_4_K},
{"12mp", dai::ColorCameraProperties::SensorResolution::THE_12_MP},
{"13mp", dai::ColorCameraProperties::SensorResolution::THE_13_MP},
{"800p", dai::ColorCameraProperties::SensorResolution::THE_800_P},
{"1200p", dai::ColorCameraProperties::SensorResolution::THE_1200_P},
{"5MP", dai::ColorCameraProperties::SensorResolution::THE_5_MP},
{"4000x3000", dai::ColorCameraProperties::SensorResolution::THE_4000X3000},
{"5312X6000", dai::ColorCameraProperties::SensorResolution::THE_5312X6000},
{"48_MP", dai::ColorCameraProperties::SensorResolution::THE_48_MP},
{"48mp", dai::ColorCameraProperties::SensorResolution::THE_48_MP},
{"1440X1080", dai::ColorCameraProperties::SensorResolution::THE_1440X1080}};
declareAndLogParam<bool>("i_publish_topic", publish);
declareAndLogParam<int>("i_board_socket_id", static_cast<int>(socket));
Expand All @@ -104,10 +106,21 @@ void SensorParamHandler::declareParams(std::shared_ptr<dai::node::ColorCamera> c
int preview_width = declareAndLogParam<int>("i_preview_width", preview_size);
int preview_height = declareAndLogParam<int>("i_preview_height", preview_size);
colorCam->setPreviewSize(preview_width, preview_height);
auto resolution = utils::getValFromMap(declareAndLogParam<std::string>("i_resolution", "1080"), rgbResolutionMap);
int width, height;
colorCam->setResolution(resolution);
sensor.getSizeFromResolution(colorCam->getResolution(), width, height);
auto resString = declareAndLogParam<std::string>("i_resolution", sensor.defaultResolution);

// if resolution not in allowed resolutions, use default
if(std::find(sensor.allowedResolutions.begin(), sensor.allowedResolutions.end(), resString) == sensor.allowedResolutions.end()) {
RCLCPP_WARN(getROSNode()->get_logger(),
"Resolution %s not supported by sensor %s. Using default resolution %s",
resString.c_str(),
sensor.name.c_str(),
sensor.defaultResolution.c_str());
resString = sensor.defaultResolution;
}

int width = colorCam->getResolutionWidth();
int height = colorCam->getResolutionHeight();
colorCam->setResolution(utils::getValFromMap(resString, rgbResolutionMap));

colorCam->setInterleaved(declareAndLogParam<bool>("i_interleaved", false));
if(declareAndLogParam<bool>("i_set_isp_scale", true)) {
Expand Down
Loading

0 comments on commit e544cb9

Please sign in to comment.