Segmentation Camera

examples/segmentation_camera/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR)
+project(ignition-rendering-segmentation-camera)
+find_package(ignition-rendering6 REQUIRED)
+
+include_directories(SYSTEM
+  ${PROJECT_BINARY_DIR}
+)
+
+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})
+
+if (NOT APPLE)
+  find_package(GLEW REQUIRED)
+  include_directories(SYSTEM ${GLEW_INCLUDE_DIRS})
+  link_directories(${GLEW_LIBRARY_DIRS})
+endif()
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations")
+
+configure_file (example_config.hh.in ${PROJECT_BINARY_DIR}/example_config.hh)
+
+add_executable(segmentation_camera Main.cc GlutWindow.cc)
+
+target_link_libraries(segmentation_camera
+  ${GLUT_LIBRARIES}
+  ${OPENGL_LIBRARIES}
+  ${GLEW_LIBRARIES}
+  ${IGNITION-RENDERING_LIBRARIES}
+)
+
+add_custom_command(TARGET segmentation_camera POST_BUILD
+  COMMAND ${CMAKE_COMMAND} -E copy_directory
+  ${CMAKE_SOURCE_DIR}/media
+  $<TARGET_FILE_DIR:segmentation_camera>/media)

examples/segmentation_camera/GlutWindow.cc

