基于最小代价函数的离群值移除 图像配准矩阵参数异常求助

昨日发布的问题未收到回复后，我自行研究了如何填充Matrix A与Matrix B的矩阵元素，以计算返回可将图像B对齐到图像A的变换矩阵。我找到一篇列有矩阵元素推导过程的研究论文，将表达式直接代入矩阵元素填充循环后，第二幅图像的离群值已有少量移除，但出现了与图像A重叠的异常，更新代码如下：

class ICPTransformation
{
    public static Transformation ComputeTransformation(List<Point> shp1, List<Point> shp2) {
        // 新建4x4矩阵A用于计算变换
        PCALib.Matrix A = new PCALib.Matrix(4,4);
        // 新建4x1矩阵B用于计算变换
        PCALib.Matrix B = new PCALib.Matrix(4, 1);
        // 遍历第一组形状的所有点
        for(int i = 0; i < shp1.Count; i++)
        {
            A[0,0] +=2*shp2[i].X*shp2[i].X+2*shp2[i].Y*shp2[i].Y;
            // 填充矩阵A剩余元素，注意矩阵A是4x4维度
            A[0, 1] += 0;
            A[0, 2] += 2 * shp2[i].X;
            A[0, 3] += 2 * shp2[i].Y;
            A[1, 0] += 0;
            A[1, 1] += 2 * shp2[i].Y * shp2[i].Y+2*shp2[i].X*shp2[i].X;
            A[1, 2] += 2 * shp2[i].Y;
            A[1, 3] += -2 * shp2[i].X;
            A[2, 0] += 2 * shp2[i].X;
            A[2, 1] += 2 * shp2[i].Y;
            A[2, 2] += 2;
            A[2, 3] += 0;
            A[3, 0] += 2 * shp2[i].Y;
            A[3, 1] += -2 * shp2[i].X;
            A[3, 2] += 0;
            A[3, 3] += -2;
            // 填充矩阵B剩余元素，矩阵B是4x1维度
            B[0, 0] += 2 * shp1[i].X * shp2[i].X + 2 * shp1[i].Y * shp2[i].Y;
            B[1, 0] += 2 * shp1[i].X * shp2[i].Y - 2 * shp2[i].X * shp1[i].Y;
            B[2, 0] += 2 * shp1[i].X;
            B[3, 0] += -2 * shp1[i].Y;
        }
    }
}

有没有人可以帮我修复代码中的这个异常？我怀疑问题出在Matrix A或者Matrix B的矩阵元素填写上。

using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using PCALib;

namespace OutlierRemoval
{
    public partial class Form1 : Form
    {
        // 存储点集的列表声明
        List<Point> Shape1 = new List<Point>();
        List<Point> Shape2 = new List<Point>();
        List<Point> Shape2Transformed = new List<Point>();
        
        public Form1()
        {
            InitializeComponent();
            // 第一幅图像初始化完成前禁用第二个按钮
            button2.Enabled = false;
        }

