Functions
FASTCV_API void	fcvGeomAffineFitf32 (const fcvCorrespondences __restrict corrs, float __restrict affine)
	Computes affine trans. for a given set of corresponding features points using a linear least square colver based on Cholkesky decomposition.
FASTCV_API int	fcvGeomAffineEvaluatef32 (const fcvCorrespondences __restrict corrs, float __restrict affine, float maxsqerr, uint16_t __restrict inliers, int32_t numinliers)
	Evaluates specified affine transformation against provided points correspondences. Checks which correspondence members have a projection error that is smaller than the given one (maxSquErr).
FASTCV_API void	fcvGeomHomographyFitf32 (const fcvCorrespondences __restrict corrs, float __restrict homography)
	Performs cholesky homography fitting on specified points correspondences. [x_to] [ a11 a12 a13 ] [ x_from ] [y_to] = [ a21 a22 a23 ] * [ y_from ] [ 1 ] [ a31 a32 a33 ] [ 1 ] note that all the correspondences are considered, if correspondence pairs are smaller than 4, the API returns. If correspondence pairs are larger than 4, the API takes all the correspondences into consideration using least squared method.
FASTCV_API int	fcvGeomHomographyEvaluatef32 (const fcvCorrespondences __restrict corrs, float __restrict homography, float maxsqerr, uint16_t __restrict inliers, int32_t numinliers)
	Evaluates specified homography against provided points correspondences. Check which correspondence members have a projection error that is smaller than the given one (maxSquErr).
FASTCV_API int	fcvGeomHomographyEvaluatef32_v2 (const fcvCorrespondences __restrict corrs, float32_t __restrict homography, float32_t maxsqerr, uint16_t __restrict inliers, float32_t __restrict errinliers, int32_t *numinliers)
	Evaluates specified homography against provided points correspondences. Check which correspondence members have a projection error that is smaller than the given one (maxSquErr).
FASTCV_API float	fcvGeomPoseRefineGNf32 (const fcvCorrespondences __restrict corrs, short minIterations, short maxIterations, float stopCriteria, float initpose, float *refinedpose)
	Performs cholesky pose fitting on specified points correspondences. Takes a pose and uses the correspondences to refine it using iterative Gauss-Newton optimization.
FASTCV_API int	fcvGeomPoseUpdatef32 (const float __restrict projected, const float __restrict reprojErr, const float __restrict invz, const float __restrict reprojVariance, unsigned int numpts, float *__restrict pose)
	Update and compute the differential pose based on the specified points correspondences This function and fcvGeomPoseOptimizeGNf32 can be used iteratively to perform poseRefine GN.
FASTCV_API int	fcvGeomPoseOptimizeGNf32 (const float __restrict projected, const float __restrict reprojErr, const float __restrict invz, const float __restrict reprojVariance, unsigned int numpts, float *__restrict pose)
	Update the pose based on the specified points correspondences using Gauss-Newton optimization. This function and fcvGeomPoseEvaluateErrorf32 can be used iteratively to perform poseRefine GN.
FASTCV_API float	fcvGeomPoseEvaluateErrorf32 (const fcvCorrespondences __restrict corrs, const float __restrict pose, float __restrict projected, float __restrict reprojErr, float __restrict invz, float __restrict reprojVariance)
	Calculate the reprojection error based on the input pose. This function and fcvGeomPoseOptimizef32 can be used iteratively to perform poseRefine (GN or LM)..
FASTCV_API int	fcvGeomPoseEvaluatef32 (const fcvCorrespondences __restrict corrs, const float pose, float maxSquErr, uint16_t __restrict inliers, uint32_t numInliers)
	Checks which members have a projection error that is smaller than the given one.
FASTCV_API void	fcvGeom3PointPoseEstimatef32 (const fcvCorrespondences __restrict corrs, float pose, int32_t *numPoses)
	Estimates a 6DOF pose NOTE: Given the coordinates of three 3D points (in world reference frame), and their corresponding perspective projections in an image, this algorithm determines the position and orientation of the camera in the world reference frame. The function provides up to four solutions that can be disambiguated using a fourth point. When used in conjunction with RANSAC, this function can perform efficient outlier rejection. Two degenerate cases should be avoided when using this function:
FASTCV_API void	fcvGeomDistortPoint2x1f32 (const float32_t __restrict cameraCalibration, const float32_t __restrict xyCamera, float32_t *__restrict xyDevice)
	Applies radial distortion to a 2D coordinate in camera coordinates and returns the distorted coordinate in device coordinates.
FASTCV_API void	fcvGeomDistortPoint2xNf32 (const float32_t __restrict cameraCalibration, const float32_t __restrict xyCamera, uint32_t srcStride, uint32_t xySize, float32_t *__restrict xyDevice, uint32_t dstStride)
	Applies radial distortion to a set of 2D coordinates in camera coordinates and returns the distorted coordinates in device coordinates. brief algorithm desribed in fcvGeomDistortPoint2x1f32.
