Projecting a Helios2 3D image to and from a 2D image

In some applications of the Helios2 time-of-flight camera, you may need to project 3D image data from the Helios2 to a 2D image, and vice versa. This knowledge base article explores the mathematical concepts behind such projections, and also provides sample code.

Note: The information below is useful for understanding LUCID’s 3D+RGB Kit, but does not cover this product specifically. For more information on the 3D+RGB Kit, check out the code samples available in the Arena SDK documentation.

Homogeneous coordinates

Homogenous coordinates let you represent points in space more simply, allowing you to perform operations such as translation and rotation more easily. A 3D point given by the vector,

can be represented in homogeneous coordinates as,

To translate a point by a vector (t_x, t_y, t_z), one can do the following transformation.

Similarly, for a rotation transformation, the matrix would be

To translate and rotate a point, the matrix becomes

Order of operations

The operations below need to be performed on the 3D points obtained by the Helios2, to project onto 2D pixel coordinates. An explanation of each step follows.

3D world coordinate system > 3D camera coordinate system > 2D image plane > 2D pixel coordinates

1. 3D world coordinate system > 3D camera coordinate system

The first step of a 3D to 2D projection is to go from the 3D world coordinate frame to the 3D camera coordinate frame, which are not necessarily the same.
In more general terms, the following needs to be performed:

As we are projecting the Helios2’s 3D points onto its own 2D plane (i.e., not onto a different plane), the world and camera coordinate systems are the same. The above-mentioned projection becomes the identity:

(Note: For LUCID’s 3D+RGB Kit, we would project the 3D points recorded by the Helios2 onto the 2D image recorded by the Triton2. In this case, we would need to perform the appropriate rotation and translation transformations on the Helios2 image to bring it into the Triton’s frame of reference, which will not simply be an identity matrix as in our case. )

2. 3D camera coordinate system > 2D image plane

Next, we project the set of 3D points on to the 2D image plane of the Helios2. This can be accomplished by using the following camera projection matrix.

where x’= X_C/Z_C and y’ = Y_C/Z_C. This scaling is done this way to keep coordinates in homogeneous coordinates for convenience.

3. 2D image plane > 2D pixel coordinates

Finally, we convert our 2D image plane coordinates into 2D pixel coordinates. This is where we use the camera intrinsic matrix

where f_x and f_y are the focal length parameters, while c_x and c_y are the optical center coordinates. These values can be obtained from nodes on the Helios2.

The operation in this step is as follows:

If Z_C ≠ 0, this transformation is equivalent to the following:

Adding lens distortion

The above derivation does not consider the distortion caused by real lenses. To consider lens distortion, our derivation is modified as follows:

where

and

k₁, k₂, k₃, p₁, and p₂ are the lens distortion parameters that can be obtained from nodes on the Helios2 as described below.

Code samples

The C++ sample code below lets you get data from the Helios2 and perform conversions with the data that you obtain. This sample code implements the concepts outlined in the previous section, and utilizes the Arena SDK and the OpenCV library.

The following helper functions let you obtain the Helios2 image in matrix format, and to obtain the calibration matrix and lens distortion coefficients.

getImageHLT
ReadCalibrationFromHelios

The C++ functions below let you convert between 3D Helios2 coordinates and 2D image pixel coordinates.

Project3DPointOnImage
Image2DCoordinateAndDepthTo3DPoint
Image2DCoordinateTo3DPointOnPlane

The C++ code described below can be downloaded here.

getImageHLT

Streams the Helios2 device, grabs an image, and returns it in a matrix format.

