奥比中光相机的深度图像数据（TUM数据集）

https://github.com/ParikaGoel/KinectFusion

https://www.microsoft.com/en-us/research/wp-content/uploads/2016/02/ismar2011.pdf

https://vision.in.tum.de/data/datasets/rgbd-dataset/download

https://dev.intelrealsense.com/docs

https://github.com/IntelRealSense/librealsense/releases

https://www.mrpt.org/tutorials/programming/maths-and-geometry/2d_3d_geometry/

https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/

import argparse
import sys
import os
import numpy


def read_file_list(filename):
    """
    Reads a trajectory from a text file. 

    File format:
    The file format is "stamp d1 d2 d3 ...", where stamp denotes the time stamp (to be matched)
    and "d1 d2 d3.." is arbitary data (e.g., a 3D position and 3D orientation) associated to this timestamp. 

    Input:
    filename -- File name

    Output:
    dict -- dictionary of (stamp,data) tuples

    """
    file = open(filename)
    data = file.read()
    lines = data.replace(",", " ").replace("\t", " ").split("\n")
    list = [[v.strip() for v in line.split(" ") if v.strip() != ""]
            for line in lines if len(line) > 0 and line[0] != "#"]
    list = [(float(l[0]), l[1:]) for l in list if len(l) > 1]
    return dict(list)


def associate(first_list, second_list, offset, max_difference):
    """
    Associate two dictionaries of (stamp,data). As the time stamps never match exactly, we aim 
    to find the closest match for every input tuple.

    Input:
    first_list -- first dictionary of (stamp,data) tuples
    second_list -- second dictionary of (stamp,data) tuples
    offset -- time offset between both dictionaries (e.g., to model the delay between the sensors)
    max_difference -- search radius for candidate generation

    Output:
    matches -- list of matched tuples ((stamp1,data1),(stamp2,data2))

    """
    first_keys = first_list.keys()
    second_keys = second_list.keys()
    potential_matches = [(abs(a - (b + offset)), a, b)
                         for a in first_keys
                         for b in second_keys
                         if abs(a - (b + offset)) < max_difference]
    potential_matches.sort()
    matches = []
    for diff, a, b in potential_matches:
        if a in first_keys and b in second_keys:
            first_keys.remove(a)
            second_keys.remove(b)
            matches.append((a, b))

    matches.sort()
    return matches


if __name__ == '__main__':

    # parse command line
    parser = argparse.ArgumentParser(description='''
    This script takes two data files with timestamps and associates them   
    ''')
    parser.add_argument(
        'first_file', help='first text file (format: timestamp data)')
    parser.add_argument(
        'second_file', help='second text file (format: timestamp data)')
    parser.add_argument(
        '--first_only', help='only output associated lines from first file', action='store_true')
    parser.add_argument(
        '--offset', help='time offset added to the timestamps of the second file (default: 0.0)', default=0.0)
    parser.add_argument(
        '--max_difference', help='maximally allowed time difference for matching entries (default: 0.02)', default=0.02)
    args = parser.parse_args()

    first_list = read_file_list(args.first_file)
    second_list = read_file_list(args.second_file)

    matches = associate(first_list, second_list, float(
        args.offset), float(args.max_difference))

    if args.first_only:
        for a, b in matches:
            print("%f %s" % (a, " ".join(first_list[a])))
    else:
        for a, b in matches:
            print("%f %s %f %s" % (a, " ".join(
                first_list[a]), b-float(args.offset), " ".join(second_list[b])))

https://svncvpr.in.tum.de/cvpr-ros-pkg/trunk/rgbd_benchmark/rgbd_benchmark_tools/src/rgbd_benchmark_tools/

https://vision.in.tum.de/data/datasets/rgbd-dataset/tools#evaluation

  1024×768×16bit = 
  (1024×768×16)/8 Byte = 
  [(1024×768×16)/8]/1024 KB = 
  1536 KB = 
  {[(1024×768×16)/8]/1024}/1024 MB 
  = 1.5 MB

focalLength = 525.0
centerX = 319.5
centerY = 239.5
scalingFactor = 5000.0

def depth2mi(depthValue):

    return depthValue * 0.001


def depth2xyz(u, v, depthValue):
    fx = 361.1027
    fy = 361.8266
    cx = 258.4545
    cy = 212.1282

    depth = depth2mi(depthValue)

    z = float(depth)
    x = float((u - cx) * z) / fx
    y = float((v - cy) * z) / fy

    result = [x, y, z]
    return result

#深度图存入np数组
arr = np.array(depth)
#彩色图上某点像素坐标（u，v）在对齐深度图的深度值
depthValue =float(arr3[v, u])
coordinate = depth2xyz(u, v, depthValue)