将相机坐标转换为世界坐标

如何解决将相机坐标转换为世界坐标

我正在使用英特尔实感 D415 摄像头来测量到物体的距离并获得相应的 3D 坐标。我得到的坐标是准确的，但是当我尝试将它们转换为另一个坐标系时出现问题。

我想做的是：

在 RGB 帧中找到一个对象
获取物体的深度并计算相对于相机的 3D 位置（必须这样做，因为相机的原点位于左侧立体相机上）
将 3D 位置计算到世界坐标系中，使其对其他应用程序有用

第 1 步和第 2 步工作正常。我的问题在第 3 步。我使用 rs2_deproject_pixel_to_point 来获取正确的坐标。我的理解是，我可以校准第二个外在矩阵之类的东西，并使用以下方法转换新系统中的点：

P_world = R * P_Camera + t = Rt * P_Camera

Rt 为 3x4 矩阵，P_Camera 为 4x1 向量

为了获得矩阵Rt，我打印了一个棋盘并将其放置在我的世界坐标中的已知位置。我选择桌子的一角作为原点。现在我正在 RGB 帧中搜索棋盘并保存深度帧中的 3D 相机坐标。在这一步之后，我试图用来自 4 个具有不同 X、Y、Z 值和已知角位置的点的 12 个线性方程求解矩阵。到目前为止一切顺利，但我得到的结果远非正确，我不知道出了什么问题。

是方法本身的错误还是只是我的实现导致了错误的值？有什么方法可以轻松地转换另一个系统中的坐标吗？

以下是我用来查找棋盘和获取 3D 位置的代码：

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,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 == "High Accuracy":
        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),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,dist,camera_info)

将相机坐标转换为世界坐标

如何解决将相机坐标转换为世界坐标

相关推荐