diff --git a/astrobee/config/robots/colmap_bumble.config b/astrobee/config/robots/colmap_bumble.config new file mode 100644 index 0000000000..8f1b437d63 --- /dev/null +++ b/astrobee/config/robots/colmap_bumble.config @@ -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 = {} diff --git a/localization/camera/src/camera_params.cc b/localization/camera/src/camera_params.cc index 4b3f880cd4..1cfe027ee9 100644 --- a/localization/camera/src/camera_params.cc +++ b/localization/camera/src/camera_params.cc @@ -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: @@ -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::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 @@ -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); @@ -330,7 +354,14 @@ void camera::CameraParameters::UndistortCentered(Eigen::Vector2d const& distorte cv::eigen2cv(GetIntrinsicMatrix(), dist_int_mat); cv::eigen2cv(GetIntrinsicMatrix(), 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(2, 0) = 0.0; + cvdist.at(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!";