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

Jump to bottom

Added option to specify x and y components of camera focal length. #374

Closed
wants to merge 1 commit into from
Closed

Added option to specify x and y components of camera focal length. #374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
3 changes: 2 additions & 1 deletion gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -147,7 +147,8 @@ namespace gazebo
protected: double cx_prime_;
protected: double cx_;
protected: double cy_;
protected: double focal_length_;
protected: double focal_length_x_;
protected: double focal_length_y_;
protected: double hack_baseline_;
protected: double distortion_k1_;
protected: double distortion_k2_;
Expand Down
50 changes: 36 additions & 14 deletions gazebo_plugins/src/gazebo_ros_camera_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -179,13 +179,34 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent,
else
this->cy_ = this->sdf->Get<double>("Cy");

if (!this->sdf->HasElement("focalLength"))
if (this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLength>, defaults to 0");
this->focal_length_= 0;
this->focal_length_x_ = this->sdf->Get<double>("focalLength");
this->focal_length_y_ = this->focal_length_x_;
}

if (!this->sdf->HasElement("focalLengthX"))
{
if (!this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLengthX>, defaults to 0");
this->focal_length_x_= 0;
}
}
else
this->focal_length_x_ = this->sdf->Get<double>("focalLengthX");

if (!this->sdf->HasElement("focalLengthY"))
{
if (!this->sdf->HasElement("focalLength"))
{
ROS_DEBUG("Camera plugin missing <focalLengthY>, defaults to 0");
this->focal_length_y_= 0;
}
}
else
this->focal_length_ = this->sdf->Get<double>("focalLength");
this->focal_length_y_ = this->sdf->Get<double>("focalLengthY");


if (!this->sdf->HasElement("hackBaseline"))
{
Expand Down Expand Up @@ -478,14 +499,15 @@ void GazeboRosCameraUtils::Init()
(2.0 * tan(this->camera_->GetHFOV().Radian() / 2.0));
# endif

if (this->focal_length_ == 0)
if (this->focal_length_x_ == 0 || this->focal_length_y_ == 0)
{
this->focal_length_ = computed_focal_length;
this->focal_length_x_ = computed_focal_length;
this->focal_length_y_ = computed_focal_length;
}
else
{
// check against float precision
if (!gazebo::math::equal(this->focal_length_, computed_focal_length))
if (!gazebo::math::equal(this->focal_length_x_, computed_focal_length))
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

you probably want to this for the y focal_length as well

Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok, nevermind

{
# if GAZEBO_MAJOR_VERSION >= 7
ROS_WARN("The <focal_length>[%f] you have provided for camera_ [%s]"
Expand All @@ -494,7 +516,7 @@ void GazeboRosCameraUtils::Init()
" focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
" the explected focal_lengtth value is [%f],"
" please update your camera_ model description accordingly.",
this->focal_length_, this->parentSensor_->Name().c_str(),
this->focal_length_x_, this->parentSensor_->Name().c_str(),
this->width_, this->camera_->HFOV().Radian(),
computed_focal_length);
# else
Expand All @@ -504,7 +526,7 @@ void GazeboRosCameraUtils::Init()
" focal_length = width_ / (2.0 * tan(HFOV/2.0)),"
" the explected focal_lengtth value is [%f],"
" please update your camera_ model description accordingly.",
this->focal_length_, this->parentSensor_->GetName().c_str(),
this->focal_length_x_, this->parentSensor_->GetName().c_str(),
this->width_, this->camera_->GetHFOV().Radian(),
computed_focal_length);

Expand All @@ -530,11 +552,11 @@ void GazeboRosCameraUtils::Init()
camera_info_msg.D[3] = this->distortion_t1_;
camera_info_msg.D[4] = this->distortion_t2_;
// original camera_ matrix
camera_info_msg.K[0] = this->focal_length_;
camera_info_msg.K[0] = this->focal_length_x_;
camera_info_msg.K[1] = 0.0;
camera_info_msg.K[2] = this->cx_;
camera_info_msg.K[3] = 0.0;
camera_info_msg.K[4] = this->focal_length_;
camera_info_msg.K[4] = this->focal_length_y_;
camera_info_msg.K[5] = this->cy_;
camera_info_msg.K[6] = 0.0;
camera_info_msg.K[7] = 0.0;
Expand All @@ -551,12 +573,12 @@ void GazeboRosCameraUtils::Init()
camera_info_msg.R[8] = 1.0;
// camera_ projection matrix (same as camera_ matrix due
// to lack of distortion/rectification) (is this generated?)
camera_info_msg.P[0] = this->focal_length_;
camera_info_msg.P[0] = this->focal_length_x_;
camera_info_msg.P[1] = 0.0;
camera_info_msg.P[2] = this->cx_;
camera_info_msg.P[3] = -this->focal_length_ * this->hack_baseline_;
camera_info_msg.P[3] = -this->focal_length_x_ * this->hack_baseline_;
camera_info_msg.P[4] = 0.0;
camera_info_msg.P[5] = this->focal_length_;
camera_info_msg.P[5] = this->focal_length_y_;
camera_info_msg.P[6] = this->cy_;
camera_info_msg.P[7] = 0.0;
camera_info_msg.P[8] = 0.0;
Expand Down
10 changes: 5 additions & 5 deletions gazebo_plugins/src/gazebo_ros_prosilica.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -247,11 +247,11 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiCameraInfoMsg->D[3] = this->distortion_t1_;
this->roiCameraInfoMsg->D[4] = this->distortion_t2_;
// original camera matrix
this->roiCameraInfoMsg->K[0] = this->focal_length_;
this->roiCameraInfoMsg->K[0] = this->focal_length_x_;
this->roiCameraInfoMsg->K[1] = 0.0;
this->roiCameraInfoMsg->K[2] = this->cx_ - req.roi.x_offset;
this->roiCameraInfoMsg->K[3] = 0.0;
this->roiCameraInfoMsg->K[4] = this->focal_length_;
this->roiCameraInfoMsg->K[4] = this->focal_length_y_;
this->roiCameraInfoMsg->K[5] = this->cy_ - req.roi.y_offset;
this->roiCameraInfoMsg->K[6] = 0.0;
this->roiCameraInfoMsg->K[7] = 0.0;
Expand All @@ -267,12 +267,12 @@ void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& re
this->roiCameraInfoMsg->R[7] = 0.0;
this->roiCameraInfoMsg->R[8] = 1.0;
// camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
this->roiCameraInfoMsg->P[0] = this->focal_length_;
this->roiCameraInfoMsg->P[0] = this->focal_length_x_;
this->roiCameraInfoMsg->P[1] = 0.0;
this->roiCameraInfoMsg->P[2] = this->cx_ - req.roi.x_offset;
this->roiCameraInfoMsg->P[3] = -this->focal_length_ * this->hack_baseline_;
this->roiCameraInfoMsg->P[3] = -this->focal_length_x_ * this->hack_baseline_;
this->roiCameraInfoMsg->P[4] = 0.0;
this->roiCameraInfoMsg->P[5] = this->focal_length_;
this->roiCameraInfoMsg->P[5] = this->focal_length_y_;
this->roiCameraInfoMsg->P[6] = this->cy_ - req.roi.y_offset;
this->roiCameraInfoMsg->P[7] = 0.0;
this->roiCameraInfoMsg->P[8] = 0.0;
Expand Down