怎么介绍ipm10逆透视变换(IPM)多种方式及代码总结
1.背景
在自动/辅助驾驶中，车道线的检测非常重要。在前视摄像头拍摄的图像中，由于透视效应的存在，本来平行的事物，在图像中确实相交的。而IPM变换就是消除这种透视效应，所以也叫逆透视。
而我们需要认识的变换主要分为三类透视变换、仿射变换、单应性变换：
1.透视变换：不能保证物体形状的“平行性”。仿射变换是透视变换的特殊形式。透视变换是将一个平面投影到另一个平面，简单理解就是把一张图片投影到另一张图片，求的是同一张图片到它的投影图片之间的变换。
2.仿射变换：保证物体形状的“平直性”和“平行性”，一般为平移旋转等操作
3.单应性变换：由三维空间拍摄两张不同的图片来获取关键点，求的是该图片到另一个角度图片的变换，但是变换过后还是这张图片，没有变成另一个角度的图片，变换过后和另一张图片还是不同，因为三维空间得到的背景不同，所以变换过后并不能得到获取关键点对的另一张图片。
图片中的物体是一个平面图，比如用相机拍摄电脑屏幕上，（假设正面是拍正方形，侧面拍是长方形，将长方形透视变换成正方形，那么两张图片应该是一样一样的）那么单应性变换和透视变换没有什么区别。
如果我们拍的是电脑，那么不同角度得到的图片是不一样的。可能变换过后得到的电脑屏幕一样，但是从侧面拍的电脑会得到电脑侧面的信息，当变换到屏幕正面的时候就会在正面看到电脑侧面的usb插口。
2.IPM变换方法
对应点对单应变换方法
输入：至少四个对应点对，不能有三点及以上共线，不需要知道摄相机参数或者平面位置的任何信息。
数学原理：利用点对，求解透视变换矩阵，其中map_matrix是一个3×3矩阵，所以可以构建一个线性方程组进行求解。如果大于4个点，可采用ransac的方法进行求解，一边具有更好的稳定性。
选点方法：一般采取手动选取，或者利用消影点（图像上平行线的交点，也叫消失点，vanish point）选取。
代码实现：代码实现比较简单，可以很容易实现IPM变换
计算变换矩阵：H = getPerspectiveTransform()
获取IPM图像：warpPerspective();
- void mywarpPerspective(Mat src,Mat &dst,Mat T) {//此处注意计算模型的坐标系与Mat的不同//图像以左上点为（0,0），向左为x轴，向下为y轴，所以前期搜索到的特征点 存的格式是（图像x，图像y）---（rows，cols）//而Mat矩阵的是向下为x轴，向左为y轴，所以存的方向为（图像y，图像x）----（cols，rows）----（width，height）//这个是计算的时候容易弄混的//创建原图的四个顶点的3*4矩阵（此处我的顺序为左上，右上，左下，右下）Mat tmp(3, 4, CV_64FC1, 1);tmp.at < double >(0, 0) = 0;tmp.at < double >(1, 0) = 0;tmp.at < double >(0, 1) = src.cols;tmp.at < double >(1, 1) = 0;tmp.at < double >(0, 2) = 0;tmp.at < double >(1, 2) = src.rows;tmp.at < double >(0, 3) = src.cols;tmp.at < double >(1, 3) = src.rows;//获得原图四个顶点变换后的坐标，计算变换后的图像尺寸Mat corner = T * tmp; //corner=(x,y)=(cols,rows)int width = 0, height = 0;double maxw = corner.at < double >(0, 0)/ corner.at < double >(2,0);double minw = corner.at < double >(0, 0)/ corner.at < double >(2,0);double maxh = corner.at < double >(1, 0)/ corner.at < double >(2,0);double minh = corner.at < double >(1, 0)/ corner.at < double >(2,0);for (int i = 1; i < 4; i++) {maxw = max(maxw, corner.at < double >(0, i) / corner.at < double >(2, i));minw = min (minw, corner.at < double >(0, i) / corner.at < double >(2, i));maxh = max(maxh, corner.at < double >(1, i) / corner.at < double >(2, i));minh = min (minh, corner.at < double >(1, i) / corner.at < double >(2, i));}//创建向前映射矩阵 map_x, map_y//size(height,width)dst.create(int(maxh - minh), int(maxw - minw), src.type());Mat map_x(dst.size(), CV_32FC1);Mat map_y(dst.size(), CV_32FC1);Mat proj(3,1, CV_32FC1,1);Mat point(3,1, CV_32FC1,1);T.convertTo(T, CV_32FC1);//本句是为了令T与point同类型（同类型才可以相乘，否则报错，也可以使用T.convertTo(T, point.type() );）Mat Tinv=T.inv();for (int i = 0; i < dst.rows; i++) {for (int j = 0; j < dst.cols; j++) {point.at<float>(0) = j + minw ;point.at<float>(1) = i + minh ;proj = Tinv * point;map_x.at<float>(i, j) = proj.at<float>(0)/ proj.at<float>(2) ;map_y.at<float>(i, j) = proj.at<float>(1) / proj.at<float>(2) ;}}remap(src,dst,map_x,map_y, CV_INTER_LINEAR);}
3.简化相机模型的逆透视变换
利用相机成像过程当中各种坐标系之间的转换关系，对其基本原理进行抽象和简化，从而得到世界坐标系和图像坐标系之间坐标的对应关系，并对逆透视变换的坐标关系进行公式化描述。这种逆透视变换形式简单，计算速度快，并且适用于复杂道路场景。
3.1 paper：Stereo inverse perspective mapping: theory and applications
这种方法需要的已知量：
这个方法同时考虑到了上下俯仰角和水平偏航角度的矫正，但是存在水平线弯曲误差，导致图像的水平横线如行道线等，恢复到世界坐标系的道路平面后，会有一定的弯曲。
3.2 Paper：Robust Inverse Perspective Mapping Based on Vanishing Point
基于消失点的俯仰角和偏航角计算
根据射影几何学原理，现实空间中的平行直线组在存在透视形变的情况下将会相交于无穷远点处，而该交点在成像平面上的投影即被称为消失点。当现实世界的平行直线组与成像平面平行时，消失点将位于成像平面的无穷远处；但是当平行直线组与成像平面部存在不平行关系时，消失点将位于成像平面的有限远处，甚至可能出现在图像区域以内。
消失点具有一些重要的性质：
-
真实道路场景中相互平行的行道线以及与其他和行道线平行的直线都指向相同的消失点；
-
某条直线对应的消失点一定位于这条直线在图像平面的投影射线方向上； -
消失点的位置和滚转角无关，仅仅和俯仰角、偏航角有关； -
当偏转角为90度时，行道线所对应的消失点位于道路图像水平方向外部； -
当俯仰角为90度时，行道线所对应的消失点位于道路图像垂直方向外部； -
当相机坐标系和世界坐标系原点完全重合时，消失点位于道路图像中心；
其中$( x , y )$是消失点坐标，$( M , N )$是图像宽高，($\alpha_v$,$\alpha_u$)是垂直、水平视场角的一半，由次公式可计算偏转和俯仰角
示意图如下：
由图像坐标到IPM坐标的关系式为：
引入偏航角：
void build_ipm_table(const int srcw,const int srch,const int dstw,const int dsth,const int vptx,const int vpty,int* maptable){const float alpha_h = 0.5f * FOV_H * DEG2RAD;const float alpha_v = 0.5f * FOV_V * DEG2RAD;const float gamma = -(float)(vptx - (srcw >> 1)) * alpha_h / (srcw >> 1); // camera pan angleconst float theta = -(float)(vpty - (srch >> 1)) * alpha_v / (srch >> 1); // camera tilt angleconst int front_map_start_position = dsth >> 1;const int front_map_end_position = front_map_start_position + dsth;const int side_map_mid_position = dstw >> 1;//scale to get better mapped imageconst int front_map_scale_factor = 4;const int side_map_scale_factor = 2;for (int y = 0; y < dstw; ++y){for (int x = front_map_start_position; x < front_map_end_position; ++x){int idx = y * dsth + (x - front_map_start_position);int deltax = front_map_scale_factor * (front_map_end_position - x - CAMERA_POS_X);int deltay = side_map_scale_factor * (y - side_map_mid_position - CAMERA_POS_Y);if (deltay == 0){maptable[idx] = maptable[idx - dsth];}else{int u = (int)((atan(CAMERA_POS_Z * sin(atan((float)deltay / deltax)) / deltay) - (theta - alpha_v)) / (2 * alpha_v / srch));int v = (int)((atan((float)deltay / deltax) - (gamma - alpha_h)) / (2 * alpha_h / srcw));if (u >= 0 && u < srch && v >= 0 && v < srcw){maptable[idx] = srcw * u + v;}else{maptable[idx] = -1;}}}}}
从对比结果看，此方法不会让水平线弯曲，同事使用偏航角进行修正，具有较好效果。
3.3 Paper：Adaptive Inverse Perspective Mapping for Lane Map Generation with SLAM
https://github.com/visionbike/AdaptiveIPM
这种方法利用相机位姿，在相邻帧中加入了俯仰角的修正，因此可以得到比较好的逆变换效果，下面就是相关原理示意图：
void AdaptiveIPM::ipm_based_on_vp(){//根据灭点来进行反投影变换Point2d ptVP = m_ptVP;Mat imgSrc = m_SrcImg;int v = imgSrc.rows;int u = imgSrc.cols;ptVP.y = MAX(0, ptVP.y);m_ipmInfo.vpPortion = m_ipmInfo.vpInitPortion;FLOAT_MAT_ELEM_TYPE eps = m_ipmInfo.vpPortion * v;//cout<<eps<<endl;m_ipmInfo.ipmLeft = MAX(0, m_ipmInfo.ipmLeft);m_ipmInfo.ipmRight = MIN(u - 1, m_ipmInfo.ipmRight);m_ipmInfo.ipmTop = ptVP.y + eps;// MAX(ptVanish.y+eps, m_ipmInfo.ipmTop);//动态转化大小m_ipmInfo.ipmBottom = MIN(v - 1, m_ipmInfo.ipmBottom);FLOAT_MAT_ELEM_TYPE uvLimitsp[] = { ptVP.x, m_ipmInfo.ipmRight, m_ipmInfo.ipmLeft, ptVP.x,m_ipmInfo.ipmTop, m_ipmInfo.ipmTop, m_ipmInfo.ipmTop, m_ipmInfo.ipmBottom };CvMat uvLimits = cvMat(2, 4, FLOAT_MAT_TYPE, uvLimitsp);//get these points on the ground planeCvMat *xyLimitsp = cvCreateMat(2, 4, FLOAT_MAT_TYPE);CvMat xyLimits = *xyLimitsp;transformImage2Ground(&uvLimits, &xyLimits, m_cam);//get extent on the ground planeCvMat row1, row2;cvGetRow(&xyLimits, &row1, 0);cvGetRow(&xyLimits, &row2, 1);double xfMax, xfMin, yfMax, yfMin;cvMinMaxLoc(&row1, (double*)&xfMin, (double*)&xfMax, 0, 0, 0);cvMinMaxLoc(&row2, (double*)&yfMin, (double*)&yfMax, 0, 0, 0);int outRow = m_IpmImg.rows; //设定512*512int outCol = m_IpmImg.cols;//cout<<"x:"<<xfMax<<" "<<xfMin<<endl<<"y:"<<yfMax<<" "<<yfMin<<endl;FLOAT_MAT_ELEM_TYPE stepRow = (yfMax - yfMin) / outRow;FLOAT_MAT_ELEM_TYPE stepCol = (xfMax - xfMin) / outCol;CvMat *xyGrid = cvCreateMat(2, outRow*outCol, FLOAT_MAT_TYPE);INT i, j;FLOAT_MAT_ELEM_TYPE x, y;//fill it with x-y values on the ground plane in world framefor (i = 0, y = yfMax - .5*stepRow; i < outRow; i++, y -= stepRow){for (j = 0, x = xfMin + .5*stepCol; j < outCol; j++, x += stepCol){//cout<<"x:"<<x<<"y:"<<y<<endl;CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 0, i*outCol + j) = x;CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 1, i*outCol + j) = y;}}//get their pixel values in image frame //获取每个像素的真实像素值，创建输出2行，outRow*outCol大小的矩阵CvMat *uvGrid = cvCreateMat(2, outRow*outCol, FLOAT_MAT_TYPE);transformGround2Image(xyGrid, uvGrid, m_cam);//now loop and find the nearest pixel value for each position//that's inside the image, otherwise put it zeroFLOAT_MAT_ELEM_TYPE ui, vi;//get mean of the input image//cout<<m_srcImg.type()<<endl;CvMat *inImage = cvCreateMat(imgSrc.rows, imgSrc.cols, imgSrc.type());CvMat tempImg = imgSrc;cvCopy(&tempImg, inImage);CvScalar means = cvAvg(inImage);double mean = means.val[0];for (i = 0; i < outRow; i++){for (j = 0; j < outCol; j++){/*get pixel coordiantes*/ui = CV_MAT_ELEM(*uvGrid, FLOAT_MAT_ELEM_TYPE, 0, i*outCol + j);vi = CV_MAT_ELEM(*uvGrid, FLOAT_MAT_ELEM_TYPE, 1, i*outCol + j);if (ui<m_ipmInfo.ipmLeft || ui>m_ipmInfo.ipmRight || vi<m_ipmInfo.ipmTop || vi>m_ipmInfo.ipmBottom){m_IpmImg.at<uchar>(i, j) = 0;}/*not out of bounds, then get nearest neighbor*/else{/*Bilinear interpolation 双线性插值*/if (m_ipmInfo.ipmInterpolation == 0){int x1 = int(ui);int x2 = int(ui + 0.5);int y1 = int(vi);int y2 = int(vi + 0.5);float x = ui - x1;float y = vi - y1;float val = CV_MAT_ELEM(*inImage, uchar, y1, x1) * (1 - x) * (1 - y) + CV_MAT_ELEM(*inImage, uchar, y1, x2) * x * (1 - y) + CV_MAT_ELEM(*inImage, uchar, y2, x1) * (1 - x) * y + CV_MAT_ELEM(*inImage, uchar, y2, x2) * x * y;m_IpmImg.at<uchar>(i, j) = (float)val;}/*nearest-neighbor interpolation最近邻插值*/else{m_IpmImg.at<uchar>(i, j) = CV_MAT_ELEM(*inImage, uchar, int(vi + .5), int(ui + .5));}}}}m_ipmInfo.xLimits[0] = CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 0, 0);m_ipmInfo.xLimits[1] = CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 0, (outRow - 1)*outCol + outCol - 1);m_ipmInfo.yLimits[1] = CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 1, 0);m_ipmInfo.yLimits[0] = CV_MAT_ELEM(*xyGrid, FLOAT_MAT_ELEM_TYPE, 1, (outRow - 1)*outCol + outCol - 1);m_ipmInfo.xScale = 1 / stepCol;m_ipmInfo.yScale = 1 / stepRow;m_ipmInfo._width = outCol;m_ipmInfo._height = outRow;cout << stepCol << endl;cout << stepRow << endl;//cleancvReleaseMat(&xyLimitsp);cvReleaseMat(&xyGrid);cvReleaseMat(&uvGrid);cvReleaseMat(&inImage);}
仅仅考虑单帧的情形：
考虑帧间自适应变换：
测试结果：
4.其他IPM变换方法
以下两点是我根据实际工作上所使用过的一些方法，相比较于以上几种方法来说，更多的是应用了多视图几何的知识来得到IPM变换。
4.1在相机坐标系下已求得路平面方程求IPM
在slam应用中，如果已知相机pose，同时在用地面点已求得路平面方程的情况下，可利用多视图几何关系求得IPM变换
输入：
-
相机坐标系下路平面方程$a x + b y + c z + d = 0$
-
相机内参
步骤：
4.2已知相机位姿，利用求单应矩阵得到IPM
同样，在已知相机pose情况下，可将相机向路面方向旋转90度-俯仰角，那么得到一个新的相机pose，此时的相机是垂直与路面向下，那么得到的图像自然也就是IPM图像。
对于多视图几何中，单应矩阵的计算公式：
$N,d$表示路平面参数
如果是纯旋转矩阵，那么单应可以表示为：
当单应矩阵求出来了，那么很容易就可以得到IPM变换图像。
dst(x,y) = src((H11x+H12y+H13)/(H31x+H32y+H33), (H21x+H22y+H23)/(H31x+H32y+H33))
void basicPanoramaStitching(const string &img1Path, const string &img2Path){Mat img1 = imread("Blender_Suzanne1.jpg");Mat img2 = imread("Blender_Suzanne2.jpg");//! [camera-pose-from-Blender-at-location-1]Mat c1Mo = (Mat_<double>(4, 4) << 0.9659258723258972, 0.2588190734386444, 0.0, 1.5529145002365112,0.08852133899927139, -0.3303661346435547, -0.9396926164627075, -0.10281121730804443,-0.24321036040782928, 0.9076734185218811, -0.342020183801651, 6.130080699920654,0, 0, 0, 1);//! [camera-pose-from-Blender-at-location-1]//! [camera-pose-from-Blender-at-location-2]Mat c2Mo = (Mat_<double>(4, 4) << 0.9659258723258972, -0.2588190734386444, 0.0, -1.5529145002365112,-0.08852133899927139, -0.3303661346435547, -0.9396926164627075, -0.10281121730804443,0.24321036040782928, 0.9076734185218811, -0.342020183801651, 6.130080699920654,0, 0, 0, 1);//! [camera-pose-from-Blender-at-location-2]Mat cameraMatrix = (Mat_<double>(3, 3) << 700.0, 0.0, 320.0,0.0, 700.0, 240.0,0, 0, 1);Mat R1 = c1Mo(Range(0, 3), Range(0, 3));Mat R2 = c2Mo(Range(0, 3), Range(0, 3));//c1Mo * oMc2Mat R_2to1 = R1*R2.t();//! [compute-homography]Mat H = cameraMatrix * R_2to1 * cameraMatrix.inv();H /= H.at<double>(2, 2);cout << "H:\n" << H << endl;//! [compute-homography]//! [stitch]Mat img_stitch;warpPerspective(img2, img_stitch, H, Size(img2.cols * 2, img2.rows));Mat half = img_stitch(Rect(0, 0, img1.cols, img1.rows));img1.copyTo(half);//! [stitch]Mat img_compare;Mat img_space = Mat::zeros(Size(50, img1.rows), CV_8UC3);hconcat(img1, img_space, img_compare);hconcat(img_compare, img2, img_compare);imshow("Compare images", img_compare);imshow("Panorama stitching", img_stitch);waitKey();}
5.参考链接
https://blog.csdn.net/feiyang_luo/article/details/103555036
https://blog.csdn.net/yeyang911/article/details/51915348
https://blog.csdn.net/lyhbkz/article/details/82254893
https://blog.csdn.net/djfjkj52/article/details/104791610/
https://blog.csdn.net/qq_40891636/article/details/106917776
https://www.cnblogs.com/riddick/p/7398311.html
《MoveIt玩转双臂机器人 • 李江浩》
带领大家一起来设计一款基于MoveIt！的双臂机械臂，同时也设计一块示教手轮，并开发对应的ROS功能包来实现在ROS系统下机械臂控制。此次教程还是一个软硬件结合开发的课程，做了双臂机械臂，你也就能玩单臂机械臂。
点击阅读原文可查看课程详情