App Note: Add RGB Color to Helios 3D Point Cloud

 // Get Helios XYZ data bytes and intensity data:
 void GetHeliosImage(Arena::IDevice* pHeliosDevice, cv::Mat& intensity_image, cv::Mat& xyz_mm)
 {
 	// Get Helios XYZ data bytes and intensity data:
 	Arena::SetNodeValue<gcstring>(pHeliosDevice->GetNodeMap(), "PixelFormat", "Coord3D_ABCY16");

 	// 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();
 	double xyz_scale_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateScale");
	Arena::SetNodeValue<gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateA");
	double x_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");
	Arena::SetNodeValue<gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateB");
	double y_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");
	Arena::SetNodeValue<gcstring>(node_map, "Scan3dCoordinateSelector", "CoordinateC");
	double z_offset_mm = Arena::GetNodeValue<double>(node_map, "Scan3dCoordinateOffset");

	pHeliosDevice->StartStream();
	Arena::IImage* image = pHeliosDevice->GetImage(2000);

	size_t height, width;
	height = image->GetHeight();
	width = image->GetWidth();

	xyz_mm = cv::Mat((int)height, (int)width, CV_32FC3);
	intensity_image = cv::Mat((int)height, (int)width, CV_16UC1);

	const uint16_t* input_data;
	input_data = (uint16_t*)image->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);

			intensity_image.at<ushort>(ir, ic) = input_data[3]; // // Intensity value

			input_data += 4;
		}
	}

	pHeliosDevice->StopStream();

	// Optional: Show the Helios intensity image
	//cv::imshow("HLS Intensity", intensity_image);
	//cv::waitKey(0);
}

void GetTritonRGBImage(Arena::IDevice* pDeviceTriton, cv::Mat& triton_rgb)
{
	Arena::SetNodeValue<gcstring>(pDeviceTriton->GetNodeMap(), "PixelFormat", "RGB8");

	pDeviceTriton->StartStream();
	Arena::IImage* image = pDeviceTriton->GetImage(2000);
	size_t height, width;
	height = image->GetHeight();
	width = image->GetWidth();

	triton_rgb = cv::Mat((int)height, (int)width, CV_8UC3);

	// Copy image data from Arena image buffer to opencv Mat buffer
	memcpy(triton_rgb.data, image->GetData(), height * width * 3);

	pDeviceTriton->StopStream();
}

#include "opencv2\calib3d.hpp"

// Index the circles in the calibration target:
void LucidIndexCalibrationCircles(const cv::Mat& image_in, std::vector& grid_centers)
{
	cv::SimpleBlobDetector::Params bright_params;
	bright_params.filterByColor = true;
	bright_params.blobColor = 255; // white circles in the calibration target
	bright_params.thresholdStep = 2;
	bright_params.minArea = 10.0;  // Min/max area can be adjusted based on size of dots in image
	bright_params.maxArea = 1000.0;
	cv::Ptr<cv::SimpleBlobDetector> blob_detector = cv::SimpleBlobDetector::create(bright_params);

	// pattern_size(num_cols, num_rows)
	//       num_cols: number of columns (number of circles in a row) of the calibration target viewed by the camera
	//       num_rows: number of rows (number of circles in a column) of the calibration target viewed by the camera
	// Specify according to the orientation of the calibration target
	cv::Size pattern_size(5, 4);

	// Find max value in input image
	double min_value, max_value;
	cv::minMaxIdx(image_in, &min_value, &max_value);

	// Scale image to 8-bit, using full 8-bit range
	cv::Mat image_8bit;
	image_in.convertTo(image_8bit, CV_8U, 255.0 / max_value);

	bool is_found = cv::findCirclesGrid(image_8bit, pattern_size, grid_centers, cv::CALIB_CB_SYMMETRIC_GRID, blob_detector);
	printf("Found %d circle centers.\n", grid_centers.size());
	
	// Display image with the found grid (optional)
	cv::Mat image_bgr;
	cv::cvtColor(image_8bit, image_bgr, cv::COLOR_GRAY2BGR);
	cv::drawChessboardCorners(image_bgr, pattern_size, cv::Mat(grid_centers), is_found);
	cv::namedWindow("Detected circles", cv::WINDOW_NORMAL);
	cv::imshow("Detected circles", image_bgr);	
	cv::waitKey(0);
}

	// Find circles in Helios and Triton images and solve for position and pose of Helios relative to Triton with solvePNP()
	// Inputs: image of calibration target from Helios (intensity_image), image of calibration target from Triton (triton_rgb), and triton lens calibration results (cameraMatrix, distCoeffs)

	std::vector<cv::Point2f> grid_centers_1, grid_centers_2;

	// convert Triton image to mono for dot finding
	cv::Mat triton_mono_image;
	cv::cvtColor(triton_rgb, triton_mono_image, cv::COLOR_RGB2GRAY);

	// Find corresponding points in intensity images
	LucidIndexCalibrationCircles(intensity_image, grid_centers_1);
	LucidIndexCalibrationCircles(triton_mono_image, grid_centers_2);

	/* Get XYZ values from the Helios 3D image that match 2D locations on the triton (corresponding points) */
	std::vector<cv::Point3f> target_points_3d_mm;
	std::vector<cv::Point2f> target_points_3d_pix;
	std::vector<cv::Point2f> target_points_2d_pix;
	unsigned int total_points = 0;

	for (int ind = 0; ind < grid_centers_2.size(); ind++)
	{
		unsigned int c1 = (unsigned int)round(grid_centers_1[ind].x);
		unsigned int r1 = (unsigned int)round(grid_centers_1[ind].y);
		unsigned int c2 = (unsigned int)round(grid_centers_2[ind].x);
		unsigned int r2 = (unsigned int)round(grid_centers_2[ind].y);

		float pt_x = xyz_mm.at<cv::Vec3f>(r1, c1)[0];
		float pt_y = xyz_mm.at<cv::Vec3f>(r1, c1)[1];
		float pt_z = xyz_mm.at<cv::Vec3f>(r1, c1)[2];

		cv::Point3f xyz_pt1(pt_x, pt_y, pt_z);
		std::cout << "xyz_pt1: " << xyz_pt1 << std::endl;

		target_points_3d_mm.push_back(xyz_pt1);
		target_points_3d_pix.push_back(grid_centers_1[ind]);
		target_points_2d_pix.push_back(grid_centers_2[ind]);
		total_points++;
	}

	cv::Mat rotation_vector, translation_vector;

	bool success = cv::solvePnP(target_points_3d_mm,
		target_points_2d_pix,
		cameraMatrix,
		distCoeffs,
		rotation_vector,
		translation_vector);