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 (tx, ty, tz), 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’= XC/ZC and y’ = YC/ZC. 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 fx and fy are the focal length parameters, while cx and cy are the optical center coordinates. These values can be obtained from nodes on the Helios2.
The operation in this step is as follows:
.
If ZC ≠ 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
.
k1, k2, k3, p1, and p2 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
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 and higher
- Helios2+: HTP003S-001 v1.16.0.0 and higher
- Helios2 Wide: HTW003S-001 v1.12.0.0 and higher
- Helios2 Ray: HTR003S-001 v1.6.0.0 and higher
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 (XW, YW, ZW), 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 ZW and returns the corresponding 3D point (XW, YW, ZW).
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 (XW, YW, ZW).
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 (XW, YW, ZW).
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 p0 is the provided point on the plane, l0 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 (XW, YW, ZW).
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; }