        private void button1_Click(object sender, EventArgs e)
        {
            Shape1.Clear();
            Shape2.Clear();
            Point p1a = new Point(20, 30);
            Point p2a = new Point(120,50);
            Point p3a = new Point(160,80);
            Point p4a = new Point(180, 300);
            Point p5a = new Point(100,220);
            Point p6a = new Point(50, 280);
            Point p7a = new Point(20, 140);
            Point[] mypoints = new Point[] {p1a,p2a,p3a,p4a,p5a,p6a,p7a};
            Shape1.AddRange(mypoints);
            // 定义变换生成第二组形状
            Transformation t2 = new Transformation();
            t2.A = 1.05;t2.B = 0.05;t2.T1 = 15;t2.T2 = 22;
            Shape2 = ApplyTransformation(t2, Shape1);
            Shape2[2] = new Point(Shape2[2].X + 10, Shape2[2].Y + 3);// 修改单个点制造偏差
            // 给两组形状添加离群值
            Point ptOutlier1 = new Point(200, 300);
            Shape1.Add(ptOutlier1);
            Point ptOutlier2 = new Point(270, 160);
            Shape2.Add(ptOutlier2);
            // 绘制形状
            Pen pBlue = new Pen(Brushes.Blue, 1);
            Pen pRED = new Pen(Brushes.Red, 1);
            Graphics g = panel1.CreateGraphics();
            DisplayShape(Shape1, pBlue, g);
            DisplayShape(Shape2, pRED, g);
            button2.Enabled = true;
        }
        List<Point> ApplyTransformation(Transformation x,List<Point> shape) {
            List<Point> Tlist = new List<Point>();
            foreach (Point c in shape) {
                double xprime = x.A * c.X + x.B * c.Y + x.T1;
                double yprime = x.B * c.X * -1 + x.A * c.Y + x.T2;
                Point ptrans = new Point((int)xprime, (int)yprime);
                Tlist.Add(ptrans);
            }
            return Tlist;
        }
        void DisplayShape(List<Point> Shp,Pen pen, Graphics G)
        {
            Point? prevPoint = null;// 可空类型存储上一个点
            foreach(Point pt in Shp)
            {
                G.DrawEllipse(pen, new Rectangle(pt.X - 2, pt.Y - 2, 4, 4));
                if (prevPoint != null)
                {
                    G.DrawLine(pen, (Point)prevPoint, pt);
                }
                prevPoint = pt;
            }
            G.DrawLine(pen, Shp[0], Shp[Shp.Count - 1]);
        }

        private void button2_Click(object sender, EventArgs e)
        {
           Transformation T = ICPTransformation.ComputeTransformation(Shape1, Shape2);
            MessageBox.Show("Cost = "+ICPTransformation.computeCost(Shape1,Shape2,T).ToString());
            List<Point> Shape2T = ApplyTransformation(T, Shape2);
            Pen pBlue = new Pen(Brushes.Blue,1);
            Pen pRed = new Pen(Brushes.Red, 1);
            Graphics g = panel2.CreateGraphics();
            DisplayShape(Shape1,pBlue,g);
            DisplayShape(Shape2T, pRed, g);
        }

        private void button3_Click(object sender, EventArgs e)
        {
           /* 预留测试代码
            Transformation x = new Transformation();
            List<Point> mypoints = ApplyTransformation(x, Shape2);
            Graphics g = panel3.CreateGraphics();
            Pen mypen = new Pen(Brushes.Blue, 1);
            DisplayShape(mypoints, mypen, g);*/
        }
    }
    public class Transformation
    {
        public double A { get; set; }
        public double B { get; set; }
        public double T1 { get; set; }
        public double T2 { get; set; }
    }
    class ICPTransformation
    {
        public static Transformation ComputeTransformation(List<Point> shp1, List<Point> shp2) {
            PCALib.Matrix A = new PCALib.Matrix(4,4);
            PCALib.Matrix B = new PCALib.Matrix(4, 1);
            for(int i = 0; i < shp1.Count; i++)
            {
                A[0, 0] += 2 * shp2[i].X * shp2[i].X + 2 * shp2[i].Y * shp2[i].Y;
                A[0, 1] += 0;
                A[0, 2] += 2 * shp2[i].X;
                A[0, 3] += 2 * shp2[i].Y;
                A[1, 0] += 0;
                A[1, 1] += 2 * shp2[i].Y * shp2[i].Y + 2 * shp2[i].X * shp2[i].X;
                A[1, 2] += 2 * shp2[i].Y;
                A[1, 3] += -2 * shp2[i].X;
                A[2, 0] += 2 * shp2[i].X;
                A[2, 1] += 2 * shp2[i].Y;
                A[2, 2] += 2;
                A[2, 3] += 0;
                A[3, 0] += 2 * shp2[i].Y;
                A[3, 1] += -2 * shp2[i].X;
                A[3, 2] += 0;
                A[3, 3] += 2;
                B[0, 0] += 2 * shp1[i].X * shp2[i].X + 2 * shp1[i].Y * shp2[i].Y;
                B[1, 0] += 2 * shp1[i].X * shp2[i].Y - 2 * shp2[i].X * shp1[i].Y;
                B[2, 0] += 2 * shp1[i].X;
                B[3, 0] += 2 * shp1[i].Y;
            }
            // 求矩阵A的逆
            PCALib.Matrix Ainv =(PCALib.Matrix) A.Inverse;
            // 矩阵相乘得到变换参数
            PCALib.Matrix Res =(PCALib.Matrix) Ainv.Multiply(Ainv);
            Transformation T = new Transformation();
            T.A = Res[0, 0];
            T.B = Res[1, 0];
            T.T1 = Res[2, 0];
            T.T2 = Res[3, 0];
            return T;
        }
        public static double computeCost(List<Point> P1List, List<Point> P2List,Transformation T) {
            double cost = 0;
            for (int i = 0; i < P1List.Count; i++) {
                double xprime = T.A * P2List[i].X + T.B * P2List[i].Y + T.T1;
                double yprime = -1 * T.B * P2List[i].X + T.A * P2List[i].Y + T.T2;
                cost += (P1List[i].X-xprime)*(P1List[i].X-xprime)+(P1List[i].Y-yprime)*(P1List[i].Y-yprime);
            }
            return cost;
        }
    }
    // 重复定义的拼写错误类，可直接删除
    public class Tranformation
    {
        public double A { get; set; }
        public double B { get; set; }
        public double T1 { get; set; }
        public double T2 { get; set; }
    }
}

