diff --git a/gazebo/rendering/Distortion.cc b/gazebo/rendering/Distortion.cc index f570da9ad6..e3aa23da1d 100644 --- a/gazebo/rendering/Distortion.cc +++ b/gazebo/rendering/Distortion.cc @@ -60,6 +60,14 @@ namespace gazebo /// black pixels at the corners of the image. public: bool distortionCrop = true; + /// \brief Use the legacy distortion mode. If this is set to false, the + /// new mode modifies how Brown's distortion equations are applied to + /// better reflect real distortion. The image is projected from image + /// plane to camera plane to apply distortion equations, then projected + /// back to image plane. Note that the new distortion doesn't allow + /// the image to be cropped. + public: bool legacyMode = true; + /// \brief Lens distortion compositor public: Ogre::CompositorInstance *lensDistortionInstance; @@ -122,6 +130,16 @@ void Distortion::Load(sdf::ElementPtr _sdf) { this->dataPtr->compositorName = _sdf->Get(compositorName); } + const std::string legacyMode = "ignition:legacy_mode"; + if (_sdf->HasElement(legacyMode)) + { + this->dataPtr->legacyMode = _sdf->Get(legacyMode); + } + if (!this->dataPtr->legacyMode && this->dataPtr->distortionCrop) + { + gzwarn << "Enable legacy mode to use distortion cropping" << std::endl; + this->dataPtr->distortionCrop = false; + } } ////////////////////////////////////////////////// @@ -160,12 +178,16 @@ void Distortion::SetCamera(CameraPtr _camera) // seems to work best with a square distortion map texture unsigned int texSide = _camera->ImageHeight() > _camera->ImageWidth() ? _camera->ImageHeight() : _camera->ImageWidth(); + // calculate focal length from largest fov + const double fov = _camera->ImageHeight() > _camera->ImageWidth() ? + _camera->VFOV().Radian() : _camera->HFOV().Radian(); + const double focalLength = texSide/(2*tan(fov/2)); this->dataPtr->distortionTexWidth = texSide - 1; this->dataPtr->distortionTexHeight = texSide - 1; unsigned int imageSize = this->dataPtr->distortionTexWidth * this->dataPtr->distortionTexHeight; - double incrU = 1.0 / this->dataPtr->distortionTexWidth; - double incrV = 1.0 / this->dataPtr->distortionTexHeight; + double colStepSize = 1.0 / this->dataPtr->distortionTexWidth; + double rowStepSize = 1.0 / this->dataPtr->distortionTexHeight; // initialize distortion map this->dataPtr->distortionMap.resize(imageSize); @@ -174,30 +196,96 @@ void Distortion::SetCamera(CameraPtr _camera) this->dataPtr->distortionMap[i] = -1; } + ignition::math::Vector2d distortionCenterCoordinates( + this->dataPtr->lensCenter.X() * this->dataPtr->distortionTexWidth, + this->dataPtr->lensCenter.Y() * this->dataPtr->distortionTexWidth); + + // declare variables before the loop + const auto unsetPixelVector = ignition::math::Vector2d(-1, -1); + ignition::math::Vector2d normalizedLocation, + distortedLocation, + newDistortedCoordinates, + currDistortedCoordinates; + unsigned int distortedIdx, + distortedCol, + distortedRow; + double normalizedColLocation, normalizedRowLocation; + // fill the distortion map - for (unsigned int i = 0; i < this->dataPtr->distortionTexHeight; ++i) + for (unsigned int mapRow = 0; mapRow < this->dataPtr->distortionTexHeight; + ++mapRow) { - double v = i*incrV; - for (unsigned int j = 0; j < this->dataPtr->distortionTexWidth; ++j) + normalizedRowLocation = mapRow*rowStepSize; + for (unsigned int mapCol = 0; mapCol < this->dataPtr->distortionTexWidth; + ++mapCol) { - double u = j*incrU; - ignition::math::Vector2d uv(u, v); - ignition::math::Vector2d out = - this->Distort(uv, this->dataPtr->lensCenter, + normalizedColLocation = mapCol*colStepSize; + + normalizedLocation[0] = normalizedColLocation; + normalizedLocation[1] = normalizedRowLocation; + + if (this->dataPtr->legacyMode) + { + distortedLocation = this->Distort( + normalizedLocation, + this->dataPtr->lensCenter, this->dataPtr->k1, this->dataPtr->k2, this->dataPtr->k3, this->dataPtr->p1, this->dataPtr->p2); + } + else + { + distortedLocation = this->Distort( + normalizedLocation, + this->dataPtr->lensCenter, + this->dataPtr->k1, this->dataPtr->k2, this->dataPtr->k3, + this->dataPtr->p1, this->dataPtr->p2, + this->dataPtr->distortionTexWidth, + focalLength); + } // compute the index in the distortion map - unsigned int idxU = out.X() * this->dataPtr->distortionTexWidth; - unsigned int idxV = out.Y() * this->dataPtr->distortionTexHeight; - - if (idxU < this->dataPtr->distortionTexWidth && - idxV < this->dataPtr->distortionTexHeight) + distortedCol = distortedLocation.X() * this->dataPtr->distortionTexWidth; + distortedRow = distortedLocation.Y() * + this->dataPtr->distortionTexHeight; + + // Note that the following makes sure that, for significant distortions, + // there is not a problem where the distorted image seems to fold over + // itself. This is accomplished by favoring pixels closer to the center + // of distortion, and this change applies to both the legacy and + // nonlegacy distortion modes. + + // Make sure the distorted pixel is within the texture dimensions + if (distortedCol < this->dataPtr->distortionTexWidth && + distortedRow < this->dataPtr->distortionTexHeight) { - unsigned int mapIdx = idxV * this->dataPtr->distortionTexWidth + idxU; - this->dataPtr->distortionMap[mapIdx] = uv; + distortedIdx = distortedRow * this->dataPtr->distortionTexWidth + + distortedCol; + + // check if the index has already been set + if (this->dataPtr->distortionMap[distortedIdx] != unsetPixelVector) + { + // grab current coordinates that map to this destination + currDistortedCoordinates = + this->dataPtr->distortionMap[distortedIdx] * + this->dataPtr->distortionTexWidth; + + // grab new coordinates to map to + newDistortedCoordinates[0] = mapCol; + newDistortedCoordinates[1] = mapRow; + + // use the new mapping if it is closer to the center of the distortion + if (newDistortedCoordinates.Distance(distortionCenterCoordinates) < + currDistortedCoordinates.Distance(distortionCenterCoordinates)) + { + this->dataPtr->distortionMap[distortedIdx] = normalizedLocation; + } + } + else + { + this->dataPtr->distortionMap[distortedIdx] = normalizedLocation; + } } - // else: pixel maps outside the image bounds. + // else: mapping is outside of the image bounds. // This is expected and normal to ensure // no black borders; carry on } @@ -395,11 +483,20 @@ ignition::math::Vector2d Distortion::Distort( const ignition::math::Vector2d &_in, const ignition::math::Vector2d &_center, double _k1, double _k2, double _k3, double _p1, double _p2) +{ + return Distort(_in, _center, _k1, _k2, _k3, _p1, _p2, 1u, 1.0); +} + +////////////////////////////////////////////////// +ignition::math::Vector2d Distortion::Distort( + const ignition::math::Vector2d &_in, + const ignition::math::Vector2d &_center, double _k1, double _k2, double _k3, + double _p1, double _p2, unsigned int _width, double _f) { // apply Brown's distortion model, see // http://en.wikipedia.org/wiki/Distortion_%28optics%29#Software_correction - ignition::math::Vector2d normalized2d = _in - _center; + ignition::math::Vector2d normalized2d = (_in - _center)*(_width/_f); ignition::math::Vector3d normalized(normalized2d.X(), normalized2d.Y(), 0); double rSq = normalized.X() * normalized.X() + normalized.Y() * normalized.Y(); @@ -416,14 +513,15 @@ ignition::math::Vector2d Distortion::Distort( dist.Y() += _p1 * (rSq + 2 * (normalized.Y()*normalized.Y())) + 2 * _p2 * normalized.X() * normalized.Y(); - return _center + ignition::math::Vector2d(dist.X(), dist.Y()); + return ((_center*_width) + + ignition::math::Vector2d(dist.X(), dist.Y())*_f)/_width; } ////////////////////////////////////////////////// void Distortion::SetCrop(const bool _crop) { // Only update the distortion scale if the crop value is going to flip. - if (this->dataPtr->distortionCrop != _crop) + if (this->dataPtr->distortionCrop != _crop && this->dataPtr->legacyMode) { this->dataPtr->distortionCrop = _crop; this->CalculateAndApplyDistortionScale(); diff --git a/gazebo/rendering/Distortion.hh b/gazebo/rendering/Distortion.hh index 786fbbd15e..c172f3df31 100644 --- a/gazebo/rendering/Distortion.hh +++ b/gazebo/rendering/Distortion.hh @@ -54,7 +54,8 @@ namespace gazebo public: void SetCamera(CameraPtr _camera); /// \brief Set whether to crop the black border around the distorted - /// image points. + /// image points. Note that cropping only occurs for the legacy mode + /// distortion implementation. /// \param[in] _crop True to crop the black border /// \sa Crop public: void SetCrop(const bool _crop); @@ -104,6 +105,24 @@ namespace gazebo double _k1, double _k2, double _k3, double _p1, double _p2); + /// \brief Apply distortion model using camera coordinates projection + /// \param[in] _in Input uv coordinate. + /// \param[in] _center Normalized distortion center. + /// \param[in] _k1 Radial distortion coefficient k1. + /// \param[in] _k2 Radial distortion coefficient k2. + /// \param[in] _k3 Radial distortion coefficient k3. + /// \param[in] _p1 Tangential distortion coefficient p1. + /// \param[in] _p2 Tangential distortion coefficient p2. + /// \param[in] _width Width of the image texture in pixels. + /// \param[in] _f Focal length in pixels. + /// \return Distorted coordinate. + public: static ignition::math::Vector2d Distort( + const ignition::math::Vector2d &_in, + const ignition::math::Vector2d &_center, + double _k1, double _k2, double _k3, + double _p1, double _p2, + unsigned int _width, double _f); + /// \brief get the distortion map value. /// \return the distortion map value at the specified index, /// or (-1, -1) if the index diff --git a/gazebo/test/ServerFixture.cc b/gazebo/test/ServerFixture.cc index d86efdb502..ba1d7f0213 100644 --- a/gazebo/test/ServerFixture.cc +++ b/gazebo/test/ServerFixture.cc @@ -578,7 +578,9 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, const std::string &_noiseType, double _noiseMean, double _noiseStdDev, bool _distortion, double _distortionK1, double _distortionK2, double _distortionK3, double _distortionP1, double _distortionP2, - double _cx, double _cy) + double _cx, double _cy, + bool _legacyMode, + double _horizontalFov) { msgs::Factory msg; std::ostringstream newModelStr; @@ -594,7 +596,7 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << " " << _rate << "" << " true" << " " - << " 0.78539816339744828" + << " " << _horizontalFov << "" << " " << " " << _width << "" << " " << _height << "" @@ -623,6 +625,9 @@ void ServerFixture::SpawnCamera(const std::string &_modelName, << " " << _distortionP1 << "" << " " << _distortionP2 << "" << "
" << _cx << " " << _cy << "
" + << " " + << (_legacyMode ? "true" : "false") + << "" << " "; } diff --git a/gazebo/test/ServerFixture.hh b/gazebo/test/ServerFixture.hh index 00d9542424..475707123a 100644 --- a/gazebo/test/ServerFixture.hh +++ b/gazebo/test/ServerFixture.hh @@ -254,6 +254,8 @@ namespace gazebo /// \param[in] _distortionP2 Distortion coefficient p2. /// \param[in] _cx Normalized optical center x, used for distortion. /// \param[in] _cy Normalized optical center y, used for distortion. + /// \param[in] _legacyMode Use the legacy distortion model. + /// \param[in] _horizontalFov The horizontal field of view. protected: void SpawnCamera(const std::string &_modelName, const std::string &_cameraName, const ignition::math::Vector3d &_pos = @@ -267,7 +269,9 @@ namespace gazebo bool _distortion = false, double _distortionK1 = 0.0, double _distortionK2 = 0.0, double _distortionK3 = 0.0, double _distortionP1 = 0.0, double _distortionP2 = 0.0, - double _cx = 0.5, double _cy = 0.5); + double _cx = 0.5, double _cy = 0.5, + bool _legacyMode = true, + double _horizontalFov = 0.78539816339744828); /// \brief Spawn a wide angle camera. /// \param[in] _modelName Name of the model. diff --git a/test/integration/camera_sensor.cc b/test/integration/camera_sensor.cc index 0b275751f4..452cc2f227 100644 --- a/test/integration/camera_sensor.cc +++ b/test/integration/camera_sensor.cc @@ -2040,4 +2040,198 @@ TEST_F(CameraSensor, SetCompositorNames) delete [] img; } #endif -} \ No newline at end of file +} + +///////////////////////////////////////////////// +TEST_F(CameraSensor, CheckNewAndLegacyDistortionModes) +{ + Load("worlds/test/issue_3003_distortion_implementation_correction.world"); + + // Make sure the render engine is available. + if (rendering::RenderEngine::Instance()->GetRenderPathType() == + rendering::RenderEngine::NONE) + { + gzerr << "No rendering engine, unable to run camera test\n"; + return; + } + + // Spawn cameras. + const std::string modelNameBarrelLegacy = "camera_model_barrel_legacy"; + const std::string cameraNameBarrelLegacy = "camera_sensor_barrel_legacy"; + const std::string modelNameBarrelNew = "camera_model_barrel_new"; + const std::string cameraNameBarrelNew = "camera_sensor_barrel_new"; + + const std::string modelNamePincushionLegacy = + "camera_model_pincushion_legacy"; + const std::string cameraNamePincushionLegacy = + "camera_sensor_pincushion_legacy"; + const std::string modelNamePincushionNew = "camera_model_pincushion_new"; + const std::string cameraNamePincushionNew = "camera_sensor_pincushion_new"; + const unsigned int width = 160; + const unsigned int height = 120; + const double updateRate = 10; + const ignition::math::Pose3d setPose( + ignition::math::Vector3d(0.5, 0, 0), + ignition::math::Quaterniond(0, 0, 0)); + const double horizontalFov = 1.6; + + // spawn a camera with pincushion distortion + SpawnCamera(modelNamePincushionLegacy, cameraNamePincushionLegacy, + setPose.Pos(), setPose.Rot().Euler(), width, height, updateRate, + "", 0, 0, true, 0.5, 0, 0, 0, 0, 0.5, 0.5, + true, horizontalFov); + SpawnCamera(modelNamePincushionNew, cameraNamePincushionNew, + setPose.Pos(), setPose.Rot().Euler(), width, height, updateRate, + "", 0, 0, true, 0.5, 0, 0, 0, 0, 0.5, 0.5, + false, horizontalFov); + // spawn a camera with barrel distortion + SpawnCamera(modelNameBarrelLegacy, cameraNameBarrelLegacy, setPose.Pos(), + setPose.Rot().Euler(), width, height, updateRate, + "", 0, 0, true, -0.5, 0, 0, 0, 0, 0.5, 0.5, + true, horizontalFov); + SpawnCamera(modelNameBarrelNew, cameraNameBarrelNew, setPose.Pos(), + setPose.Rot().Euler(), width, height, updateRate, + "", 0, 0, true, -0.5, 0, 0, 0, 0, 0.5, 0.5, + false, horizontalFov); + + sensors::SensorPtr sensorPincushionLegacy = + sensors::get_sensor(cameraNamePincushionLegacy); + sensors::CameraSensorPtr camSensorPincushionLegacy = + std::dynamic_pointer_cast(sensorPincushionLegacy); + sensors::SensorPtr sensorPincushionNew = + sensors::get_sensor(cameraNamePincushionNew); + sensors::CameraSensorPtr camSensorPincushionNew = + std::dynamic_pointer_cast(sensorPincushionNew); + + sensors::SensorPtr sensorBarrelLegacy = + sensors::get_sensor(cameraNameBarrelLegacy); + sensors::CameraSensorPtr camSensorBarrelLegacy = + std::dynamic_pointer_cast(sensorBarrelLegacy); + sensors::SensorPtr sensorBarrelNew = + sensors::get_sensor(cameraNameBarrelNew); + sensors::CameraSensorPtr camSensorBarrelNew = + std::dynamic_pointer_cast(sensorBarrelNew); + + imageCount = 0; + imageCount2 = 0; + imageCount3 = 0; + imageCount4 = 0; + + img = new unsigned char[width * height*3]; + img2 = new unsigned char[width * height*3]; + img3 = new unsigned char[width * height*3]; + img4 = new unsigned char[width * height*3]; + + event::ConnectionPtr c1 = + camSensorPincushionLegacy->Camera()->ConnectNewImageFrame( + std::bind(&::OnNewCameraFrame, &imageCount, img, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + event::ConnectionPtr c2 = + camSensorPincushionNew->Camera()->ConnectNewImageFrame( + std::bind(&::OnNewCameraFrame, &imageCount2, img2, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + event::ConnectionPtr c3 = + camSensorBarrelLegacy->Camera()->ConnectNewImageFrame( + std::bind(&::OnNewCameraFrame, &imageCount3, img3, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + event::ConnectionPtr c4 = + camSensorBarrelNew->Camera()->ConnectNewImageFrame( + std::bind(&::OnNewCameraFrame, &imageCount4, img4, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + // Get some images + while ( + imageCount < 10 || imageCount2 < 10 || + imageCount3 < 10 || imageCount4 < 10) + { + common::Time::MSleep(10); + } + + unsigned int colorSum1 = 0; + unsigned int colorSum2 = 0; + unsigned int colorSum3 = 0; + unsigned int colorSum4 = 0; + const unsigned int middleRow = height / 2; + for (unsigned int x = 0; x < width*3; x+=3) + { + unsigned int r1 = img[(middleRow*width*3) + x]; + unsigned int g1 = img[(middleRow*width*3) + x + 1]; + unsigned int b1 = img[(middleRow*width*3) + x + 2]; + colorSum1 += r1 + g1 + b1; + unsigned int r2 = img2[(middleRow*width*3) + x]; + unsigned int g2 = img2[(middleRow*width*3) + x + 1]; + unsigned int b2 = img2[(middleRow*width*3) + x + 2]; + colorSum2 += r2 + g2 + b2; + unsigned int r3 = img3[(middleRow*width*3) + x]; + unsigned int g3 = img3[(middleRow*width*3) + x + 1]; + unsigned int b3 = img3[(middleRow*width*3) + x + 2]; + colorSum3 += r3 + g3 + b3; + unsigned int r4 = img4[(middleRow*width*3) + x]; + unsigned int g4 = img4[(middleRow*width*3) + x + 1]; + unsigned int b4 = img4[(middleRow*width*3) + x + 2]; + colorSum4 += r4 + g4 + b4; + } + + // Check that the legacy mode distorts the pincushion images less + EXPECT_GT(colorSum1, colorSum2+800); + // Check that there is a good difference in the barrel images + // this difference is largely caused by the edges of the new + // distortion model which appear gray when they have no value + // and gray (192) has a much lower value than white (765) + EXPECT_GT(colorSum3, colorSum4+20000); + + // Check that the corners contain rendered pixels. + // Specifically, the corners in each image should be white and + // have a value of 765=255*3 since the background of the environment + // is white + unsigned int cornerColorSumImg1 = img[0] + img[1] + img[2]; + unsigned int cornerColorSumImg2 = img2[0] + img2[1] + img2[2]; + unsigned int cornerColorSumImg3 = img3[0] + img3[1] + img3[2]; + EXPECT_EQ(cornerColorSumImg1, 765u); + EXPECT_EQ(cornerColorSumImg2, 765u); + EXPECT_EQ(cornerColorSumImg3, 765u); + // Check that this image is not cropped and that the corner pixel + // has the gray value assigned to unrendered pixels + unsigned int cornerColorSumImg4 = img4[0] + img4[1] + img4[2]; + EXPECT_EQ(cornerColorSumImg4, 192u); + + auto getFirstColIdxOfMineCart = [](const unsigned char* img) + { + for (unsigned int x = 0; x < width*3; x+=3) + { + const unsigned int r = img[(middleRow*width*3) + x]; + const unsigned int g = img[(middleRow*width*3) + x + 1]; + const unsigned int b = img[(middleRow*width*3) + x + 2]; + const unsigned int pixelSum = r + g + b; + if (pixelSum != 765u && pixelSum != 192u) { + return x; + } + } + return (width-1) * 3; + }; + + // Check mine cart location meets expectations + const unsigned int mineCartColIdx1 = getFirstColIdxOfMineCart(img); + const unsigned int mineCartColIdx2 = getFirstColIdxOfMineCart(img2); + const unsigned int mineCartColIdx3 = getFirstColIdxOfMineCart(img3); + const unsigned int mineCartColIdx4 = getFirstColIdxOfMineCart(img4); + + // Check that, in the pin cushion case, the mine cart is seen closer to the + // left in the new version. This makes sense as the new distortion mode + // should have more significant distortion than the legacy mode. + EXPECT_GT(mineCartColIdx1, mineCartColIdx2+15); + // Check that, in the barrel case, the mine cart is seen closer to the left + // in the legacy distortion mode than in the new mode. This makes sense + // because the legacy mode crops the image removing the edges where there + // is no image information. + EXPECT_LT(mineCartColIdx3, mineCartColIdx4-25); + + delete[] img; + delete[] img2; + delete[] img3; + delete[] img4; +} diff --git a/worlds/test/issue_3003_distortion_implementation_correction.world b/worlds/test/issue_3003_distortion_implementation_correction.world new file mode 100644 index 0000000000..5c310434ef --- /dev/null +++ b/worlds/test/issue_3003_distortion_implementation_correction.world @@ -0,0 +1,66 @@ + + + + 0 + 0.4 0.4 0.4 1 + 1 1 1 1 + + + 1 + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/mine cart/4/files/meshes/minecart.dae + + + + 10 + + + + + + + + + + + + + + + + + https://fuel.ignitionrobotics.org/1.0/openrobotics/models/mine cart/4/files/meshes/minecart.dae + + polySurface1278 +
0
+ + + + + 1 1 1 1 + 1 1 1 1 + + + materials/textures/MineCart_Albedo.png + materials/textures/MineCart_Metalness.png + materials/textures/MineCart_Roughness.png + + + + + + 0 + 0 + 0 + + 3 1 0 0 -0 0 + + +