FASTCV_API void	fcvGeomUndistortPoint2x1f32 (const float32_t __restrict cameraCalibration, const float32_t __restrict xyDevice, float32_t *__restrict xyCamera)
	Applies radial undistortion to a 2D coordinate in device coordinates and returns the undistorted coordinate in camera coordinates.
FASTCV_API void	fcvGeomUndistortPoint2xNf32 (const float32_t __restrict cameraCalibration, const float32_t __restrict xyDevice, uint32_t srcStride, uint32_t xySize, float32_t *__restrict xyCamera, uint32_t dstStride)
	Applies radial undistortion to a 2D coordinate in device coordinates and returns the undistorted coordinate in camera coordinates. brief algorithm desribed in fcvGeomUndistortPoint2x1f32.
FASTCV_API int32_t	fcvGeomProjectPoint3x1f32 (const float32_t __restrict pose, const float32_t __restrict cameraCalibration, const float32_t __restrict xyz, float32_t __restrict xyCamera, float32_t *__restrict xyDevice)
	Transforms a 3D point using a pose-matrix, projects the transformed point, distorts the projected 2D point and converts to device coordinates.
FASTCV_API void	fcvGeomProjectPoint3xNf32 (const float32_t __restrict pose, const float32_t __restrict cameraCalibration, const float32_t __restrict xyz, uint32_t srcStride, uint32_t xyzSize, float32_t __restrict xyCamera, float32_t __restrict xyDevice, uint32_t dstStride, uint32_t inFront)
	Transforms a 3D point using a pose-matrix, projects the transformed point, distorts the projected 2D point and converts to device coordinates. brief algorithm desribed in fcvGeomProjectPoint3x1f32.
FASTCV_API fcvStatus	fcvInvertAffineTransformf32 (const float32_t __restrict M, float32_t __restrict invAffineMat)
	Invert an affine transformation.
FASTCV_API fcvStatus	fcvGeomHomographyRobustFitf32 (const fcvCorrespondences __restrict corr, float32_t __restrict homography, float32_t reprojThreshold)
	Performs robust homography fitting on specified points correspondences.
FASTCV_API fcvStatus	fcvICPJacobianErrorSE3f32 (const uint16_t __restrict depthData, uint32_t depthWidth, uint32_t depthHeight, uint32_t depthStride, const float32_t __restrict refPointsNormals, uint32_t numPoints, const float32_t __restrict refPose, const float32_t __restrict camera, float32_t sqDistThreshold, float32_t __restrict sqDists, float32_t __restrict errors, float32_t *__restrict jacobian)
	Compute Point-to-Plane ICP Jacobian matrix and errors for SE3 motion.
FASTCV_API fcvStatus	fcvGeomHomographyFitf32_v2 (const fcvCorrespondences __restrict corrs, float32_t __restrict homography, uint32_t mode)
	Performs homography fitting on specified points correspondences. [x_to] [ a11 a12 a13 ] [ x_from ] [y_to] = [ a21 a22 a23 ] * [ y_from ] [ 1 ] [ a31 a32 a33 ] [ 1 ] note that all the correspondences are considered, if correspondence pairs are smaller than 4, the API returns. If correspondence pairs are larger than 4, the API takes all the correspondences into consideration using least squared method.
FASTCV_API fcvStatus	fcvGeomHomographyRobustFitf32_v2 (const fcvCorrespondences __restrict corr, float32_t __restrict homography, float32_t reprojThreshold, uint32_t mode)
	Performs robust homography fitting on specified points correspondences.
FASTCV_API fcvStatus	fcvDepthFusion8x8x8xNs16 (const fcvDepthFusionBlockConfig __restrict configs, int16_t __restrict volumes, uint32_t numBlocks, uint32_t volumeStride, const float32_t __restrict depthData, uint32_t depthWidth, uint32_t depthHeight, uint32_t depthStride, const float32_t __restrict cameraCalibration, int16_t maxWeight)
	Volumetric depth fusion into multiple 8x8x8 blocks.
FASTCV_API fcvStatus	fcvUndistortDisparityConvertDepthf32 (const float32_t __restrict src, uint32_t srcWidth, uint32_t srcHeight, uint32_t srcStride, const uint8_t __restrict mask, uint32_t maskStride, const float32_t __restrict pixelDistortion, uint32_t pixelDistortionStride, uint32_t convertDepth, const float32_t __restrict imageDistortion, const float32_t __restrict depthParam, float32_t __restrict dst, uint32_t dstStride)
	Undistort a disparity image and optionally convert the undistorted disparity image into a depth image.
FASTCV_API fcvStatus	fcvRegisterDepthImagef32 (const float32_t __restrict src, uint32_t srcWidth, uint32_t srcHeight, uint32_t srcStride, const float32_t __restrict Kdinv, const float32_t __restrict Kc, const float32_t __restrict Rd2c, const float32_t __restrict Td2c, float32_t __restrict dst, uint32_t dstStride)
	Registers an input Depth Image with an input Color Image.