修复方案 #

代码存在两处核心错误：

1. 矩阵运算逻辑错误：求解线性方程Ax=B的正确公式为x = A⁻¹ * B，你的代码误写为Ainv.Multiply(Ainv)，即A逆乘A逆，完全偏离了正确计算逻辑。
2. 冗余重复定义：代码末尾多定义了一个拼写错误的Tranformation类（少了字母s），会引发类型冲突，直接删除即可。

修正后的核心代码 #

public static Transformation ComputeTransformation(List<Point> shp1, List<Point> shp2) {
    PCALib.Matrix A = new PCALib.Matrix(4,4);
    PCALib.Matrix B = new PCALib.Matrix(4, 1);
    for(int i = 0; i < shp1.Count; i++)
    {
        A[0, 0] += 2 * shp2[i].X * shp2[i].X + 2 * shp2[i].Y * shp2[i].Y;
        A[0, 1] += 0;
        A[0, 2] += 2 * shp2[i].X;
        A[0, 3] += 2 * shp2[i].Y;
        A[1, 0] += 0;
        A[1, 1] += 2 * shp2[i].Y * shp2[i].Y + 2 * shp2[i].X * shp2[i].X;
        A[1, 2] += 2 * shp2[i].Y;
        A[1, 3] += -2 * shp2[i].X;
        A[2, 0] += 2 * shp2[i].X;
        A[2, 1] += 2 * shp2[i].Y;
        A[2, 2] += 2;
        A[2, 3] += 0;
        A[3, 0] += 2 * shp2[i].Y;
        A[3, 1] += -2 * shp2[i].X;
        A[3, 2] += 0;
        A[3, 3] += 2;
        B[0, 0] += 2 * shp1[i].X * shp2[i].X + 2 * shp1[i].Y * shp2[i].Y;
        B[1, 0] += 2 * shp1[i].X * shp2[i].Y - 2 * shp2[i].X * shp1[i].Y;
        B[2, 0] += 2 * shp1[i].X;
        B[3, 0] += 2 * shp1[i].Y;
    }
    PCALib.Matrix Ainv =(PCALib.Matrix) A.Inverse;
    // 修正为A逆乘B矩阵
    PCALib.Matrix Res =(PCALib.Matrix) Ainv.Multiply(B);
    Transformation T = new Transformation();
    T.A = Res[0, 0];
    T.B = Res[1, 0];
    T.T1 = Res[2, 0];
    T.T2 = Res[3, 0];
    return T;
}

内容的提问来源于stack exchange，提问作者user16612111