相机工作原理和理解SDK流程

Posted 2022-11-23 东东就是我

1.理论知识

https://zhuanlan.zhihu.com/c_1263787122512822272
相机工作原理介绍

http://t.zoukankan.com/eve612-p-13841736.html
相机SDK介绍

https://zhuanlan.zhihu.com/p/468033445

2.代码解读

（sick的visionary-s双目3D相机的SDK）

1. 链接相机

deviceStreaming = Device.Streaming(args.ipAddress,args.tcpPort)
deviceStreaming.openStream()

''' Opens the streaming channel. '''
def openStream(self):
    logging.warning("HPF Opening streaming socket..."),
    self.sock_stream = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # 2,1
    self.sock_stream.settimeout(5)
    try:
        self.sock_stream.connect((self.ipAddress, self.tcpPort))
    except socket.error as err:
        logging.error("Error on connecting to %s:%d: %s" % (self.ipAddress, self.tcpPort, err))
        sys.exit(2)
    logging.info("...done.")

2.发送接收数据请求

通过socket发送blob请求

deviceStreaming.sendBlobRequest()
def sendBlobRequest(self):
    """ Sending a blob request. """
    MSG_BLREQ_TX = b'BlbReq'

    logging.debug("Sending BlbReq: %s" % (to_hex(MSG_BLREQ_TX)))
    self.sock_stream.send(MSG_BLREQ_TX)

3.定义一个data类接受数据

dontStop = True

myData = Data.Data()

4.接受数据放在devicestreaming.frame

while dontStop:
    try:
        deviceStreaming.getFrame() # 下面有实现
        wholeFrame = deviceStreaming.frame

        myData.read(wholeFrame)

        if myData.hasDepthMap:
            print('\\nData contains depth map data:')
            print('Frame number: ', myData.depthmap.frameNumber)
            distanceData = myData.depthmap.distance

前面向相机发送了字符“BlbReq”，然后执行了getframe，

def getFrame(self):
    """ Receives the raw data frame from the device via the streaming channel."""
    logging.warning('HPF !!_> Reading image from stream...')
    keepRunning = True

    BLOB_HEAD_LEN = 11
    header = self.sock_stream.recv(BLOB_HEAD_LEN)  # minimum header
    frameAcqStart = time.perf_counter()
    logging.debug("len(header) = %d dump: %s" % (len(header),to_hex(header)))
    if len(header) < BLOB_HEAD_LEN:
        raise RuntimeError("Uh, not enough bytes for BLOB_HEAD_LEN, only %s" % (len(header)))

    # check if the header content is as expected
    (magicword, pkgLength, protocolVersion, packetType) = \\
        struct.unpack('>IIHB', header)  #  Python中按一定的格式取出某字符串中的子字符串 
    if magicword != 0x02020202:
        logging.error("Unknown magic word: %0x" % (magicword))
        keepRunning = False
    if protocolVersion != 0x0001:
        logging.error("Unknown protocol version: %0x" % (protocolVersion))
        keepRunning = False
    if packetType != 0x62:
        logging.error("Unknown packet type: %0x" % (packetType))
        keepRunning = False

    if not keepRunning:
        raise RuntimeError('something is wrong with the buffer')

    # -3 for protocolVersion and packetType already received
    # +1 for checksum
    toread = pkgLength - 3 + 1
    logging.debug("pkgLength: %d" % (pkgLength))
    logging.debug("toread: %d" % (toread))

    data = bytearray(len(header) + toread)
    view = memoryview(data)
    view[:len(header)] = header
    view = view[len(header):]
    while toread:
        nBytes = self.sock_stream.recv_into(view, toread)
        if nBytes==0:
            # premature end of connection
            raise RuntimeError("received  but requested  bytes".format(len(data)-len(view), pkgLength))
        view = view[nBytes:]
        toread -= nBytes

    self.frame = str(data)

    frameAcqStop = time.perf_counter()
    logging.info("Receiving took %0.1f ms"%((frameAcqStop-frameAcqStart)*1000))
    # full frame should be received now
    logging.debug("...done.")

首先定义一个长度是head+plglength的data数组。然后把数组的内存地址给到view，操作view就相当于操作了data，所有后面获取sock的数据都是存在data里，然后赋值给self.frame

5. 获取frame赋值给刚开始定义的data类

wholeFrame = deviceStreaming.frame

myData.read(wholeFrame)

def read(self, dataBuffer):
""" Extracts necessary data segments and triggers parsing of segments. """

# first 11 bytes contain some internal definitions
tempBuffer = dataBuffer[0:11]
(magicword, pkglength, protocolVersion, packetType) = \\
    unpack('>IIHB', tempBuffer)
assert (magicword == 0x02020202)
logging.debug("Package length: %s", pkglength)
logging.debug("Protocol version: %s", protocolVersion)  # expected to be == 1
logging.debug("Packet type: %s", packetType)  # expected to be  == 98

# next four bytes an id (should equal 1) and
# the number of segments (should be 3)
tempBuffer = dataBuffer[11:15]
(segid, numSegments) = unpack('>HH', tempBuffer)
logging.debug("Blob ID: %s", segid)  # expected to be == 1
logging.debug("Number of segments: %s", numSegments)  # expected to be  == 3

# offset and changedCounter, 4 bytes each per segment
offset = [None] * numSegments
changedCounter = [None] * numSegments
tempBuffer = dataBuffer[15:15 + numSegments * 2 * 4]
for i in range(numSegments):
    index = i * 8
    (offset[i], changedCounter[i]) = \\
        unpack('>II', tempBuffer[index:index + 8])
    offset[i] += 11