FASTCV_API fcvStatus	fcvConvertDepthImageToPointCloudf32 (const float32_t __restrict src, uint32_t srcWidth, uint32_t srcHeight, uint32_t srcStride, const float32_t __restrict Kdinv, float32_t *__restrict dst, uint32_t dstStride)
	Convert depth image into 3D point Cloud.

Detailed Description

Homography, pose evaluation functions

Function Documentation

FASTCV_API fcvStatus fcvConvertDepthImageToPointCloudf32	(	const float32_t *__restrict	src,
		uint32_t	srcWidth,
		uint32_t	srcHeight,
		uint32_t	srcStride,
		const float32_t *__restrict	Kdinv,
		float32_t *__restrict	dst,
		uint32_t	dstStride
	)

Convert depth image into 3D point Cloud.

Parameters:

src	Pointer to the depth image
srcWidth	Width of the depth image in pixel. The number of pixels in a row.
srcHeight	Height of the depth image in pixel
srcStride	Stride of depth image is the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to srcWidthsizeof(float32_t). NOTE:* should be multiple of 8.
Kdinv	Kdinv is an array of 9 elements representing the inverse of the 3x3 depth camera matrix arranged row wise.
dst	Pointer to the 3D point cloud in an interleaved fashion x0,y0,z0,x1,y1,z1. It has a width of 3*srcWidth and has the same height as the src buffer.
dstStride	Stride for output image, i.e. the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to 3srcWidthsizeof(float32_t) NOTE: should be multiple of 8.

Returns:: FASTCV_SUCCESS upon success. Other status codes upon failure.

3

FASTCV_API fcvStatus fcvDepthFusion8x8x8xNs16	(	const fcvDepthFusionBlockConfig *__restrict	configs,
		int16_t *__restrict	volumes,
		uint32_t	numBlocks,
		uint32_t	volumeStride,
		const float32_t *__restrict	depthData,
		uint32_t	depthWidth,
		uint32_t	depthHeight,
		uint32_t	depthStride,
		const float32_t *__restrict	cameraCalibration,
		int16_t	maxWeight
	)

Volumetric depth fusion into multiple 8x8x8 blocks.

Fuses a depth map into one or more volume blocks of 8x8x8 samples each. The volume blocks have a size of 9x9x9 samples, but only 8x8x8 samples will be processed, leaving a border of one sample that can be used to create an overlap with neighboring blocks. Each sample consists of two 16-bit values encoding the distance (scaled by the ramp) and the fusion count. The volume block samples must be initialized to (32766, 0) before the first fusion. The samples of the blocks are updated by calculating a running average with respect to depth map samples.

Parameters:

configs	Setup details for the fusion blocks. Must contain numBlocks items.
volumes	An array of numBlocks pointers to the raw fusion data. Each block consists of 8x8x8 samples, which each consist of two 16-bit values.
numBlocks	Number of blocks to fuse.
volumeStride	Volume stride, i.e. the gap (in terms of bytes) between the first element of a volume and that of the successive volume. If srcStride is equal to 0, it will be set to 2048 (the size of volume).
depthData	Pointer to the depth map to fuse into the blocks. Depth data is stored as float in scale of scene units (e.g. meters).
depthWidth	Width of the depth map in pixels
depthHeight	Height of the depth map in pixels
depthStride	Stride of the depth map in bytes
cameraCalibration	Camera calibration with 4 parameters: focal-length (x and y) and principal point (x and y) in pixels
maxWeight	Maximum weight for the running average. After maxWeight fusions a sample is being updated with a weight of 1/maxWeight. Must be smaller than 128.

Returns:: FASTCV_SUCCESS upon success, Other status codes upon failure

3

FASTCV_API void fcvGeom3PointPoseEstimatef32	(	const fcvCorrespondences *__restrict	corrs,
		float *	pose,
		int32_t *	numPoses
	)

Estimates a 6DOF pose
NOTE: Given the coordinates of three 3D points (in world reference frame), and their corresponding perspective projections in an image, this algorithm determines the position and orientation of the camera in the world reference frame. The function provides up to four solutions that can be disambiguated using a fourth point. When used in conjunction with RANSAC, this function can perform efficient outlier rejection. Two degenerate cases should be avoided when using this function:

Indeterminate configuration: When the three points are collinear in space, there will be a family of poses mapping the three points to the same image points.
Unstable configuration: The camera center is located on a circular cylinder passing through the three points and the camera optical axis is perpendicular to the plane derived by the three points. With this configuration, a small change in the position of the three points will result in a large change of the estimated pose..

Parameters:

corrs	2D-3D correspondence points
pose	computed pose (numPoses * 12 data)
numPoses	(max = 4)

3

FASTCV_API int fcvGeomAffineEvaluatef32	(	const fcvCorrespondences *__restrict	corrs,
		float *__restrict	affine,
		float	maxsqerr,
		uint16_t *__restrict	inliers,
		int32_t *	numinliers
	)

Evaluates specified affine transformation against provided points correspondences. Checks which correspondence members have a projection error that is smaller than the given one (maxSquErr).

Parameters:

