1 題目介紹

已知魚眼相機的參數，

1. image_width，表示圖像的寬度
2. image_height，表示圖像的高度
3. $\xi$，表示魚眼相機參數
4. $k_1$、$k_2$，表示徑向相機參數
5. $p_1$、$p_2$，表示切向相機參數
6. $f_x$、$f_y$，表示相機的焦距
7. $c_x$、$c_y$，表示相機的光心

已知相機坐標系到車體坐標系的變換矩陣，

8. $t$，表示平移向量
9. $R$，表示旋轉矩陣

給出4組這樣的參數及相應的圖片，請求解最終的IPM圖片。

2 求解

(1)設置最終IPM圖像的尺寸為$1000\times 1000$（用符號$ipm\_img\_h\times ipm\_img\_w$表示），其中每個像素的長度和寬度為0.02米（用符號$pixel\_scale\_$表示）。

(2)遍歷IPM圖像中的每個像素$(u,v)$，那么可以直到機體坐標系的點的坐標值為，
$$p_v=\left[\begin{array}{c}?(0.5?ipm\_img\_h?u)?pixel\_scale\\ 0.5?(ipm\_img\_w?v)?pixel\_scale\\ 0\end{array}\right]\text{\hspace{1em}(1)}$$
(3)求得在相機坐標系下的坐標值為，
$$p_c=R^T?p_v?R^T?t\text{\hspace{1em}(2)}$$

將$p_c$歸一化操作，得到$p_{c\_norm}$。

(4)此時，得到單位平面上的點$(x_1,y_1)$，
$$\begin{array}{r}{x}_1=\frac{p_{c\_norm}[0]}{p_{c\_norm}[2]+\xi }\\ y_1=\frac{p_{c\_norm}[1]}{p_{c\_norm}[2]+\xi }\end{array}\text{\hspace{1em}(3)}$$

(5)得到畸變后的點$(x_2,y_2)$，
$$\begin{array}{rl}r& =\sqrt{x_1^2+y_1^2}\\ x_2& =x_1(1+k_1{r}^2+k_2{r}^4)+2p_1{x}_1{y}_1+p_2(r^2+2x_1^2)\\ y_2& =y_1(1+k_1{r}^2+k_2{r}^4)+p_1(r^2+2y_1^2)+2p_2{x}_1{y}_1\end{array}\text{\hspace{1em}(4)}$$

(6)將點$(x_2,y_2)$投影到圖像坐標系，得到$(u_1,v_1)$，
$$\begin{array}{rl}{u}_1& =f_x{x}_2+c_x\\ v_1& =f_y{y}_2+c_y\end{array}\text{\hspace{1em}(5)}$$
將魚眼相機中的點$(u_1,v_1)$的顏色賦給IPM圖像中的$(u.v)$坐標。

此處需要注意：

tip1: 獲取顏色時需要四舍五入$u_1$和$v_1$。

tip2: 如果IPM圖像中$(u,v)$坐標已經被賦予了顏色信息，記錄它的次數，那么最終的顏色值通過次數來加權平均，
$$color=\frac{total?1}{total}?color+\frac{1}{total}?new\_color\text{\hspace{1em}(6)}$$

最終的代碼如下，

