diff --git a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h index 313e18264..6eb1310e5 100644 --- a/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h +++ b/gazebo_plugins/include/gazebo_plugins/gazebo_ros_camera_utils.h @@ -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_; diff --git a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp index 4776cae45..e54836321 100644 --- a/gazebo_plugins/src/gazebo_ros_camera_utils.cpp +++ b/gazebo_plugins/src/gazebo_ros_camera_utils.cpp @@ -179,13 +179,34 @@ void GazeboRosCameraUtils::Load(sensors::SensorPtr _parent, else this->cy_ = this->sdf->Get("Cy"); - if (!this->sdf->HasElement("focalLength")) + if (this->sdf->HasElement("focalLength")) { - ROS_DEBUG("Camera plugin missing , defaults to 0"); - this->focal_length_= 0; + this->focal_length_x_ = this->sdf->Get("focalLength"); + this->focal_length_y_ = this->focal_length_x_; + } + + if (!this->sdf->HasElement("focalLengthX")) + { + if (!this->sdf->HasElement("focalLength")) + { + ROS_DEBUG("Camera plugin missing , defaults to 0"); + this->focal_length_x_= 0; + } + } + else + this->focal_length_x_ = this->sdf->Get("focalLengthX"); + + if (!this->sdf->HasElement("focalLengthY")) + { + if (!this->sdf->HasElement("focalLength")) + { + ROS_DEBUG("Camera plugin missing , defaults to 0"); + this->focal_length_y_= 0; + } } else - this->focal_length_ = this->sdf->Get("focalLength"); + this->focal_length_y_ = this->sdf->Get("focalLengthY"); + if (!this->sdf->HasElement("hackBaseline")) { @@ -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)) { # if GAZEBO_MAJOR_VERSION >= 7 ROS_WARN("The [%f] you have provided for camera_ [%s]" @@ -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 @@ -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); @@ -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; @@ -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; diff --git a/gazebo_plugins/src/gazebo_ros_prosilica.cpp b/gazebo_plugins/src/gazebo_ros_prosilica.cpp index dd0203290..0eb34f1e4 100644 --- a/gazebo_plugins/src/gazebo_ros_prosilica.cpp +++ b/gazebo_plugins/src/gazebo_ros_prosilica.cpp @@ -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; @@ -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;