Improve result of minimize scipy function

Viewed 72

I'm using minimize() function on the loop, trying to find rotation-translation matrix between pointcloud and camera pixels. As a parameters I'm giving 6 points coordinates in camera frame (uvs) and corresponding xyz (points) and calculate error:

result = minimize( costFunction, settings[ 'initialTransform' ], args = ( 1.0, False ), bounds = settings[ 'bounds' ], method = 'SLSQP', options = { 'disp': True, 'maxiter': 1000 } )

    while not result.success or result.fun > 50:
        for i in range( 0, len( settings[ 'initialTransform' ] ) ):
            settings[ 'initialTransform' ][ i ] = random.uniform( settings[ 'bounds' ][ i ][ 0 ], settings[ 'bounds' ][ i ][ 1 ] )
        result = minimize( costFunction, settings[ 'initialTransform' ], args = ( 1.0, False ), bounds = settings[ 'bounds' ], method = 'SLSQP', options = { 'disp': True, 'maxiter': 1000 } )

It does work, however it looks for me like calculations stuck in the infinite loop - it just can't find an optimal solution. Here is a costFunction implementation:

    def costFunction( x, sign = 1.0, debug = False ):
        global cameraModel, settings
        parameters = x
        translation = [ parameters[ 0 ], parameters[ 1 ], parameters[ 2 ], 1.0 ]
        rotationMatrix = tf.transformations.euler_matrix( parameters[ 5 ], parameters[ 4 ], parameters[ 3 ] )
        rotationMatrix[ :, 3 ] = translation
        error = 0
        for i in range( 0, len( settings[ 'points' ] ) ):
            point = settings[ 'points' ][ i ]
            expectedUV = settings[ 'uvs' ][ i ]
            rotatedPoint = rotationMatrix.dot( point )
            uv = cameraModel.project3dToPixel( rotatedPoint )
            diff = np.array( uv ) - np.array( expectedUV )
            error = error + math.sqrt( np.sum( diff ** 2 ) )
        return sign * error

So I'm wondering if I give more than 6 point will it help or should I increase threshold for error? Appreciate any hints and algorithm improvements.

0 Answers
Related