title: Transform camera- to world-coordinates
body: <p>I am using a Intel Realsense D415 camera to measure the distance to objects and get the corresponding 3D-coordinates. The coordinates I am getting are accurate but I get problems when I try to transform them to another coordinate-system.</p>
  <p>What I am trying to do is:</p>
  <ol>
  <li>Find an object in the RGB-Frame</li>
  <li>Get the depth of the object and calculate the 3D-Position relative to the camera (This has to be done because the camera has it's origin on the left stereo-camera)</li>
  <li>Calculate the 3D-position into a world coordinate-system to make them useful for another application</li>
  </ol>
  <p>Step 1 and 2 are working properly. My problem is in step 3. I am using <em>rs2_deproject_pixel_to_point</em> to get the right coordinates. My understanding is that I could calibrate something like a second extrinsic matrix and transform the points in the new system with:</p>
  <p><code>P_world = R * P_Camera + t = Rt * P_Camera</code></p>
  <p>with <code>Rt</code> as a 3x4 matrix and <code>P_Camera</code> as a 4x1 vector</p>
  <p>To get the matrix <em>Rt</em> I printed a chessboard and placed it at a known position in my world-coordinates. As the origin I choose the corner of my table. Now I am searching for the chessboard in the RGB-frame and save the 3D-camera-coordinates from the depth-frame. After this step I am trying to solve the matrix with 12 linear equations from 4 points with different X,Y,Z-values and the known positions of the corners. So far so good but the results I get are nowhere near correct and I have no idea what's the mistake.</p>
  <p>Is the mistake in the method itself or is it just my implementation that leads to wrong values? Is there any way to easily transform the coordinates in another system?</p>
  <p>Below is the code I use to find the chessboard and get the 3D-positions:</p>
  <pre><code>def convert_depth_to_phys_coord_using_realsense(x, y, depth, cameraInfo):
      _intrinsics = rs.intrinsics()
      _intrinsics.width = cameraInfo.width
      _intrinsics.height = cameraInfo.height
      _intrinsics.ppx = cameraInfo.ppx
      _intrinsics.ppy = cameraInfo.ppy
      _intrinsics.fx = cameraInfo.fx
      _intrinsics.fy = cameraInfo.fy
      # _intrinsics.model = cameraInfo.distortion_model
      _intrinsics.model  = rs.distortion.none
      _intrinsics.coeffs = [i for i in cameraInfo.coeffs]
      result = rs.rs2_deproject_pixel_to_point(_intrinsics, [x, y], depth)
      # result[0]: right, result[1]: down, result[2]: forward
      return result[0], -result[1], -result[2]
  
  ...
  
  # Setup the pipeline
  pipe = rs.pipeline()
  cfg = rs.config()
  cfg.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)
  cfg.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
  profile = pipe.start(cfg)
  
  # Setup the 'High Accuracy'-mode
  depth_sensor = profile.get_device().first_depth_sensor()
  preset_range = depth_sensor.get_option_range(rs.option.visual_preset)
  for i in range(int(preset_range.max)):
      visulpreset = depth_sensor.get_option_value_description(rs.option.visual_preset,i)
      print('%02d: %s'%(i,visulpreset))
      if visulpreset == &quot;High Accuracy&quot;:
          depth_sensor.set_option(rs.option.visual_preset, i)
  # enable higher laser-power for better detection
  depth_sensor.set_option(rs.option.laser_power, 180)
  # lower the depth unit for better accuracy and shorter distance covered
  depth_sensor.set_option(rs.option.depth_units, 0.0005)
  
  # Skip first frames for auto-exposure to adjust
  for x in range(5):
      pipe.wait_for_frames()
  
  while(1):
  
      # Stores next frameset
      frameset = pipe.wait_for_frames()
      color_frame = frameset.get_color_frame()
      depth_frame = frameset.get_depth_frame()
  
      if color_frame:
          color = np.asanyarray(color_frame.get_data())
          depth = np.asanyarray(depth_frame.get_data())
  
          # Create alignment primitive with color as its target stream:
          align = rs.align(rs.stream.color)
          frameset = align.process(frameset)
  
          # Update color and depth frames:
          colorizer = rs.colorizer()
          aligned_depth_frame = frameset.get_depth_frame()
          colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
  
          # termination criteria
          criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
  
          # processing
          # resize the image to match the size of the depth-frame
          gray = cv2.cvtColor(color, cv2.COLOR_BGR2GRAY)
          resize = cv2.resize(gray,(1280,720))
  
          # Find the chessboard corners
          ret, corners = cv2.findChessboardCorners(resize, (rows, columns), None)
          #corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
  
          if ret:
              detected = cv2.drawChessboardCorners(resize, (7,9), corners, ret)
              for corner in corners:
                  x,y = corner.ravel()
  
                  dist = aligned_depth_frame.get_distance(x, y)
                  camera_info = aligned_depth_frame.profile.as_video_stream_profile().intrinsics
  
                  x_w, y_w, z_w = convert_depth_to_phys_coord_using_realsense(x, y, dist, camera_info)
  
  </code></pre>