Skip to content

Commit 5abc73f

Browse files
andrewjongandrewjong
committed
Make DROAN work with smaller 480x300 images
1 parent 6ace1df commit 5abc73f

File tree

4 files changed

+38
-26
lines changed

4 files changed

+38
-26
lines changed

‎robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/config/disparity_expansion_params.yaml

Lines changed: 3 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,12 @@
11
/**:
22
ros__parameters:
33
# parameters
4-
scale: 1.0 # units size of a meter. depth value corresponds to this many meters
5-
downsample_scale: 1.0 # the ratio to scale down the disparity image before processing
4+
metric_depth_scale: 1.0 # units size of a meter. depth value corresponds to this many meters
5+
downsample_scale: 0.5 # the ratio to scale down the disparity image before processing. DROAN expects 960x600. So if the incoming image is 480x300, set this to 0.5 so that it scales up
66
baseline_fallback: 0.12 # if the baseline is 0 from the camera_info, use this value instead
77
lut_max_disparity: 180 # maximum disparity value in the disparity image
88
# expansion_radius: 0.325 # 0.325 is the spirit drone blade to blade
99
expansion_radius: 0.1 # debug hack to make disparity expansion not crash
1010
bg_multiplier: 1.0
1111
# sensor_pixel_error: 0.5 # what is this? and why was it 0.5?
12-
sensor_pixel_error: 0.0
13-
14-
12+
sensor_pixel_error: 0.0

‎robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/include/disparity_expansion/disparity_expansion.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ class DisparityExpansionNode : public rclcpp::Node {
6262
double padding;
6363
double bg_multiplier;
6464
double pixel_error;
65-
double scale;
65+
double metric_depth_scale;
6666
std::vector<std::vector<LUTCell>> table_u;
6767
std::vector<std::vector<LUTCell>> table_v;
6868
std::vector<double> table_d;

‎robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/src/disparity_expansion.cpp

Lines changed: 33 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -10,8 +10,8 @@
1010

1111
DisparityExpansionNode::DisparityExpansionNode(const rclcpp::NodeOptions& options)
1212
: Node("DisparityExpansionNode", options) {
13-
this->declare_parameter("scale", 2.0);
14-
this->get_parameter("scale", this->scale);
13+
this->declare_parameter("metric_depth_scale", 2.0);
14+
this->get_parameter("metric_depth_scale", this->metric_depth_scale);
1515
this->declare_parameter("expansion_radius", 2.0);
1616
this->get_parameter("expansion_radius", this->expansion_radius);
1717
this->declare_parameter("lut_max_disparity", 164);
@@ -73,7 +73,7 @@ void DisparityExpansionNode::set_cam_info(
7373
"Baseline from camera_info was 0, setting to this->baseline_fallback:"
7474
<< this->baseline);
7575
}
76-
this->baseline *= this->downsample_scale;
76+
// this->baseline *= this->downsample_scale;
7777
this->generate_expansion_lookup_table();
7878
this->got_cam_info = true;
7979
RCLCPP_INFO(this->get_logger(), "Baseline: %.4f meters", this->baseline);
@@ -98,8 +98,8 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
9898
double disparity;
9999

100100
for (unsigned int disp_idx = 1; disp_idx < lut_max_disparity; disp_idx++) {
101-
disparity = disp_idx / this->scale; // 1 cell = 0.5m, z is in meters
102-
r = this->expansion_radius; // * exp(DEPTH_ERROR_COEFF*z);
101+
disparity = disp_idx / this->metric_depth_scale; // 1 cell = 0.5m, z is in meters
102+
r = this->expansion_radius; // * exp(DEPTH_ERROR_COEFF*z);
103103
z = this->baseline * this->fx / disparity;
104104

105105
double disp_new = this->baseline * this->fx / (z - this->expansion_radius) + 0.5;
@@ -117,7 +117,11 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
117117
v2 = this->fy * r2 / z + this->cy;
118118

119119
if ((v2 - v1) < 0)
120-
RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", v1, v2, disp_idx);
120+
RCLCPP_ERROR(this->get_logger(),
121+
"Something MESSED disp_idx=%d v=%d cy=%f fy=%f r=%f x=%f y=%f z=%f "
122+
"beta=%f beta1=%f r1=%f r2=%f v1=%d v2=%d",
123+
disp_idx, v, this->cy, this->fy, r, x, y, z, beta, beta1, r1, r2, v1,
124+
v2);
121125

122126
if (v1 < 0) v1 = 0;
123127
if (v1 > (height - 1)) v1 = height - 1;
@@ -141,7 +145,11 @@ void DisparityExpansionNode::generate_expansion_lookup_table() {
141145
u2 = this->fx * r2 / z + this->cx;
142146

143147
if ((u2 - u1) < 0)
144-
RCLCPP_ERROR(this->get_logger(), "Something MESSED %d %d %d", u1, u2, disp_idx);
148+
RCLCPP_ERROR(this->get_logger(),
149+
"Something MESSED disp_idx=%d u=%d cx=%f fx=%f r=%f x=%f y=%f z=%f "
150+
"alpha=%f alpha1=%f r1=%f r2=%f u1=%d u2=%d",
151+
disp_idx, u, this->cx, this->fx, r, x, y, z, alpha, alpha1, r1, r2, u1,
152+
u2);
145153

146154
if (u1 < 0) u1 = 0;
147155
if (u1 > (this->width - 1)) u1 = this->width - 1;
@@ -281,19 +289,22 @@ void DisparityExpansionNode::process_disparity_image(
281289
for (int u = (int)this->width - 1; u >= 0; u -= 1) {
282290
float disparity_value = disparity32F.at<float>(v, u);
283291

284-
if (std::isnan(double(disparity_value * this->scale)) ||
285-
((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) ||
286-
((int(disparity_value * this->scale) + 1) <= 0)) {
292+
if (std::isnan(double(disparity_value * this->metric_depth_scale)) ||
293+
((int(disparity_value * this->metric_depth_scale) + 1) >=
294+
this->lut_max_disparity) ||
295+
((int(disparity_value * this->metric_depth_scale) + 1) <= 0)) {
287296
RCLCPP_INFO_STREAM_THROTTLE(
288297
this->get_logger(), *this->get_clock(), 5000,
289298
"Step 1 Expand u: Disparity out of range: "
290299
<< disparity_value << ", lut_max_disparity: " << this->lut_max_disparity
291-
<< " Scale: " << this->scale);
300+
<< " metric_depth_scale: " << this->metric_depth_scale);
292301
continue;
293302
}
294303

295-
unsigned int u1 = this->table_u.at(int(disparity_value * this->scale) + 1).at(u).idx1;
296-
unsigned int u2 = this->table_u.at(int(disparity_value * this->scale) + 1).at(u).idx2;
304+
unsigned int u1 =
305+
this->table_u.at(int(disparity_value * this->metric_depth_scale) + 1).at(u).idx1;
306+
unsigned int u2 =
307+
this->table_u.at(int(disparity_value * this->metric_depth_scale) + 1).at(u).idx2;
297308

298309
if (disparity32F.empty()) {
299310
RCLCPP_ERROR(this->get_logger(), "disparity32F matrix is empty.");
@@ -365,19 +376,22 @@ void DisparityExpansionNode::process_disparity_image(
365376
float disparity_value = disparity32F.at<float>(v, u) +
366377
this->pixel_error; // disparity_row[v * row_step + u];// + 0.5;
367378

368-
if (std::isnan(double(disparity_value * this->scale)) ||
369-
((int(disparity_value * this->scale) + 1) >= this->lut_max_disparity) ||
370-
((int(disparity_value * this->scale) + 1) <= 0)) {
379+
if (std::isnan(double(disparity_value * this->metric_depth_scale)) ||
380+
((int(disparity_value * this->metric_depth_scale) + 1) >=
381+
this->lut_max_disparity) ||
382+
((int(disparity_value * this->metric_depth_scale) + 1) <= 0)) {
371383
RCLCPP_INFO_STREAM_THROTTLE(
372384
this->get_logger(), *this->get_clock(), 5000,
373385
"Step 2 Expand v: Disparity out of range: "
374386
<< disparity_value << ", lut_max_disparity: " << this->lut_max_disparity
375-
<< " Scale: " << this->scale);
387+
<< " metric_depth_scale: " << this->metric_depth_scale);
376388
continue;
377389
}
378390

379-
unsigned int v1 = this->table_v.at(int(disparity_value * this->scale) + 1).at(v).idx1;
380-
unsigned int v2 = this->table_v.at(int(disparity_value * this->scale) + 1).at(v).idx2;
391+
unsigned int v1 =
392+
this->table_v.at(int(disparity_value * this->metric_depth_scale) + 1).at(v).idx1;
393+
unsigned int v2 =
394+
this->table_v.at(int(disparity_value * this->metric_depth_scale) + 1).at(v).idx2;
381395

382396
cv::Rect roi = cv::Rect(u, v1, 1, (v2 - v1));
383397

‎robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@
3636
<remap from="look_ahead" to="/$(env ROBOT_NAME)/trajectory_controller/look_ahead" />
3737
<remap from="tracking_point"
3838
to="/$(env ROBOT_NAME)/trajectory_controller/tracking_point" />
39-
<remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/left/camera_info" />
39+
<remap from="camera_info" to="/$(env ROBOT_NAME)/sensors/front_stereo/right/camera_info" />
4040
<remap from="trajectory_segment_to_add" to="/$(env ROBOT_NAME)/trajectory_controller/trajectory_segment_to_add" />
4141
</node>
4242
<!-- DROAN local model -->

0 commit comments

Comments
 (0)