void getImageHLT(Arena::IDevice* pHeliosDevice, Arena::IImage** ppOutImage, cv::Mat& xyz_mm, size_t& width, size_t& height, double& xyz_scale_mm, double& x_offset_mm, double& y_offset_mm, double& z_offset_mm)
{
	// Read the scale factor and offsets to convert from unsigned 16-bit values 
	//    in the Coord3D_ABCY16 pixel format to coordinates in mm
	GenApi::INodeMap* node_map = pHeliosDevice->GetNodeMap();
	xyz_scale_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateScale");
	Arena::SetNodeValue<GenICam::gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateA");
	x_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");
	Arena::SetNodeValue<GenICam::gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateB");
	y_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");
	Arena::SetNodeValue<GenICam::gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateC");
	z_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");
	pHeliosDevice->StartStream();
	Arena::IImage* pHeliosImage = pHeliosDevice->GetImage(2000);
	// copy image because original will be delited after function call
	Arena::IImage* pCopyImage = Arena::ImageFactory::Copy(pHeliosImage);
	*ppOutImage = pCopyImage;
	width = pHeliosImage->GetWidth();
	height = pHeliosImage->GetHeight();
	xyz_mm = cv::Mat((int)height, (int)width, CV_32FC3);
	const uint16_t* input_data = reinterpret_cast<const uint16_t*>(pHeliosImage->GetData());
	for (unsigned int ir = 0; ir < height; ++ir)
	{
		for (unsigned int ic = 0; ic < width; ++ic)
		{
			// Get unsigned 16 bit values for X,Y,Z coordinates
			ushort x_u16 = input_data[0];
			ushort y_u16 = input_data[1];
			ushort z_u16 = input_data[2];
			// Convert 16-bit X,Y,Z to float values in mm
			xyz_mm.at<cv::Vec3f>(ir, ic)[0] = (float)(x_u16 * xyz_scale_mm + x_offset_mm);
			xyz_mm.at<cv::Vec3f>(ir, ic)[1] = (float)(y_u16 * xyz_scale_mm + y_offset_mm);
			xyz_mm.at<cv::Vec3f>(ir, ic)[2] = (float)(z_u16 * xyz_scale_mm + z_offset_mm) + 3; //the z-coordinate of a point in a Helios point cloud has a -3mm offset that needs to be compensated for, hence the addition of 3mm. 
			input_data += 4;
		}
	}
	pHeliosDevice->RequeueBuffer(pHeliosImage);
	pHeliosDevice->StopStream();
}

The 3mm added above to the z-coordinate measured by the Helios compensates for the -3mm offset in the z-coordinate values in a Helios point cloud. This makes the coordinate system align with the case front, instead of the lens.

In newer Helios firmware, this value equals the value for the node ToFXyzConversionOffsetZ. If you have any concerns / questions at this stage, please reach out to us at support@thinklucid.com.

The following firmware versions have this node:

Helios2: HLT003S-001 v1.21.0.0
Helios2+: HTP003S-001 v1.16.0.0
Helios2 Wide: HTW003S-001 v1.12.0.0
Helios2 Ray: HTR003S-001 v1.6.0.0

ReadCalibrationFromHelios

Obtains the camera intrinsic matrix and lens distortion coefficients.

void ReadCalibrationFromHelios(Arena::IDevice* pDeviceHLT, cv::Mat& cameraMatrix, cv::Mat& distortionCoeffs)
{
	GenApi::INodeMap* nodeMap = pDeviceHLT->GetNodeMap();
	cameraMatrix = cv::Mat::zeros(3, 3, CV_64FC1);
	printf_s("Reading intrinsics from nodes: CalibFocalLengthX, CalibFocalLengthY, CalibOpticalCenterX, CalibOpticalCenterY\n");
	cameraMatrix.at<double>(0, 0) = Arena::GetNodeValue<double>(nodeMap, "CalibFocalLengthX");
	cameraMatrix.at<double>(1, 1) = Arena::GetNodeValue<double>(nodeMap, "CalibFocalLengthY");
	cameraMatrix.at<double>(0, 2) = Arena::GetNodeValue<double>(nodeMap, "CalibOpticalCenterX");
	cameraMatrix.at<double>(1, 2) = Arena::GetNodeValue<double>(nodeMap, "CalibOpticalCenterY");
	cameraMatrix.at<double>(2, 2) = 1;
	printf_s("Focal length is %0.5f pixels\n", cameraMatrix.at<double>(0, 0));
	printf_s("Optical center is (%0.3f, %0.3f) pixels\n", cameraMatrix.at<double>(0, 2), cameraMatrix.at<double>(1, 2));
	const int n_distortion_coefficients = 5; // 5 for HLT/HTP, 8 for HTW
	distortionCoeffs = cv::Mat::zeros(1, n_distortion_coefficients, CV_64FC1);
	printf_s("Reading %d distortion values from camera\n", n_distortion_coefficients);
	for (int i = 0; i < n_distortion_coefficients; i++)
	{
		char selector_value[32];
		sprintf_s(selector_value, "Value%d", i);
		Arena::SetNodeValue(nodeMap, "CalibLensDistortionValueSelector", GenICam::gcstring(selector_value));
		distortionCoeffs.at<double>(0, i) = Arena::GetNodeValue<double>(nodeMap, "CalibLensDistortionValue");
		printf_s("Value of distortion coeffient %d is %0.6f\n", i, distortionCoeffs.at<double>(0, i));
	}
}

