UnderwaterCameraROSPlugin.cc
Go to the documentation of this file.
1 // Copyright (c) 2016 The UUV Simulator Authors.
2 // All rights reserved.
3 //
4 // Licensed under the Apache License, Version 2.0 (the "License");
5 // you may not use this file except in compliance with the License.
6 // You may obtain a copy of the License at
7 //
8 // http://www.apache.org/licenses/LICENSE-2.0
9 //
10 // Unless required by applicable law or agreed to in writing, software
11 // distributed under the License is distributed on an "AS IS" BASIS,
12 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 // See the License for the specific language governing permissions and
14 // limitations under the License.
15 
17 
18 namespace gazebo
19 {
22  : DepthCameraPlugin(), lastImage(NULL)
23 { }
24 
27 {
28  if (this->lastImage)
29  delete[] lastImage;
30 
31  if (this->depth2rangeLUT)
32  delete[] depth2rangeLUT;
33 }
34 
36 void UnderwaterCameraROSPlugin::Load(sensors::SensorPtr _sensor,
37  sdf::ElementPtr _sdf)
38 {
39  try
40  {
41  DepthCameraPlugin::Load(_sensor, _sdf);
42 
43  // Copying from DepthCameraPlugin into GazeboRosCameraUtils
44  this->parentSensor_ = this->parentSensor;
45  this->width_ = this->width;
46  this->height_ = this->height;
47  this->depth_ = this->depth;
48  this->format_ = this->format;
49  this->camera_ = this->depthCamera;
50 
51  GazeboRosCameraUtils::Load(_sensor, _sdf);
52  }
53  catch(gazebo::common::Exception &_e)
54  {
55  gzerr << "Error loading UnderwaterCameraROSPlugin" << std::endl;
56  return;
57  }
58 
59  if (!ros::isInitialized())
60  {
61  gzerr << "Not loading UnderwaterCameraROSPlugin since ROS has not "
62  << " been properly initialized." << std::endl;
63  return;
64  }
65 
66  lastImage = new unsigned char[this->width * this->height * this->depth];
67 
68  // Only need to load settings specific to this sensor.
69  GetSDFParam<float>(_sdf, "attenuationR", this->attenuation[0], 1.f / 30.f);
70  GetSDFParam<float>(_sdf, "attenuationG", this->attenuation[1], 1.f / 30.f);
71  GetSDFParam<float>(_sdf, "attenuationB", this->attenuation[2], 1.f / 30.f);
72 
73  this->background[0] = (unsigned char)0;
74  this->background[1] = (unsigned char)0;
75  this->background[2] = (unsigned char)0;
76 
77  if (_sdf->HasElement("backgroundR"))
78  this->background[0] = (unsigned char)_sdf->GetElement(
79  "backgroundR")->Get<int>();
80  if (_sdf->HasElement("backgroundG"))
81  this->background[1] = (unsigned char)_sdf->GetElement(
82  "backgroundG")->Get<int>();
83  if (_sdf->HasElement("backgroundB"))
84  this->background[2] = (unsigned char)_sdf->GetElement(
85  "backgroundB")->Get<int>();
86  // Compute camera intrinsics fx, fy from FOVs:
87 #if GAZEBO_MAJOR_VERSION >= 7
88  ignition::math::Angle hfov = ignition::math::Angle(this->depthCamera->HFOV().Radian());
89  ignition::math::Angle vfov = ignition::math::Angle(this->depthCamera->VFOV().Radian());
90 #else
91  ignition::math::Angle hfov = this->depthCamera->GetHFOV();
92  ignition::math::Angle vfov = this->depthCamera->GetVFOV();
93 #endif
94 
95  double fx = (0.5*this->width) / tan(0.5 * hfov.Radian());
96  double fy = (0.5*this->height) / tan(0.5 * vfov.Radian());
97 
98  // Assume the camera's principal point to be at the sensor's center:
99  double cx = 0.5 * this->width;
100  double cy = 0.5 * this->height;
101 
102  // Create and fill depth2range LUT
103  this->depth2rangeLUT = new float[this->width * this->height];
104  float * lutPtr = this->depth2rangeLUT;
105  for (int v = 0; v < this->height; v++)
106  {
107  double y_z = (v - cy)/fy;
108  for (int u = 0; u < this->width; u++)
109  {
110  double x_z = (u - cx)/fx;
111  // Precompute the per-pixel factor in the following formula:
112  // range = || (x, y, z) ||_2
113  // range = || z * (x/z, y/z, 1.0) ||_2
114  // range = z * || (x/z, y/z, 1.0) ||_2
115  *(lutPtr++) = sqrt(1.0 + x_z*x_z + y_z*y_z);
116  }
117  }
118 }
119 
122  unsigned int _width, unsigned int _height, unsigned int _depth,
123  const std::string &_format)
124 {
125  // TODO: Can we assume this pointer to always remain valid?
126  this->lastDepth = _image;
127 }
128 
131  unsigned int _width, unsigned int _height, unsigned int _depth,
132  const std::string &_format)
133 { }
134 
136 void UnderwaterCameraROSPlugin::OnNewImageFrame(const unsigned char *_image,
137  unsigned int _width, unsigned int _height, unsigned int _depth,
138  const std::string& _format)
139 {
140  // Only create cv::Mat wrappers around existing memory
141  // (neither allocates nor copies any images).
142  const cv::Mat input(_height, _width, CV_8UC3,
143  const_cast<unsigned char*>(_image));
144  const cv::Mat depth(_height, _width, CV_32FC1,
145  const_cast<float*>(lastDepth));
146 
147  cv::Mat output(_height, _width, CV_8UC3, lastImage);
148 
149  this->SimulateUnderwater(input, depth, output);
150 
151  if (!this->initialized_ || this->height_ <= 0 || this->width_ <= 0)
152  return;
153 
154 #if GAZEBO_MAJOR_VERSION >= 7
155  this->sensor_update_time_ = this->parentSensor->LastUpdateTime();
156 #else
157  this->sensor_update_time_ = this->parentSensor->GetLastUpdateTime();
158 #endif
159 
160  if (!this->parentSensor->IsActive())
161  {
162  if ((*this->image_connect_count_) > 0)
163  // Do this first so there's chance for sensor to run 1 frame after
164  // activate
165  this->parentSensor->SetActive(true);
166  }
167  else
168  {
169  if ((*this->image_connect_count_) > 0)
170  this->PutCameraData(this->lastImage);
171  this->PublishCameraInfo();
172  }
173 }
174 
176 void UnderwaterCameraROSPlugin::SimulateUnderwater(const cv::Mat& _inputImage,
177  const cv::Mat& _inputDepth, cv::Mat& _outputImage)
178 {
179  const float * lutPtr = this->depth2rangeLUT;
180  for (unsigned int row = 0; row < this->height; row++)
181  {
182  const cv::Vec3b* inrow = _inputImage.ptr<cv::Vec3b>(row);
183  const float* depthrow = _inputDepth.ptr<float>(row);
184  cv::Vec3b* outrow = _outputImage.ptr<cv::Vec3b>(row);
185 
186  for (int col = 0; col < this->width; col++)
187  {
188  // Convert depth to range using the depth2range LUT
189  float r = *(lutPtr++)*depthrow[col];
190  const cv::Vec3b& in = inrow[col];
191  cv::Vec3b& out = outrow[col];
192 
193  if (r < 1e-3)
194  r = 1e10;
195 
196  for (int c = 0; c < 3; c++)
197  {
198  // Simplifying assumption: intensity ~ irradiance.
199  // This is not really the case but a good enough approximation
200  // for now (it would be better to use a proper Radiometric
201  // Response Function).
202  float e = std::exp(-r*attenuation[c]);
203  out[c] = e*in[c] + (1.0f-e)*background[c];
204  }
205  }
206  }
207 }
208 
210 GZ_REGISTER_SENSOR_PLUGIN(UnderwaterCameraROSPlugin)
211 }
void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
Load plugin and its configuration from sdf.
float attenuation[3]
Attenuation constants per channel (RGB)
ROSCPP_DECL bool isInitialized()
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
boost::shared_ptr< int > image_connect_count_
unsigned char background[3]
Background constants per channel (RGB)
virtual void OnNewRGBPointCloud(const float *_pcd, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
virtual void SimulateUnderwater(const cv::Mat &_inputImage, const cv::Mat &_inputDepth, cv::Mat &_outputImage)
Add underwater light damping to image.
float * depth2rangeLUT
Depth to range lookup table (LUT)
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
void PutCameraData(const unsigned char *_src)
rendering::CameraPtr camera_
unsigned char * lastImage
Latest simulated image.
virtual ~UnderwaterCameraROSPlugin()
Class destructor.
const float * lastDepth
Temporarily store pointer to previous depth image.
sensors::SensorPtr parentSensor_


uuv_sensor_ros_plugins
Author(s): Musa Morena Marcusso Manhaes , Sebastian Scherer , Luiz Ricardo Douat
autogenerated on Mon Jul 1 2019 19:39:20