OpenCVとPythonと私 vol.5

#!/usr/bin/python

import cv, numpy
from opencv import highgui

import sys
import getopt

filename = None
debug    = False
rows     = 0
columns  = 0
span     = 0

def main () :

        global filename
        global debug
        global rows
        global columns
        global span

        try:
                opts, args = getopt.getopt( sys.argv[1:], "hdr:c:s:",["help","debug","rows","columns","span"])
        except getopt.GetoptError, err :
                print str( err )
                usage()
                sys.exit(2)

        if len( args ) != 0 :
                filename = args[:]
        else :
                usage()
                sys.exit(0)

        for o, a in opts :
                if o in ( "-h", "--help" ) :
                        usage()
                        sys.exit(0)
                elif o in ( "-d", "--debug" ) :
                        debug = True
                elif o in ( "-r", "--rows" ) :
                        rows = int( a )
                elif o in ( "-c", "--columns" ) :
                        columns = int( a )
                elif o in ( "-s", "--span" ) :
                        span = int( a )
                else :
                        assert False, "unhandled option"

def debugout ( str ) :
        if debug == True :
                print str
    
def usage () :
        print "Usage: calibrateCamera.py [OPTION] FILE"
        print " "
        print " -h, --help    Display this message."
        print " -d, --debug   Output debug log."
        print " -r, --rows    Row counts of chessboard's corners"
        print " -c, --columns Column counts of chessboard's corners"
        print " -s, --span    Length of between 2 corners"

if __name__ == "__main__" :

        main()

        objectPointsList =
        imagePointsList =

        for element in filename :

                debugout( "Input file name is " + element )
                
                img = cv.LoadImage( element , cv.CV_LOAD_IMAGE_GRAYSCALE )
                corners = cv.FindChessboardCorners( img, ( columns, rows ) )

                for corner in corners[1] :
                        imagePointsList.append( list( corner ) )
                        tmp = list( corner )
                        debugout( "x_1 = " + str( tmp[0] ) + " / x_2 = " + str( tmp[1] ) )
                        del tmp

                ytmp = range( columns )
                ytmp.reverse()
                for x in range( rows ) :
                        for y in ytmp :
                                objectPoint = []
                                objectPoint.append( x * span )
                                objectPoint.append( y * span )
                                objectPoint.append( 0 )
                                debugout( "X_1 = " + str( x * span ) + " / X_2 = " + str( y * span ) )
                                objectPointsList.append( objectPoint )
                                del objectPoint

        imagePointsArray = numpy.array( imagePointsList, 'float' )
        imagePoints = cv.fromarray( imagePointsArray )
        debugout( "imagepoints: <size : " + str( cv.GetDims(imagePoints) ) + " >\n" + \
                          "              <channels : " + str( cv.CV_MAT_CN(cv.GetElemType(imagePoints) ) ) + " >" )

        objectPointsArray = numpy.array( objectPointsList, 'float' )
        objectPoints = cv.fromarray( objectPointsArray )
        debugout( "objectpoints: <size : " + str( cv.GetDims(objectPoints) ) + " >\n" + \
                          "              <channels : " + str( cv.CV_MAT_CN(cv.GetElemType(objectPoints) ) ) + " >" )

        cameraMatrix = cv.CreateMat( 3, 3, cv.CV_32FC1 )
        distortionCoeffs = cv.CreateMat( 1, 4, cv.CV_32FC1 )
        rotationVector = cv.CreateMat( len( filename ), 3, cv.CV_32FC1 )
        translationVector = cv.CreateMat( len( filename ), 3, cv.CV_32FC1 )

        pointCountsArray = rows * columns * numpy.ones( ( len( filename ), 1 ) ,'int32')
        pointCounts = cv.fromarray( pointCountsArray )

        debugout( "pointcounts: <size : " + str( cv.GetDims(pointCounts) ) + " >\n" + \
                          "              <channels : " + str( cv.CV_MAT_CN(cv.GetElemType(pointCounts) ) ) + " >" )
        try:
                cv.CalibrateCamera2( objectPoints, imagePoints, \
                                             pointCounts ,\
                                             ( img.height, img.width ) ,\
                                             cameraMatrix, distortionCoeffs,\
                                             rotationVector, translationVector, 0 )
        except cv.error, err :
                print str( err )
                sys.exit(2)

        print "CameraMatrix :"
        print [ cameraMatrix[0,i] for i in range(3) ]
        print [ cameraMatrix[1,i] for i in range(3) ]
        print [ cameraMatrix[2,i] for i in range(3) ]
        print "\n"

        print "Distortion Coeffecients:"
        print [ distortionCoeffs[0,i] for i in range(4) ]
        print "\n"

        print "Rotation Vector:"
        print [ rotationVector[0,i] for i in range(3) ]
        print "\n"

        print "Translation Vector:"
        print [ translationVector[0,i] for i in range(3) ]
        print "\n"