logging.debug("Offsets: %s", offset)  # offset in bytes for each segment
logging.debug("Changed counter: %s", changedCounter)  # counter for changes in the data

# first segment describes the data format in XML
xmlSegment = dataBuffer[offset[0]:offset[1]]
logging.debug("The whole XML segment:")
logging.debug(xmlSegment)
# second segment contains the binary data
binarySegment = dataBuffer[offset[1]:offset[2]]

if (numSegments == 3):
    overlaySegment = dataBuffer[offset[2]:pkglength+4+4] # numBytes(magicword) = 4, numBytes(pkglength) = 4
    logging.debug("The whole overlay XML segment:")
    logging.debug(overlaySegment)

checksum = dataBuffer[pkglength+8]
if checksum != self.checksum:
  logging.error("Checksum is wrong: %s (expected %s)" % (checksum, self.checksum)) # checksum of whole data
  self.corrupted = True
else:
  logging.debug("Checksum: %s", checksum) # checksum of whole data
  self.corrupted = False

# parsing the XML in order to extract necessary image information
# only parse if something has changed
if (self.changedCounter < changedCounter[0]):
    logging.debug("XML did change, parsing started.")
    myXMLParser = XMLParser()
    myXMLParser.parse(xmlSegment)
    self.xmlParser = myXMLParser
    self.changedCounter = changedCounter[0]
else:
    logging.debug("XML did not change, not parsing again.")
    myXMLParser = self.xmlParser

myBinaryParser = BinaryParser()

self.hasDepthMap = False
self.hasPolar2D = False
self.hasCartesian = False

if myXMLParser.hasDepthMap:
    logging.debug("Data contains depth map, reading camera params")
    self.hasDepthMap = True
    self.cameraParams = \\
        CameraParameters(width=myXMLParser.imageWidth,
                         height=myXMLParser.imageHeight,
                         cam2worldMatrix=myXMLParser.cam2worldMatrix,
                         fx=myXMLParser.fx, fy=myXMLParser.fy,
                         cx=myXMLParser.cx, cy=myXMLParser.cy,
                         k1=myXMLParser.k1, k2=myXMLParser.k2,
                         f2rc=myXMLParser.f2rc)

    # extracting data from the binary segment (distance, intensity
    # and confidence).
    if myXMLParser.stereo:
        numBytesDistance = myXMLParser.imageHeight * \\
                           myXMLParser.imageWidth * \\
                           myXMLParser.numBytesPerZValue
    else:
        numBytesDistance = myXMLParser.imageHeight * \\
                           myXMLParser.imageWidth * \\
                           myXMLParser.numBytesPerDistanceValue
    numBytesIntensity = myXMLParser.imageHeight * \\
                        myXMLParser.imageWidth * \\
                        myXMLParser.numBytesPerIntensityValue
    numBytesConfidence = myXMLParser.imageHeight * \\
                         myXMLParser.imageWidth * \\
                         myXMLParser.numBytesPerConfidenceValue

    try:
        numBytesFrameNumber = myXMLParser.numBytesFrameNumber
        numBytesQuality = myXMLParser.numBytesQuality
        numBytesStatus = myXMLParser.numBytesStatus
    except AttributeError:
        numBytesFrameNumber = 0
        numBytesQuality = 0
        numBytesStatus = 0

    logging.info("Reading binary segment...")
    myBinaryParser.getDepthMap(binarySegment,
                               numBytesFrameNumber,
                               numBytesQuality,
                               numBytesStatus,
                               numBytesDistance,
                               numBytesIntensity,
                               myXMLParser.numBytesPerIntensityValue,
                               numBytesConfidence)
    logging.info("...done.")

    if myXMLParser.stereo:
        distance = list(myBinaryParser.depthmap.distance)
        for i in range(0, len(distance)):
            distance[i] = distance[i] / 10.0 #account for sub-millimeter values
        myBinaryParser.depthmap.distance = tuple(distance)
    self.depthmap = myBinaryParser.depthmap

if myXMLParser.hasPolar2DData:
    self.hasPolar2D = True
    if (myXMLParser.hasDepthMap):
        myBinaryParser.getPolar2D(myBinaryParser.remainingBuffer, myXMLParser.numPolarValues)
    else:
        myBinaryParser.getPolar2D(binarySegment, myXMLParser.numPolarValues)
    if hasattr(myBinaryParser, 'polardata'):
        self.polarData2D = myBinaryParser.polardata
    else:
        self.hasPolar2D = False
elif myXMLParser.hasCartesianData:
    self.hasCartesian = True
    if (myXMLParser.hasDepthMap):
        myBinaryParser.getCartesian(myBinaryParser.remainingBuffer)
    else:
        myBinaryParser.getCartesian(binarySegment)
    if hasattr(myBinaryParser, 'cartesianData'):
        self.cartesianData = myBinaryParser.cartesianData
    else:
        self.hasCartesian = False

1. 先取出11个长度的头文件
   2.取出4个长度
   3.给offest和changecounter赋值 offest表示数据片段大小
   
   4.第一个片段，是xml数据，第二个是binary数据，第三个是覆盖xml数据（？？？）
   
   
   

然后把切割出来的数据分发到相应的实例中

这里myxmlparser.parse 是读取xml文件内容 ，然后把self.xmlparser=myXMLParser

根据xml的参数 获取binarysegment中的数据，然后把深度数据放入myBinaryParser的self.depthmap

然后在赋值给self.depthmap