Skip to content

Commit 02ad6ec

Browse files
peci1peci1
committed
Fixed Melodic.
1 parent ffbd3b7 commit 02ad6ec

File tree

2 files changed

+84
-18
lines changed

2 files changed

+84
-18
lines changed

‎include/robot_model_renderer/pinhole_camera.hpp‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,10 @@ class PinholeCameraModel : public image_geometry::PinholeCameraModel
3636
PinholeCameraModel& operator=(const PinholeCameraModel& other);
3737

3838
virtual ~PinholeCameraModel();
39+
40+
virtual bool fromCameraInfo(const sensor_msgs::CameraInfo& msg);
41+
virtual bool fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg);
42+
3943
virtual void initUnrectificationMaps() const;
4044
virtual cv::Rect rectifyRoi(const cv::Rect& roi_raw, const cv::Mat& P) const;
4145
cv::Rect rectifyRoi(const cv::Rect& roi_raw) const;

‎src/pinhole_camera.cpp‎

Lines changed: 80 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,9 @@ enum DistortionModel { EQUIDISTANT, PLUMB_BOB_OR_RATIONAL_POLYNOMIAL, UNKNOWN_MO
3333
struct PinholeCameraModel::Cache
3434
{
3535
DistortionState distortion_state;
36+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
3637
DistortionModel distortion_model;
38+
#endif
3739

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

@@ -42,23 +44,27 @@ struct PinholeCameraModel::Cache
4244

4345
mutable bool reduced_maps_dirty;
4446
mutable cv::Mat reduced_map1, reduced_map2;
45-
47+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
4648
mutable bool unrectify_full_maps_dirty;
4749
mutable cv::Mat unrectify_full_map1, unrectify_full_map2;
4850

4951
mutable bool unrectify_reduced_maps_dirty;
5052
mutable cv::Mat unrectify_reduced_map1, unrectify_reduced_map2;
51-
53+
#endif
5254
mutable bool rectified_roi_dirty;
5355
mutable cv::Rect rectified_roi;
5456

55-
Cache()
56-
: distortion_state(UNKNOWN),
57+
Cache() :
58+
distortion_state(UNKNOWN),
59+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
5760
distortion_model(UNKNOWN_MODEL),
61+
#endif
5862
full_maps_dirty(true),
5963
reduced_maps_dirty(true),
64+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
6065
unrectify_full_maps_dirty(true),
6166
unrectify_reduced_maps_dirty(true),
67+
#endif
6268
rectified_roi_dirty(true)
6369
{
6470
}
@@ -76,6 +82,13 @@ struct PinholeCameraModel::ExtraCache
7682

7783
mutable bool unrectify_reduced_float_maps_dirty {true};
7884
mutable cv::Mat unrectify_reduced_float_map;
85+
86+
// Copied from Cache for Melodic compatibility
87+
mutable bool unrectify_full_maps_dirty {true};
88+
mutable cv::Mat unrectify_full_map1, unrectify_full_map2;
89+
90+
mutable bool unrectify_reduced_maps_dirty {true};
91+
mutable cv::Mat unrectify_reduced_map1, unrectify_reduced_map2;
7992
};
8093

8194
void initInverseRectificationMap(cv::InputArray _cameraMatrix, cv::InputArray _distCoeffs,
@@ -213,7 +226,7 @@ PinholeCameraModel::PinholeCameraModel() : extraCache(std::make_unique<ExtraCach
213226
{
214227
}
215228

216-
PinholeCameraModel::PinholeCameraModel(const sensor_msgs::CameraInfo& msg)
229+
PinholeCameraModel::PinholeCameraModel(const sensor_msgs::CameraInfo& msg) : PinholeCameraModel()
217230
{
218231
this->fromCameraInfo(msg);
219232
}
@@ -222,15 +235,17 @@ PinholeCameraModel::~PinholeCameraModel() = default;
222235

223236

224237
PinholeCameraModel::PinholeCameraModel(const PinholeCameraModel& other)
225-
: image_geometry::PinholeCameraModel(other)
238+
: image_geometry::PinholeCameraModel(other), extraCache(std::make_unique<ExtraCache>())
226239
{
240+
*this->extraCache = *other.extraCache;
227241
}
228242

229243
PinholeCameraModel& PinholeCameraModel::operator=(const PinholeCameraModel& other)
230244
{
231245
if (this == &other)
232246
return *this;
233247
image_geometry::PinholeCameraModel::operator=(other);
248+
*this->extraCache = *other.extraCache;
234249
return *this;
235250
}
236251

@@ -296,6 +311,21 @@ cv::Point2d PinholeCameraModel::_rectifyPoint(const cv::Point2d& uv_raw, const c
296311
#endif
297312
}
298313

314+
bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfoConstPtr& msg)
315+
{
316+
return this->fromCameraInfo(*msg);
317+
}
318+
319+
bool PinholeCameraModel::fromCameraInfo(const sensor_msgs::CameraInfo& msg)
320+
{
321+
const auto changed = image_geometry::PinholeCameraModel::fromCameraInfo(msg);
322+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
323+
this->extraCache->unrectify_full_maps_dirty |= this->cache_->unrectify_full_maps_dirty;
324+
this->extraCache->unrectify_reduced_maps_dirty |= this->cache_->unrectify_reduced_maps_dirty;
325+
#endif
326+
return changed;
327+
}
328+
299329
void PinholeCameraModel::initUnrectificationMaps() const
300330
{
301331
if (extraCache->unrectify_full_float_maps_dirty)
@@ -345,18 +375,31 @@ void PinholeCameraModel::initUnrectificationMaps() const
345375
extraCache->unrectify_full_float_maps_dirty = false;
346376
}
347377

348-
if (cache_->unrectify_full_maps_dirty)
378+
if (extraCache->unrectify_full_maps_dirty
379+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
380+
|| cache_->unrectify_full_maps_dirty
381+
#endif
382+
)
349383
{
350384
// Note: m1type=CV_16SC2 to use fast fixed-point maps (see cv::remap)
351385
convertMaps(extraCache->unrectify_full_float_map,
352386
cv::Mat(),
353-
cache_->unrectify_full_map1,
354-
cache_->unrectify_full_map2,
387+
extraCache->unrectify_full_map1,
388+
extraCache->unrectify_full_map2,
355389
CV_16SC2);
390+
extraCache->unrectify_full_maps_dirty = false;
391+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
392+
cache_->unrectify_full_map1 = extraCache->unrectify_full_map1;
393+
cache_->unrectify_full_map2 = extraCache->unrectify_full_map2;
356394
cache_->unrectify_full_maps_dirty = false;
395+
#endif
357396
}
358397

359-
if (cache_->unrectify_reduced_maps_dirty || extraCache->unrectify_reduced_float_maps_dirty)
398+
if (extraCache->unrectify_reduced_maps_dirty || extraCache->unrectify_reduced_float_maps_dirty
399+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
400+
|| cache_->unrectify_reduced_maps_dirty
401+
#endif
402+
)
360403
{
361404
/// @todo Use rectified ROI
362405
cv::Rect roi(cam_info_.roi.x_offset,
@@ -373,11 +416,19 @@ void PinholeCameraModel::initUnrectificationMaps() const
373416
roi.y /= binningY();
374417
roi.width /= binningX();
375418
roi.height /= binningY();
376-
if (cache_->unrectify_reduced_maps_dirty)
419+
if (extraCache->unrectify_reduced_maps_dirty
420+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
421+
|| cache_->unrectify_reduced_maps_dirty
422+
#endif
423+
)
377424
{
378-
cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1(roi) -
425+
extraCache->unrectify_reduced_map1 = extraCache->unrectify_full_map1(roi) -
379426
cv::Scalar(roi.x, roi.y);
380-
cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2(roi);
427+
extraCache->unrectify_reduced_map2 = extraCache->unrectify_full_map2(roi);
428+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
429+
cache_->unrectify_reduced_map1 = extraCache->unrectify_reduced_map1;
430+
cache_->unrectify_reduced_map2 = extraCache->unrectify_reduced_map2;
431+
#endif
381432
}
382433
if (extraCache->unrectify_reduced_float_maps_dirty)
383434
{
@@ -387,17 +438,28 @@ void PinholeCameraModel::initUnrectificationMaps() const
387438
else
388439
{
389440
// Otherwise we're rectifying the full image
390-
if (cache_->unrectify_reduced_maps_dirty)
441+
if (extraCache->unrectify_reduced_maps_dirty
442+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
443+
|| cache_->unrectify_reduced_maps_dirty
444+
#endif
445+
)
391446
{
392-
cache_->unrectify_reduced_map1 = cache_->unrectify_full_map1;
393-
cache_->unrectify_reduced_map2 = cache_->unrectify_full_map2;
447+
extraCache->unrectify_reduced_map1 = extraCache->unrectify_full_map1;
448+
extraCache->unrectify_reduced_map2 = extraCache->unrectify_full_map2;
449+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
450+
cache_->unrectify_reduced_map1 = extraCache->unrectify_full_map1;
451+
cache_->unrectify_reduced_map2 = extraCache->unrectify_full_map2;
452+
#endif
394453
}
395454
if (extraCache->unrectify_reduced_float_maps_dirty)
396455
{
397456
extraCache->unrectify_reduced_float_map = extraCache->unrectify_full_float_map;
398457
}
399458
}
459+
#if IMAGE_GEOMETRY_VERSION_MAJOR > 1 || IMAGE_GEOMETRY_VERSION_MINOR >= 16
400460
cache_->unrectify_reduced_maps_dirty = false;
461+
#endif
462+
extraCache->unrectify_reduced_maps_dirty = false;
401463
extraCache->unrectify_reduced_float_maps_dirty = false;
402464
}
403465
}
@@ -496,11 +558,11 @@ void PinholeCameraModel::unrectifyImage(const cv::Mat& rectified, cv::Mat& raw,
496558
initUnrectificationMaps();
497559
if (rectified.depth() == CV_32F || rectified.depth() == CV_64F)
498560
{
499-
cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation,
561+
cv::remap(rectified, raw, extraCache->unrectify_reduced_map1, extraCache->unrectify_reduced_map2, interpolation,
500562
cv::BORDER_CONSTANT, std::numeric_limits<float>::quiet_NaN());
501563
}
502564
else {
503-
cv::remap(rectified, raw, cache_->unrectify_reduced_map1, cache_->unrectify_reduced_map2, interpolation);
565+
cv::remap(rectified, raw, extraCache->unrectify_reduced_map1, extraCache->unrectify_reduced_map2, interpolation);
504566
}
505567
break;
506568
default:

0 commit comments

Comments
 (0)