Skip to content

Commit

Permalink
Adding a parameter for camera calibration
Browse files Browse the repository at this point in the history
Signed-off-by: CursedRock17 <[email protected]>

Revert "Initial Merging: Setting up ImageSaverNode"

This reverts commit 4534c95.

DisparityNode: replace full_dp parameter with sgbm_mode (ros-perception#945)

Previously, only the SGBM and HH modes were allowed

add invalid_depth param (ros-perception#943)

Add option to set all invalid depth pixels to a specified value, typically the maximum range.

 * Updates convertDepth parameter name and optimizes use of the parameter.
 * Updates PointCloudXYZ, PointCloudXYZI, and PointCloudXYZRGB with new invalid_depth parameter

Adding scale parameter to camera calibrator

Signed-off-by: CursedRock17 <[email protected]>
  • Loading branch information
CursedRock17 committed Feb 26, 2024
1 parent 14f604a commit 7f5d1d1
Show file tree
Hide file tree
Showing 15 changed files with 83 additions and 36 deletions.
2 changes: 1 addition & 1 deletion .gitignore
Original file line number Diff line number Diff line change
@@ -1,3 +1,3 @@
*pyc
.vscode/settings.json
.vscode/
*/doc/generated
8 changes: 7 additions & 1 deletion camera_calibration/src/camera_calibration/calibrator.py
Original file line number Diff line number Diff line change
Expand Up @@ -343,6 +343,7 @@ def __init__(self, boards, flags=0, fisheye_flags = 0, pattern=Patterns.Chessboa
self.pattern = pattern
self.br = cv_bridge.CvBridge()
self.camera_model = CAMERA_MODEL.PINHOLE
self.declare_parameter('vga_scale', 0)
# self.db is list of (parameters, image) samples for use in calibration. parameters has form
# (X, Y, size, skew) all normalized to [0,1], to keep track of what sort of samples we've taken
# and ensure enough variety.
Expand Down Expand Up @@ -535,7 +536,12 @@ def downsample_and_detect(self, img):
# Scale the input image down to ~VGA size
height = img.shape[0]
width = img.shape[1]
scale = math.sqrt( (width*height) / (640.*480.) )
vga_scale_param = self.get_parameter('vga_scale').get_parameter_value()
if vga_scale_param == 0:
scale = math.sqrt((width*height) / (640.*480.))
else:
scale = vga_scale_param

if scale > 1.0:
scrib = cv2.resize(img, (int(width / scale), int(height / scale)))
else:
Expand Down
6 changes: 6 additions & 0 deletions depth_image_proc/doc/components.rst
Original file line number Diff line number Diff line change
Expand Up @@ -116,6 +116,8 @@ Parameters
for the depth topic subscriber.
* **queue_size** (int, default: 5): Size of message queue for synchronizing
subscribed topics.
* **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
values (if 0.0 the parameter has no effect).

depth_image_proc::PointCloudXyzRadialNode
-----------------------------------------
Expand Down Expand Up @@ -165,6 +167,8 @@ Parameters
the intensity image subscriber.
* **queue_size** (int, default: 5): Size of message queue for synchronizing
subscribed topics.
* **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
values (if 0.0 the parameter has no effect).

depth_image_proc::PointCloudXyziRadialNode
------------------------------------------
Expand Down Expand Up @@ -235,6 +239,8 @@ Parameters
* **exact_sync** (bool, default: False): Whether to use exact synchronizer.
* **queue_size** (int, default: 5): Size of message queue for synchronizing
subscribed topics.
* **invalid_depth** (double, default: 0.0): Value used for replacing invalid depth
values (if 0.0 the parameter has no effect).

depth_image_proc::PointCloudXyzrgbRadialNode
--------------------------------------------
Expand Down
29 changes: 18 additions & 11 deletions depth_image_proc/include/depth_image_proc/conversions.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ namespace depth_image_proc
template<typename T>
void convertDepth(
const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const image_geometry::PinholeCameraModel & model,
double range_max = 0.0)
double invalid_depth = 0.0)
{
// Use correct principal point from calibration
float center_x = model.cx();
Expand All @@ -65,19 +65,26 @@ void convertDepth(
float constant_y = unit_scaling / model.fy();
float bad_point = std::numeric_limits<float>::quiet_NaN();

// ensure that the computation only happens in case we have a default depth
T invalid_depth_cvt = T(0);
bool use_invalid_depth = invalid_depth != 0.0;
if (use_invalid_depth) {
invalid_depth_cvt = DepthTraits<T>::fromMeters(invalid_depth);
}
sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");

const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
int row_step = depth_msg->step / sizeof(T);
for (int v = 0; v < static_cast<int>(cloud_msg->height); ++v, depth_row += row_step) {
for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) {
uint32_t row_step = depth_msg->step / sizeof(T);
for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) {
for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) {
T depth = depth_row[u];

// Missing points denoted by NaNs
if (!DepthTraits<T>::valid(depth)) {
if (range_max != 0.0) {
depth = DepthTraits<T>::fromMeters(range_max);
if (use_invalid_depth) {
depth = invalid_depth_cvt;
} else {
*iter_x = *iter_y = *iter_z = bad_point;
continue;
Expand All @@ -96,8 +103,8 @@ void convertDepth(
template<typename T>
void convertDepthRadial(
const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
cv::Mat & transform)
const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const cv::Mat & transform)
{
// Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
float bad_point = std::numeric_limits<float>::quiet_NaN();
Expand Down Expand Up @@ -129,7 +136,7 @@ void convertDepthRadial(
template<typename T>
void convertIntensity(
const sensor_msgs::msg::Image::ConstSharedPtr & intensity_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg)
const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg)
{
sensor_msgs::PointCloud2Iterator<float> iter_i(*cloud_msg, "intensity");
const T * inten_row = reinterpret_cast<const T *>(&intensity_msg->data[0]);
Expand All @@ -145,7 +152,7 @@ void convertIntensity(
// Handles RGB8, BGR8, and MONO8
void convertRgb(
const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
sensor_msgs::msg::PointCloud2::SharedPtr cloud_msg,
int red_offset, int green_offset, int blue_offset, int color_step);

cv::Mat initMatrix(cv::Mat cameraMatrix, cv::Mat distCoeffs, int width, int height, bool radial);
Expand Down
3 changes: 3 additions & 0 deletions depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,6 +67,9 @@ class PointCloudXyzNode : public rclcpp::Node
image_transport::CameraSubscriber sub_depth_;
int queue_size_;

// Parameters
double invalid_depth_;

// Publications
std::mutex connect_mutex_;
rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,9 @@ class PointCloudXyziNode : public rclcpp::Node
using Synchronizer = message_filters::Synchronizer<SyncPolicy>;
std::shared_ptr<Synchronizer> sync_;

// parameters
float invalid_depth_;

// Publications
std::mutex connect_mutex_;
rclcpp::Publisher<PointCloud>::SharedPtr pub_point_cloud_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,9 @@ class PointCloudXyzrgbNode : public rclcpp::Node
std::shared_ptr<Synchronizer> sync_;
std::shared_ptr<ExactSynchronizer> exact_sync_;

// parameters
float invalid_depth_;

// Publications
std::mutex connect_mutex_;
rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;
Expand Down
6 changes: 3 additions & 3 deletions depth_image_proc/src/conversions.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ cv::Mat initMatrix(

void convertRgb(
const sensor_msgs::msg::Image::ConstSharedPtr & rgb_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
int red_offset, int green_offset, int blue_offset, int color_step)
{
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
Expand All @@ -101,8 +101,8 @@ void convertRgb(
// force template instantiation
template void convertDepth<uint16_t>(
const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
const image_geometry::PinholeCameraModel & model,
double range_max);
double invalid_depth);

} // namespace depth_image_proc
9 changes: 6 additions & 3 deletions depth_image_proc/src/point_cloud_xyz.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,9 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
// Read parameters
queue_size_ = this->declare_parameter<int>("queue_size", 5);

// values used for invalid points for pcd conversion
invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);

// Create publisher with connect callback
rclcpp::PublisherOptions pub_options;
pub_options.event_callbacks.matched_callback =
Expand Down Expand Up @@ -94,7 +97,7 @@ void PointCloudXyzNode::depthCb(
const Image::ConstSharedPtr & depth_msg,
const CameraInfo::ConstSharedPtr & info_msg)
{
auto cloud_msg = std::make_shared<PointCloud2>();
const PointCloud2::SharedPtr cloud_msg = std::make_shared<PointCloud2>();
cloud_msg->header = depth_msg->header;
cloud_msg->height = depth_msg->height;
cloud_msg->width = depth_msg->width;
Expand All @@ -109,9 +112,9 @@ void PointCloudXyzNode::depthCb(

// Convert Depth Image to Pointcloud
if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) {
convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
} else if (depth_msg->encoding == enc::TYPE_32FC1) {
convertDepth<float>(depth_msg, cloud_msg, model_);
convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
} else {
RCLCPP_ERROR(
get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
Expand Down
7 changes: 5 additions & 2 deletions depth_image_proc/src/point_cloud_xyzi.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,9 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options)
this->declare_parameter<std::string>("image_transport", "raw");
this->declare_parameter<std::string>("depth_image_transport", "raw");

// value used for invalid points for pcd conversion
invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);

// Read parameters
int queue_size = this->declare_parameter<int>("queue_size", 5);

Expand Down Expand Up @@ -202,9 +205,9 @@ void PointCloudXyziNode::imageCb(

// Convert Depth Image to Pointcloud
if (depth_msg->encoding == enc::TYPE_16UC1) {
convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
} else if (depth_msg->encoding == enc::TYPE_32FC1) {
convertDepth<float>(depth_msg, cloud_msg, model_);
convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
} else {
RCLCPP_ERROR(
get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
Expand Down
7 changes: 5 additions & 2 deletions depth_image_proc/src/point_cloud_xyzrgb.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,9 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options)
this->declare_parameter<std::string>("image_transport", "raw");
this->declare_parameter<std::string>("depth_image_transport", "raw");

// value used for invalid points for pcd conversion
invalid_depth_ = this->declare_parameter<double>("invalid_depth", 0.0);

// Read parameters
int queue_size = this->declare_parameter<int>("queue_size", 5);
bool use_exact_sync = this->declare_parameter<bool>("exact_sync", false);
Expand Down Expand Up @@ -256,9 +259,9 @@ void PointCloudXyzrgbNode::imageCb(

// Convert Depth Image to Pointcloud
if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_16UC1) {
convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
} else if (depth_msg->encoding == sensor_msgs::image_encodings::TYPE_32FC1) {
convertDepth<float>(depth_msg, cloud_msg, model_);
convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
} else {
RCLCPP_ERROR(
get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
Expand Down
4 changes: 4 additions & 0 deletions image_pipeline/doc/changelog.rst
Original file line number Diff line number Diff line change
Expand Up @@ -38,3 +38,7 @@ There are several major change between ``Iron`` and ``Jazzy``:
``rgb/image_rect_color`` to ``rgb/image_raw`` to make clear that the
unrectified camera projection matrix is used, and for consistency with
other radial nodes.
* The boolen parameter ``full_dp`` from the DisparityNode has been deleted
and a new integer parameter ``sgbm_mode`` added to enable all the
variations of the stereo matching algorithm SGBM available from the
OpenCV library.
8 changes: 8 additions & 0 deletions stereo_image_proc/doc/components.rst
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,14 @@ Published Topics
Parameters
^^^^^^^^^^

*Disparity algorithm variant*
* **sgbm_mode** (int, default: 0): Stereo matching algorithm variation:

* SGBM (0)
* HH (1)
* SGBM_3WAY (2)
* HH4 (3)

*Disparity pre-filtering*

* **prefilter_size** (int, default: 9): Normalization window size, pixels.
Expand Down
6 changes: 3 additions & 3 deletions stereo_image_proc/launch/stereo_image_proc.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ def generate_launch_description():
'uniqueness_ratio': LaunchConfiguration('uniqueness_ratio'),
'P1': LaunchConfiguration('P1'),
'P2': LaunchConfiguration('P2'),
'full_dp': LaunchConfiguration('full_dp'),
'sgbm_mode': LaunchConfiguration('sgbm_mode'),
}],
remappings=[
('left/image_rect', [LaunchConfiguration('left_namespace'), '/image_rect']),
Expand Down Expand Up @@ -199,8 +199,8 @@ def generate_launch_description():
'(Semi-Global Block Matching only)'
),
DeclareLaunchArgument(
name='full_dp', default_value='False',
description='Run the full variant of the algorithm (Semi-Global Block Matching only)'
name='sgbm_mode', default_value='0',
description='The mode of the SGBM matcher to be used'
),
ComposableNodeContainer(
condition=LaunchConfigurationEquals('container', ''),
Expand Down
18 changes: 8 additions & 10 deletions stereo_image_proc/src/stereo_image_proc/disparity_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -258,6 +258,12 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
"Maximum allowed difference in the left-right disparity check in pixels"
" (Semi-Global Block Matching only)",
0, 0, 128, 1);
add_param_to_map(
int_params,
"sgbm_mode",
"Mode of the SGBM stereo matcher."
"",
0, 0, 3, 1);

// Describe double parameters
std::map<std::string, std::pair<double, rcl_interfaces::msg::ParameterDescriptor>> double_params;
Expand All @@ -277,17 +283,9 @@ DisparityNode::DisparityNode(const rclcpp::NodeOptions & options)
"The second parameter ccontrolling the disparity smoothness (Semi-Global Block Matching only)",
400.0, 0.0, 4000.0, 0.0);

// Describe bool parameters
std::map<std::string, std::pair<bool, rcl_interfaces::msg::ParameterDescriptor>> bool_params;
rcl_interfaces::msg::ParameterDescriptor full_dp_descriptor;
full_dp_descriptor.description =
"Run the full variant of the algorithm (Semi-Global Block Matching only)";
bool_params["full_dp"] = std::make_pair(false, full_dp_descriptor);

// Declaring parameters triggers the previously registered callback
this->declare_parameters("", int_params);
this->declare_parameters("", double_params);
this->declare_parameters("", bool_params);

// Publisher options to allow reconfigurable qos settings and connect callback
rclcpp::PublisherOptions pub_opts;
Expand Down Expand Up @@ -424,8 +422,8 @@ rcl_interfaces::msg::SetParametersResult DisparityNode::parameterSetCb(
block_matcher_.setSpeckleSize(param.as_int());
} else if ("speckle_range" == param_name) {
block_matcher_.setSpeckleRange(param.as_int());
} else if ("full_dp" == param_name) {
block_matcher_.setSgbmMode(param.as_bool());
} else if ("sgbm_mode" == param_name) {
block_matcher_.setSgbmMode(param.as_int());
} else if ("P1" == param_name) {
block_matcher_.setP1(param.as_double());
} else if ("P2" == param_name) {
Expand Down

0 comments on commit 7f5d1d1

Please sign in to comment.