corrs	Pointer to correspondences structure.
affine	Affine matrix representing relationship between ptTo and ptFrom correspondences stored as 3x3 floating point matrix formatted as

Parameters:

maxsqerr	Maximum error value squared.
inliers	Output array for those indices that passed the test - the array MUST be able to store numIndices items.
numinliers	Output number of corrs that passed the test.

Returns:: 0 if successfull -1 if error value square is >= maxsqerr

3

FASTCV_API void fcvGeomAffineFitf32	(	const fcvCorrespondences *__restrict	corrs,
		float *__restrict	affine
	)

Computes affine trans. for a given set of corresponding features points using a linear least square colver based on Cholkesky decomposition.

Parameters:

corrs	Correspondence data struct containing coords of points in two frames
affine	3 x 3 affine matrix (computed best fit affine transformation)

3

FASTCV_API void fcvGeomDistortPoint2x1f32	(	const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyCamera,
		float32_t *__restrict	xyDevice
	)

Applies radial distortion to a 2D coordinate in camera coordinates and returns the distorted coordinate in device coordinates.

input: (x,y) focal length: f1,f2 principle point: p1,p2 radical distortion: k1,k2 tangential distortion: t1,t2 Output (xd,yd) r^2 = x^2+y^2 cdist = 1+k1*r^2 + k2*r^4 a0 = 2*x*y, a1 = 3*x^2 + y^2, a2 = x^2+3*y^2 xd = (x*cdist + t1*a1 + t2*a2)*f1 + p1 yd = (y*cdist + t1*a3 + t2*a1)*f2 + p2

Parameters:

cameraCalibration	Camera calibration with 8 parameter: focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: should be 128-bit aligned.
xyCamera	Input of the undistorted 2D camera coordinate (2 float values).
xyDevice	Output of the distorted 2D device coordinate (2 float values).

3

FASTCV_API void fcvGeomDistortPoint2xNf32	(	const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyCamera,
		uint32_t	srcStride,
		uint32_t	xySize,
		float32_t *__restrict	xyDevice,
		uint32_t	dstStride
	)

Applies radial distortion to a set of 2D coordinates in camera coordinates and returns the distorted coordinates in device coordinates. brief algorithm desribed in fcvGeomDistortPoint2x1f32.

Parameters:

