代码不能直接使用。
for (int j = strRY; j<endRY; ++j)
{
float * pR = result.ptr<float>(j);
for (int i = strRX; i < endRX; ++i)
{
// To make sure z value;
float z(FLOAT32_MIN);
int i_(i - pOffset.x), j_(j - pOffset.y);
float x = m_mapX.at<float>(j - strRY, i - strRY);
float y = m_mapY.at<float>(j - strRY, i - strRY);
// whether out of range.
if (x < 0 || x >= src2.cols || y < 0 || y >= src2.rows)
continue;
int lRow = static_cast<int>(y);
int nRow = lRow + 1;
int lCol = static_cast<int>(x);
int nCol = lCol + 1;
double u = y - lRow;
double v = x - lCol;
// interpolation
// right bottom.
if (lRow >= src2.rows - 1 && lCol >= src2.cols - 1)
{
float data = m_mapZ.at<float>(lRow, lCol);
if (data == -1.0e6)
continue;
z = (1 - u) * (1 - v) * data;
}
else if (lRow >= src2.rows - 1)//last row
{
float data = m_mapZ.at<float>(lRow, lCol);
float data2 = m_mapZ.at<float>(lRow, nCol);
if (data == -1.0e6 || data2 == -1.0e6)
continue;
z = (1 - u) * (1 - v) * data +
(1 - u) * v * data2;
}
else if (lCol >= src2.cols - 1)//last col
{
float data = m_mapZ.at<float>(lRow, lCol);
float data3 = m_mapZ.at<float>(nRow, lCol);
if (data == -1.0e6 || data3 == -1.0e6)
continue;
z = (1 - u) * (1 - v) * data + u * (1 - v) * data3;
}
else
{
float data = m_mapZ.at<float>(lRow, lCol);
float data2 = m_mapZ.at<float>(lRow, nCol);
float data3 = m_mapZ.at<float>(nRow, lCol);
float data4 = m_mapZ.at<float>(nRow, nCol);
if (data == -1.0e6 || data2 == -1.0e6 || data3 == -1.0e6 || data4 == -1.0e6)
continue;
z = (1 - u) * (1 - v) * data + (1 - u) * v * data2 +
u * (1 - v) * data3 + u * v * data4;
}
pR[i] = pR[i] == -1.0e6 ? z : 0.5*(pR[i] + z);
}
}