private void OnMouseDown(object sender, MouseButtonEventArgs mouseButtonEventArgs)
{
if (Mouse.RightButton == MouseButtonState.Pressed)
{
MainWindowViewModel viewModel = (MainWindowViewModel)DataContext;
var mousePosition = mouseButtonEventArgs.GetPosition(this);
Vector <double> mouseVec = _vecBuilder.DenseOfArray(new[] { mousePosition.X, mousePosition.Y, 1.0f });
var featureBands = viewModel.Layers.Where(b => b.UseFeature).OrderBy(b => b.Name).ToList();
ushort[] bandPixels = new ushort[featureBands.Count];
var viewToToWorld = viewModel.ClassifierViewModel.ScreenToWorld * _scaleMat.Inverse() * _screenToViewMat.Inverse();
for (int bandIndex = 0; bandIndex < featureBands.Count; ++bandIndex)
{
var band = featureBands[bandIndex];
var viewToPixelMat = band.WorldToImage * viewToToWorld;
var bandPixelPosition = viewToPixelMat * mouseVec;
ushort bandPixelValue = band.BandImage.GetScaledToUshort((int)bandPixelPosition[0], (int)bandPixelPosition[1]);
bandPixels[bandIndex] = bandPixelValue;
}
var worldMousePosition = viewToToWorld * mouseVec;
int selectedIndex = viewModel.LandcoverTypes.Values.ToList().FindIndex(f => f.Id == viewModel.SelectedLandCoverTypeViewModel.Id);
var classifiedFeatureVector = new ClassifiedFeatureVector(selectedIndex, new FeatureVector(bandPixels), new Point(worldMousePosition[0], worldMousePosition[1]));
viewModel.ClassifierViewModel.FeaturesViewModel.AddFeature(new ClassifiedFeatureVectorViewModel(classifiedFeatureVector));
}
}
public void Update(params double[] z)
{
var z_ = CheckZ(z);
var zv = Vbuilder.DenseOfArray(z_);
FilterSharp.Update(ref x, ref P, zv, h, R);
}
public static PointD[] Straight(double[][] initPoints, CameraParameters param)
{
int N = initPoints.Length;
MathNet.Numerics.LinearAlgebra.Vector <double> C = param.Pos;
Matrix <double> R = param.R;
R = M.DenseOfDiagonalArray(new double[] { 1, -1, -1 }) * R;
MathNet.Numerics.LinearAlgebra.Vector <double>[] Apoints = new MathNet.Numerics.LinearAlgebra.Vector <double> [N];
for (int i = 0; i < N; i++)
{
Apoints[i] = V.DenseOfArray(initPoints[i]);
}
MathNet.Numerics.LinearAlgebra.Vector <double>[] Cpoints = new MathNet.Numerics.LinearAlgebra.Vector <double> [N];
for (int i = 0; i < N; i++)
{
Cpoints[i] = R * (Apoints[i] - C);
}
PointD[] Bpoints = new PointD[N];
for (int i = 0; i < N; i++)
{
Bpoints[i] = new PointD(Cpoints[i][0] / Cpoints[i][2], Cpoints[i][1] / Cpoints[i][2]);
}
return(Bpoints);
}
/// <summary>
/// 正規化した座標のリストを作成
/// </summary>
/// <param name="tail"></param>
private void NormalizePoints(int listnum)
{
VectorBuilder <double> Vector = Vector <double> .Build;
MatrixBuilder <double> Matrix = Matrix <double> .Build;
for (int i = 0; i < listnum; i++)
{
//体が常に横を向くように正規化
//HipRightからHipLeftへのびるXZ平面ベクトルに対して垂直なXZ平面ベクトルを計算
double[] hip = new double[3] {
-(kinectPoints[12][i][2] - kinectPoints[16][i][2]),
0,
kinectPoints[12][i][0] - kinectPoints[16][i][0]
};
var VecHip = Vector.DenseOfArray(hip);
//求めたベクトルの絶対値を1に
var e_x = VecHip.Divide((float)VecHip.L2Norm());
//e_y = (0,1,0)
var e_y = Vector.DenseOfArray(new double[3] {
0, 1, 0
});
//e_z = e_x × e_y
var e_z = Vector.DenseOfArray(new double[3] {
-e_x.At(2), 0, e_x.At(0)
});
//e_x,e_y,e_zを一つの行列に
var mat = Matrix.DenseOfColumnVectors(new Vector <double>[] { e_x, e_y, e_z }).Inverse();
double[] norm = new double[3];//正規化した座標を格納する三次元座標
for (int j = 0; j < 21; j++)
{
var VecJ = Vector.DenseOfArray(new double[3] {
kinectPoints[j][i][0] - kinectPoints[0][i][0],
kinectPoints[j][i][1] - kinectPoints[0][i][1],
kinectPoints[j][i][2] - kinectPoints[0][i][2]
});
var E = mat * VecJ;
for (int k = 0; k < 3; k++)
{
norm[k] = E.At(k);
}
kinectNormalizedPoints[j].Add((double[])norm.Clone());
}
//頭の高さを1として正規化
double[] head = kinectNormalizedPoints[3][i];
double headVec = Math.Sqrt(head[0] * head[0] + head[1] * head[1] + head[2] * head[2]);
for (int j = 0; j < 21; j++)//jointnum:3 = head
{
double[] points = kinectNormalizedPoints[j][i];
double Vec = Math.Sqrt(points[0] * points[0] + points[1] * points[1] + points[2] * points[2]);
Vec /= headVec;
//kinectNormalizedPoints[j][i][0] *= Vec;
}
}
}
private void OnMove(object sender, MouseEventArgs args)
{
MainWindowViewModel viewModel = (MainWindowViewModel)DataContext;
if (viewModel == null)
{
return;
}
var position = args.GetPosition(this);
var posVec = _vecBuilder.DenseOfArray(new[] { position.X, position.Y, 1 });
var featureBands = viewModel.Layers.Where(b => b.UseFeature).ToList();
var indices = viewModel.Layers.Where(b => b.UseFeature).Select((b, i) => i);
ushort[] bandIntensities = new ushort[featureBands.Count];
foreach (int bandIndex in indices)
{
var band = featureBands[bandIndex];
var viewToPixelMat = band.WorldToImage * viewModel.ClassifierViewModel.ScreenToWorld * _scaleMat.Inverse() * _screenToViewMat.Inverse();
var bandPixelPosition = viewToPixelMat * posVec;
ushort bandIntensity = band.BandImage.GetScaledToUshort((int)bandPixelPosition[0], (int)bandPixelPosition[1]);
bandIntensities[bandIndex] = bandIntensity;
}
if (viewModel.ClassifierViewModel.IsTrained)
{
var featureVector = new FeatureVector(bandIntensities);
var predictedType = viewModel.ClassifierViewModel.CurrentClassifierViewModel.Predict(featureVector);
viewModel.PredictionViewModel.MousePredictionTypeViewModel = viewModel.LandcoverTypes.Values.ToList()[predictedType];
}
}
// Use this for initialization
void Start()
{
/*SparseMatrix A = SparseMatrix.OfRowArrays(new double[7][]
* {
* new double[] {0, -1, -1, -1, -1, -1, -1},
* new double[] {-1, 1, 0, 0, 0, 0, 0},
* new double[] {-1, 0, 1, 0, 0, 0, 0},
* new double[] {-1, 0, 0, 1, 0, 0, 0},
* new double[] {-1, 0, 0, 0, 1, 0, 0},
* new double[] {-1, 0, 0, 0, 0, 1, 0},
* new double[] {-1, 0, 1, 0, 0, 0, 1}
* });*/
Func <int, int, double> init = cValues;
SparseMatrix A = SparseMatrix.Create(7, 7, init);
double lambda = -993f / (6f * 0.01f);
lambda *= 1f - 5f / 7f;
double nDefault = 5f;
VectorBuilder <double> Builder = Vector <double> .Build;
Vector <double> B = Builder.DenseOfArray(new double[7]
{
0,
lambda,
lambda,
lambda,
lambda,
lambda,
lambda,
});
VectorBuilder <double> resultB = Vector <double> .Build;
Vector <double> result = resultB.Dense(7);
A.Solve(B, result);
Debug.Log("Matrix A is : " + A.ToString());
Debug.Log("Matrix B is : " + B.ToString());
Debug.Log("Lambda is : " + lambda);
Debug.Log("The result vector3 is : " + result.ToString());
}
public static void Main()
{
// Write a message to the Console
Console.WriteLine("Welcome to Bridge.NET");
Vector <double> k = v_builder.DenseOfArray(new[] { 1.0, 0.0, 0.0 });
Matrix <double> I = m_builder.DenseIdentity(3);
Matrix <double> khat = Hat(k);
Matrix <double> khat2 = khat.Multiply(khat);
Console.WriteLine(I + khat + khat2);
// After building (Ctrl + Shift + B) this project,
// browse to the /bin/Debug or /bin/Release folder.
// A new bridge/ folder has been created and
// contains your projects JavaScript files.
// Open the bridge/index.html file in a browser by
// Right-Click > Open With..., then choose a
// web browser from the list
// This application will then run in the browser.
}
static void Main(string[] args)
{
//初始化一个矩阵和向量的构建对象
var mb = Matrix <double> .Build;
var mb1 = Matrix <double> .Build;
var vb = Vector <double> .Build;
DateTime time = new DateTime(2005, 12, 05);
double[] AiC = { 1, 2, 3 };
double[,] AiC1 = { { 1, 2, 3 }, { 1, 2, 3 }, { 1, 2, 3 } };
double[] AiR = new double[3];
DenseMatrix AiMatrix = DenseMatrix.OfArray(new[, ] {
{ 1.0, 2.0, 3.0 }, { 4.0, 5.0, 6.0 }, { 7.0, 8.0, 9.0 }
});; //创建一个对角矩阵对象
VectorBuilder <double> vector = Vector <double> .Build;
var t = vector.DenseOfArray(AiC);
//var c = AiMatrix.DenseOfArray(AiC1);
var r = t * AiMatrix;
var matrixA = DenseMatrix.OfArray(new[, ] {
{ 1.0, 2.0, 3.0 }, { 4.0, 5.0, 6.0 }, { 7.0, 8.0, 9.0 }
});
var matrixB = DenseMatrix.OfArray(new[, ] {
{ 1.0, 3.0, 5.0 }, { 2.0, 4.0, 6.0 }, { 3.0, 5.0, 7.0 }
});
var s = matrixA * matrixB;
string str = "20100531";
DateTime dtime = DateTime.ParseExact(str, "yyyyMMdd", null);
DateTime dtime1 = DateTime.Now;
Dictionary <int, double> dic = new Dictionary <int, double>();
dic.Add(0, 1);
dic.Add(1, 2);
dic.Add(2, 3);
dic.Add(3, 4);
//var matrix2 = mb.Dense(2, 3, (i, j) => 3 * i + j);
var diagMaxtrix = mb.DenseDiagonal(4, 4, (i) => i);
//先生成数据集合
var chiSquare = new ChiSquared(5);
Console.WriteLine(@"2. Generate 1000 samples of the ChiSquare(5) distribution");
var data = new double[1000];
//var data1 = new double[1000];
//for (var i = 0; i < data.Length; i++)
//{
// data[i] = chiSquare.Sample();
//}
List <double> a = new List <double>();
List <double> b = new List <double>();
//data1 = new double []{1,2,3,4,5};
data[0] = 10;
data[1] = 0;
data[2] = 0;
data[3] = 0;
data[4] = 0;
data[5] = 0;
data[6] = 0;
data[7] = 0;
data[8] = 0;
data[9] = -0.1;
var data1 = new double[] { 1, 2, 3 };
var data2 = new double[] { 1, 2, 6 };
data1.Mean();
data1.Variance();
data2.Mean();
var ss = data1.Covariance(data2);
//使用扩展方法进行相关计算
Console.WriteLine(@"3.使用扩展方法获取生成数据的基本统计结果");
Console.WriteLine(@"{0} - 最大值", data.Maximum().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 最小值", data.Minimum().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 均值", data.Mean().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 中间值", data.Median().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 有偏方差", data.PopulationVariance().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 无偏方差", data.Variance().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 标准偏差", data.StandardDeviation().ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - 标准有偏偏差", data.PopulationStandardDeviation().ToString(" #0.00000;-#0.00000"));
Console.WriteLine();
Console.WriteLine(@"4. 使用DescriptiveStatistics类进行基本的统计计算");
var descriptiveStatistics = new DescriptiveStatistics(data);//使用数据进行类型的初始化
//直接使用属性获取结果
Console.WriteLine(@"{0} - Kurtosis", descriptiveStatistics.Kurtosis.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Largest element", descriptiveStatistics.Maximum.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Smallest element", descriptiveStatistics.Minimum.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Mean", descriptiveStatistics.Mean.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Variance", descriptiveStatistics.Variance.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Standard deviation", descriptiveStatistics.StandardDeviation.ToString(" #0.00000;-#0.00000"));
Console.WriteLine(@"{0} - Skewness", descriptiveStatistics.Skewness.ToString(" #0.00000;-#0.00000"));
Console.WriteLine();
}
public static Vector <double> Invhat(Matrix <double> khat)
{
double[] inv = { (-khat[1, 2] + khat[2, 1]), (khat[0, 2] - khat[2, 0]), (-khat[0, 1] + khat[1, 0]) };
Vector <double> output = v_builder.DenseOfArray(inv);
output /= 2;
return(output);
}
public double Normalize(int joint_index, double theta)
{
double u = Robot.Joint_upper_limit[joint_index];
double l = Robot.Joint_lower_limit[joint_index];
if (check_limits)
{
if (!(l < theta && theta < u))
{
Vector <double> a = v_builder.DenseOfArray(new[] { -1.0, 1.0 });
a = a * 2 * Math.PI;
Vector <double> b = a.Add(theta);
int index = -1;
for (int i = 0; i < b.Count; i++)
{
if (l < b[i] && b[i] < u)
{
index = i;
break;
}
}
if (index == -1)
{
return(default(double));
}
else
{
theta = theta + a[index];
}
}
}
if (use_last_joints)
{
double diff = Last_joints[joint_index] - theta;
int n_diff = (int)Math.Floor(diff / (Math.PI * 2.0));
double r_diff = diff % (Math.PI * 2.0);
if (r_diff > Math.PI)
{
n_diff += 1;
}
if (Math.Abs(n_diff) > 0)
{
if (!check_limits)
{
theta += 2 * Math.PI * n_diff;
}
else
{
double theta_vs = theta + 2 * Math.PI;
Vector <double> theta_v = v_builder.Dense(Math.Abs(n_diff));
for (int i = 0; i < theta_v.Count; i++)
{
if (n_diff > 0)
{
theta_v[i] = n_diff - i;
}
else if (n_diff < 0)
{
theta_v[i] = n_diff + i;
}
}
theta_v = theta_v * 2 * Math.PI;
theta_v = theta_v.Add(theta);
int index = -1;
for (int i = 0; i < theta_v.Count; i++)
{
if (l < theta_v[i] && theta_v[i] < u)
{
index = i;
break;
}
}
if (index != -1)
{
theta = theta_v[index];
}
}
}
}
return(theta);
}
public static double[][] robot6_sphericalwrist_invkin(Robot robot, Transform desired_pose, double[] last_joints = default(double[]))
{
if (robot.R_tool != default(Matrix <double>) && robot.P_tool != default(Vector <double>))
{
Matrix <double> transposed = robot.R_tool.Transpose();
desired_pose.R = desired_pose.R.Multiply(transposed);
desired_pose.P = desired_pose.P - desired_pose.R * robot.P_tool;
}
Matrix <double> H = robot.H.Clone();
Matrix <double> P = robot.P.Clone();
List <double[]> theta_v = new List <double[]>();
Vector <double> zeros = v_builder.DenseOfArray(new[] { 0.0, 0.0, 0.0 });
Vector <double> ex = v_builder.DenseOfArray(new[] { 1.0, 0.0, 0.0 });
Vector <double> ey = v_builder.DenseOfArray(new[] { 0.0, 1.0, 0.0 });
Vector <double> ez = v_builder.DenseOfArray(new[] { 0.0, 0.0, 1.0 });
if (!P.Column(4).Equals(zeros))
{
double P4_d = P.Column(4) * H.Column(3);
if (!(P.Column(4).Subtract(P4_d * H.Column(3)).Equals(zeros)))
{
throw new ArgumentException(String.Format("Robot may not have spherical wrist"));
}
P.SetColumn(3, P.Column(3) + P.Column(4));
P.SetColumn(4, zeros);
}
if (!P.Column(5).Equals(zeros))
{
double P5_d = P.Column(5) * H.Column(5);
if (!(P.Column(5).Subtract(P5_d * H.Column(5)).Equals(zeros)))
{
throw new ArgumentException(String.Format("Robot may not have spherical wrist"));
}
P.SetColumn(6, P.Column(6) + P.Column(5));
P.SetColumn(5, zeros);
}
double d1 = ey * (P.Column(1).Add(P.Column(2)).Add(P.Column(3)));
Vector <double> v1 = desired_pose.P.Subtract(desired_pose.R * P.Column(6));
double[] Q1 = GeneralRoboticsToolbox.Subproblem4(ey, v1, -H.Column(0), d1);
NormalizeJoints normalize = new NormalizeJoints(robot, last_joints);
double[] first_normalize = normalize.FindNormalizedJoints(0, Q1);
foreach (double q1 in first_normalize)
{
Matrix <double> R01 = GeneralRoboticsToolbox.Rot(H.Column(0), q1);
Vector <double> p26_q1 = R01.TransposeThisAndMultiply(desired_pose.P - desired_pose.R * P.Column(6)).Subtract(P.Column(0).Add(P.Column(1)));
double d3 = p26_q1.L2Norm();
Vector <double> v3 = P.Column(2);
Vector <double> p3 = P.Column(3);
double[] Q3 = GeneralRoboticsToolbox.Subproblem3(p3, v3, H.Column(2), d3);
double[] second_normalize = normalize.FindNormalizedJoints(2, Q3);
foreach (double q3 in second_normalize)
{
Matrix <double> R23 = GeneralRoboticsToolbox.Rot(H.Column(2), q3);
Vector <double> v2 = p26_q1;
Vector <double> p2 = P.Column(2) + R23 * P.Column(3);
double q2 = GeneralRoboticsToolbox.Subproblem1(p2, v2, H.Column(1));
double[] q2_array = normalize.FindNormalizedJoints(1, new[] { q2 });
if (q2_array.Length == 0)
{
continue;
}
q2 = q2_array[0];
Matrix <double> R12 = GeneralRoboticsToolbox.Rot(H.Column(1), q2);
Matrix <double> R03 = R01 * R12 * R23;
Matrix <double> R36 = R03.Transpose() * desired_pose.R;
Vector <double> v4 = R36 * H.Column(5);
Vector <double> p4 = H.Column(5);
double[] Q4_Q5 = GeneralRoboticsToolbox.Subproblem2(p4, v4, H.Column(3), H.Column(4));
double[][] third_normalize = normalize.FindNormalizedJoints(new[] { 3, 4 }, Q4_Q5);
Console.WriteLine("size of qs {0}, {1}", third_normalize[0].Length, third_normalize[1].Length);
int minoftwo = Math.Min(third_normalize[0].Length, third_normalize[1].Length);
Console.WriteLine(minoftwo);
for (int q = 0; q < minoftwo; q++)
{
Matrix <double> R35 = GeneralRoboticsToolbox.Rot(H.Column(3), third_normalize[0][q]) * GeneralRoboticsToolbox.Rot(H.Column(4), third_normalize[1][q]);
Matrix <double> R05 = R03 * R35;
Matrix <double> R56 = R05.Transpose() * desired_pose.R;
Vector <double> v6 = R56 * H.Column(4);
Vector <double> p6 = H.Column(4);
double q6 = GeneralRoboticsToolbox.Subproblem1(p6, v6, H.Column(5));
double[] q6_array = normalize.FindNormalizedJoints(5, new[] { q6 });
if (q6_array.Length == 0)
{
continue;
}
q6 = q6_array[0];
double[] theta_v_entry = new double[] { q1, q2, q3, third_normalize[0][q], third_normalize[1][q], q6 };
theta_v.Add(theta_v_entry);
}
}
}
if (last_joints != default(double[]))
{
double[] theta_dist_arr = new double[theta_v.Count];
for (int i = 0; i < theta_v.Count; i++)
{
Vector <double> theta_v_vec = v_builder.DenseOfArray(theta_v[i]);
Vector <double> theta_dist_vec = theta_v_vec.Subtract(last_joints[i]);
theta_dist_arr[i] = theta_dist_vec.L2Norm();
}
int index = 0;
List <double> theta_dist_list = theta_dist_arr.ToList();
List <double[]> returned = new List <double[]>();
for (int z = 0; z < theta_dist_list.Count; z++)
{
index = theta_dist_list.IndexOf(theta_dist_list.Min());
returned[z] = theta_v[index];
theta_dist_list.RemoveAt(index);
}
return(returned.ToArray());
}
else
{
return(theta_v.ToArray());
}
}
static void TestHat()
{
//Test Hat
Console.WriteLine("Testing Hat...");
Vector <double> k = v_builder.DenseOfArray(new[] { 1.0, 2.0, 3.0 });
Matrix <double> khat = m_builder.Dense(3, 3);
khat[0, 1] = -3;
khat[0, 2] = 2;
khat[1, 0] = 3;
khat[1, 2] = -1;
khat[2, 0] = -2;
khat[2, 1] = 1;
Matrix <double> k_hat = GeneralRoboticsToolbox.Hat(k);
if (!AlmostEqualsMatrix(k_hat, khat))
{
Console.WriteLine("hat failed");
}
else
{
Console.WriteLine("hat succeeded");
}
}
public static Vector <double> Quaternion(this VectorBuilder <double> b, double w, double x, double y, double z)
{
return(b.DenseOfArray(new double[] { w, x, y, z }));
}
public static Vector <double> Dense3(this VectorBuilder <double> b, double x, double y, double z)
{
return(b.DenseOfArray(new double[] { x, y, z }));
}
