Проблема в рото-переводе области
Я попытался вставить код, ранее написанный в моей компании, но у меня возникли проблемы. Код работал над старым проектом. К сожалению, разработчик больше не работает со мной, поэтому мне сложно понять причины неисправности. Цель кода состоит в том, чтобы с учетом набора координат вершин области, видимой с 4 разных точек обзора, повернуть координаты трех точек обзора, чтобы отобразить их на одном графике в соответствии с системой отсчета первой точки обзора. Прежде всего, я создал экземпляр класса, используемого для ротационного перевода:
List<double[,]> data = new List<double[,]>();
for ( int i = 0 ; i < radarList.Count ; i++ )
{
if ( radarList[i].ConfiguredFromFile == true )
{
var p = radarList[i];
var matrix = new double[,] {
{p.SafetyArea.PointA.X, p.SafetyArea.PointA.Y},
{p.SafetyArea.PointB.X, p.SafetyArea.PointB.Y},
{p.SafetyArea.PointC.X, p.SafetyArea.PointC.Y},
{p.SafetyArea.PointD.X, p.SafetyArea.PointD.Y},
};
data.Add( matrix );
}
}
this.roto = new Rototranslator( data );
Конструктор:
public Rototranslator(List<double[,]> calibrationPoints)
{
// the number of reference systems
var len = calibrationPoints.Count;
// let's build a dense matrix of the 4 points
// in each reference system
Matrix[] data = new Matrix[len];
for (int i = 0; i < len; i++)
{
var p = calibrationPoints[i];
var matrix = DenseMatrix.OfArray( new double[,] {
{p[0,0], p[0,1], 1.0},
{p[1,0], p[1,1], 1.0},
{p[2,0], p[2,1], 1.0},
{p[3,0], p[3,1], 1.0}
});
data[i] = matrix;
}
// let's build len-1 rotoTraslation from
// system(i) to system(0).
// remarks: beta[0] will be null
var beta = new Matrix[len];
Matrix system0 = data[0];
for (int i = 1; i < len; i++)
{
// from X to Y
// BETA=(X' * X)^-1 * X' * Y
var systemI = data[i];
var systemIt = systemI.Transpose();
beta[i] = (Matrix)systemIt.Multiply(systemI).Inverse()
.Multiply(systemIt).Multiply(system0);
//Console.WriteLine(this.beta[i]);
}
// let's find the origin(1) coordinate in system(0)
var origin1 = DenseMatrix.OfArray( new [,] { {0.0, 0.0, 1.0} });
var origin1in0 = origin1.Multiply(beta[1]);
// rotation from system(0) to our reference system
this.theta = -Math.Atan2(origin1in0[0, 1], origin1in0[0, 0]);
var roto0 = DenseMatrix.OfArray( new[,] {
{ Math.Cos(this.theta), Math.Sin(this.theta), 0.0 },
{ -Math.Sin(this.theta), Math.Cos(this.theta), 0.0 },
{ 0.0, 0.0, 1.0 }});
this.roto = new Matrix[len];
this.inverse = new Matrix[len];
this.roto[0] = this.composeRototrans(new DiagonalMatrix(3, 3, 1), roto0);
this.inverse[0] = (Matrix)this.roto[0].Inverse();
//Console.WriteLine("ROTO[0]");
//Console.WriteLine(this.roto[0]);
// and finally build all the rototranslation from system(i)
// to reference system
for (int i = 1; i < len; i++)
{
this.roto[i] = this.composeRototrans(beta[i], this.roto[0]);
this.inverse[i] = (Matrix)this.roto[i].Inverse();
//Console.WriteLine("ROTO[" + i + "]");
//Console.WriteLine(this.roto[i]);
}
}
private Matrix composeRototrans(Matrix a, Matrix b)
{
var result = a.Multiply(b);
//cleaning prospective components
result[0, 2] = 0.0;
result[1, 2] = 0.0;
return (Matrix)result;
}
А затем я хочу изобразить на графике новую вершину:
for ( int i = 1 ; i < radarList.Count ; i++ )
{
if ( radarList[i].ConfiguredFromFile == true )
{
double rX = 0, rY = 0;
this.roto.Rototranslate( i , 0 , 0 , out rX , out rY );
var p = radarList[i];
double x, y;
DataPoint dataPoint1;
DataPoint dataPoint2=new DataPoint( 1D , 0D );
//this.roto.Rototranslate( i , p.SafetyArea.PointA.X , p.SafetyArea.PointA.Y , out x , out y );
dataPoint1 = new DataPoint( rX , new double[] { rY , 0 } );
dataPoint1.BorderWidth = 0;
dataPoint1.IsValueShownAsLabel = false;
dataPoint1.LabelBorderDashStyle = System.Windows.Forms.DataVisualization.Charting.ChartDashStyle.NotSet;
dataPoint1.LabelBorderWidth = 0;
dataPoint1.MarkerBorderWidth = 0;
dataPoint1.MarkerSize = 15;
dataPoint2.MarkerSize = 0;
listRadarSeries[i].Points.Add( dataPoint1 );
//listRadarSeries[i].Points.Add( dataPoint2 );
this.roto.Rototranslate( i , p.SafetyArea.PointA.X , p.SafetyArea.PointA.Y , out x , out y );
p.RotoSafetyArea.PointA.X=x;
p.RotoSafetyArea.PointA.Y = y;
DataPoint dpA = new DataPoint( x , new double[] { y , 5 } );
listRadarSeries[i].Points.Add( dpA );
this.roto.Rototranslate( i , p.SafetyArea.PointB.X , p.SafetyArea.PointB.Y , out x , out y );
p.RotoSafetyArea.PointB.X = x;
p.RotoSafetyArea.PointB.Y = y;
DataPoint dpB = new DataPoint( x , new double[] { y , 5 } );
listRadarSeries[i].Points.Add( dpB );
this.roto.Rototranslate( i , p.SafetyArea.PointC.X , p.SafetyArea.PointC.Y , out x , out y );
p.RotoSafetyArea.PointC.X = x;
p.RotoSafetyArea.PointC.Y = y;
DataPoint dpC = new DataPoint( x , new double[] { y , 5 } );
listRadarSeries[i].Points.Add( dpC );
this.roto.Rototranslate( i , p.SafetyArea.PointD.X , p.SafetyArea.PointD.Y , out x , out y );
p.RotoSafetyArea.PointD.X = x;
p.RotoSafetyArea.PointD.Y = y;
DataPoint dpD = new DataPoint( x , new double[] { y , 5 } );
listRadarSeries[i].Points.Add( dpD );
}
}
Функция rototranlsate:
public void Rototranslate(int systemIdx, double x, double y,
out double xRotot, out double yRotot)
{
var coordInSource = DenseMatrix.OfArray( new [,] { {x, y, 1.0} });
var rototranslatedPoint = coordInSource.Multiply(this.roto[systemIdx]);
xRotot = rototranslatedPoint[0,0];
yRotot = rototranslatedPoint[0,1];
}
Но в конце я получаю что-то не так
Есть ли у кого-нибудь представление о том, что не так в коде? Странно то, что это код, используемый в проекте, где он отлично работает (по крайней мере, я помню)
Спасибо