Skip to content

Commit

Permalink
Support radial fisheye colmap distortion. (#801)
Browse files Browse the repository at this point in the history
  • Loading branch information
bcoltin authored Jul 31, 2024
1 parent c4b509c commit c1d2d76
Show file tree
Hide file tree
Showing 2 changed files with 169 additions and 2 deletions.
136 changes: 136 additions & 0 deletions astrobee/config/robots/colmap_bumble.config
Original file line number Diff line number Diff line change
@@ -0,0 +1,136 @@
-- Copyright (c) 2017, United States Government, as represented by the
-- Administrator of the National Aeronautics and Space Administration.
--
-- All rights reserved.
--
-- The Astrobee platform is licensed under the Apache License, Version 2.0
-- (the "License"); you may not use this file except in compliance with the
-- License. You may obtain a copy of the License at
--
-- http://www.apache.org/licenses/LICENSE-2.0
--
-- Unless required by applicable law or agreed to in writing, software
-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
-- License for the specific language governing permissions and limitations
-- under the License.

-- bumble config

robot_llp_address = "10.42.0.8"
robot_mlp_address = "10.42.0.9"

robot_i2c_bus = "/dev/i2c-1"

robot_imu_drdy_pin = 4

robot_geometry = {
nav_cam_to_haz_cam_transform = transform(vec3(0.072789474, 0.0010512418, -0.048805003), quat4(-0.00014444836, 0.020741299, 0.99976044, 0.006988339)),
nav_cam_to_sci_cam_transform = transform(vec3(-0.035350038, 0.019131216, 0.0083921009), quat4(0.0007718104, -0.0043221056, -0.0069533111, 0.99996619)),
haz_cam_depth_to_image_transform = {
0.91037857, 8.4134405e-05, 0.0050848035, 0.0020508297,
-0.0003594422, 0.9113776, 0.023508033, -0.0011003268,
-0.0044235394, -0.018620972, 0.88773717, -0.028333293,
0, 0, 0, 1},

-- Engineering positions with idealized orientations
perch_cam_transform = transform(vec3(-0.1331, 0.0509, -0.0166), quat4(0.000, -0.70710678118, 0.000, 0.70710678118)),-- placeholder, not valid!
haz_cam_transform = transform(vec3(0.1328, 0.0362, -0.0826), quat4(-0.500, 0.500, -0.500, 0.500)), -- placeholder, not valid!
nav_cam_transform = transform(vec3(0.1157+0.002, -0.0422, -0.0826), quat4(0.500, 0.500, 0.500, 0.500) ),
dock_cam_transform = transform(vec3(-0.1032-0.0029, -0.0540, -0.0064), quat4(0.500, -0.500, -0.500, 0.500) ),
--imu_transform = transform(vec3(0.0247, 0.0183, 0.0094), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ),
imu_transform = transform(vec3(0.0386, 0.0247, -0.01016), quat4(0.000, 0.000, 0.70710678118, 0.70710678118) ),
-- Not accurate, only for sim purposes
sci_cam_transform = transform(vec3(0.118, 0.0, -0.096), quat4(0.500, 0.500, 0.500, 0.500) )
};

robot_camera_calibrations = {
-- calibrated with colmap from ISAAC15 USL data in July 2024
nav_cam = {
distortion_coeff = {-0.046076881777018734, -0.0045533546413852235},
intrinsic_matrix = {
666.6402966821737, 0.0, 633.87549809563473,
0.0, 666.6402966821737, 540.87895986772969,
0.0, 0.0, 1.0
},
gain=50,
exposure=175
},
dock_cam = {
distortion_coeff = 1.00762,
intrinsic_matrix = {
753.51021, 0.0, 631.11512,
0.0, 751.3611, 508.69621,
0.0, 0.0, 1.0
},
gain=50,
exposure=150
},
perch_cam = {
distortion_coeff = {-0.366735, 0.182027, 0.00218105, 0.0114682},
intrinsic_matrix = {
209.21199, 0.0, 94.688486,
0.0, 207.62067, 84.04047,
0.0, 0.0, 1.0
},
gain=100,
exposure=150
},
haz_cam = {
distortion_coeff = {-0.259498, -0.08484934, 0.0032980311, -0.00024045673},
intrinsic_matrix = {
206.19095, 0.0, 112.48999,
0.0, 206.19095, 81.216598,
0.0, 0.0, 1.0
},
gain=50,
exposure=150
},
sci_cam = {
distortion_coeff = {-0.025598438, 0.048258987, -0.00041380657, 0.0056673533},
intrinsic_matrix = {
1023.6054, 0.0, 683.97547,
0.0, 1023.6054, 511.2185,
0.0, 0.0, 1.0
},
gain=50,
exposure=150
},
nav_cam_to_haz_cam_timestamp_offset = -0.02,
nav_cam_to_sci_cam_timestamp_offset = 0.08
}

-- PMC bus ordering and i2c trims {stbd, port}
robot_pmc_i2c_addrs = {0x21, 0x20}
robot_stbd_nozzle_calibration = {
{0, 0, 0, 0, 0, 0},
{0, 0, 0, 0, 0, 0}
}
robot_port_nozzle_calibration = {
{0, 0, 0, 0, 0, 0},
{5, 0, 5, 5, 0, 0}
}

robot_haz_cam_device = "0005-1207-0034-0713"
robot_perch_cam_device = "0005-1209-0034-1304"

-- Location of the trackers in the body frame
robot_vive_extrinsics = {
{
-- port
serial = "LHR-08DE963B", pose = transform(
vec3(0, -0.1397, -0.1397),
quat4(1.0, 0.0, 0.0, 0.0))
},{
-- starboard
serial = "LHR-1FC0DEF4", pose = transform(
vec3(0, 0.1397, -0.1397),
quat4(0.0, 1.0, 0.0, 0.0))
}
}

agent_name = "Bumble"

heartbeat_queue_size = 20

nodes_not_running = {}
35 changes: 33 additions & 2 deletions localization/camera/src/camera_params.cc
Original file line number Diff line number Diff line change
Expand Up @@ -229,6 +229,8 @@ void camera::CameraParameters::SetDistortion(Eigen::VectorXd const& distortion)
// Inside tangent function
distortion_precalc2_ = 2 * tan(distortion[0] / 2);
break;
case 2:
// Fall through intended.
case 4:
// Fall through intended.
case 5:
Expand Down Expand Up @@ -266,6 +268,27 @@ void camera::CameraParameters::DistortCentered(Eigen::Vector2d const& undistorte
}
*distorted_c = (optical_offset_ - distorted_half_size_) +
conv * norm.cwiseProduct(focal_length_);
} else if (distortion_coeffs_.size() == 2) {
// colmap radial fisheye model
double k1 = distortion_coeffs_[0];
double k2 = distortion_coeffs_[1];

// To relative coordinates
Eigen::Vector2d norm = undistorted_c.cwiseQuotient(focal_length_);
double r = norm.norm();
if (r > std::numeric_limits<double>::epsilon()) {
double theta = atan(r);
double theta2 = theta * theta;
double thetad = theta * (1 + k1 * theta2 + k2 * theta2 * theta2);

*distorted_c = thetad / r * norm;
} else {
*distorted_c = 0.0 * norm;
}

// Back to absolute coordinates.
*distorted_c = distorted_c->cwiseProduct(focal_length_) +
(optical_offset_ - distorted_half_size_);
} else if (distortion_coeffs_.size() == 4 ||
distortion_coeffs_.size() == 5) {
// Tsai lens distortion
Expand Down Expand Up @@ -317,7 +340,8 @@ void camera::CameraParameters::UndistortCentered(Eigen::Vector2d const& distorte
if (rd > 1e-5)
conv = ru / rd;
*undistorted_c = conv * norm.cwiseProduct(focal_length_);
} else if (distortion_coeffs_.size() == 4 ||
} else if (distortion_coeffs_.size() == 2 ||
distortion_coeffs_.size() == 4 ||
distortion_coeffs_.size() == 5) {
// Tsai lens distortion
cv::Mat src(1, 1, CV_64FC2);
Expand All @@ -330,7 +354,14 @@ void camera::CameraParameters::UndistortCentered(Eigen::Vector2d const& distorte
cv::eigen2cv(GetIntrinsicMatrix<DISTORTED>(), dist_int_mat);
cv::eigen2cv(GetIntrinsicMatrix<UNDISTORTED>(), undist_int_mat);
src_map = distorted_c + distorted_half_size_;
cv::undistortPoints(src, dst, dist_int_mat, cvdist, cv::Mat(), undist_int_mat);
if (distortion_coeffs_.size() == 2) {
cvdist.resize(4);
cvdist.at<double>(2, 0) = 0.0;
cvdist.at<double>(3, 0) = 0.0;
cv::fisheye::undistortPoints(src, dst, dist_int_mat, cvdist, cv::Mat(), undist_int_mat);
} else {
cv::undistortPoints(src, dst, dist_int_mat, cvdist, cv::Mat(), undist_int_mat);
}
*undistorted_c = dst_map - undistorted_half_size_;
} else {
LOG(ERROR) << "Unknown distortion vector size!";
Expand Down

0 comments on commit c1d2d76

Please sign in to comment.