Project3DPointOnImage

Projects a 3D point from Helios2 image onto its own 2D image plane. Takes in a 3D point (X_W, Y_W, Z_W), and returns 2D pixel coordinate (i, j).

Parameters:

3D point coordinates
Rotation vector
Translation vector
Camera matrix
Lens distortion coefficients

Returns:

2D pixel coordinates of projected point.

This function uses OpenCV’s function “projectPoints”, which implements the 3D to 2D projection explained in the previous section.

The rotation and translation vectors sent to the function as parameters are set to 0 (since the world and camera coordinate frames are the same and no transformation is needed.

cv::Point2f Project3DPointOnImage(float X, float Y, float Z, cv::Mat cameraMatrix, cv::Mat distortionCoeffs)
{
	const int N_points = 1; // Project 1 3D point
	cv::Mat input_XYZ(1, 1, CV_32FC3);
	input_XYZ.at<cv::Point3f>(0) = cv::Point3f(X, Y, Z);
	// 3D points are already in camera coordinate system so no rotation/translation vectors are zero
	cv::Mat rotationVector = cv::Mat::zeros(3, 1, CV_32FC1);
	cv::Mat translationVector = cv::Mat::zeros(3, 1, CV_32FC1);
	// project points from 3D XYZ in mm to 2D (row,col) in pixels
	cv::Mat projectedPoints;
	cv::projectPoints(
		input_XYZ,
		rotationVector,
		translationVector,
		cameraMatrix,
		distortionCoeffs,
		projectedPoints);
	cv::Vec2f point_2D = projectedPoints.at<cv::Vec2f>(0);
	return point_2D;
}

Image2DCoordinateAndDepthTo3DPoint

Takes in a 2D pixel coordinate (i, j) and a depth coordinate Z_W and returns the corresponding 3D point (X_W, Y_W, Z_W).

Parameters:

2D pixel coordinates
Depth coordinate
Camera matrix
Lens distortion coefficients

Returns

3D point

This function essentially solves the projection problem in reverse. It does it by using OpenCV’s “undistortPoints”, which iteratively solves for a 3D vector pointing towards the “undistorted” 3D point, which when projected on the 2D image plane outputs the given ‘distorted’ pixel coordinates (i, j). This 3D vector is then scaled by the provided depth coordinate to give the 3D point (X_W, Y_W, Z_W).

cv::Point3f Image2DCoordinateAndDepthTo3DPoint(float pixel_col, float pixel_row, float Z_world, cv::Mat cameraMatrix, cv::Mat distortionCoeffs)
{
	const int N_points = 1;
	cv::Mat distorted_xy(1, N_points, CV_32FC2);
	distorted_xy.at<cv::Point2f>(0) = cv::Point2f(pixel_col, pixel_row);
	// Solve for the ideal pinhole camera model 2D image location that maps to the input distorted pixel location (x,y)=(pixel_col, pixel_row)
	// Note there is no closed form solution to this problem so undistortPoints uses an iterative solver.
	cv::Mat undistorted_point;
	cv::undistortPoints(distorted_xy, undistorted_point, cameraMatrix, distortionCoeffs);
	// Undistort points returns the direction the sub-pixel location points to as an (X,Y) direction with Z=1
	cv::Point3f pixel_vector(undistorted_point.at<cv::Point2f>(0).x, undistorted_point.at<cv::Point2f>(0).y, 1.0f);
	// Scale the pixel direction by the input world depth to get the 3D location
	cv::Point3f world_XYZ = Z_world * pixel_vector;
	return world_XYZ;
}

Image2DCoordinateTo3DPointOnPlane

Takes in a 2D pixel coordinate (i, j) and the desired plane information and returns the corresponding 3D point (X_W, Y_W, Z_W).

Parameters:

2D pixel coordinates
A 2D point on the desired plane
Unit vector normal to the plane
Camera matrix
Lens distortion coefficients

Returns:

3D point

This function works like the sample function Image2DCoordinateAndDepthTo3DPoint, in that it also uses “undistortPoints” to output a 3D vector pointing towards its “undistorted” 3D point. Then, the distance of the given plane from the origin (in other words, how much to scale this vector to obtain the final 3D point) can be found using the formula,

where p₀ is the provided point on the plane, l₀ is the origin, n is the unit normal to the plane, and l is the vector output from “undistortPoints” (normalized). This relation is explained in the Wikipedia article on the Line–plane intersection in Wikipedia.

This factor d can then be used to scale our vector to obtain the 3D point (X_W, Y_W, Z_W).

cv::Point3f Image2DCoordinateTo3DPointOnPlane(float pixel_col, float pixel_row, cv::Vec3f plane_normal, cv::Point3f point_on_plane, cv::Mat cameraMatrix, cv::Mat distortionCoeffs)
{
	const int N_points = 1;
	cv::Mat distorted_xy(1, N_points, CV_32FC2);
	distorted_xy.at<cv::Point2f>(0) = cv::Point2f(pixel_col, pixel_row);

	// Solve for the ideal pinhole camera model 2D image location that maps to the input distorted pixel location (x,y)=(pixel_col, pixel_row)
	// Note there is no closed form solution to this problem so undistortPoints uses an iterative solver.
	cv::Mat undistorted_point;
	cv::undistortPoints(distorted_xy, undistorted_point, cameraMatrix, distortionCoeffs);

	// Undistort points returns the direction the sub-pixel location points to as an (X,Y) direction with Z=1
	cv::Point3f pixel_vector(undistorted_point.at<cv::Point2f>(0).x, undistorted_point.at<cv::Point2f>(0).y, 1.0f);

	// normalize so magnitude = 1
	pixel_vector = pixel_vector / cv::norm(pixel_vector);

	// Solve for the intersection of the 3D plane and a line in the direction of the pixel vector originating from (0,0,0)
	// https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection
	// ensure the plane normal vector has unit magnitude
	plane_normal = plane_normal / cv::norm(plane_normal);
	cv::Point3f point_on_line(0, 0, 0);
	// equation from wikipedia page
	double d = (point_on_plane - point_on_line).dot(plane_normal) / (pixel_vector.dot(plane_normal));
	cv::Point3f world_XYZ_on_plane = d * pixel_vector;
	return world_XYZ_on_plane;
}

Projecting a Helios2 3D image to and from a 2D image

Homogeneous coordinates

Order of operations

1. 3D world coordinate system > 3D camera coordinate system

2. 3D camera coordinate system > 2D image plane

3. 2D image plane > 2D pixel coordinates

Code samples

getImageHLT

ReadCalibrationFromHelios

Project3DPointOnImage

Image2DCoordinateAndDepthTo3DPoint

Image2DCoordinateTo3DPointOnPlane

Related Articles

Focusing a Triton2 EVS Camera