Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add equidistant distortion model #358

Merged
merged 2 commits into from
Mar 1, 2021
Merged

Add equidistant distortion model #358

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f2b8437
Add equidistant distortion model
mintar Aug 31, 2020
9900ec1
Fix travis
mintar Sep 7, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions .travis.sh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@ function travis_time_end {
set -x
}

apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg # for docker
apt-get update -qq && apt-get install -y -q wget sudo lsb-release gnupg build-essential # for docker
DEBIAN_FRONTEND=noninteractive apt-get install -y tzdata # https://stackoverflow.com/questions/44331836/apt-get-install-tzdata-noninteractive

travis_time_start setup.before_install
Expand All @@ -33,7 +33,7 @@ sudo sh -c 'echo "deb http://repositories.ros.org/ubuntu/testing $(lsb_release -
wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
sudo apt-get update -qq || echo Ignore error on apt-get update
# Install ROS
sudo apt-get install -qq -y python3-catkin-pkg python3-catkin-tools python3-rosdep python3-wstool ros-$ROS_DISTRO-catkin
sudo apt-get install -qq -y python3-catkin-pkg python3-catkin-tools python3-osrf-pycommon python3-rosdep python3-wstool ros-$ROS_DISTRO-catkin
source /opt/ros/$ROS_DISTRO/setup.bash
# Setup for rosdep
sudo rosdep init
Expand Down
65 changes: 57 additions & 8 deletions image_geometry/src/pinhole_camera_model.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,12 @@
namespace image_geometry {

enum DistortionState { NONE, CALIBRATED, UNKNOWN };
enum DistortionModel { EQUIDISTANT, PLUMB_BOB_OR_RATIONAL_POLYNOMIAL, UNKNOWN_MODEL };

struct PinholeCameraModel::Cache
{
DistortionState distortion_state;
DistortionModel distortion_model;

cv::Mat_<double> K_binned, P_binned; // Binning applied, but not cropping

Expand All @@ -24,7 +26,9 @@ struct PinholeCameraModel::Cache
mutable cv::Rect rectified_roi;

Cache()
: full_maps_dirty(true),
: distortion_state(UNKNOWN),
distortion_model(UNKNOWN_MODEL),
full_maps_dirty(true),
reduced_maps_dirty(true),
rectified_roi_dirty(true)
{
Expand Down Expand Up @@ -132,7 +136,8 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)

// Figure out how to handle the distortion
if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL ||
cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
// If any distortion coefficient is non-zero, then need to apply the distortion
cache_->distortion_state = NONE;
for (size_t i = 0; i < cam_info_.D.size(); ++i)
Expand All @@ -147,6 +152,17 @@ bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
else
cache_->distortion_state = UNKNOWN;

// Get the distortion model, if supported
if (cam_info_.distortion_model == sensor_msgs::distortion_models::PLUMB_BOB ||
cam_info_.distortion_model == sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL) {
cache_->distortion_model = PLUMB_BOB_OR_RATIONAL_POLYNOMIAL;
}
else if(cam_info_.distortion_model == sensor_msgs::distortion_models::EQUIDISTANT) {
cache_->distortion_model = EQUIDISTANT;
}
else
cache_->distortion_model = UNKNOWN_MODEL;

// If necessary, create new K_ and P_ adjusted for binning and ROI
/// @todo Calculate and use rectified ROI
bool adjust_binning = (binning_x > 1) || (binning_y > 1);
Expand Down Expand Up @@ -342,7 +358,18 @@ cv::Point2d PinholeCameraModel::rectifyPoint(const cv::Point2d& uv_raw) const
cv::Point2f raw32 = uv_raw, rect32;
const cv::Mat src_pt(1, 1, CV_32FC2, &raw32.x);
cv::Mat dst_pt(1, 1, CV_32FC2, &rect32.x);
cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);

switch (cache_->distortion_model) {
case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
cv::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
break;
case EQUIDISTANT:
cv::fisheye::undistortPoints(src_pt, dst_pt, K_, D_, R_, P_);
break;
default:
assert(cache_->distortion_model == UNKNOWN_MODEL);
throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
}
return rect32;
}

Expand All @@ -363,7 +390,18 @@ cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
cv::Rodrigues(R_.t(), r_vec);
std::vector<cv::Point2d> image_point;
cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);

switch (cache_->distortion_model) {
case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
break;
case EQUIDISTANT:
cv::fisheye::projectPoints(std::vector<cv::Point3d>(1, ray), image_point, r_vec, t_vec, K_, D_);
break;
default:
assert(cache_->distortion_model == UNKNOWN_MODEL);
throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
}

return image_point[0];
}
Expand Down Expand Up @@ -448,10 +486,21 @@ void PinholeCameraModel::initRectificationMaps() const
P_binned(1,3) *= scale_y;
}
}

// Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
CV_16SC2, cache_->full_map1, cache_->full_map2);

switch (cache_->distortion_model) {
case PLUMB_BOB_OR_RATIONAL_POLYNOMIAL:
// Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
cv::initUndistortRectifyMap(K_binned, D_, R_, P_binned, binned_resolution,
CV_16SC2, cache_->full_map1, cache_->full_map2);
break;
case EQUIDISTANT:
cv::fisheye::initUndistortRectifyMap(K_binned,D_, R_, P_binned, binned_resolution,
CV_16SC2, cache_->full_map1, cache_->full_map2);
break;
default:
assert(cache_->distortion_model == UNKNOWN_MODEL);
throw Exception("Wrong distortion model. Supported models: PLUMB_BOB, RATIONAL_POLYNOMIAL and EQUIDISTANT.");
}
cache_->full_maps_dirty = false;
}

Expand Down
3 changes: 3 additions & 0 deletions image_geometry/test/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,3 +2,6 @@ catkin_add_nosetests(directed.py)

catkin_add_gtest(${PROJECT_NAME}-utest utest.cpp)
target_link_libraries(${PROJECT_NAME}-utest ${PROJECT_NAME} ${OpenCV_LIBS})

catkin_add_gtest(${PROJECT_NAME}-utest-equi utest_equi.cpp)
target_link_libraries(${PROJECT_NAME}-utest-equi ${PROJECT_NAME} ${OpenCV_LIBS})
Loading