cv::Mat IPM::GenerateIPMImage(const std::vector<cv::Mat>& images) const {// Initialize a black IPM image with dimensions ipm_img_h_ x ipm_img_w_ and 3 channels (RGB)cv::Mat ipm_image = cv::Mat::zeros(ipm_img_h_, ipm_img_w_, CV_8UC3);std::vector<std::vector<int>> uv_cnt(ipm_img_h_, std::vector<int>(ipm_img_w_, 0)); // Check if the number of input images matches the number of camerasif (images.size() != cameras_.size()) {// If not, print an error message and return the initialized black IPM imagestd::cout << "IPM not init normaly !" << std::endl;return ipm_image;}// Iterate over each pixel in the IPM imagefor (int u = 0; u < ipm_img_w_; ++u) {for (int v = 0; v < ipm_img_h_; ++v) {// Calculate the point p_v in vehicle coordinates, p_v is corresponding to the current pixel (u, v).// Assume the height of the ipm_image in vehicle coordinate is 0.Eigen::Vector3d p_v(-(0.5 * ipm_img_h_ - u) * pixel_scale_,(0.5 * ipm_img_w_ - v) * pixel_scale_, 0);// Iterate over each camerafor (size_t i = 0; i < cameras_.size(); ++i) {// Project the vehile point p_v into the image plane uvTODO begin/////將vehicle系下的點轉到camera系下Eigen::Vector3d p_c = cameras_[i].T_vehicle_cam_.inverse() * p_v;// Eigen::Maxtrix3d R1 = cameras_[i].T_vehicle_cam_.linear();// Eigen::Vector3d t1 = cameras_[i].translation();if (p_c[2] < 0.0) { //過濾z值小于0的點continue;}//將camera系下的點轉到圖像系下 Eigen::Vector3d norm_p_c = p_c.normalized();norm_p_c[2] += cameras_[i].xi_;//unit plane上的點(x1,y1)double x1 = norm_p_c[0] / norm_p_c[2];double y1 = norm_p_c[1] / norm_p_c[2];double r_2 = x1 * x1 + y1 * y1;//獲取參數k1,k2,p1,p2double k1 = cameras_[i].D_.at<double>(0, 0);double k2 = cameras_[i].D_.at<double>(1, 0);double p1 = cameras_[i].D_.at<double>(2, 0);double p2 = cameras_[i].D_.at<double>(3, 0);//畸變后的點(x2,y2)double x2 = x1 * (1 + k1 * r_2 + k2 * r_2 * r_2) + 2.0 * p1 * x1 * y1 + p2 * (r_2 + 2 * x1 * x1);double y2 = y1 * (1 + k1 * r_2 + k2 * r_2 * r_2) + p1 * (r_2 + 2 * y1 * y1) + 2.0 * p2 * x1 * y1;//獲取參數fx,fy,cx,cydouble fx = cameras_[i].K_.at<double>(0, 0);double fy = cameras_[i].K_.at<double>(1, 1);double cx = cameras_[i].K_.at<double>(0, 2);double cy = cameras_[i].K_.at<double>(1, 2);//圖像坐標系下的點(u1,v1)double u1 = fx * x2 + cx;double v1 = fy * y2 + cy;//四舍五入，得到最終的uv0,uv1int uv0 = static_cast<int>(std::round(u1));int uv1 = static_cast<int>(std::round(v1)); TODO end/// (uv0, uv1) is the projected pixel from p_v to cameras_[i]// Skip this point if the projected coordinates are out of bounds of the camera imageif (uv0 < 0 || uv0 >= cameras_[i].width_ || uv1 < 0 ||uv1 >= cameras_[i].height_) {continue;}// Get the pixel color from the camera image and set it to the IPM image// If the IPM image pixel is still black (not yet filled), directly assign the colorif (ipm_image.at<cv::Vec3b>(v, u) == cv::Vec3b(0, 0, 0)) {ipm_image.at<cv::Vec3b>(v, u) = images[i].at<cv::Vec3b>(uv1, uv0);uv_cnt[v][u] += 1;} else {// Otherwise, average the existing color with the new coloruv_cnt[v][u] += 1;double total = 1.0 * uv_cnt[v][u]; ipm_image.at<cv::Vec3b>(v, u) = (total - 1.0) / total * ipm_image.at<cv::Vec3b>(v, u) + 1.0 / total * images[i].at<cv::Vec3b>(uv1, uv0); }}}}// Return the generated IPM imagereturn ipm_image;
}

最終的IPM圖像如下，

給出的參考IPM圖像如下，

本方法得到的IPM圖像有更清晰的停車位線！