@@ -0,0 +1,357 @@
+/*
+ * 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 <OpenGL/gl.h>
+  #include <OpenGL/OpenGL.h>
+  #include <GLUT/glut.h>
+#else
+  #include <GL/glew.h>
+  #include <GL/gl.h>
+  #include <GL/glut.h>
+#endif
+
+#if !defined(__APPLE__) && !defined(_WIN32)
+  #include <GL/glx.h>
+#endif
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include <ignition/common/Console.hh>
+#include <ignition/math/Vector2.hh>
+#include <ignition/rendering/Camera.hh>
+#include <ignition/rendering/Image.hh>
+#include <ignition/rendering/RayQuery.hh>
+#include <ignition/rendering/Scene.hh>
+#include <ignition/rendering/OrbitViewController.hh>
+#include <ignition/rendering/SegmentationCamera.hh>
+#include "GlutWindow.hh"
+
+#define KEY_ESC 27
+#define UNSUPPORTED_BUTTONS 5
+
+//////////////////////////////////////////////////
+unsigned int imgw = 0;
+unsigned int imgh = 0;
+
+ir::CameraPtr g_camera;
+ir::ImagePtr g_image;
+ignition::common::ConnectionPtr g_connection;
+
+bool g_initContext = false;
+
+#if __APPLE__
+  CGLContextObj g_context;
+  CGLContextObj g_glutContext;
+#elif _WIN32
+#else
+  GLXContext g_context;
+  Display *g_display;
+  GLXDrawable g_drawable;
+  GLXContext g_glutContext;
+  Display *g_glutDisplay;
+  GLXDrawable g_glutDrawable;
+#endif
+
+// view control variables
+ir::RayQueryPtr g_rayQuery;
+ir::OrbitViewController g_viewControl;
+ir::RayQueryResult g_target;
+struct mouseButton
+{
+  int button = 0;
+  int state = GLUT_UP;
+  int x = 0;
+  int y = 0;
+  int motionX = 0;
+  int motionY = 0;
+  int dragX = 0;
+  int dragY = 0;
+  int scroll = 0;
+  bool buttonDirty = false;
+  bool motionDirty = false;
+};
+struct mouseButton g_mouse;
+std::mutex g_mouseMutex;
+
+//////////////////////////////////////////////////
+void mouseCB(int _button, int _state, int _x, int _y)
+{
+  // ignore unknown mouse button numbers
+  if (_button >= UNSUPPORTED_BUTTONS)
+    return;
+
+  std::lock_guard<std::mutex> lock(g_mouseMutex);
+  g_mouse.button = _button;
+  g_mouse.state = _state;
+  g_mouse.x = _x;
+  g_mouse.y = _y;
+  g_mouse.motionX = _x;
+  g_mouse.motionY = _y;
+  g_mouse.buttonDirty = true;
+}
+
+//////////////////////////////////////////////////
+void motionCB(int _x, int _y)
+{
+  std::lock_guard<std::mutex> lock(g_mouseMutex);
+  int deltaX = _x - g_mouse.motionX;
+  int deltaY = _y - g_mouse.motionY;
+  g_mouse.motionX = _x;
+  g_mouse.motionY = _y;
+
+  if (g_mouse.motionDirty)
+  {
+    g_mouse.dragX += deltaX;
+    g_mouse.dragY += deltaY;
+  }
+  else
+  {
+    g_mouse.dragX = deltaX;
+    g_mouse.dragY = deltaY;
+  }
+  g_mouse.motionDirty = true;
+}
+
+//////////////////////////////////////////////////
+void handleMouse()
+{
+  std::lock_guard<std::mutex> lock(g_mouseMutex);
+  // only ogre supports ray query for now so use
+  // ogre camera located at camera index = 0.
+  ir::CameraPtr rayCamera = g_camera;
+  if (!g_rayQuery)
+  {
+    g_rayQuery = rayCamera->Scene()->CreateRayQuery();
+    if (!g_rayQuery)
+    {
+      ignerr << "Failed to create Ray Query" << std::endl;
+      return;
+    }
+  }
+
+  if (g_mouse.buttonDirty)
+  {
+    g_mouse.buttonDirty = false;
+    double nx =
+        2.0 * g_mouse.x / static_cast<double>(rayCamera->ImageWidth()) - 1.0;
+    double ny = 1.0 -
+        2.0 * g_mouse.y / static_cast<double>(rayCamera->ImageHeight());
+
+    // g_rayQuery->SetFromCamera(rayCamera, ignition::math::Vector2d(nx, ny));
+    // g_target  = g_rayQuery->ClosestPoint();
+    // if (!g_target)
+    // {
+    //   // set point to be 10m away if no intersection found
+    //   g_target.point = g_rayQuery->Origin() + g_rayQuery->Direction() * 10;
+    //   return;
+    // }
+
+    // mouse wheel scroll zoom
+    if ((g_mouse.button == 3 || g_mouse.button == 4) &&
+        g_mouse.state == GLUT_UP)
+    {
+      double scroll = (g_mouse.button == 3) ? -1.0 : 1.0;
+      double distance = rayCamera->WorldPosition().Distance(
+          g_target.point);
+      int factor = 1;
+      double amount = -(scroll * factor) * (distance / 5.0);
+
+      g_viewControl.SetCamera(g_camera);
+      g_viewControl.SetTarget(g_target.point);
+      g_viewControl.Zoom(amount);
+    }
+  }
+
+  if (g_mouse.motionDirty)
+  {
+    g_mouse.motionDirty = false;
+    auto drag = ignition::math::Vector2d(g_mouse.dragX, g_mouse.dragY);
+
+    // left mouse button pan
+    if (g_mouse.button == GLUT_LEFT_BUTTON && g_mouse.state == GLUT_DOWN)
+    {
+      g_viewControl.SetCamera(g_camera);
+      g_viewControl.SetTarget(g_target.point);
+      g_viewControl.Pan(drag);
+    }
+    else if (g_mouse.button == GLUT_MIDDLE_BUTTON && g_mouse.state == GLUT_DOWN)
+    {
+      g_viewControl.SetCamera(g_camera);
+      g_viewControl.SetTarget(g_target.point);
+      g_viewControl.Orbit(drag);
+    }
+    // right mouse button zoom
+    else if (g_mouse.button == GLUT_RIGHT_BUTTON && g_mouse.state == GLUT_DOWN)
+    {
+      double hfov = rayCamera->HFOV().Radian();
+      double vfov = 2.0f * atan(tan(hfov / 2.0f) /
+          rayCamera->AspectRatio());
+      double distance = rayCamera->WorldPosition().Distance(
+          g_target.point);
+      double amount = ((-g_mouse.dragY /
+          static_cast<double>(rayCamera->ImageHeight()))
+          * distance * tan(vfov/2.0) * 6.0);
+
+      g_viewControl.SetCamera(g_camera);
+      g_viewControl.SetTarget(g_target.point);
+      g_viewControl.Zoom(amount);
+    }
+  }
+}
+
+//////////////////////////////////////////////////
+void displayCB()
+{
+#if __APPLE__
+  CGLSetCurrentContext(g_context);
+#elif _WIN32
+#else
+  if (g_display)
+  {
+    glXMakeCurrent(g_display, g_drawable, g_context);
+  }
+#endif
+
+  g_camera->Update();
+
+  handleMouse();
+
+#if __APPLE__
+  CGLSetCurrentContext(g_glutContext);
+#elif _WIN32
+#else
+  glXMakeCurrent(g_glutDisplay, g_glutDrawable, g_glutContext);
+#endif
+
+  unsigned char *data = g_image->Data<unsigned char>();
+
+  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);
+  }
+}
+
+//////////////////////////////////////////////////
+void OnNewSegmentationFrame(const uint8_t *_scan,
+                    unsigned int _width, unsigned int _height,
+                    unsigned int /*_channels*/,
+                    const std::string &_format)
+{
+  unsigned char *data = g_image->Data<unsigned char>();
+  auto bufferSize = _width * _height * 3;
+  memcpy(data, _scan, bufferSize);
+}
+
+//////////////////////////////////////////////////
+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<ir::Image>(image);
+
+  ir::SegmentationCameraPtr camera = std::dynamic_pointer_cast<ir::SegmentationCamera>(
+      g_camera);
+
+  // callback when new segmentation frame is received.
+  g_connection = camera->ConnectNewSegmentationFrame(
+        std::bind(OnNewSegmentationFrame,
+        std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
+        std::placeholders::_4, std::placeholders::_5));
+
+  g_camera->Update();
+}
+
+//////////////////////////////////////////////////
+void initContext()
+{
+  glutInitDisplayMode(GLUT_DOUBLE);
+  glutInitWindowPosition(0, 0);
+  glutInitWindowSize(imgw, imgh);
+  glutCreateWindow("Segmentation Camera");
+  glutDisplayFunc(displayCB);
+  glutIdleFunc(idleCB);
+  glutKeyboardFunc(keyboardCB);
+
+  glutMouseFunc(mouseCB);
+  glutMotionFunc(motionCB);
+}
+
+//////////////////////////////////////////////////
+void printUsage()
+{
+  std::cout << "===============================" << std::endl;
+  std::cout << "  ESC - Exit                   " << std::endl;
+  std::cout << "===============================" << std::endl;
+}
+
+//////////////////////////////////////////////////
+void run(ir::CameraPtr _camera)
+{
+  if (!_camera)
+  {
+    ignerr << "No camera found. Scene will not be rendered" << std::endl;
+    return;
+  }
+
+#if __APPLE__
+  g_context = CGLGetCurrentContext();
+#elif _WIN32
+#else
+  g_context = glXGetCurrentContext();
+  g_display = glXGetCurrentDisplay();
+  g_drawable = glXGetCurrentDrawable();
+#endif
+
+  initCamera(_camera);
+  initContext();
+  printUsage();
+
+#if __APPLE__
+  g_glutContext = CGLGetCurrentContext();
+#elif _WIN32
+#else
+  g_glutDisplay = glXGetCurrentDisplay();
+  g_glutDrawable = glXGetCurrentDrawable();
+  g_glutContext = glXGetCurrentContext();
+#endif
+
+  glutMainLoop();
+}