cameraCalibration	Camera calibration with 8 parameter: focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: should be 128-bit aligned.
xyCamera	Input of the undistorted 2D camera coordinates (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the input points. Total memory allocated must be large enough to accomodate the input+padding, which has size of NsrcStride (in bytes) NOTE:* should be 128-bit aligned.
srcStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the input array has points as follows x0 y0 0 0 0 x1 y1 0 0 0 x2 y2 ..., then the stride is 5 * size(float32_t) = 20 NOTE: should be a multiple of 8.
xySize	Number of points N
xyDevice	Output of the distorted 2D device coordinates (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the output points. Total memory allocated must be large enough to accomodate the output+padding, which has size of NdstStride (in bytes) NOTE:* should be 128-bit aligned.
dstStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the output array has points as follows x0 y0 0 0 0 x1 y1 0 0 0 x2 y2 ..., then the stride is 5 * size(float32_t) = 20 NOTE: should be a multiple of 8.

3

FASTCV_API int fcvGeomHomographyEvaluatef32	(	const fcvCorrespondences *__restrict	corrs,
		float *__restrict	homography,
		float	maxsqerr,
		uint16_t *__restrict	inliers,
		int32_t *	numinliers
	)

Evaluates specified homography against provided points correspondences. Check which correspondence members have a projection error that is smaller than the given one (maxSquErr).

ATTENTION: This function's signature will become OBSOLETE in a future release of this library (2.0.0). The new interface is specified in the function: fcvGeomHomographyEvaluatef32_v2(). In the 2.0.0 release, fcvGeomHomographyEvaluatef32_v2 will be renamed to fcvGeomHomographyEvaluatef32 and the signature of fcvGeomHomographyEvaluatef32 as it appears now, will be removed.

Parameters:

corrs	Pointer to correspondences structure.
homography	Homography representing relationship between ptTo and ptFrom correspondences stored as 3x3 floating point matrix formatted as

Parameters:

maxsqerr	Maximum error value squared.
inliers	Output array for those indices that passed the test - the array MUST be able to store numIndices items.
numinliers	Output number of corrs that passed the test.

Returns:: 0 that error is less than maximum error, -1 greater or equal to maximum error.

3

FASTCV_API int fcvGeomHomographyEvaluatef32_v2	(	const fcvCorrespondences *__restrict	corrs,
		float32_t *__restrict	homography,
		float32_t	maxsqerr,
		uint16_t *__restrict	inliers,
		float32_t *__restrict	errinliers,
		int32_t *	numinliers
	)

Evaluates specified homography against provided points correspondences. Check which correspondence members have a projection error that is smaller than the given one (maxSquErr).

ATTENTION: This function is a duplication of fcvGeomHomographyEvaluatef32() with the addition of extra parameters. This function has been added to allow for backward compatibility with the original function. When the 2.0.0 release of this library is made, this function will be renamed to: fcvGeomHomographyEvaluatef32, fcvGeomHomographyEvaluatef32_v2 will be removed, and the current signature for fcvGeomHomographyEvaluatef32 will be removed. Until 2.0.0, the developer should use this implementation with the expectation of renaming it to fcvGeomHomographyEvaluatef32 when transitioning to 2.0.0.

Parameters:

corrs	Pointer to correspondences structure.
homography	Homography representing relationship between ptTo and ptFrom correspondences stored as 3x3 floating point matrix formatted as

Parameters:

maxsqerr	Maximum error value squared.
inliers	Output array for those indices that passed the test - the array MUST be able to store numIndices items.
errinliers	Output array for the error of indices of correspondences that passed the test.
numinliers	Output number of corrs that passed the test.

Returns:: 0 that error is less than maximum error, -1 greater or equal to maximum error.

3

FASTCV_API void fcvGeomHomographyFitf32	(	const fcvCorrespondences *__restrict	corrs,
		float *__restrict	homography
	)

Performs cholesky homography fitting on specified points correspondences.

[x_to] [ a11 a12 a13 ] [ x_from ]
[y_to] = [ a21 a22 a23 ] * [ y_from ]
[ 1 ] [ a31 a32 a33 ] [ 1 ]
note that all the correspondences are considered, if correspondence pairs are smaller than 4, the API returns. If correspondence pairs are larger than 4, the API takes all the correspondences into consideration using least squared method.

Output precision is within 3e-3

Parameters:

corrs	Pointer to correspondences structure.
homography	3x3 floating point matrix formatted as

3

FASTCV_API fcvStatus fcvGeomHomographyFitf32_v2	(	const fcvCorrespondences *__restrict	corrs,
		float32_t *__restrict	homography,
		uint32_t	mode
	)

Performs homography fitting on specified points correspondences.

[x_to] [ a11 a12 a13 ] [ x_from ]
[y_to] = [ a21 a22 a23 ] * [ y_from ]
[ 1 ] [ a31 a32 a33 ] [ 1 ]
note that all the correspondences are considered, if correspondence pairs are smaller than 4, the API returns. If correspondence pairs are larger than 4, the API takes all the correspondences into consideration using least squared method.

Parameters:

corrs	Pointer to correspondences structure.
homography	3x3 floating point matrix formatted as

Parameters:

mode	Mode set to 0 for high speed, 1 for high accuracy

Returns:: FASTCV_SUCCESS upon success. Other status codes upon failure.

3

FASTCV_API fcvStatus fcvGeomHomographyRobustFitf32	(	const fcvCorrespondences *__restrict	corr,
		float32_t *__restrict	homography,
		float32_t	reprojThreshold
	)

Performs robust homography fitting on specified points correspondences.

Perform RANSAC-based robust method to estimate the perspective transformation fitting on specified points correspondences.

Parameters:

corr	Pointer to correspondences structure
homography	Homography stored as 3x3 floating point matrix
reprojThreshold	Threshold of reprojection error to treat the feature pair as an inliers

3

FASTCV_API fcvStatus fcvGeomHomographyRobustFitf32_v2	(	const fcvCorrespondences *__restrict	corr,
		float32_t *__restrict	homography,
		float32_t	reprojThreshold,
		uint32_t	mode
	)

Performs robust homography fitting on specified points correspondences.

Perform RANSAC-based robust method to estimate the perspective transformation fitting on specified points correspondences.

Parameters:

corr	Pointer to correspondences structure
homography	Homography stored as 3x3 floating point matrix
reprojThreshold	Threshold of reprojection error to treat the feature pair as an inliers
mode	Mode set to 0 for high speed, 1 for high accuracy

Returns:: FASTCV_SUCCESS upon success. Other status codes upon failure.

3

FASTCV_API float fcvGeomPoseEvaluateErrorf32	(	const fcvCorrespondences *__restrict	corrs,
		const float *__restrict	pose,
		float *__restrict	projected,
		float *__restrict	reprojErr,
		float *__restrict	invz,
		float *__restrict	reprojVariance
	)

Calculate the reprojection error based on the input pose. This function and fcvGeomPoseOptimizef32 can be used iteratively to perform poseRefine (GN or LM)..

Parameters:

corrs	Pointer to correspondences structure.
pose	Pose representing updated pose correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.
projected	2D position after projection
reprojErr	2D reprojection error in camera coordinates (not in pixels!)
invz	Inverse depth (z)
reprojVariance	Reprojection variance in camera coordinates

Returns:: Reprojection error.

3

FASTCV_API int fcvGeomPoseEvaluatef32	(	const fcvCorrespondences *__restrict	corrs,
		const float *	pose,
		float	maxSquErr,
		uint16_t *__restrict	inliers,
		uint32_t *	numInliers
	)

Checks which members have a projection error that is smaller than the given one.

Parameters:

corrs	Pointer to correspondences structure.
pose	Pose representing relationship between ptTo and ptFrom correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.
maxSquErr	Maximum error value squared.
inliers	Output array for those indices that passed the test - the array MUST be able to store numIndices items.
numInliers	Output number of corrs that passed the test.

Returns:: 0 that error is less than maximum error, -1 greater or equal to maximum error.

3

FASTCV_API int fcvGeomPoseOptimizeGNf32	(	const float *__restrict	projected,
		const float *__restrict	reprojErr,
		const float *__restrict	invz,
		const float *__restrict	reprojVariance,
		unsigned int	numpts,
		float *__restrict	pose
	)

Update the pose based on the specified points correspondences using Gauss-Newton optimization. This function and fcvGeomPoseEvaluateErrorf32 can be used iteratively to perform poseRefine GN.

Parameters:

projected	2D position after projection
reprojErr	2D reprojection error in camera coordinates (not in pixels!)
invz	Inverse depth (z)
reprojVariance	Reprojection variance in camera coordinates
numpts	Number of points
pose	Pose representing updated pose correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.

Returns:: 0 if successfully clustered, otherwise error code

3

FASTCV_API float fcvGeomPoseRefineGNf32	(	const fcvCorrespondences *__restrict	corrs,
		short	minIterations,
		short	maxIterations,
		float	stopCriteria,
		float *	initpose,
		float *	refinedpose
	)

Performs cholesky pose fitting on specified points correspondences. Takes a pose and uses the correspondences to refine it using iterative Gauss-Newton optimization.

Parameters:

corrs	Pointer to correspondences structure.
minIterations	Minimum number of iterations to refine.
maxIterations	Maximum number of iterations to refine.
stopCriteria	Improvement threshold, iterations stop if improvement is less than this value.
initpose	Pose representing initial pose correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.
refinedpose	Pose representing refined pose correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.

Returns:: Final reprojection error.

3

FASTCV_API int fcvGeomPoseUpdatef32	(	const float *__restrict	projected,
		const float *__restrict	reprojErr,
		const float *__restrict	invz,
		const float *__restrict	reprojVariance,
		unsigned int	numpts,
		float *__restrict	pose
	)

Update and compute the differential pose based on the specified points correspondences This function and fcvGeomPoseOptimizeGNf32 can be used iteratively to perform poseRefine GN.

Parameters:

projected	2D position after projection
reprojErr	2D reprojection error in camera coordinates (not in pixels!)
invz	Inverse depth (z)
reprojVariance	Reprojection variance in camera coordinates
numpts	Number of points
pose	Pose representing differential pose correspondences stored as a 3x4 transformation matrix in the form [R\|t], where R is a 3x3 rotation matrix and t is the translation vector. The matrix stored in pose is row major ordering: a11, a12, a13, a14, a21, a22, a23, a24, a31, a32, a33, a34 where the matrix is: \| a11, a12, a13 , a14\| \| a21, a22, a23, a24 \| \| a31, a32, a33, a34 \| Pointer storage must be at least a 12-element floating point array.

Returns:: 0 if successfully clustered, otherwise error code

3

FASTCV_API int32_t fcvGeomProjectPoint3x1f32	(	const float32_t *__restrict	pose,
		const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyz,
		float32_t *__restrict	xyCamera,
		float32_t *__restrict	xyDevice
	)

Transforms a 3D point using a pose-matrix, projects the transformed point, distorts the projected 2D point and converts to device coordinates.

(x_camera, y_camera, z_camera) = Pose * (x,y,z,1)' xCamera = x_camera/z_camera, yCamera = y_camera/z_camera xyDevice = distortion(xyCamera) - described in fcvGeomDistortPoint2x1f32

Parameters:

pose	Pose matrix of size 3x4 (12 float values) in row-major format.
cameraCalibration	Camera calibration with 8 parameter: focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: should be 128-bit aligned.
xyz	3D point (x,y,z) as three float values
xyCamera	Output of the projected 2D camera coordinate (2 float values)
xyDevice	Output of the projected and distorted 2D device coordinate (2 float values)

Returns:: 1 if transformed point lies in front of the camera plane. 0 otherwise

3

FASTCV_API void fcvGeomProjectPoint3xNf32	(	const float32_t *__restrict	pose,
		const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyz,
		uint32_t	srcStride,
		uint32_t	xyzSize,
		float32_t *__restrict	xyCamera,
		float32_t *__restrict	xyDevice,
		uint32_t	dstStride,
		uint32_t *	inFront
	)

Transforms a 3D point using a pose-matrix, projects the transformed point, distorts the projected 2D point and converts to device coordinates. brief algorithm desribed in fcvGeomProjectPoint3x1f32.

Parameters:

pose	Pose matrix of size 3x4 (12 float values) in row-major format. NOTE: should be 128-bit aligned.
cameraCalibration	Camera calibration with 8 parameter: focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: should be 128-bit aligned.
xyz	3D points (x,y,z) as sets of three float values. While allocating, allocate enough memory to accomodate the points (Nx3) keeping in mind the stride of the input points. Total memory allocated must be large enough to accomodate the input +padding. NOTE: should be 128-bit aligned.
srcStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the input array has points as follows x0 y0 z0 0 0 x1 y1 z1 0 0 x2 y2 z2.., then the stride is 5 * size(float32_t) = 20
xyzSize	Number of points N
xyCamera	Output of the projected 2D camera coordinates (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the output points. Total memory allocated must be large enough to accomodate the output+padding. NOTE: should be 128-bit aligned.
xyDevice	Output of the projected and distorted 2D device coordinates. (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the output points. Total memory allocated must be large enough to accomodate the output+padding. NOTE: should be 128-bit aligned.
dstStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the output array has points as follows x0 y0 0 0 0 x1 y1 0 0 0 x2 y2 ..., then the stride is 5 * size(float32_t) = 20 NOTE: should be a multiple of 8.
inFront	Is 1 if transformed point lies in front of the camera plane and 0 otherwise It must be allocated as a Nx1 vector.

3

FASTCV_API void fcvGeomUndistortPoint2x1f32	(	const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyDevice,
		float32_t *__restrict	xyCamera
	)

Applies radial undistortion to a 2D coordinate in device coordinates and returns the undistorted coordinate in camera coordinates.

Parameters:

cameraCalibration	Camera calibration with 8 parameter: Inverse focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: The first two entries of this parameter for this function are the Inverse of the focal length and not the focal length itself. NOTE: should be 128-bit aligned.
xyDevice	Input of the distorted 2D device coordinate (2 float values).
xyCamera	Output of the undistorted 2D camera coordinate (2 float values).

3

FASTCV_API void fcvGeomUndistortPoint2xNf32	(	const float32_t *__restrict	cameraCalibration,
		const float32_t *__restrict	xyDevice,
		uint32_t	srcStride,
		uint32_t	xySize,
		float32_t *__restrict	xyCamera,
		uint32_t	dstStride
	)

Applies radial undistortion to a 2D coordinate in device coordinates and returns the undistorted coordinate in camera coordinates. brief algorithm desribed in fcvGeomUndistortPoint2x1f32.

Parameters:

cameraCalibration	Camera calibration with 8 parameter: inverse focal-length (x and y), principal point (x & y), radial distortion (2 parameters), tangential distortion (2 parameters). NOTE: should be 128-bit aligned.
xyDevice	Input of the distorted 2D device coordinates (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the input points. Total memory allocated must be large enough to accomodate the input+padding, which has size of NsrcStride (in bytes) NOTE:* should be 128-bit aligned.
srcStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the input array has points as follows x0 y0 0 0 0 x1 y1 0 0 0 x2 y2 ..., then the stride is 5 * size(float32_t) = 20 NOTE: should be a multiple of 8.
xySize	Number of points N
xyCamera	Output of the undistorted 2D camera coordinates (Nx2 float values). While allocating, allocate enough memory to accomodate the points (Nx2) keeping in mind the stride of the output points. Total memory allocated must be large enough to accomodate the output+padding, which has size of NdstStride (in bytes) NOTE:* should be 128-bit aligned.
dstStride	Stride between consecutive input camera coordinates. Stride here is defined as the number of units between consecutive x coordinates. For example, if the output array has points as follows x0 y0 0 0 0 x1 y1 0 0 0 x2 y2 ..., then the stride is 5 * size(float32_t) = 20 NOTE: should be a multiple of 8.

3

FASTCV_API fcvStatus fcvICPJacobianErrorSE3f32	(	const uint16_t *__restrict	depthData,
		uint32_t	depthWidth,
		uint32_t	depthHeight,
		uint32_t	depthStride,
		const float32_t *__restrict	refPointsNormals,
		uint32_t	numPoints,
		const float32_t *__restrict	refPose,
		const float32_t *__restrict	camera,
		float32_t	sqDistThreshold,
		float32_t *__restrict	sqDists,
		float32_t *__restrict	errors,
		float32_t *__restrict	jacobian
	)

Compute Point-to-Plane ICP Jacobian matrix and errors for SE3 motion.

This function computes the Jacobian matrix and errors for point-to-plane ICP (Iterative Closest Point) algorithm given reference 3D points and normal vectors and a depth map.

Parameters:

depthData	Pointer to the depth map. Depth data is stored as unsigned short scaled by 1000x with respect to scene units (e.g. millimeters vs meters).
depthWidth	Width of the depth map
depthHeight	Height of the depth map
depthStride	Number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left as 0 depthMapStride is set to default: depthMapWidth * sizeof(uint16_t).
refPointsNormals	Reference 3D points and 3D normal vectors of total N (numPoints) points (x1,y1,z1,nx1,ny1,nz1, x2,y2,z2,nx2,ny2,nz2, ..., xN,yN,zN,nxN,nyN,nzN)
numPoints	Number of reference points and normals
refPose	Reference pose stored in 3 x 4 matrix
camera	Camera calibration parameters (fx, fy, cx, cy, 1/fx, 1/fy)
sqDistThreshold	Squared distance threshold for filtering out outliers
sqDists	Output squared distances. Memory should be allocated beforehand (numPoints x sizeof(float32_t)).
errors	Output errors. Memory should be allocated beforehand (numPoints x sizeof(float32_t)).
jacobian	Output Jacobian matrix. Memory should be allocated beforehand (numPoints x 6 x sizeof(float32_t)).

Returns:: FASTCV_SUCCESS upon success, Other status codes upon failure

3

FASTCV_API fcvStatus fcvInvertAffineTransformf32	(	const float32_t *__restrict	M,
		float32_t *__restrict	invAffineMat
	)

Invert an affine transformation.

Invert an affine transformation which is commonly used to transform back

Parameters:

M	Input 3x3 affine transformation matrix
invAffineMat	Output 3x3 reverse affine transformation

3

FASTCV_API fcvStatus fcvRegisterDepthImagef32	(	const float32_t *__restrict	src,
		uint32_t	srcWidth,
		uint32_t	srcHeight,
		uint32_t	srcStride,
		const float32_t *__restrict	Kdinv,
		const float32_t *__restrict	Kc,
		const float32_t *__restrict	Rd2c,
		const float32_t *__restrict	Td2c,
		float32_t *__restrict	dst,
		uint32_t	dstStride
	)

Registers an input Depth Image with an input Color Image.

Parameters:

src	Pointer to the input depth image
srcWidth	The width of the input depth image.
srcHeight	The height of the input depth image
srcStride	Stride of image is the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to srcWidthsizeof(float32_t). NOTE:* should be multiple of 8.
Kdinv	Kdinv is an array of 9 elements representing the inverse of the 3x3 depth camera matrix arranged row wise.
Kc	Kc is an array of 9 elements which represents the 3x3 color camera intrinsic parameters arranged row wise
Rd2c	Rd2c is an array of 9 element which represents the 3D Rotation (3x3) matrix from the Depth Camera to the Color Camera arranged row wise
Td2c	Td2c is an array of 3 element which represents the transform parameter from the Depth Camera to the Color Camera.
dst	Output matrix which has the same width, length and channel number as src
dstStride	Stride for output image, i.e. the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to srcWidthsizeof(float32_t) NOTE:* should be multiple of 8.

Returns:: FASTCV_SUCCESS upon success. Other status codes upon failure.

3

FASTCV_API fcvStatus fcvUndistortDisparityConvertDepthf32	(	const float32_t *__restrict	src,
		uint32_t	srcWidth,
		uint32_t	srcHeight,
		uint32_t	srcStride,
		const uint8_t *__restrict	mask,
		uint32_t	maskStride,
		const float32_t *__restrict	pixelDistortion,
		uint32_t	pixelDistortionStride,
		uint32_t	convertDepth,
		const float32_t *__restrict	imageDistortion,
		const float32_t *__restrict	depthParam,
		float32_t *__restrict	dst,
		uint32_t	dstStride
	)

Undistort a disparity image and optionally convert the undistorted disparity image into a depth image.

Parameters:

src	Pointer to the disparity input image
srcWidth	Width of the disparity input image in pixel. The number of pixels in a row.
srcHeight	Height of the disparity input image in pixel.
srcStride	Stride of image is the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to srcWidthsizeof(float32_t) NOTE:* should be multiple of 8.
mask	Pointer to the returned foreground mask image. It's a 1-channel image, same width and height as src. If an entry set to 1, there will be a valid undistorted dispartity or depth value. If an entry set to 0, there will be no valid undistorted dispartity or depth value.
maskStride	Stride of the foreground mask image. It's the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 MaskStride is default to srcWidth. NOTE: should be multiple of 8.
pixelDistortion	Pointer to the pixel distortion value for each pixel in the image. It has the same dimensions as the input image src.
pixelDistortionStride	Stride of pixel distortion value for each pixel in the image. It's the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 pixelDistortionStride is default to srcWidthsizeof(float32_t). WARNING:* should be multiple of 8
convertDepth	This parameter is a flag to enable or disable the conversion of disparity to depth. If set to 1, then the undistorted disparity values will be converted to depth and returned in the dst buffer. If set to 0, then the undistorted disparity will be returned in the dst buffer.
imageDistortion	ImageDistortion contains two elements. The First part contains the spatial distortion pattern, and the second part contains the decay of the distortion effect.
depthParam	Parameters used to transform disparity to depth values. The equation is undistortedDepthValue = 1 / (undistortedDisparityValue * depthParam[0] + depthParam[1])
dst	Output matrix which has the same width, length and channel number as src. The buffer contains undistorted disparity values if convertDepth is set to 0 and contains the undistorted depth values if convertDepth is set to 1
dstStride	Stride for output image, i.e. the number of bytes between column 0 of row 1 and column 0 of row 2 in data memory. If left at 0 srcStride is default to srcWidthsizeof(float32_t) NOTE:* should be multiple of 8.

Returns:: FASTCV_SUCCESS upon success. Other status codes upon failure.

3

Functions

Detailed Description

Function Documentation