/// <summary>
/// 将位图转换为层信息
/// </summary>
/// <param name="bmp">位图</param>
/// <returns>返回LayerImageCollection</returns>
public LayerImageCollection TransformBmpToLayerImg(Bitmap bmp)
{
if (bmp == null || bmp.PixelFormat != PixelFormat.Format8bppIndexed)
{
throw new Exception("原位图为空或者不是灰度图。");
}
int width = bmp.Width;
int height = bmp.Height;
Rectangle rect = new Rectangle(0, 0, width, height);
BitmapData bmpdata = bmp.LockBits(rect, ImageLockMode.ReadOnly, bmp.PixelFormat);
// Stride为位图中每一行以4字节对齐的行宽
int strideSource = bmpdata.Stride;
byte[] byteData = new byte[width * height];
LayerImage li = null;
LayerImageCollection layerCollection = null;
unsafe
{
byte *ptr = (byte *)bmpdata.Scan0.ToPointer();
for (int row = 0; row < height; row++)
{
for (int col = 0; col < width; col++)
{
// 位图的在内存中的排列是从左到右，从下到上的，但BitmapData貌似做了优化，把位图数据在内存中的排列
// 顺序改为与坐标系一致，即从左到右、从上到下
byteData[col + row * width] = *ptr;
ptr++;
}
}
bmp.UnlockBits(bmpdata);
}
layerCollection = new LayerImageCollection();
li = new LayerImage(byteData, width, height);
layerCollection.Add(li); // 加入I的第0层数据
for (int l = 1; l < LN; l++)
{
int w, h; // 当前层图像的宽和高
// width和height为前一层图像的宽和高
w = (width + 1) / 2;
h = (height + 1) / 2;
byteData = new byte[w * h];
for (int row = 0; row < h; row++)
{
for (int col = 0; col < w; col++)
{
int leftX = 2 * col - 1;
int rightX = 2 * col + 1;
int lowY = 2 * row - 1;
int upY = 2 * row + 1;
int centerX = 2 * col;
int centerY = 2 * row;
if (leftX < 0)
{
leftX = 0;
}
if (rightX >= width)
{
rightX = width - 1;
}
if (lowY < 0)
{
lowY = 0;
}
if (upY >= height)
{
upY = height - 1;
}
// 根据上一层的数据生成当前层的数据
byteData[col + row * w] = (byte)(0.25 * li.ImageData[centerX + centerY * width] +
0.125 * (li.ImageData[leftX + centerY * width] + li.ImageData[rightX + centerY * width] +
li.ImageData[centerX + lowY * width] + li.ImageData[centerX + upY * width]) +
0.0625 * (li.ImageData[leftX + lowY * width] + li.ImageData[rightX + lowY * width] +
li.ImageData[leftX + upY * width] + li.ImageData[rightX + upY * width]));
}
}
li = new LayerImage(byteData, w, h);
layerCollection.Add(li);
width = w;
height = h;
}
return(layerCollection);
}
/// <summary>
/// 根据两幅图像金字塔和已选定的初始点，计算整体位移，返回位移值和缩放系数
/// </summary>
/// <param name="firstlayercollection">第一幅图像金字塔</param>
/// <param name="secondlayercollection">第二幅图像金字塔</param>
/// <param name="ipoints">第一幅图像中选中的初始点</param>
/// <param name="wx">计算每个点时，积分邻域横向半径，此值寻找初始点时只需要取1，跟踪时要取的大一些</param>
/// <param name="wy">计算每个点时，积分邻域纵向半径，此值寻找初始点时只需要取1，跟踪时要取的大一些</param>
/// <param name="reversebias">反向跟踪时，输出点和原来点之间的距离差阈值（像素值），该差值超过阈值被认为不是好的跟踪点，当被删除，建议取1</param>
/// <param name="medianbias">每个点的位移与中值位移之间的最大距离(像素值)，超过这个距离的点认为是跟踪失败的，建议取10</param>
/// <param name="scalestep">缩放台阶，缩放系数的变化刻度，如变化差小于0.2的视为不变，大于0.2小于0.4的视为0.2，以此类推</param>
/// <param name="firstlayerpoints">第一幅图像的最终输出点</param>
/// <param name="secondlayerpoints">第二幅图像的最终输出点</param>
/// <returns>位移值和缩放系数</returns>
public float[] ComputerDisplacement(LayerImageCollection firstlayercollection, LayerImageCollection secondlayercollection, ArrayList ipoints,
float reversebias, // 这个参数不好控制，导致跟踪非常不稳定
int wx, int wy, float medianbias, float scalestep,
ref ArrayList firstlayerpoints, ref ArrayList secondlayerpoints)
{
if (ipoints == null || secondlayercollection == null || firstlayercollection == null)
{
return(null);
}
// 输出结果，前两个维度表示位移，第三个维度表示区域缩放系数
float[] outvector = new float[3];
firstlayerpoints = new ArrayList();
secondlayerpoints = new ArrayList();
ArrayList jpoints = new ArrayList();
ValuePointCollection fbPointsArray = new ValuePointCollection(); // 用于计算Forward-Backward的集合，ValuePoint类型的
ValuePoint vp = null;
foreach (PointF p in ipoints)
{
PointF jpoint = TrackPoint(firstlayercollection, secondlayercollection, p, wx, wy); // 正跟踪，第一幅图跟踪到第二幅图
PointF ipoint;
if (jpoint.X >= 0 && jpoint.Y >= 0)
{
vp = new ValuePoint();
vp.Ptf1 = p; // 第一幅图的点加入队列
vp.Ptf2 = jpoint; // 第二幅图的对应点加入队列
fbPointsArray.Add(vp);
// 这段代码导致跟踪极不稳定
ipoint = TrackPoint(secondlayercollection, firstlayercollection, jpoint, wx, wy); // 反跟踪，第二幅图跟踪到第一幅图
vp.Val = (float)Math.Sqrt((p.X - ipoint.X) * (p.X - ipoint.X) + (p.Y - ipoint.Y) * (p.Y - ipoint.Y)); // 反向跟踪后产生的点与原来点的距离
if (vp.Val < reversebias)
{
fbPointsArray.Add(vp);
}
}
}
if (fbPointsArray.Count == 0)
{
return(null);
}
//// 这段代码不是很合适
//fbPointsArray = ValuePoint.SortValuePointCollection(fbPointsArray);
//// 根据Forward-Backward值，去掉一半的不稳定点
//int arraycont = fbPointsArray.Count;
//for (int i = arraycont / 2; i < arraycont; i++)
//{
// fbPointsArray.RemoveAt(arraycont / 2);
//} // Forward-Backward计算完成
// 将fbPointsArray用于中值位移计算
for (int i = 0; i < fbPointsArray.Count; i++)
{
vp = fbPointsArray[i];
// 求每一点的位移值，不作开方减小计算量，不影响结果
vp.Val = (vp.Ptf2.X - vp.Ptf1.X) * (vp.Ptf2.X - vp.Ptf1.X) + (vp.Ptf2.Y - vp.Ptf1.Y) * (vp.Ptf2.Y - vp.Ptf1.Y);
}
fbPointsArray = ValuePoint.SortValuePointCollection(fbPointsArray);
ValuePoint median_vp = fbPointsArray[fbPointsArray.Count / 2];
outvector[0] = median_vp.Ptf2.X - median_vp.Ptf1.X;
outvector[1] = median_vp.Ptf2.Y - median_vp.Ptf1.Y; // 中值位移计算完毕
// 去掉位移偏离中值位移过大的点
ValuePointCollection tmpCollection = new ValuePointCollection();
foreach (ValuePoint vpoint in fbPointsArray)
{
float dist = (float)Math.Sqrt(Math.Pow(vpoint.Ptf2.X - vpoint.Ptf1.X - median_vp.Ptf2.X + median_vp.Ptf1.X, 2) +
Math.Pow(vpoint.Ptf2.Y - vpoint.Ptf1.Y - median_vp.Ptf2.Y + median_vp.Ptf1.Y, 2));
if (dist > medianbias) // 超过中值偏移的点删掉
{
tmpCollection.Add(vpoint);
}
else // 没超过中值位移的点加入图像金字塔
{
firstlayerpoints.Add(vpoint.Ptf1);
secondlayerpoints.Add(vpoint.Ptf2);
}
}
foreach (ValuePoint vpoint in tmpCollection)
{
fbPointsArray.Remove(vpoint);
}
// 计算区域的缩放值
float[] scales = new float[fbPointsArray.Count - 1];
for (int i = 0; i < fbPointsArray.Count - 1; i++)
{
ValuePoint v1 = fbPointsArray[i];
ValuePoint v2 = fbPointsArray[i + 1];
if ((v2.Ptf1.X - v1.Ptf1.X) * (v2.Ptf1.X - v1.Ptf1.X) + (v2.Ptf1.Y - v1.Ptf1.Y) * (v2.Ptf1.Y - v1.Ptf1.Y) == 0)
{
scales[i] = 1;
}
else
{
scales[i] = (float)(Math.Sqrt((v2.Ptf2.X - v1.Ptf2.X) * (v2.Ptf2.X - v1.Ptf2.X) + (v2.Ptf2.Y - v1.Ptf2.Y) * (v2.Ptf2.Y - v1.Ptf2.Y)) /
Math.Sqrt((v2.Ptf1.X - v1.Ptf1.X) * (v2.Ptf1.X - v1.Ptf1.X) + (v2.Ptf1.Y - v1.Ptf1.Y) * (v2.Ptf1.Y - v1.Ptf1.Y)));
}
}
outvector[2] = GetMedian(scales);
if (outvector[2] < 0)
{
outvector[2] = 1;
}
else
{
outvector[2] = 1 + (int)((outvector[2] - 1) / scalestep) * scalestep;
}
return(outvector);
}
/// <summary>
/// 跟踪给定点的算法
/// </summary>
/// <param name="firstlayercollection">第一幅图像金字塔</param>
/// <param name="secondlayercollection">第二幅图像金字塔</param>
/// <param name="u">第一幅图像中给定的点</param>
/// <param name="wx">计算每个点时，积分邻域横向半径，此值寻找初始点时只需要取1，跟踪时要取的大一些</param>
/// <param name="wy">计算每个点时，积分邻域纵向半径，此值寻找初始点时只需要取1，跟踪时要取的大一些</param>
/// <returns>返回第二幅图像中对应的点</returns>
public PointF TrackPoint(LayerImageCollection firstlayercollection, LayerImageCollection secondlayercollection, PointF u, int wx, int wy)
{
float[] G = null;
float[] G_1 = null; // G的逆矩阵
PointF[] g = new PointF[LN]; // 前一层估计的光流
PointF[] d = new PointF[LN]; // 本层计算出来的剩余光流
PointF[] p = new PointF[LN]; // 原始点坐标在本层的映射坐标
PointF[] v = new PointF[K + 1]; // 本层每次迭代计算出来的剩余光流
Wx = wx;
Wy = wy;
if (firstlayercollection == null || secondlayercollection == null)
{
throw new Exception("有图像为空，无法跟踪。");
}
if (firstlayercollection[0].Width != secondlayercollection[0].Width)
{
throw new Exception("两幅图像大小不一致，无法跟踪。");
}
g[LN - 1].X = 0;
g[LN - 1].Y = 0;
for (int l = LN - 1; l >= 0; l--)
{
p[l].X = (float)(u.X / Math.Pow(2, l));
p[l].Y = (float)(u.Y / Math.Pow(2, l));
// 原图中给定的点已经在边界上，无法跟踪
if (p[l].X > firstlayercollection[l].Width - 1 || p[l].Y > firstlayercollection[l].Height - 1)
{
// 返回值中带有负数表示没有正确的结果
return(new PointF(-1, -1));
}
G = new float[4] {
0, 0, 0, 0
};
for (float x = p[l].X - Wx; x <= p[l].X + Wx; x++)
{
for (float y = p[l].Y - Wy; y <= p[l].Y + Wy; y++)
{
float ix = Ix(l, x, y);
float iy = Iy(l, x, y);
G[0] += ix * ix; // 第0行第0列
G[1] += ix * iy; // 第0行第1列
G[2] += ix * iy; // 第1行第0列
G[3] += iy * iy; // 第1行第1列
}
}
// 求G的逆矩阵G_1
float gdet = G[0] * G[3] - G[1] * G[2];
// G的行列式为0，无法计算，跟踪丢失
if (gdet == 0)
{
return(new PointF(-1, -1));
}
G_1 = new float[4] {
0, 0, 0, 0
};
G_1[0] = G[3] / gdet;
G_1[1] = -G[2] / gdet;
G_1[2] = -G[1] / gdet;
G_1[3] = G[0] / gdet;
v[0].X = 0;
v[0].Y = 0;
for (int k = 1; k < K + 1; k++)
{
int delt;
PointF b = new PointF(0, 0);
PointF yeta = new PointF(0, 0);
for (float x = p[l].X - Wx; x <= p[l].X + Wx; x++)
{
for (float y = p[l].Y - Wy; y <= p[l].Y + Wy; y++)
{
// 第二幅图的点已超出边界，跟踪丢失
if (x + g[l].X + v[k - 1].X <0 || x + g[l].X + v[k - 1].X> secondlayercollection[l].Width - 1 ||
y + g[l].Y + v[k - 1].Y <0 || y + g[l].Y + v[k - 1].Y> secondlayercollection[l].Height - 1)
{
return(new PointF(-1, -1));
}
delt = firstlayercollection[l][x, y] - secondlayercollection[l][x + g[l].X + v[k - 1].X, y + g[l].Y + v[k - 1].Y];
b.X += delt * Ix(l, x, y);
b.Y += delt * Iy(l, x, y);
}
}
yeta.X = G_1[0] * b.X + G_1[1] * b.Y;
yeta.Y = G_1[2] * b.X + G_1[3] * b.Y;
v[k].X = v[k - 1].X + yeta.X;
v[k].Y = v[k - 1].Y + yeta.Y;
}
d[l].X = v[K].X;
d[l].Y = v[K].Y;
if (l > 0)
{
g[l - 1].X = 2 * (g[l].X + d[l].X);
g[l - 1].Y = 2 * (g[l].Y + d[l].Y);
}
}
return(new PointF(u.X + g[0].X + d[0].X, u.Y + g[0].Y + d[0].Y));
}