diff --git a/examples/wide_angle_camera/CMakeLists.txt b/examples/wide_angle_camera/CMakeLists.txt new file mode 100644 index 000000000..68f17634f --- /dev/null +++ b/examples/wide_angle_camera/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +project(ignition-rendering-wide-angle-camera) +find_package(ignition-rendering7 REQUIRED) + +include_directories(SYSTEM + ${PROJECT_BINARY_DIR} +) + +if (APPLE OR UNIX) + find_package(GLUT REQUIRED) + include_directories(SYSTEM ${GLUT_INCLUDE_DIRS}) + link_directories(${GLUT_LIBRARY_DIRS}) + + find_package(OpenGL REQUIRED) + include_directories(SYSTEM ${OpenGL_INCLUDE_DIRS}) + link_directories(${OpenGL_LIBRARY_DIRS}) + + set(TARGET_THIRD_PARTY_DEPENDS + ${TARGET_THIRD_PARTY_DEPENDS} + ${OPENGL_LIBRARIES} + ${GLUT_LIBRARIES} + ) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations") +endif() + +if (NOT APPLE) + find_package(GLEW REQUIRED) + include_directories(SYSTEM ${GLEW_INCLUDE_DIRS}) + link_directories(${GLEW_LIBRARY_DIRS}) + + set(TARGET_THIRD_PARTY_DEPENDS + ${TARGET_THIRD_PARTY_DEPENDS} + ${GLEW_LIBRARIES} + ) +endif() + +if (WIN32) + find_package(FreeGLUT REQUIRED) + set(TARGET_THIRD_PARTY_DEPENDS ${TARGET_THIRD_PARTY_DEPENDS} FreeGLUT::freeglut) +endif() + +add_executable(wide_angle_camera Main.cc GlutWindow.cc) + +target_link_libraries(wide_angle_camera + ${IGNITION-RENDERING_LIBRARIES} + ${TARGET_THIRD_PARTY_DEPENDS} +) + +if (WIN32) + set_target_properties(wide_angle_camera + PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${PROJECT_BINARY_DIR} + RUNTIME_OUTPUT_DIRECTORY_DEBUG ${PROJECT_BINARY_DIR} + RUNTIME_OUTPUT_DIRECTORY_RELEASE ${PROJECT_BINARY_DIR} + RUNTIME_OUTPUT_DIRECTORY_RELWITHDEBINFO ${PROJECT_BINARY_DIR} + RUNTIME_OUTPUT_DIRECTORY_MINSIZEREL ${PROJECT_BINARY_DIR} + ) +endif() diff --git a/examples/wide_angle_camera/GlutWindow.cc b/examples/wide_angle_camera/GlutWindow.cc new file mode 100644 index 000000000..e3cda1977 --- /dev/null +++ b/examples/wide_angle_camera/GlutWindow.cc @@ -0,0 +1,232 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#if __APPLE__ + #include + #include + #include +#elif _WIN32 + #define NOMINMAX + #include + #include + #include + #include + #include "Wingdi.h" +#else + #include + #include + #include +#endif + +#if !defined(__APPLE__) && !defined(_WIN32) + #include +#endif + +#include + +#include +#include +#include +#include + +#include "GlutWindow.hh" + +#define KEY_ESC 27 +#define KEY_TAB 9 + +////////////////////////////////////////////////// +unsigned int imgw = 0; +unsigned int imgh = 0; + +std::vector g_cameras; +ir::CameraPtr g_camera; +ir::CameraPtr g_currCamera; +unsigned int g_cameraIndex = 0; +ir::ImagePtr g_image; +ignition::common::ConnectionPtr g_connection; + +bool g_initContext = false; + +#if __APPLE__ + CGLContextObj g_context; + CGLContextObj g_glutContext; +#elif _WIN32 + HGLRC g_context = 0; + HDC g_display = 0; + HGLRC g_glutContext = 0; + HDC g_glutDisplay = 0; +#else + GLXContext g_context; + Display *g_display; + GLXDrawable g_drawable; + GLXContext g_glutContext; + Display *g_glutDisplay; + GLXDrawable g_glutDrawable; +#endif + +double g_offset = 0.0; + +////////////////////////////////////////////////// +void displayCB() +{ +#if __APPLE__ + CGLSetCurrentContext(g_context); +#elif _WIN32 + if (!wglMakeCurrent(g_display, g_context)) + { + std::cerr << "Error callcing wglMakeCurrent" << '\n'; + exit(-1); + } +#else + if (g_display) + { + glXMakeCurrent(g_display, g_drawable, g_context); + } +#endif + + g_cameras[g_cameraIndex]->Update(); + +#if __APPLE__ + CGLSetCurrentContext(g_glutContext); +#elif _WIN32 + wglMakeCurrent(g_glutDisplay, g_glutContext); +#else + glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext); +#endif + + unsigned char *data = g_image->Data(); + + glClearColor(0.5, 0.5, 0.5, 1); + glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT); + glPixelZoom(1, -1); + glRasterPos2f(-1, 1); + glDrawPixels(imgw, imgh, GL_RGB, GL_UNSIGNED_BYTE, data); + + glutSwapBuffers(); +} + +////////////////////////////////////////////////// +void idleCB() +{ + glutPostRedisplay(); +} + +////////////////////////////////////////////////// +void keyboardCB(unsigned char _key, int, int) +{ + if (_key == KEY_ESC || _key == 'q' || _key == 'Q') + { + exit(0); + } + else if (_key == KEY_TAB) + { + g_cameraIndex = (g_cameraIndex + 1) % g_cameras.size(); + } +} + +////////////////////////////////////////////////// +void OnNewWideAngleFrame(const void *_data, + unsigned int _width, unsigned int _height, + unsigned int _channels, + const std::string &/*_format*/) +{ + unsigned char *data = g_image->Data(); + memcpy(data, _data,_width * _height * _channels); +} + +////////////////////////////////////////////////// +void initCamera(ir::CameraPtr _camera) +{ + g_camera = _camera; + imgw = g_camera->ImageWidth(); + imgh = g_camera->ImageHeight(); + ir::Image image = g_camera->CreateImage(); + g_image = std::make_shared(image); + + ir::WideAngleCameraPtr wideAngleCamera = + std::dynamic_pointer_cast( + g_camera); + + // connect to new image event + g_connection = wideAngleCamera->ConnectNewWideAngleFrame( + std::bind(OnNewWideAngleFrame, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + + // update the camera once + g_camera->Update(); +} + +////////////////////////////////////////////////// +void initContext() +{ + glutInitDisplayMode(GLUT_DOUBLE); + glutInitWindowPosition(0, 0); + glutInitWindowSize(imgw, imgh); + glutCreateWindow("Wide Angle Camera"); + glutDisplayFunc(displayCB); + glutIdleFunc(idleCB); + glutKeyboardFunc(keyboardCB); +} + +////////////////////////////////////////////////// +void printUsage() +{ + std::cout << "===============================" << std::endl; + std::cout << " TAB - Switch render engines " << std::endl; + std::cout << " ESC - Exit " << std::endl; + std::cout << "===============================" << std::endl; +} + +////////////////////////////////////////////////// +void run(std::vector _cameras) +{ + if (_cameras.empty()) + { + ignerr << "No cameras found. Scene will not be rendered" << std::endl; + return; + } + +#if __APPLE__ + g_context = CGLGetCurrentContext(); +#elif _WIN32 + g_context = wglGetCurrentContext(); + g_display = wglGetCurrentDC(); +#else + g_context = glXGetCurrentContext(); + g_display = glXGetCurrentDisplay(); + g_drawable = glXGetCurrentDrawable(); +#endif + + g_cameras = _cameras; + initCamera(_cameras[0]); + initContext(); + printUsage(); + +#if __APPLE__ + g_glutContext = CGLGetCurrentContext(); +#elif _WIN32 + g_glutContext = wglGetCurrentContext(); + g_glutDisplay = wglGetCurrentDC(); +#else + g_glutDisplay = glXGetCurrentDisplay(); + g_glutDrawable = glXGetCurrentDrawable(); + g_glutContext = glXGetCurrentContext(); +#endif + + glutMainLoop(); +} diff --git a/examples/wide_angle_camera/GlutWindow.hh b/examples/wide_angle_camera/GlutWindow.hh new file mode 100644 index 000000000..94b2b497a --- /dev/null +++ b/examples/wide_angle_camera/GlutWindow.hh @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_EXAMPLES_WIDE_ANGLE_CAMERA_GLUTWINDOW_HH_ +#define IGNITION_RENDERING_EXAMPLES_WIDE_ANGLE_CAMERA_GLUTWINDOW_HH_ + +#include +#include "ignition/rendering/RenderTypes.hh" + +namespace ir = ignition::rendering; + +/// \brief Run the demo and render the scene from the cameras +/// \param[in] _cameras Cameras in the scene +void run(std::vector _cameras); + +#endif diff --git a/examples/wide_angle_camera/Main.cc b/examples/wide_angle_camera/Main.cc new file mode 100644 index 000000000..1a107b3f5 --- /dev/null +++ b/examples/wide_angle_camera/Main.cc @@ -0,0 +1,195 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#if defined(__APPLE__) + #include + #include +#elif _WIN32 + #define NOMINMAX + #include + #include + #include + #include +#else + #include + #include + #include +#endif + +#include +#include + +#include +#include + +#include "GlutWindow.hh" + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +void buildScene(ScenePtr _scene) +{ + // initialize _scene + _scene->SetAmbientLight(0.3, 0.3, 0.3); + _scene->SetBackgroundColor(0.3, 0.3, 0.3); + VisualPtr root = _scene->RootVisual(); + + // create directional light + DirectionalLightPtr light0 = _scene->CreateDirectionalLight(); + light0->SetDirection(0.5, 0.5, -1); + light0->SetDiffuseColor(0.8, 0.8, 0.8); + light0->SetSpecularColor(0.5, 0.5, 0.5); + root->AddChild(light0); + + // create blue material + MaterialPtr blue = _scene->CreateMaterial(); + blue->SetAmbient(0.0, 0.0, 0.3); + blue->SetDiffuse(0.0, 0.0, 0.8); + blue->SetSpecular(0.5, 0.5, 0.5); + blue->SetShininess(50); + blue->SetReflectivity(0); + + // create box visual + VisualPtr box = _scene->CreateVisual("box"); + box->AddGeometry(_scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(5, 0, 0); + box->SetLocalScale(3, 3, 3); + box->SetMaterial(blue); + root->AddChild(box); + + // create red material + MaterialPtr red = _scene->CreateMaterial(); + red->SetAmbient(0.3, 0.0, 0.0); + red->SetDiffuse(0.8, 0.0, 0.0); + red->SetSpecular(0.5, 0.5, 0.5); + + // create another box + VisualPtr box2 = _scene->CreateVisual("box2"); + box2->AddGeometry(_scene->CreateBox()); + box2->SetLocalPosition(3, -2, 1); + box2->SetLocalRotation(0, 0.3, 0.7); + box2->SetMaterial(red); + root->AddChild(box2); + + // create green material + MaterialPtr green = _scene->CreateMaterial(); + green->SetAmbient(0.0, 0.2, 0.0); + green->SetDiffuse(0.0, 0.6, 0.0); + green->SetSpecular(0.5, 0.5, 0.5); + + // create a sphere + VisualPtr sphere = _scene->CreateVisual("sphere"); + sphere->AddGeometry(_scene->CreateSphere()); + sphere->SetLocalPosition(3, 2.5, 0); + sphere->SetLocalScale(1.5, 1.5, 1.5); + sphere->SetMaterial(green); + root->AddChild(sphere); + + // create gray material + MaterialPtr gray = _scene->CreateMaterial(); + gray->SetAmbient(0.7, 0.7, 0.7); + gray->SetDiffuse(0.7, 0.7, 0.7); + gray->SetSpecular(0.7, 0.7, 0.7); + + VisualPtr grid = _scene->CreateVisual(); + GridPtr gridGeom = _scene->CreateGrid(); + gridGeom->SetCellCount(20); + gridGeom->SetCellLength(1); + gridGeom->SetVerticalCellCount(0); + grid->AddGeometry(gridGeom); + grid->SetLocalPosition(3, 0, 0.0); + grid->SetMaterial(gray); + root->AddChild(grid); + +//! [create wide angle camera] + CameraLens lens; + lens.SetCustomMappingFunction(1.05, 4.0, AFT_TAN, 1.0, 0.0); + lens.SetType(MFT_CUSTOM); + lens.SetCutOffAngle(IGN_PI); + + WideAngleCameraPtr camera = _scene->CreateWideAngleCamera("camera"); + camera->SetLens(lens); + camera->SetHFOV(3.0); + camera->SetLocalPosition(0.0, 0.0, 0.5); + camera->SetLocalRotation(0.0, 0.0, 0.0); + camera->SetImageWidth(800); + camera->SetImageHeight(600); + camera->SetAntiAliasing(2); + camera->SetAspectRatio(1.333); + root->AddChild(camera); +//! [create wide angle camera] +} + +////////////////////////////////////////////////// +CameraPtr createCamera(const std::string &_engineName) +{ + // create and populate scene + RenderEngine *engine = rendering::engine(_engineName); + if (!engine) + { + ignwarn << "Engine '" << _engineName + << "' is not supported" << std::endl; + return CameraPtr(); + } + ScenePtr scene = engine->CreateScene("scene"); + buildScene(scene); + + // return camera sensor + SensorPtr sensor = scene->SensorByName("camera"); + return std::dynamic_pointer_cast(sensor); +} + +////////////////////////////////////////////////// +int main(int _argc, char** _argv) +{ + glutInit(&_argc, _argv); + + // Expose engine name to command line because we can't instantiate both + // ogre and ogre2 at the same time + std::string ogreEngineName("ogre"); + if (_argc > 1) + { + ogreEngineName = _argv[1]; + } + + common::Console::SetVerbosity(4); + std::vector engineNames; + std::vector cameras; + + engineNames.push_back(ogreEngineName); + engineNames.push_back("optix"); + + for (auto engineName : engineNames) + { + try + { + CameraPtr camera = createCamera(engineName); + if (camera) + { + cameras.push_back(camera); + } + } + catch (...) + { + std::cerr << "Error starting up: " << engineName << std::endl; + } + } + run(cameras); + return 0; +} diff --git a/include/ignition/rendering/CameraLens.hh b/include/ignition/rendering/CameraLens.hh new file mode 100644 index 000000000..64babc9bf --- /dev/null +++ b/include/ignition/rendering/CameraLens.hh @@ -0,0 +1,190 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef IGNITION_RENDERING_CAMERALENS_HH_ +#define IGNITION_RENDERING_CAMERALENS_HH_ + +#include + +#include +#include + +#include "ignition/rendering/config.hh" +#include "ignition/rendering/Export.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Enum for mapping function types + enum IGNITION_RENDERING_VISIBLE MappingFunctionType + { + /// \brief Gnomonic + MFT_GNOMONIC = 0, + + /// \brief Stereographic + MFT_STEREOGRAPHIC = 1, + + /// \brief Equidistant + MFT_EQUIDISTANT = 2, + + /// \brief Equisolid angle + MFT_EQUISOLID_ANGLE = 3, + + /// \brief Orthographic + MFT_ORTHOGRAPHIC = 4, + + /// \brief custom + MFT_CUSTOM = 5 + }; + + /// \brief Enum for angle function types + enum IGNITION_RENDERING_VISIBLE AngleFunctionType + { + /// \brief identity + AFT_IDENTITY = 0, + + /// \brief sin + AFT_SIN = 1, + + /// \brief TAN + AFT_TAN = 2 + }; + + /// \brief Describes a lens of a camera + /// as amapping function of type r = c1*f*fun(theta/c2+c3) + class IGNITION_RENDERING_VISIBLE CameraLens + { + /// \brief Constructor + public: CameraLens(); + + /// \brief Destructor + public: virtual ~CameraLens(); + + /// \brief Constructor + /// \param[in] _other The other camera lens + public: explicit CameraLens(const CameraLens &_other); + + /// \brief Set custom camera lens with specified parameters + /// \param[in] _c1 Image scaling factor + /// \param[in] _c2 Angle scaling factor + /// \param[in] _fun Angle transform function + /// \param[in] _f Focal length of the optical system + /// \param[in] _c3 Angle shift parameter, should be 0 in most cases + public: void SetCustomMappingFunction(double _c1, double _c2, + AngleFunctionType _fun, double _f, double _c3); + + /// \brief Get lens projection type + /// \return Lens projection / mapping function type + public: MappingFunctionType Type() const; + + /// \brief Checks if lens type is of the custom type + /// \return True if this->Type() == MFT_CUSTOM + public: bool IsCustom() const; + + /// \brief Gets c1 parameter + /// \return c1 parameter + public: double C1() const; + + /// \brief Gets c2 parameter + /// \return c2 parameter + public: double C2() const; + + /// \brief Gets c3 parameter + /// \return c3 parameter + public: double C3() const; + + /// \brief Gets f parameter + /// \return f parameter + public: double F() const; + + /// \brief Gets angle transform function + /// \return Angle transform function + public: AngleFunctionType AngleFunction() const; + + /// \brief Gets cut off angle + /// \return Cut off angle + public: double CutOffAngle() const; + + /// \brief Checks if image should be scaled to fit horizontal FOV + /// \return True if the image will be scaled + public: bool ScaleToHFOV() const; + + /// \brief Set lens projection type + /// \param[in] _type Lens projection / mapping function type + public: void SetType(MappingFunctionType _type); + + /// \brief Sets c1 parameter + /// \param[in] _c c1 parameter + public: void SetC1(double _c); + + /// \brief Sets c2 parameter + /// \param[in] _c c2 parameter + public: void SetC2(double _c); + + /// \brief Sets c3 parameter + /// \param[in] _c c3 parameter + public: void SetC3(double _c); + + /// \brief Sets f parameter + /// \param[in] _f f parameter + public: void SetF(double _f); + + /// \brief Sets angle transform function + /// \param[in] _fun Angle transform function + public: void SetAngleFunction(AngleFunctionType _fun); + + /// \brief Sets cut-off angle + /// \param[in] _angle cut-off angle + public: void SetCutOffAngle(double _angle); + + /// \brief Sets whether the image should be scaled to fit horizontal FOV + /// If True, the projection will compute a new focal length for achieving + /// the desired FOV + /// \param[in] _scale true if it should, + /// note: c1 and f parameters are ignored in this case + public: void SetScaleToHFOV(bool _scale); + + /// \brief Apply mapping function to input number + /// \param[in] _f Input floating point number to apply the mapping + /// function to. + /// \return Result of the application + public: float ApplyMappingFunction(float _f) const; + + /// \internal + /// \brief Get mapping function as vector3d + /// return unit vector, either unit x, y, or z. + public: math::Vector3d MappingFunctionAsVector3d() const; + + /// \brief Assignment operator + /// \param[in] _other The other camera lens + public: CameraLens &operator=(const CameraLens &_other); + + /// \internal + /// \brief Converts projection type from one of presets to `custom` + private: void ConvertToCustom(); + + /// \internal + /// \brief Private data pointer + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/ignition/rendering/RenderTypes.hh b/include/ignition/rendering/RenderTypes.hh index c7fc5dcd8..a44116ae9 100644 --- a/include/ignition/rendering/RenderTypes.hh +++ b/include/ignition/rendering/RenderTypes.hh @@ -91,6 +91,7 @@ namespace ignition class Text; class ThermalCamera; class Visual; + class WideAngleCamera; class WireBox; /// \typedef ArrowVisualPtr @@ -121,6 +122,10 @@ namespace ignition /// \brief Shared pointer to Segmentation Camera typedef shared_ptr SegmentationCameraPtr; + /// \typedef WideAngleCameraPtr + /// \brief Shared pointer to Wide Angle Camera + typedef shared_ptr WideAngleCameraPtr; + /// \typedef GpuRaysPtr /// \brief Shared pointer to GpuRays typedef shared_ptr GpuRaysPtr; @@ -295,6 +300,10 @@ namespace ignition /// \brief Shared pointer to const Segmentation Camera typedef shared_ptr ConstSegmentationCameraPtr; + /// \typedef const SegmentationCameraPtr + /// \brief Shared pointer to const Wide Angle Camera + typedef shared_ptr ConstWideAngleCameraPtr; + /// \typedef const GpuRaysPtr /// \brief Shared pointer to const GpuRays typedef shared_ptr ConstGpuRaysPtr; diff --git a/include/ignition/rendering/Scene.hh b/include/ignition/rendering/Scene.hh index d88fd6bd8..c769bedd5 100644 --- a/include/ignition/rendering/Scene.hh +++ b/include/ignition/rendering/Scene.hh @@ -737,6 +737,35 @@ namespace ignition public: virtual SegmentationCameraPtr CreateSegmentationCamera( unsigned int _id, const std::string &_name) = 0; + /// \brief Create new wide angle camera. A unique ID and name will + /// automatically be assigned to the camera. + /// \return The created camera + public: virtual WideAngleCameraPtr CreateWideAngleCamera() = 0; + + /// \brief Create wide angle camera with the given ID. + /// A unique name will automatically be assigned to the camera. + /// If the given ID is already in use, NULL will be returned. + /// \param[in] _id ID of the new camera + /// \return The created camera + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + unsigned int _id) = 0; + + /// \brief Create new wide angle camera with the given name. + /// A unique ID will automatically be assigned to the camera. + /// If the given name is already in use, NULL will be returned. + /// \param[in] _name Name of the new camera + /// \return The created camera + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + const std::string &_name) = 0; + + /// \brief Create new wide angle camera with the given name and ID. If + /// either the given ID or name is already in use, will return NULL. + /// \param[in] _id ID of the new camera + /// \param[in] _name Name of the new camera + /// \return The created camera + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + unsigned int _id, const std::string &_name) = 0; + /// \brief Create new gpu rays caster. A unique ID and name will /// automatically be assigned to the gpu rays caster. /// \return The created gpu rays caster diff --git a/include/ignition/rendering/WideAngleCamera.hh b/include/ignition/rendering/WideAngleCamera.hh new file mode 100644 index 000000000..083c79764 --- /dev/null +++ b/include/ignition/rendering/WideAngleCamera.hh @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_WIDEANGLECAMERA_HH_ +#define IGNITION_RENDERING_WIDEANGLECAMERA_HH_ + +#include + +#include + +#include "ignition/rendering/Camera.hh" +#include "ignition/rendering/CameraLens.hh" +#include "ignition/rendering/Scene.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \class WideAngleCamera WideAngleCamera.hh + /// ignition/rendering/WideAngleCamera.hh + /// \brief Wide angle camera class + class IGNITION_RENDERING_VISIBLE WideAngleCamera : + public virtual Camera + { + /// \brief Destructor + public: virtual ~WideAngleCamera() { } + + /// \brief Set the camera lens to use for this wide angle camera + /// \param[in] _lens Camera lens to set + public: virtual void SetLens(const CameraLens &_lens) = 0; + + /// \brief Get the camera lens used by this wide angle camera + /// \return Camera lens set to this wide angle camera + public: virtual const CameraLens &Lens() const = 0; + + /// \brief Project 3D world coordinates to screen coordinates + /// \param[in] _pt 3D world coodinates + /// \return Screen coordinates. Z is the distance of point from camera + /// optical center. + public: virtual math::Vector3d Project3d(const math::Vector3d &_pt) const + = 0; + + /// \brief Subscribes a new listener to this camera's new frame event + /// \param[in] _subscriber New camera listener callback + public: virtual common::ConnectionPtr ConnectNewWideAngleFrame( + std::function _subscriber) = 0; + }; + } + } +} +#endif diff --git a/include/ignition/rendering/base/BaseScene.hh b/include/ignition/rendering/base/BaseScene.hh index 29d30a015..c301b28b5 100644 --- a/include/ignition/rendering/base/BaseScene.hh +++ b/include/ignition/rendering/base/BaseScene.hh @@ -368,6 +368,21 @@ namespace ignition public: virtual SegmentationCameraPtr CreateSegmentationCamera( const unsigned int _id, const std::string &_name) override; + // Documentation inherited. + public: virtual WideAngleCameraPtr CreateWideAngleCamera() override; + + // Documentation inherited. + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + const unsigned int _id) override; + + // Documentation inherited. + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + const std::string &_name) override; + + // Documentation inherited. + public: virtual WideAngleCameraPtr CreateWideAngleCamera( + const unsigned int _id, const std::string &_name) override; + // Documentation inherited. public: virtual GpuRaysPtr CreateGpuRays() override; @@ -661,6 +676,22 @@ namespace ignition return SegmentationCameraPtr(); } + /// \brief Implementation for creating a wide angle camera. + /// \param[in] _id Unique id + /// \param[in] _name Name of wide angle camera + /// \return Pointer to wide angle camera + protected: virtual WideAngleCameraPtr CreateWideAngleCameraImpl( + unsigned int _id, + const std::string &_name) + { + // The following two lines will avoid doxygen warnings + (void)_id; + (void)_name; + ignerr << "Wide angle camera not supported by: " + << this->Engine()->Name() << std::endl; + return WideAngleCameraPtr(); + } + /// \brief Implementation for creating GpuRays sensor. /// \param[in] _id Unique id /// \param[in] _name Name of GpuRays sensor diff --git a/include/ignition/rendering/base/BaseWideAngleCamera.hh b/include/ignition/rendering/base/BaseWideAngleCamera.hh new file mode 100644 index 000000000..7ee048d46 --- /dev/null +++ b/include/ignition/rendering/base/BaseWideAngleCamera.hh @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_RENDERING_BASE_BASEWIDEANGLECAMERA_HH_ +#define IGNITION_RENDERING_BASE_BASEWIDEANGLECAMERA_HH_ + +#include + +#include + +#include "ignition/rendering/base/BaseCamera.hh" +#include "ignition/rendering/CameraLens.hh" +#include "ignition/rendering/WideAngleCamera.hh" + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + template + class BaseWideAngleCamera : + public virtual WideAngleCamera, + public virtual BaseCamera, + public virtual T + { + /// \brief Constructor + protected: BaseWideAngleCamera(); + + /// \brief Destructor + public: virtual ~BaseWideAngleCamera(); + + // Documentation inherited. + public: virtual void CreateWideAngleTexture(); + + // Documentation inherited. + public: virtual void SetLens(const CameraLens &_lens) override; + + // Documentation inherited. + public: virtual const CameraLens &Lens() const override; + + // Documentation inherited. + public: virtual math::Vector3d Project3d(const math::Vector3d &_pt) const + override; + + // Documentation inherited. + public: virtual common::ConnectionPtr ConnectNewWideAngleFrame( + std::function _subscriber) override; + + /// \brief Camera lens used by this wide angle camera + protected: CameraLens lens; + }; + + ////////////////////////////////////////////////// + template + BaseWideAngleCamera::BaseWideAngleCamera() + { + } + + ////////////////////////////////////////////////// + template + BaseWideAngleCamera::~BaseWideAngleCamera() + { + } + + ////////////////////////////////////////////////// + template + void BaseWideAngleCamera::SetLens(const CameraLens &_lens) + { + this->lens = _lens; + } + + ////////////////////////////////////////////////// + template + const CameraLens &BaseWideAngleCamera::Lens() const + { + return this->lens; + } + + ////////////////////////////////////////////////// + template + void BaseWideAngleCamera::CreateWideAngleTexture() + { + } + + ////////////////////////////////////////////////// + template + math::Vector3d BaseWideAngleCamera:: Project3d(const math::Vector3d &) + const + { + ignerr << "Project3d is not supported for " + << "render engine: " << this->Scene()->Engine()->Name() + << std::endl; + return math::Vector3d(); + } + + ////////////////////////////////////////////////// + template + common::ConnectionPtr BaseWideAngleCamera::ConnectNewWideAngleFrame( + std::function) + { + return nullptr; + } + } + } +} +#endif diff --git a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh index 9e8292e2b..d4d83f71b 100644 --- a/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh +++ b/ogre/include/ignition/rendering/ogre/OgreRenderTypes.hh @@ -66,6 +66,7 @@ namespace ignition class OgreText; class OgreThermalCamera; class OgreVisual; + class OgreWideAngleCamera; class OgreWireBox; typedef BaseSceneStore OgreSceneStore; @@ -125,6 +126,7 @@ namespace ignition typedef shared_ptr OgreThermalCameraPtr; typedef shared_ptr OgreVisualPtr; typedef shared_ptr OgreVisualStorePtr; + typedef shared_ptr OgreWideAngleCameraPtr; typedef shared_ptr OgreWireBoxPtr; } } diff --git a/ogre/include/ignition/rendering/ogre/OgreScene.hh b/ogre/include/ignition/rendering/ogre/OgreScene.hh index 81278e160..2631c25dd 100644 --- a/ogre/include/ignition/rendering/ogre/OgreScene.hh +++ b/ogre/include/ignition/rendering/ogre/OgreScene.hh @@ -112,6 +112,11 @@ namespace ignition const unsigned int _id, const std::string &_name) override; + // Documentation inherited + protected: virtual WideAngleCameraPtr CreateWideAngleCameraImpl( + const unsigned int _id, + const std::string &_name) override; + protected: virtual GpuRaysPtr CreateGpuRaysImpl( const unsigned int _id, const std::string &_name) override; diff --git a/ogre/include/ignition/rendering/ogre/OgreWideAngleCamera.hh b/ogre/include/ignition/rendering/ogre/OgreWideAngleCamera.hh new file mode 100644 index 000000000..94bb31b63 --- /dev/null +++ b/ogre/include/ignition/rendering/ogre/OgreWideAngleCamera.hh @@ -0,0 +1,146 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef IGNITION_RENDERING_OGRE_WIDEANGLECAMERA_HH_ +#define IGNITION_RENDERING_OGRE_WIDEANGLECAMERA_HH_ + +#include +#include +#include + +#include + +#include "ignition/rendering/base/BaseWideAngleCamera.hh" +#include "ignition/rendering/ogre/OgreRenderTarget.hh" +#include "ignition/rendering/ogre/OgreRenderTypes.hh" +#include "ignition/rendering/ogre/OgreScene.hh" +#include "ignition/rendering/ogre/OgreSensor.hh" + +namespace Ogre +{ + class Material; + class RenderTarget; + class Viewport; +} + +namespace ignition +{ + namespace rendering + { + inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Ogre implementation of WideAngleCamera + class IGNITION_RENDERING_OGRE_VISIBLE OgreWideAngleCamera : + public BaseWideAngleCamera, + protected Ogre::CompositorInstance::Listener + { + /// \brief Constructor + protected: OgreWideAngleCamera(); + + /// \brief Destructor + public: virtual ~OgreWideAngleCamera(); + + // Documentation inherited + public: virtual void Init() override; + + /// \brief Create a texture + public: virtual void CreateRenderTexture(); + + /// \brief Render the camera + public: virtual void PostRender() override; + + // Documentation inherited + public: virtual void Destroy() override; + + /// \brief Gets the environment texture size + /// \return Texture size + public: unsigned int EnvTextureSize() const; + + /// \brief Sets environment texture size + /// \param[in] _size Texture size + public: void SetEnvTextureSize(int _size); + + /// \brief Set the background color of the camera + /// \param[in] _color Color to set the background to + /// \return True if the background color is set successfully, + /// false otherwise + public: bool SetBackgroundColor(const math::Color &_color); + + /// \brief Project 3D world coordinates to screen coordinates + /// \param[in] _pt 3D world coodinates + /// \return Screen coordinates. Z is the distance of point from camera + /// optical center. + public: math::Vector3d Project3d(const math::Vector3d &_pt) const + override; + + /// \brief Get the list of ogre cameras used to create the cube map for + /// generating the wide angle camera image + /// \return A list of OGRE cameras + public: std::vector OgreEnvCameras() const; + + /// \brief Set uniform variables of a shader + /// for the provided material technique pass + /// \param[in] _pass Ogre::Pass used for rendering + /// \param[in] _ratio Frame aspect ratio + /// \param[in] _hfov Horizontal field of view + public: void SetUniformVariables(Ogre::Pass *_pass, + float _ratio, float _hfov); + + // Documentation inherited. + public: virtual void PreRender() override; + + /// \brief Implementation of the render call + public: virtual void Render() override; + + // Documentation inherited + public: common::ConnectionPtr ConnectNewWideAngleFrame( + std::function _subscriber) override; + + // Documentation inherited. + public: virtual void SetVisibilityMask(uint32_t _mask) override; + + /// \brief Set the camera's render target + protected: void CreateWideAngleTexture() override; + + /// \brief Create the camera. + protected: void CreateCamera(); + + /// \brief Creates a set of 6 cameras pointing in different directions + protected: void CreateEnvCameras(); + + /// \brief Callback that is used to set mapping material uniform values, + /// implements Ogre::CompositorInstance::Listener interface + /// \param[in] _pass_id Pass identifier + /// \param[in] _material Material whose parameters should be set + protected: void notifyMaterialRender(Ogre::uint32 _pass_id, + Ogre::MaterialPtr &_material) override; + + /// \brief Get a pointer to the render target. + /// \return Pointer to the render target + protected: virtual RenderTargetPtr RenderTarget() const override; + + private: friend class OgreScene; + + /// \cond warning + /// \brief Private data pointer + IGN_UTILS_UNIQUE_IMPL_PTR(dataPtr) + /// \endcond + }; + } + } +} +#endif diff --git a/ogre/src/OgreScene.cc b/ogre/src/OgreScene.cc index a84a7f28a..dd74dabfd 100644 --- a/ogre/src/OgreScene.cc +++ b/ogre/src/OgreScene.cc @@ -47,6 +47,7 @@ #include "ignition/rendering/ogre/OgreText.hh" #include "ignition/rendering/ogre/OgreThermalCamera.hh" #include "ignition/rendering/ogre/OgreVisual.hh" +#include "ignition/rendering/ogre/OgreWideAngleCamera.hh" #include "ignition/rendering/ogre/OgreWireBox.hh" namespace ignition @@ -450,6 +451,15 @@ ThermalCameraPtr OgreScene::CreateThermalCameraImpl(const unsigned int _id, return (result) ? camera : nullptr; } +/////////////////////////////////////////////////// +WideAngleCameraPtr OgreScene::CreateWideAngleCameraImpl(const unsigned int _id, + const std::string &_name) +{ + OgreWideAngleCameraPtr camera(new OgreWideAngleCamera); + bool result = this->InitObject(camera, _id, _name); + return (result) ? camera : nullptr; +} + /////////////////////////////////////////////////// GpuRaysPtr OgreScene::CreateGpuRaysImpl(const unsigned int _id, const std::string &_name) diff --git a/ogre/src/OgreWideAngleCamera.cc b/ogre/src/OgreWideAngleCamera.cc new file mode 100644 index 000000000..8475a5b2a --- /dev/null +++ b/ogre/src/OgreWideAngleCamera.cc @@ -0,0 +1,666 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifdef __APPLE__ + #define GL_SILENCE_DEPRECATION + #include + #include +#else +#ifndef _WIN32 + #include + #include + + // OGRE/RenderSystems/GL/GL/glext.h does not seem to have + // this defined + #ifndef GL_TEXTURE_CUBE_MAP_SEAMLESS + #define GL_TEXTURE_CUBE_MAP_SEAMLESS 0x884F + #endif +#endif +#endif + +#include "ignition/rendering/CameraLens.hh" + +#include "ignition/rendering/ogre/OgreConversions.hh" +#include "ignition/rendering/ogre/OgreRenderEngine.hh" +#include "ignition/rendering/ogre/OgreRTShaderSystem.hh" +#include "ignition/rendering/ogre/OgreWideAngleCamera.hh" + +/// \brief Private data for the WideAngleCamera class +class ignition::rendering::OgreWideAngleCamera::Implementation +{ + /// \brief Environment texture size + public: int envTextureSize = 512u; + + /// \brief Compositor used to render rectangle with attached cube map + public: Ogre::CompositorInstance *cubeMapCompInstance = nullptr; + + /// \brief Number of cameras used to create the cubemap + public: static const unsigned int kEnvCameraCount = 6u; + + /// \brief A Set of 6 cameras, + /// each pointing in different direction with FOV of 90deg + public: Ogre::Camera *envCameras[kEnvCameraCount]; + + /// \brief Render targets for envCameras + public: Ogre::RenderTarget *envRenderTargets[6]; + + /// \brief Viewports for the render targets + public: Ogre::Viewport *envViewports[6]; + + /// \brief Pixel format for cube map texture + public: Ogre::PixelFormat envCubeMapTextureFormat; + + /// \brief A single cube map texture + public: Ogre::Texture *envCubeMapTexture = nullptr; + + /// \brief Output texture + public: Ogre::Texture *ogreRenderTexture = nullptr; + + /// \brief Pointer to material, used for second rendering pass + public: Ogre::MaterialPtr compMat; + + /// \brief Camera lens description + public: CameraLens lens; + + /// \brief Pointer to the ogre camera + public: Ogre::Camera *ogreCamera = nullptr; + + /// \brief Dummy texture + public: OgreRenderTexturePtr wideAngleTexture; + + /// \brief The image buffer + public: unsigned char *imageBuffer = nullptr; + + /// \brief Outgoing image data, used by newImageFrame event. + public: unsigned char *wideAngleImage = nullptr; + + /// \brief Event used to signal camera data + public: ignition::common::EventT newImageFrame; +}; + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +OgreWideAngleCamera::OgreWideAngleCamera() + : dataPtr(utils::MakeUniqueImpl()) +{ + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + this->dataPtr->envCameras[i] = nullptr; + this->dataPtr->envRenderTargets[i] = nullptr; + this->dataPtr->envViewports[i] = nullptr; + } +} +////////////////////////////////////////////////// +OgreWideAngleCamera::~OgreWideAngleCamera() +{ + this->Destroy(); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::Init() +{ + BaseWideAngleCamera::Init(); + this->CreateCamera(); + this->CreateRenderTexture(); + this->Reset(); +} + +///////////////////////////////////////////////// +void OgreWideAngleCamera::CreateRenderTexture() +{ + RenderTexturePtr base = this->scene->CreateRenderTexture(); + this->dataPtr->wideAngleTexture = + std::dynamic_pointer_cast(base); + this->dataPtr->wideAngleTexture->SetWidth(1); + this->dataPtr->wideAngleTexture->SetHeight(1); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::PreRender() +{ + BaseCamera::PreRender(); + if (!this->dataPtr->ogreRenderTexture) + this->CreateWideAngleTexture(); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::Destroy() +{ + if (this->dataPtr->imageBuffer) + { + delete [] this->dataPtr->imageBuffer; + this->dataPtr->imageBuffer = nullptr; + } + + if (this->dataPtr->wideAngleImage) + { + delete [] this->dataPtr->wideAngleImage; + this->dataPtr->wideAngleImage = nullptr; + } + + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + OgreRTShaderSystem::DetachViewport(this->dataPtr->envViewports[i], + this->scene); + + this->dataPtr->envRenderTargets[i]->removeAllViewports(); + this->dataPtr->envRenderTargets[i] = nullptr; + + this->scene->OgreSceneManager()->destroyCamera( + this->dataPtr->envCameras[i]->getName()); + this->dataPtr->envCameras[i] = nullptr; + } + + if (this->dataPtr->envCubeMapTexture) + { + Ogre::TextureManager::getSingleton().remove( + this->dataPtr->envCubeMapTexture->getName()); + this->dataPtr->envCubeMapTexture = nullptr; + } + + if (this->dataPtr->ogreRenderTexture) + { + Ogre::TextureManager::getSingleton().remove( + this->dataPtr->ogreRenderTexture->getName()); + this->dataPtr->ogreRenderTexture = nullptr; + } +} + +////////////////////////////////////////////////// +unsigned int OgreWideAngleCamera::EnvTextureSize() const +{ + return this->dataPtr->envTextureSize; +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::SetEnvTextureSize(int _size) +{ + this->dataPtr->envTextureSize = _size; +} + +///////////////////////////////////////////////// +void OgreWideAngleCamera::CreateCamera() +{ + // Create dummy ogre camera object + Ogre::SceneManager *ogreSceneManager = this->scene->OgreSceneManager(); + if (ogreSceneManager == nullptr) + { + ignerr << "Scene manager cannot be obtained" << std::endl; + return; + } + + this->dataPtr->ogreCamera = ogreSceneManager->createCamera( + this->Name() + "_Camera"); + if (this->dataPtr->ogreCamera == nullptr) + { + ignerr << "Ogre camera cannot be created" << std::endl; + return; + } + + this->ogreNode->attachObject(this->dataPtr->ogreCamera); + this->dataPtr->ogreCamera->setFixedYawAxis(false); + this->dataPtr->ogreCamera->yaw(Ogre::Degree(-90)); + this->dataPtr->ogreCamera->roll(Ogre::Degree(-90)); + this->dataPtr->ogreCamera->setAutoAspectRatio(true); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::CreateEnvCameras() +{ + double nearPlane = this->NearClipPlane(); + double farPlane = this->FarClipPlane(); + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + std::stringstream name_str; + + name_str << this->name << "_env_" << i; + + this->dataPtr->envCameras[i] = + this->scene->OgreSceneManager()->createCamera(name_str.str()); + + this->dataPtr->envCameras[i]->setFixedYawAxis(false); + this->dataPtr->envCameras[i]->setFOVy(Ogre::Degree(90)); + this->dataPtr->envCameras[i]->setAspectRatio(1); + + this->dataPtr->envCameras[i]->yaw(Ogre::Degree(-90.0)); + this->dataPtr->envCameras[i]->roll(Ogre::Degree(-90.0)); + + this->dataPtr->envCameras[i]->setNearClipDistance(nearPlane); + this->dataPtr->envCameras[i]->setFarClipDistance(farPlane); + } + + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + this->ogreNode->attachObject(this->dataPtr->envCameras[i]); + } + + // set environment cameras orientation + this->dataPtr->envCameras[0]->yaw(Ogre::Degree(-90)); + this->dataPtr->envCameras[1]->yaw(Ogre::Degree(90)); + this->dataPtr->envCameras[2]->pitch(Ogre::Degree(90)); + this->dataPtr->envCameras[3]->pitch(Ogre::Degree(-90)); + this->dataPtr->envCameras[5]->yaw(Ogre::Degree(180)); +} + + +////////////////////////////////////////////////// +bool OgreWideAngleCamera::SetBackgroundColor(const math::Color &_color) +{ + bool retVal = true; + Ogre::ColourValue clr = OgreConversions::Convert(_color); + if (this->dataPtr->ogreCamera->getViewport()) + { + this->dataPtr->ogreCamera->getViewport()->setBackgroundColour(clr); + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + if (this->dataPtr->envViewports[i]) + { + this->dataPtr->envViewports[i]->setBackgroundColour(clr); + } + else + { + retVal = false; + } + } + } + else + { + retVal = false; + } + return retVal; +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::CreateWideAngleTexture() +{ + if (this->dataPtr->ogreCamera == nullptr) + { + ignerr << "Ogre camera cannot be created" << std::endl; + return; + } + + if (!this->dataPtr->ogreRenderTexture) + { + this->dataPtr->ogreRenderTexture = + Ogre::TextureManager::getSingleton().createManual( + this->Name() + "_wideAngleCamera", "General", Ogre::TEX_TYPE_2D, + this->ImageWidth(), this->ImageHeight(), 0, + Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET, + 0, false, 0).get(); + Ogre::RenderTarget *rt = + this->dataPtr->ogreRenderTexture->getBuffer()->getRenderTarget(); + rt->setAutoUpdated(false); + Ogre::Viewport *vp = rt->addViewport(this->dataPtr->ogreCamera); + vp->setClearEveryFrame(true); + vp->setShadowsEnabled(false); + vp->setOverlaysEnabled(false); + } + + double ratio = static_cast(this->ImageWidth()) / + static_cast(this->ImageHeight()); + + double vfov = 2.0 * atan(tan(this->HFOV().Radian() / 2.0) / ratio); + this->dataPtr->ogreCamera->setAspectRatio(ratio); + this->dataPtr->ogreCamera->setFOVy(Ogre::Radian(vfov)); + + // create the env cameras and textures + this->CreateEnvCameras(); + + unsigned int fsaa = 0; + std::vector fsaaLevels = + OgreRenderEngine::Instance()->FSAALevels(); + // check if target fsaa is supported + unsigned int targetFSAA = this->antiAliasing; + auto const it = std::find(fsaaLevels.begin(), fsaaLevels.end(), targetFSAA); + if (it != fsaaLevels.end()) + fsaa = targetFSAA; + else + { + // output warning but only do it once + static bool ogreFSAAWarn = false; + if (ogreFSAAWarn) + { + ignwarn << "Anti-aliasing level of '" << this->antiAliasing << "' " + << "is not supported. Setting to 0" << std::endl; + ogreFSAAWarn = true; + } + } + + this->dataPtr->envCubeMapTexture = + Ogre::TextureManager::getSingleton().createManual( + this->name + "_env_tex", + "General", + Ogre::TEX_TYPE_CUBE_MAP, + this->dataPtr->envTextureSize, + this->dataPtr->envTextureSize, + 0, + this->dataPtr->envCubeMapTextureFormat, + Ogre::TU_RENDERTARGET, + 0, + false, + fsaa).getPointer(); + + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + Ogre::RenderTarget *rtt; + rtt = this->dataPtr->envCubeMapTexture->getBuffer(i)->getRenderTarget(); + rtt->setAutoUpdated(false); + + Ogre::Viewport *vp = rtt->addViewport(this->dataPtr->envCameras[i]); + vp->setClearEveryFrame(true); + vp->setShadowsEnabled(true); + vp->setOverlaysEnabled(false); + + OgreRTShaderSystem::AttachViewport(vp, this->scene); + + auto const &bgColor = this->scene->BackgroundColor(); + vp->setBackgroundColour(OgreConversions::Convert(bgColor)); + vp->setVisibilityMask(this->VisibilityMask()); + + this->dataPtr->envViewports[i] = vp; + this->dataPtr->envRenderTargets[i] = rtt; + } + + // create the compositor + this->dataPtr->cubeMapCompInstance = + Ogre::CompositorManager::getSingleton().addCompositor( + this->dataPtr->ogreCamera->getViewport(), + "WideCameraLensMap/ParametrisedMap"); + + this->dataPtr->compMat = + Ogre::MaterialManager::getSingleton().getByName("WideLensMap"); + + auto pass = this->dataPtr->compMat->getTechnique(0)->getPass(0); + + if (!pass->getNumTextureUnitStates()) + pass->createTextureUnitState(); + this->dataPtr->cubeMapCompInstance->addListener(this); + this->dataPtr->cubeMapCompInstance->setEnabled(true); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::Render() +{ + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + this->dataPtr->envRenderTargets[i]->update(); + } + + this->dataPtr->compMat->getTechnique(0)->getPass(0)->getTextureUnitState(0)-> + setTextureName(this->dataPtr->envCubeMapTexture->getName()); + + Ogre::RenderTarget *rt = + this->dataPtr->ogreRenderTexture->getBuffer()->getRenderTarget(); + rt->setAutoUpdated(false); + rt->update(false); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::notifyMaterialRender(Ogre::uint32 /*_pass_id*/, + Ogre::MaterialPtr &_material) +{ + if (_material.isNull()) + return; + + Ogre::Technique *pTechnique = _material->getBestTechnique(); + if (!pTechnique) + return; + + Ogre::Pass *pPass = pTechnique->getPass(0); + if (!pPass || !pPass->hasFragmentProgram()) + return; + + this->SetUniformVariables(pPass, + this->AspectRatio(), + this->HFOV().Radian()); + +#ifndef _WIN32 + // OGRE doesn't allow to enable cubemap filtering extention thru its API + // suppose that this function was invoked in a thread that has OpenGL context + glEnable(GL_TEXTURE_CUBE_MAP_SEAMLESS); +#endif +} + + +////////////////////////////////////////////////// +void OgreWideAngleCamera::SetUniformVariables(Ogre::Pass *_pass, + float _ratio, float _hfov) +{ + Ogre::GpuProgramParametersSharedPtr uniforms = + _pass->getFragmentProgramParameters(); + + uniforms->setNamedConstant("c1", static_cast( + this->Lens().C1())); + uniforms->setNamedConstant("c2", static_cast( + this->Lens().C2())); + uniforms->setNamedConstant("c3", static_cast( + this->Lens().C3())); + + if (this->Lens().ScaleToHFOV()) + { + float param = (_hfov/2)/this->Lens().C2() + + this->Lens().C3(); + float funRes = this->Lens().ApplyMappingFunction( + static_cast(param)); + + float newF = 1.0f/(this->Lens().C1() * funRes); + + uniforms->setNamedConstant("f", static_cast(newF)); + } + else + { + uniforms->setNamedConstant("f", + static_cast(this->Lens().F())); + } + + auto vecFun = this->Lens().MappingFunctionAsVector3d(); + + uniforms->setNamedConstant("fun", Ogre::Vector3( + vecFun.X(), vecFun.Y(), vecFun.Z())); + + uniforms->setNamedConstant("cutOffAngle", + static_cast(this->Lens().CutOffAngle())); + + Ogre::GpuProgramParametersSharedPtr uniformsVs = + _pass->getVertexProgramParameters(); + + uniformsVs->setNamedConstant("ratio", static_cast(_ratio)); +} + +////////////////////////////////////////////////// +math::Vector3d OgreWideAngleCamera::Project3d( + const math::Vector3d &_pt) const +{ + // project onto cubemap face then onto + ignition::math::Vector3d screenPos; + // loop through all env cameras can find the one that sees the 3d world point + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + // project world point to camera clip space. + auto viewProj = this->dataPtr->envCameras[i]->getProjectionMatrix() * + this->dataPtr->envCameras[i]->getViewMatrix(); + auto pos = viewProj * Ogre::Vector4(OgreConversions::Convert(_pt)); + pos.x /= pos.w; + pos.y /= pos.w; + // check if point is visible + if (std::fabs(pos.x) <= 1 && std::fabs(pos.y) <= 1 && pos.z > 0) + { + // determine dir vector to projected point from env camera + // work in y up, z forward, x right clip space + ignition::math::Vector3d dir(pos.x, pos.y, 1); + ignition::math::Quaterniond rot = ignition::math::Quaterniond::Identity; + + // rotate dir vector into wide angle camera frame based on the + // face of the cube. Note: operate in clip space so + // left handed coordinate system rotation + if (i == 0) + rot = ignition::math::Quaterniond(0.0, IGN_PI * 0.5, 0.0); + else if (i == 1) + rot = ignition::math::Quaterniond(0.0, -IGN_PI * 0.5, 0.0); + else if (i == 2) + rot = ignition::math::Quaterniond(-IGN_PI * 0.5, 0.0, 0.0); + else if (i == 3) + rot = ignition::math::Quaterniond(IGN_PI * 0.5, 0.0, 0.0); + else if (i == 5) + rot = ignition::math::Quaterniond(0.0, IGN_PI, 0.0); + dir = rot * dir; + dir.Normalize(); + + // compute theta and phi from the dir vector + // theta is angle to dir vector from z (forward) + // phi is angle from x in x-y plane + // direction vector (x, y, z) + // x = sin(theta)cos(phi) + // y = sin(theta)sin(phi) + // z = cos(theta) + double theta = std::atan2( + std::sqrt(dir.X() * dir.X() + dir.Y() * dir.Y()), dir.Z()); + double phi = std::atan2(dir.Y(), dir.X()); + // this also works: + // double theta = std::acos(dir.Z()); + // double phi = std::asin(dir.Y() / std::sin(theta)); + + double f = this->Lens().F(); + double fov = this->HFOV().Radian(); + // recompute f if scale to HFOV is true + if (this->Lens().ScaleToHFOV()) + { + double param = (fov/2.0) / this->Lens().C2() + this->Lens().C3(); + double funRes = this->Lens().ApplyMappingFunction( + static_cast(param)); + f = 1.0 / (this->Lens().C1() * funRes); + } + + // Apply fisheye lens mapping function + // r is distance of point from image center + double r = this->Lens().C1() * f * + this->Lens().ApplyMappingFunction( + theta/this->Lens().C2() + this->Lens().C3()); + + // compute projected x and y in clip space + double x = cos(phi) * r; + double y = sin(phi) * r; + + unsigned int vpWidth = + this->dataPtr->ogreCamera->getViewport()->getActualWidth(); + unsigned int vpHeight = + this->dataPtr->ogreCamera->getViewport()->getActualHeight(); + // env cam cube map texture is square and likely to be different size from + // viewport. We need to adjust projected pos based on aspect ratio + double asp = static_cast(vpWidth) / + static_cast(vpHeight); + y *= asp; + + // convert to screen space + screenPos.X() = ((x / 2.0) + 0.5) * vpWidth; + screenPos.Y() = (1 - ((y / 2.0) + 0.5)) * vpHeight; + + // r will be > 1.0 if point is not visible (outside of image) + screenPos.Z() = r; + return screenPos; + } + } + + return screenPos; +} + +////////////////////////////////////////////////// +std::vector OgreWideAngleCamera::OgreEnvCameras() const +{ + return std::vector( + std::begin(this->dataPtr->envCameras), std::end(this->dataPtr->envCameras)); +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::PostRender() +{ + if (this->dataPtr->newImageFrame.ConnectionCount() <= 0u) + return; + + unsigned int width = this->ImageWidth(); + unsigned int height = this->ImageHeight(); + unsigned int len = width * height; + + PixelFormat format = PF_R8G8B8; + unsigned int channelCount = PixelUtil::ChannelCount(format); + unsigned int bytesPerChannel = PixelUtil::BytesPerChannel(format); + + if (!this->dataPtr->wideAngleImage) + this->dataPtr->wideAngleImage = new unsigned char[len * channelCount]; + if (!this->dataPtr->imageBuffer) + this->dataPtr->imageBuffer = new unsigned char[len * channelCount]; + + // get image data + Ogre::RenderTarget *rt = + this->dataPtr->ogreRenderTexture->getBuffer()->getRenderTarget(); + Ogre::PixelBox ogrePixelBox(width, height, 1, + OgreConversions::Convert(format), this->dataPtr->imageBuffer); + rt->copyContentsToMemory(ogrePixelBox); + + // fill image data + memcpy(this->dataPtr->wideAngleImage, this->dataPtr->imageBuffer, + height*width*channelCount*bytesPerChannel); + + this->dataPtr->newImageFrame( + this->dataPtr->wideAngleImage, width, height, channelCount, "PF_R8G8B8"); + + // Uncomment to debug wide angle cameraoutput + // igndbg << "wxh: " << width << " x " << height << std::endl; + // for (unsigned int i = 0; i < height; ++i) + // { + // for (unsigned int j = 0; j < width * channelCount; j += channelCount) + // { + // unsigned int idx = i * width * channelCount + j; + // unsigned int r = this->dataPtr->wideAngleImage[idx]; + // unsigned int g = this->dataPtr->wideAngleImage[idx + 1]; + // unsigned int b = this->dataPtr->wideAngleImage[idx + 2]; + // std::cout << "[" << r << "," << g << "," << b << "]"; + // } + // std::cout << std::endl; + // } +} + +////////////////////////////////////////////////// +common::ConnectionPtr OgreWideAngleCamera::ConnectNewWideAngleFrame( + std::function _subscriber) +{ + return this->dataPtr->newImageFrame.Connect(_subscriber); +} + +////////////////////////////////////////////////// +RenderTargetPtr OgreWideAngleCamera::RenderTarget() const +{ + return this->dataPtr->wideAngleTexture; +} + +////////////////////////////////////////////////// +void OgreWideAngleCamera::SetVisibilityMask(uint32_t _mask) +{ + BaseCamera::SetVisibilityMask(_mask); + if (this->dataPtr->envViewports[0] == nullptr) + return; + for (unsigned int i = 0u; i < this->dataPtr->kEnvCameraCount; ++i) + { + auto *vp = this->dataPtr->envViewports[i]; + vp->setVisibilityMask(_mask); + } +} diff --git a/ogre/src/media/materials/programs/wide_lens_map_fp.glsl b/ogre/src/media/materials/programs/wide_lens_map_fp.glsl new file mode 100644 index 000000000..6b207e3f2 --- /dev/null +++ b/ogre/src/media/materials/programs/wide_lens_map_fp.glsl @@ -0,0 +1,61 @@ +#version 120 + +uniform samplerCube envMap; + +uniform float cutOffAngle; + +// focal length +uniform float f; + +// linear scaling constant +uniform float c1; + +// angle scaling constant +uniform float c2; + +// angle offset constant +uniform float c3; + +// unit axis +// depends on the type of math function sin (X), tan (Y), or identity (Z) +uniform vec3 fun; + +varying vec2 frag_pos; + +float pi = 3.141592653; +float r = length(frag_pos); + +vec3 map(float th) +{ + // spherical to cartesian conversion + return vec3(-sin(th)*frag_pos.x/r, sin(th)*frag_pos.y/r, cos(th)); +} + +void main() +{ + // calculate angle from optical axis based on the mapping function specified + float param = r/(c1*f); + float theta = 0.0; + if (fun.x > 0) + theta = asin(param); + else if (fun.y > 0) + theta = atan(param); + else if (fun.z > 0) + theta = param; + theta = (theta-c3)*c2; + + // compute the direction vector that will be used to sample from the cubemap + vec3 tc = map(theta); + + // sample and set resulting color + gl_FragColor = vec4(textureCube(envMap, tc).rgb, 1); + + // limit to visible fov + //TODO: move to vertex shader + float param2 = cutOffAngle/c2+c3; + float cutRadius = c1*f*(fun.x*sin(param2)+fun.y*tan(param2)+fun.z*param2); + + // smooth edges + // gl_FragColor.rgb *= 1.0-step(cutRadius,r); + gl_FragColor.rgb *= 1.0-smoothstep(cutRadius-0.02,cutRadius,r); +} diff --git a/ogre/src/media/materials/programs/wide_lens_map_vs.glsl b/ogre/src/media/materials/programs/wide_lens_map_vs.glsl new file mode 100644 index 000000000..7ac01bf99 --- /dev/null +++ b/ogre/src/media/materials/programs/wide_lens_map_vs.glsl @@ -0,0 +1,14 @@ +#version 120 + +// aspect ratio +uniform float ratio; + +varying vec2 frag_pos; + +void main() +{ + gl_Position = gl_ModelViewProjectionMatrix * gl_Vertex; + + // get normalized fragment coordinate (3D to 2D window space transformation) + frag_pos = gl_Position.xy/gl_Position.w*vec2(-1.0,-1.0/ratio); +} diff --git a/ogre/src/media/materials/scripts/wide_angle_camera.material b/ogre/src/media/materials/scripts/wide_angle_camera.material new file mode 100644 index 000000000..ed1b8ac62 --- /dev/null +++ b/ogre/src/media/materials/scripts/wide_angle_camera.material @@ -0,0 +1,37 @@ +vertex_program WideLensMapVS glsl +{ + source wide_lens_map_vs.glsl + default_params + { + param_named ratio float 1 + } +} + +fragment_program WideLensMapFS glsl +{ + source wide_lens_map_fp.glsl + default_params + { + param_named envMap int 0 + param_named c1 float 1 + param_named c2 float 1 + param_named c3 float 0 + param_named f float 1 + param_named fun float3 0 0 1 + param_named cutOffAngle float 3.14 + } +} + +material WideLensMap +{ + technique + { + pass + { + vertex_program_ref WideLensMapVS { } + fragment_program_ref WideLensMapFS { } + } + } +} + + diff --git a/ogre/src/media/materials/scripts/wide_camera_lens_map.compositor b/ogre/src/media/materials/scripts/wide_camera_lens_map.compositor new file mode 100644 index 000000000..dc9f27c13 --- /dev/null +++ b/ogre/src/media/materials/scripts/wide_camera_lens_map.compositor @@ -0,0 +1,14 @@ +compositor WideCameraLensMap/ParametrisedMap +{ + technique + { + target_output + { + input none + pass render_quad + { + material WideLensMap + } + } + } +} diff --git a/src/CameraLens.cc b/src/CameraLens.cc new file mode 100644 index 000000000..eb6793ad9 --- /dev/null +++ b/src/CameraLens.cc @@ -0,0 +1,388 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include +#include + +#include +#include + +#include "ignition/rendering/CameraLens.hh" + +/// \brief Private fields of camera lens +class ignition::rendering::CameraLens::Implementation +{ + /// \brief Linear scale factor + public: double c1 = 1.0; + + /// \brief Angle scale factor + public: double c2 = 1.0; + + /// \brief Angle offset factor + public: double c3 = 0.0; + + /// \brief Linear scale factor, may be adjusted in runtime + public: double f = 1.0; + + /// \brief Visible field of view + public: double cutOffAngle = IGN_PI * 0.5; + + /// \brief True to scale image to hfov, false to render as output according + /// to projection parameters + public: bool scaleToHFov = true; + + /// \brief Mapping function type + public: MappingFunctionType type = MFT_GNOMONIC; + + /// \brief Enumeration of functions that can be casted to some other types + public: class MapFunctionEnum + { + /// \brief Constructor + /// \param[in] _fun Angle function MFT_SIN, MFT_TAN, or MFT_IDENTITY + public: explicit MapFunctionEnum(AngleFunctionType _fun) + { + variants.push_back(std::make_tuple(AFT_SIN, + math::Vector3d::UnitX, + std::function( + static_cast(&std::sin)))); + + variants.push_back(std::make_tuple(AFT_TAN, + math::Vector3d::UnitY, + std::function( + static_cast(&std::tan)))); + + variants.push_back(std::make_tuple(AFT_IDENTITY, + math::Vector3d::UnitZ, + std::function( + [](float t) -> float + { + return t; + }))); + + for (auto item : variants) + { + if (std::get<0>(item) == _fun) + { + value = item; + return; + } + } + + // function provided is not in array + throw std::invalid_argument("Unknown angle function"); + } + + /// \brief Cast to ignition::math::Vector3d, + /// this vector is passed to shader to avoid branching + /// \return Vector3 Vector whose one component is 1 + /// and the rest are nulls + public: math::Vector3d AsVector3d() const + { + return std::get<1>(value); + } + + /// \brief Get the angle transformation function + /// \return The same function which was passed to constructor + public: AngleFunctionType AngleFunction() const + { + return std::get<0>(value); + } + + /// \brief Apply function to a float value + /// \param _f input float value + /// \return The result of application + public: float Apply(float _f) + { + return std::get<2>(value)(_f); + } + + /// \brief Assignment operator + /// \param[in] _fun Rvalue + /// \return Reference to (*this) + public: MapFunctionEnum &operator=(const MapFunctionEnum &_fun) + { + this->value = _fun.value; + return *this; + } + + /// \brief List of all available functions + /// and its associated representations + private: std::vector > > variants; + + /// \brief Current value of enumeration + private: decltype(variants)::value_type value; + }; + + /// \brief Angle function component of the mapping function, + /// \see CameraLens description + public: MapFunctionEnum fun = MapFunctionEnum(AFT_IDENTITY); +}; + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +CameraLens::CameraLens() + : dataPtr(utils::MakeUniqueImpl()) +{ +} + +////////////////////////////////////////////////// +CameraLens::CameraLens(const CameraLens &_other) + : dataPtr(utils::MakeUniqueImpl()) +{ + // Avoid incorrect cppcheck error about dataPtr being assigned in constructor + CameraLens::Implementation &dp = *(this->dataPtr); + dp = *(_other.dataPtr); +} + +////////////////////////////////////////////////// +CameraLens::~CameraLens() +{ +} + +////////////////////////////////////////////////// +CameraLens &CameraLens::operator=(const CameraLens &_other) +{ + *(this->dataPtr) = *(_other.dataPtr); + return *this; +} + +////////////////////////////////////////////////// +void CameraLens::SetCustomMappingFunction(double _c1, double _c2, + AngleFunctionType _fun, double _f, double _c3) +{ + this->dataPtr->c1 = _c1; + this->dataPtr->c2 = _c2; + this->dataPtr->c3 = _c3; + this->dataPtr->f = _f; + + try + { + this->dataPtr->fun = CameraLens::Implementation::MapFunctionEnum(_fun); + } + catch(const std::exception &ex) + { + ignerr << "Angle functionis not known, " + << "[tan] will be used instead" << std::endl; + + this->dataPtr->fun = CameraLens::Implementation::MapFunctionEnum(AFT_TAN); + } + if (!this->IsCustom()) + this->ConvertToCustom(); +} + +////////////////////////////////////////////////// +MappingFunctionType CameraLens::Type() const +{ + return this->dataPtr->type; +} + +////////////////////////////////////////////////// +bool CameraLens::IsCustom() const +{ + return this->Type() == MFT_CUSTOM; +} + +////////////////////////////////////////////////// +double CameraLens::C1() const +{ + return this->dataPtr->c1; +} + +////////////////////////////////////////////////// +double CameraLens::C2() const +{ + return this->dataPtr->c2; +} + +////////////////////////////////////////////////// +double CameraLens::C3() const +{ + return this->dataPtr->c3; +} + +////////////////////////////////////////////////// +double CameraLens::F() const +{ + return this->dataPtr->f; +} + +////////////////////////////////////////////////// +AngleFunctionType CameraLens::AngleFunction() const +{ + return this->dataPtr->fun.AngleFunction(); +} + +////////////////////////////////////////////////// +double CameraLens::CutOffAngle() const +{ + return this->dataPtr->cutOffAngle; +} + +////////////////////////////////////////////////// +bool CameraLens::ScaleToHFOV() const +{ + return this->dataPtr->scaleToHFov; +} + +////////////////////////////////////////////////// +void CameraLens::SetType(MappingFunctionType _type) +{ + // c1, c2, c3, f, angle function + std::map > funTypes = { + {MFT_GNOMONIC, std::make_tuple(1.0, 1.0, 0.0, 1.0, AFT_TAN)}, + {MFT_STEREOGRAPHIC, std::make_tuple(2.0, 2.0, 0.0, 1.0, AFT_TAN)}, + {MFT_EQUIDISTANT, std::make_tuple(1.0, 1.0, 0.0, 1.0, AFT_IDENTITY)}, + {MFT_EQUISOLID_ANGLE, std::make_tuple(2.0, 2.0, 0.0, 1.0, AFT_SIN)}, + {MFT_ORTHOGRAPHIC, std::make_tuple(1.0, 1.0, 0.0, 1.0, AFT_SIN)}}; + + funTypes.emplace(MFT_CUSTOM, + std::make_tuple(this->C1(), this->C2(), this->C3(), this->F(), + this->AngleFunction())); + + decltype(funTypes)::mapped_type params; + + try + { + params = funTypes.at(_type); + } + catch(...) + { + ignerr << "Unknown lens type." << std::endl; + return; + } + + this->dataPtr->type = _type; + + if (_type == MFT_CUSTOM) + { + this->SetC1(std::get<0>(params)); + this->SetC2(std::get<1>(params)); + this->SetC3(std::get<2>(params)); + this->SetF(std::get<3>(params)); + this->SetAngleFunction(std::get<4>(params)); + } + else + { + this->dataPtr->c1 = std::get<0>(params); + this->dataPtr->c2 = std::get<1>(params); + this->dataPtr->c3 = std::get<2>(params); + this->dataPtr->f = std::get<3>(params); + + try + { + this->dataPtr->fun = + CameraLens::Implementation::MapFunctionEnum(std::get<4>(params)); + } + catch(const std::exception &ex) + { + ignerr << "`fun` value [" << std::get<4>(params) + << "] is not known, keeping the old one" << std::endl; + } + } +} + +////////////////////////////////////////////////// +void CameraLens::SetC1(double _c) +{ + this->dataPtr->c1 = _c; + + if (!this->IsCustom()) + this->ConvertToCustom(); +} + +////////////////////////////////////////////////// +void CameraLens::SetC2(double _c) +{ + this->dataPtr->c2 = _c; + + if (!this->IsCustom()) + this->ConvertToCustom(); +} + +////////////////////////////////////////////////// +void CameraLens::SetC3(double _c) +{ + this->dataPtr->c3 = _c; + + if (!this->IsCustom()) + this->ConvertToCustom(); +} + +////////////////////////////////////////////////// +void CameraLens::SetF(double _f) +{ + this->dataPtr->f = _f; + + if (!this->IsCustom()) + this->ConvertToCustom(); +} + +////////////////////////////////////////////////// +void CameraLens::SetAngleFunction(AngleFunctionType _fun) +{ + if (!this->IsCustom()) + this->ConvertToCustom(); + + try + { + this->dataPtr->fun = CameraLens::Implementation::MapFunctionEnum(_fun); + } + catch(const std::exception &ex) + { + ignerr << "`Fun` value [" << _fun << "] is not known, " + << "keeping the old one" << std::endl; + return; + } +} + +////////////////////////////////////////////////// +void CameraLens::SetCutOffAngle(const double _angle) +{ + this->dataPtr->cutOffAngle = _angle; +} + +////////////////////////////////////////////////// +void CameraLens::SetScaleToHFOV(bool _scale) +{ + this->dataPtr->scaleToHFov = _scale; +} + +////////////////////////////////////////////////// +void CameraLens::ConvertToCustom() +{ + this->SetType(MFT_CUSTOM); +} + +////////////////////////////////////////////////// +float CameraLens::ApplyMappingFunction(float _f) const +{ + return this->dataPtr->fun.Apply(_f); +} + +////////////////////////////////////////////////// +math::Vector3d CameraLens::MappingFunctionAsVector3d() const +{ + return this->dataPtr->fun.AsVector3d(); +} diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index 775dec699..fe161d792 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -46,6 +46,7 @@ #include "ignition/rendering/ThermalCamera.hh" #include "ignition/rendering/SegmentationCamera.hh" #include "ignition/rendering/Visual.hh" +#include "ignition/rendering/WideAngleCamera.hh" #include "ignition/rendering/base/BaseStorage.hh" #include "ignition/rendering/base/BaseScene.hh" @@ -876,6 +877,36 @@ SegmentationCameraPtr BaseScene::CreateSegmentationCamera( return (result) ? camera : nullptr; } +////////////////////////////////////////////////// +WideAngleCameraPtr BaseScene::CreateWideAngleCamera() +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateWideAngleCamera(objId); +} +////////////////////////////////////////////////// +WideAngleCameraPtr BaseScene::CreateWideAngleCamera( + const unsigned int _id) +{ + std::string objName = this->CreateObjectName(_id, "WideAngleCamera"); + return this->CreateWideAngleCamera(_id, objName); +} +////////////////////////////////////////////////// +WideAngleCameraPtr BaseScene::CreateWideAngleCamera( + const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateWideAngleCamera(objId, _name); +} +////////////////////////////////////////////////// +WideAngleCameraPtr BaseScene::CreateWideAngleCamera( + const unsigned int _id, + const std::string &_name) +{ + WideAngleCameraPtr camera = this->CreateWideAngleCameraImpl(_id, _name); + bool result = this->RegisterSensor(camera); + return (result) ? camera : nullptr; +} + ////////////////////////////////////////////////// GpuRaysPtr BaseScene::CreateGpuRays() { diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 8b7e5be96..41b0f4000 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -11,6 +11,7 @@ set(tests sky.cc thermal_camera.cc lidar_visual.cc + wide_angle_camera.cc ) link_directories(${PROJECT_BINARY_DIR}/test) diff --git a/test/integration/wide_angle_camera.cc b/test/integration/wide_angle_camera.cc new file mode 100644 index 000000000..be8095f4c --- /dev/null +++ b/test/integration/wide_angle_camera.cc @@ -0,0 +1,383 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include + +#include +#include +#include + +#include + +#include "test_config.h" // NOLINT(build/include) + +#include "ignition/rendering/RenderEngine.hh" +#include "ignition/rendering/RenderingIface.hh" +#include "ignition/rendering/Scene.hh" +#include "ignition/rendering/WideAngleCamera.hh" + +using namespace ignition; +using namespace rendering; + +////////////////////////////////////////////////// +class WideAngleCameraTest: public testing::Test, + public testing::WithParamInterface +{ + /// \brief Verify generated image and compare with regular camera + public: void WideAngleCamera(const std::string &_renderEngine); + + /// \brief Test 2d to 3d projection with wide angle camera + public: void Projection(const std::string &_renderEngine); + + // Documentation inherited + protected: void SetUp() override + { + ignition::common::Console::SetVerbosity(4); + } +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief WideAngle buffer +unsigned char *g_buffer = nullptr; + +/// \brief counter of received wideAngle msgs +int g_counter = 0; + +////////////////////////////////////////////////// +/// \brief callback to get the wide angle camera image data +void OnNewWideAngleFrame(const unsigned char *_data, + unsigned int _width, unsigned int _height, + unsigned int _channels, + const std::string &/*_format*/) +{ + g_mutex.lock(); + auto bufferSize = _width * _height * _channels; + + if (!g_buffer) + g_buffer = new unsigned char[bufferSize]; + + memcpy(g_buffer, _data, bufferSize); + + g_counter++; + g_mutex.unlock(); +} + +////////////////////////////////////////////////// +void WideAngleCameraTest::WideAngleCamera( + const std::string &_renderEngine) +{ + // Currently, only ogre supports wide angle cameras + if (_renderEngine.compare("ogre") != 0) + { + ignerr << "Engine '" << _renderEngine + << "' doesn't support wide angle cameras" << std::endl; + return; + } + + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + ignerr << "Engine '" << _renderEngine + << "' was unable to be retrieved" << std::endl; + return; + } + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + scene->SetAmbientLight(1.0, 1.0, 1.0); + scene->SetBackgroundColor(0.2, 0.2, 0.2); + + rendering::VisualPtr root = scene->RootVisual(); + + unsigned int width = 320u; + unsigned int height = 240u; + + // Create Wide Angle camera + auto camera = scene->CreateWideAngleCamera("WideAngleCamera"); + ASSERT_NE(camera, nullptr); + + CameraLens lens; + lens.SetCustomMappingFunction(1.05, 4.0, AFT_TAN, 1.0, 0.0); + lens.SetType(MFT_CUSTOM); + lens.SetCutOffAngle(IGN_PI); + + camera->SetLens(lens); + camera->SetHFOV(2.6); + camera->SetImageWidth(width); + camera->SetImageHeight(height); + camera->SetAspectRatio(1.333); + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + scene->RootVisual()->AddChild(camera); + + // create regular camera for comparison + CameraPtr cameraRegular = scene->CreateCamera(); + ASSERT_TRUE(camera != nullptr); + cameraRegular->SetImageWidth(width); + cameraRegular->SetImageHeight(height); + cameraRegular->SetAspectRatio(1.333); + cameraRegular->SetHFOV(2.6); + cameraRegular->SetLocalPosition(0.0, 0.0, 0.0); + cameraRegular->SetLocalRotation(0.0, 0.0, 0.0); + root->AddChild(cameraRegular); + + // create blue material + MaterialPtr blue = scene->CreateMaterial(); + blue->SetAmbient(0.0, 0.0, 0.3); + blue->SetDiffuse(0.0, 0.0, 0.8); + blue->SetSpecular(0.5, 0.5, 0.5); + + // create box visual in front of cameras + VisualPtr box = scene->CreateVisual(); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(2, 0, 0); + box->SetLocalScale(1, 1, 1); + box->SetMaterial(blue); + root->AddChild(box); + + // capture original image + Image imageRegular = cameraRegular->CreateImage(); + cameraRegular->Capture(imageRegular); + unsigned char *dataRegular = imageRegular.Data(); + + // Set a callback on the camera sensor to get a wide angle camera frame + ignition::common::ConnectionPtr connection = + camera->ConnectNewWideAngleFrame( + std::bind(OnNewWideAngleFrame, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + ASSERT_NE(nullptr, connection); + + // Update once to create image + camera->Update(); + EXPECT_EQ(1, g_counter); + + // Compare image pixels + unsigned int channelCount = PixelUtil::ChannelCount(camera->ImageFormat()); + unsigned int step = width * channelCount; + + // verify both cameras can see the blue box in the middle + unsigned int mid = static_cast( + height / 2.0 * step + step / 2.0); + unsigned int r = g_buffer[mid]; + unsigned int g = g_buffer[mid + 1]; + unsigned int b = g_buffer[mid + 2]; + EXPECT_GT(b, g); + EXPECT_GT(b, r); + + r = dataRegular[mid]; + g = dataRegular[mid + 1]; + b = dataRegular[mid + 2]; + EXPECT_GT(b, g); + EXPECT_GT(b, r); + + // get sum of pixel colors in both wide angle camera and regular camera images + unsigned int rSum = 0u; + unsigned int gSum = 0u; + unsigned int bSum = 0u; + unsigned int rRegularSum = 0u; + unsigned int gRegularSum = 0u; + unsigned int bRegularSum = 0u; + + for (unsigned int i = 0; i < height; ++i) + { + for (unsigned int j = 0; j < step; j += channelCount) + { + unsigned int idx = i * step + j; + rSum += g_buffer[idx]; + gSum += g_buffer[idx + 1]; + bSum += g_buffer[idx + 2]; + + rRegularSum += dataRegular[idx]; + gRegularSum += dataRegular[idx + 1]; + bRegularSum += dataRegular[idx + 2]; + } + } + + // wide angle camera image should not be black + EXPECT_GT(rSum, 0u); + EXPECT_GT(gSum, 0u); + EXPECT_GT(bSum, 0u); + + // sum of regular camera image should be brighter than wide angle camera image + // as there are more background visible + EXPECT_GT(rRegularSum, rSum); + EXPECT_GT(gRegularSum, rSum); + EXPECT_GT(bRegularSum, rSum); + + // The wide angle camera should have more blue pixels than regular camera + // because the box is larger in the image due to distortion + EXPECT_GT(bSum, rRegularSum); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +void WideAngleCameraTest::Projection(const std::string &_renderEngine) +{ + // Currently, only ogre supports wideAngle cameras + if (_renderEngine.compare("ogre") != 0) + { + ignerr << "Engine '" << _renderEngine + << "' doesn't support wide angle cameras" << std::endl; + return; + } + + // Setup ign-rendering with an empty scene + auto *engine = ignition::rendering::engine(_renderEngine); + if (!engine) + { + ignerr << "Engine '" << _renderEngine + << "' was unable to be retrieved" << std::endl; + return; + } + ignition::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + scene->SetAmbientLight(1.0, 1.0, 1.0); + scene->SetBackgroundColor(0.2, 0.2, 0.2); + + rendering::VisualPtr root = scene->RootVisual(); + + unsigned int width = 320u; + unsigned int height = 240u; + // Create Wide Angle camera + auto camera = scene->CreateWideAngleCamera("WideAngleCamera"); + ASSERT_NE(camera, nullptr); + + CameraLens lens; + lens.SetCustomMappingFunction(1.05, 4.0, AFT_TAN, 1.0, 0.0); + lens.SetType(MFT_CUSTOM); + lens.SetCutOffAngle(IGN_PI); + + camera->SetLens(lens); + camera->SetHFOV(2.6); + camera->SetImageWidth(width); + camera->SetImageHeight(height); + camera->SetAspectRatio(1.333); + camera->SetLocalPosition(0.0, 0.0, 0.0); + camera->SetLocalRotation(0.0, 0.0, 0.0); + scene->RootVisual()->AddChild(camera); + + camera->Update(); + + // point directly in front of camera + auto worldPoint = ignition::math::Vector3d::UnitX; + auto screenPt = camera->Project3d(worldPoint); + EXPECT_FLOAT_EQ(camera->ImageWidth() * 0.5, screenPt.X()); + EXPECT_FLOAT_EQ(camera->ImageHeight() * 0.5, screenPt.Y()); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point behind camera + worldPoint = -ignition::math::Vector3d::UnitX; + screenPt = camera->Project3d(worldPoint); + // z is distance of point from image center + // in this case it'll be outside of image so greater than 1.0 + EXPECT_GT(screenPt.Z(), 1.0); + + // point at right side of camera image + worldPoint = ignition::math::Vector3d(1, -0.5, 0.0); + screenPt = camera->Project3d(worldPoint); + EXPECT_GT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_FLOAT_EQ(camera->ImageHeight() * 0.5, screenPt.Y()); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at left side of camera image + worldPoint = ignition::math::Vector3d(1, 0.5, 0.0); + screenPt = camera->Project3d(worldPoint); + EXPECT_LT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_FLOAT_EQ(camera->ImageHeight() * 0.5, screenPt.Y()); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at top half of camera image + worldPoint = ignition::math::Vector3d(1, 0.0, 0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_FLOAT_EQ(camera->ImageWidth() * 0.5, screenPt.X()); + EXPECT_LT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at bottom half of camera image + worldPoint = ignition::math::Vector3d(1, 0.0, -0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_FLOAT_EQ(camera->ImageWidth() * 0.5, screenPt.X()); + EXPECT_GT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at top left quadrant of camera image + worldPoint = ignition::math::Vector3d(1, 0.5, 0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_LT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_LT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at top right quadrant of camera image + worldPoint = ignition::math::Vector3d(1, -0.5, 0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_GT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_LT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at bottom left quadrant of camera image + worldPoint = ignition::math::Vector3d(1, 0.5, -0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_LT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_GT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // point at bottom right quadrant of camera image + worldPoint = ignition::math::Vector3d(1, -0.5, -0.5); + screenPt = camera->Project3d(worldPoint); + EXPECT_GT(screenPt.X(), camera->ImageWidth() * 0.5); + EXPECT_GT(screenPt.Y(), camera->ImageHeight() * 0.5); + EXPECT_GT(screenPt.Z(), 0.0); + EXPECT_LT(screenPt.Z(), 1.0); + + // Clean up + engine->DestroyScene(scene); + ignition::rendering::unloadEngine(engine->Name()); +} + +TEST_P(WideAngleCameraTest, WideAngleCamera) +{ + WideAngleCamera(GetParam()); +} + +TEST_P(WideAngleCameraTest, Projection) +{ + Projection(GetParam()); +} + +INSTANTIATE_TEST_CASE_P(WideAngleCamera, WideAngleCameraTest, + RENDER_ENGINE_VALUES, ignition::rendering::PrintToStringParam()); + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}