ROS CameraInfo message gets populated with incorrect information for Femto Mega

[swankun]

When running the Femto Mega camera at the 3840x2160 resolution, the CameraInfo ROS message gets populated with the following intrinsic matrix:

$$ K = \begin{bmatrix} f_{x} & 0 & c_{x} \\ 0 & f_{y} & c_{y} \\ 0 & 0 & 1 \end{bmatrix} = \begin{bmatrix} 13489.9306640625 & 0 & 11447.90771484375 \\ 0 & 13487.794921875 & 6276.52734375 \\ 0 & 0 & 1 \end{bmatrix}, $$

which is clearly incorrect, as the camera center point $\left( c_{x}, c_{y} \right)$ are much different the nominal values of $\left( \frac{3840}{2}, \frac{2160}{2} \right)$.

However, when we request the camera parameters using the provided get_camera_params ROS service, the camera matrix (denoted by l_intr_p in the service response) the we get is as follows

$$ K = \begin{bmatrix} 2248.32177734375 & 0 & 1907.984619140625 \\ 0 & 2247.9658203125 & 1046.087890625 \\ 0 & 0 & 1 \end{bmatrix}. $$

This leads us to believe that the base width and base height are incorrectly populated in the following code by the SDK

OrbbecSDK_ROS1/src/ros_setup.cpp

Lines 280 to 281 in 09325b1

	int base_depth_width = param->depthIntrinsic.width == 0 ? 640 : param->depthIntrinsic.width;
	int base_rgb_width = param->rgbIntrinsic.width == 0 ? 640 : param->rgbIntrinsic.width;

which leads to the incorrect scaling factor in

OrbbecSDK_ROS1/src/utils.cpp

Lines 183 to 191 in 09325b1

	double scaling = static_cast<double>(width) / 640;
	info.K[0] *= scaling; // fx
	info.K[2] *= scaling; // cx
	info.K[4] *= scaling; // fy
	info.K[5] *= scaling; // cy
	info.P[0] *= scaling; // fx
	info.P[2] *= scaling; // cx
	info.P[5] *= scaling; // fy
	info.P[6] *= scaling; // cy

The CameraInfo message is also incorrect for the registered depth image provided by the driver.