/*
/// <summary>
/// Create a LevMarqSparse solver
/// </summary>
public LevMarqSparse()
{
_ptr = CvInvoke.CvCreateLevMarqSparse();
}*/
/// <summary>
/// Useful function to do simple bundle adjustment tasks
/// </summary>
/// <param name="points">Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment</param>
/// <param name="imagePoints">Projections of 3d points for every camera</param>
/// <param name="visibility">Visibility of 3d points for every camera</param>
/// <param name="cameraMatrix">Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="R">rotation matrices of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="T">translation vector of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="distCoeffcients">distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment</param>
/// <param name="termCrit">Termination criteria, a reasonable value will be (30, 1.0e-12) </param>
public static void BundleAdjust(
MCvPoint3D64f[] points, MCvPoint2D64f[][] imagePoints, int[][] visibility,
Matrix<double>[] cameraMatrix, Matrix<double>[] R, Matrix<double>[] T, Matrix<double>[] distCoeffcients, MCvTermCriteria termCrit)
{
using (Matrix<double> imagePointsMat = CvToolbox.GetMatrixFromPoints(imagePoints))
using (Matrix<int> visibilityMat = CvToolbox.GetMatrixFromArrays(visibility))
using (VectorOfMat cameraMatVec = new VectorOfMat())
using (VectorOfMat rMatVec = new VectorOfMat())
using (VectorOfMat tMatVec = new VectorOfMat())
using (VectorOfMat distorMatVec = new VectorOfMat())
{
cameraMatVec.Push(cameraMatrix);
rMatVec.Push(R);
tMatVec.Push(T);
distorMatVec.Push(distCoeffcients);
GCHandle handlePoints = GCHandle.Alloc(points, GCHandleType.Pinned);
CvInvoke.CvLevMarqSparseAdjustBundle(
cameraMatrix.Length,
points.Length, handlePoints.AddrOfPinnedObject(),
imagePointsMat, visibilityMat, cameraMatVec, rMatVec, tMatVec, distorMatVec, ref termCrit);
handlePoints.Free();
}
}
public static double CalculateAngle3D(MCvPoint3D64f p1, MCvPoint3D64f p2, MCvPoint3D64f norm)
{
var ip = GetDistance(p1, p2);
var cat1 = GetDistance(p1, norm);
var cat2 = GetDistance(p2, norm);
return CalculateAngle2D(new PointF(0, 0), new PointF((float)cat1, (float)cat2));
}
public void FilterAccMagnet(ref MCvPoint3D64f newAcc, ref MCvPoint3D64f newMagnet, double newCoeff)
{
double oldCoeff = 1 - newCoeff;
if (this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
{
return;
}
MCvPoint3D64f ta = new MCvPoint3D64f((this.OldAMGFusedAcc.Value.x * oldCoeff + newAcc.x * newCoeff), (this.OldAMGFusedAcc.Value.y * oldCoeff + newAcc.y * newCoeff), (this.OldAMGFusedAcc.Value.z * oldCoeff + newAcc.z * newCoeff));
MCvPoint3D64f tm = new MCvPoint3D64f((this.OldAMGFusedMagnet.Value.x * oldCoeff + newMagnet.x * newCoeff), (this.OldAMGFusedMagnet.Value.y * oldCoeff + newMagnet.y * newCoeff), (this.OldAMGFusedMagnet.Value.z * oldCoeff + newMagnet.z * newCoeff));
newAcc = ta;
newMagnet = tm;
}
public static Matrix<double> MCvPoint3D64fToMatrix(MCvPoint3D64f point, bool vertical = false)
{
if (vertical)
{
return new Matrix<double>(new double[,]
{
{point.x},
{point.y},
{point.z}
});
}
return new Matrix<double>(new double[,]
{
{point.x, point.y, point.z}
});
}
/// <summary>
/// Convert geodetic coordinate to ECEF coordinate
/// </summary>
/// <param name="coordinate">the geodetic coordinate</param>
/// <returns>The ECEF coordinate</returns>
public static MCvPoint3D64f Geodetic2ECEF(GeodeticCoordinate coordinate)
{
#if PINVOKE
MCvPoint3D64f res = new MCvPoint3D64f();
transformGeodetic2ECEF(ref coordinate, ref res);
return res;
#else
double sinPhi = Math.Sin(coordinate.Latitude);
double N = A / Math.Sqrt(1.0 - E * E * sinPhi * sinPhi);
double tmp1 = (N + coordinate.Altitude) * Math.Cos(coordinate.Latitude);
return new MCvPoint3D64f(
tmp1 * Math.Cos(coordinate.Longitude),
tmp1 * Math.Sin(coordinate.Longitude),
((B * B) / (A * A) * N + coordinate.Altitude) * sinPhi);
#endif
}
private extern static void transformGeodetic2ECEF(IntPtr datum, ref GeodeticCoordinate coordinate, ref MCvPoint3D64f ecef);
public MCvPoint3D64f GetNormalizedPoint(MCvPoint3D64f point)
{
var norm = this.GetPointNorm(point);
var res = new MCvPoint3D64f(point.x / norm, point.y / norm, point.z / norm);
return res;
}
public void TestQuaternion2()
{
Random r = new Random();
Quaternions q1 = new Quaternions();
q1.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
Quaternions q2 = new Quaternions();
q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);
MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
double epsilon = 1.0e-8;
Assert.Less(delta.x, epsilon);
Assert.Less(delta.y, epsilon);
Assert.Less(delta.z, epsilon);
}
/// <summary>
/// Check if the specific point is in the Cuboid
/// </summary>
/// <param name="point">The point to be checked</param>
/// <returns>True if the point is in the cuboid</returns>
public bool Contains(MCvPoint3D64f point)
{
return point.x >= Min.x && point.y >= Min.y && point.z >= Min.z
&& point.x <= Max.x && point.y <= Max.y && point.z <= Max.z;
}
/// <summary>
/// Check if the specific point is in the Cuboid
/// </summary>
/// <param name="point">The point to be checked</param>
/// <returns>True if the point is in the cuboid</returns>
public bool Contains(MCvPoint3D64f point)
{
return(point.X >= Min.X && point.Y >= Min.Y && point.Z >= Min.Z &&
point.X <= Max.X && point.Y <= Max.Y && point.Z <= Max.Z);
}
public static double GetDistanceToPoint(MCvPoint3D64f origin, MCvPoint3D64f destination)
{
return GetPoint3dMagnitude(destination - origin);
}
public Matrix<double> MCvPoint3D64fToMatrix(MCvPoint3D64f point)
{
return new Matrix<double>(new double[,]
{
{point.x, point.y, point.z}
});
}
private extern static void transformECEF2Geodetic(IntPtr datum, ref MCvPoint3D64f ecef, ref GeodeticCoordinate coordinate);
public void GetOrthoNormalAccMagnetBasis(MCvPoint3D64f accPoint, MCvPoint3D64f magnetPoint, out MCvPoint3D64f x, out MCvPoint3D64f y, out MCvPoint3D64f z)
{
var accMagnetCross = accPoint.CrossProduct(magnetPoint);
var magnetNormal = accMagnetCross.CrossProduct(accPoint);
var aNorm = this.GetNormalizedPoint(accPoint);
var mNorm = this.GetNormalizedPoint(magnetNormal);
var amNorm = this.GetNormalizedPoint(accMagnetCross);
x = this.GetPointNorm(mNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(1, 0, 0) : mNorm;
//y = aNorm.Norm.CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 1, 0) : new MCvPoint3D64f(-aNorm.x, -aNorm.y, -aNorm.z);
y = this.GetPointNorm(aNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 1, 0) : aNorm;
z = this.GetPointNorm(amNorm).CompareTo(double.NaN) == 0 ? new MCvPoint3D64f(0, 0, 1) : amNorm;
}
/// <summary>
/// Convert ECEF coordinate to geodetic coordinate
/// </summary>
/// <param name="ecef">The ecef coordinate</param>
/// <returns>The geodetic coordinate</returns>
public GeodeticCoordinate ECEF2Geodetic(MCvPoint3D64f ecef)
{
GeodeticCoordinate res = new GeodeticCoordinate();
transformECEF2Geodetic(_ptr, ref ecef, ref res);
return res;
}
/// <summary>
/// Convert geodetic coordinate to ECEF coordinate
/// </summary>
/// <param name="coordinate">the geodetic coordinate</param>
/// <returns>The ECEF coordinate</returns>
public MCvPoint3D64f Geodetic2ECEF(GeodeticCoordinate coordinate)
{
MCvPoint3D64f res = new MCvPoint3D64f();
transformGeodetic2ECEF(_ptr, ref coordinate, ref res);
return res;
}
private extern static void transformNED2Geodetic(IntPtr datum, ref MCvPoint3D64f ned, ref GeodeticCoordinate refCoor, ref MCvPoint3D64f refEcef, ref GeodeticCoordinate coor);
private extern static void transformGeodetic2ENU(IntPtr datum, ref GeodeticCoordinate coor, ref GeodeticCoordinate refCoor, ref MCvPoint3D64f refEcef, ref MCvPoint3D64f enu);
public double GetPointNorm(MCvPoint3D64f point)
{
return Math.Sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
}
public static double GetDistance(MCvPoint3D64f p1, MCvPoint3D64f p2)
{
return Math.Sqrt(Math.Pow(p1.x - p2.x, 2) + Math.Pow(p1.y - p2.y, 2) + Math.Pow(p1.z - p2.z, 2));
}
public MCvPoint3D64f ScalePoint(MCvPoint3D64f point, double scale)
{
return new MCvPoint3D64f(point.x * scale, point.y * scale, point.z * scale);
}
private void RenderTranslatoin(MCvPoint3D64f position)
{
double xScale = this.GetTranslationRenderCoeffX();
double yScale = this.GetTranslationRenderCoeffY();
Bitmap bmp = new Bitmap(1000, 1000);
double xOffset = bmp.Width / 2;
double yOffset = bmp.Height / 2;
var pointSize = new Size(bmp.Width / 100, bmp.Height / 100);
var g = Graphics.FromImage(bmp);
g.Clear(Color.White);
g.DrawRectangle(Pens.Red, new Rectangle(new Point((int)(position.x / xScale + xOffset), (int)(position.z / yScale + yOffset)), pointSize));
this.videoForm.RenderToStuffPictureBox2(bmp);
}
private void ProcessFrameFindFaces()
{
if (Options.StereoCalibrationOptions == null)
{
return;
}
var leftImageR = new Image<Gray, byte>(new Size(_cameras[0].Image.Width, _cameras[0].Image.Height));
var rightImageR = new Image<Gray, byte>(new Size(_cameras[1].Image.Width, _cameras[1].Image.Height));
CvInvoke.cvRemap(_cameras[0].Image.Ptr, leftImageR.Ptr,
Options.StereoCalibrationOptions.MapXLeft, Options.StereoCalibrationOptions.MapYLeft, 0, new MCvScalar(0));
CvInvoke.cvRemap(_cameras[1].Image.Ptr, rightImageR.Ptr,
Options.StereoCalibrationOptions.MapXRight, Options.StereoCalibrationOptions.MapYRight, 0, new MCvScalar(0));
//PointCollection.ReprojectImageTo3D()
// find first face points
var leftFaceRegions = Helper2D.GetFaceRegion2Ds(leftImageR, FaceWidth, FaceHeight, true, true);
var rightFaceRegions = Helper2D.GetFaceRegion2Ds(rightImageR, FaceWidth, FaceHeight, true, true);
FaceRegion2D leftFace;
FaceRegion2D rightFace;
if (leftFaceRegions != null
&& rightFaceRegions != null
&& (leftFace = leftFaceRegions.FirstOrDefault()) != null
&& (rightFace = rightFaceRegions.FirstOrDefault()) != null)
{
var leftPoints = new List<Point>();
var rightPoints = new List<Point>();
#region Points
// face
leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Face.Width / 2, leftFace.Face.Location.Y + leftFace.Face.Height / 2));
rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Face.Width / 2, rightFace.Face.Location.Y + rightFace.Face.Height / 2));
// left eye
if (leftFace.LeftEye != null && rightFace.LeftEye != null)
{
leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.LeftEye.Location.X + leftFace.LeftEye.Width / 2,
leftFace.Face.Location.Y + leftFace.LeftEye.Location.Y + leftFace.LeftEye.Height / 2));
rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.LeftEye.Location.X + rightFace.LeftEye.Width / 2,
rightFace.Face.Location.Y + rightFace.LeftEye.Location.Y + rightFace.LeftEye.Height / 2));
}
// right eye
if (leftFace.RightEye != null && rightFace.RightEye != null)
{
leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.RightEye.Location.X + leftFace.RightEye.Width / 2,
leftFace.Face.Location.Y + leftFace.RightEye.Location.Y + leftFace.RightEye.Height / 2));
rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.RightEye.Location.X + rightFace.RightEye.Width / 2,
rightFace.Face.Location.Y + rightFace.RightEye.Location.Y + rightFace.RightEye.Height / 2));
}
// mouth
if (leftFace.Mouth != null && rightFace.Mouth != null)
{
leftPoints.Add(new Point(leftFace.Face.Location.X + leftFace.Mouth.Location.X + leftFace.Mouth.Width / 2,
leftFace.Face.Location.Y + leftFace.Mouth.Location.Y + leftFace.Mouth.Height / 2));
rightPoints.Add(new Point(rightFace.Face.Location.X + rightFace.Mouth.Location.X + rightFace.Mouth.Width / 2,
rightFace.Face.Location.Y + rightFace.Mouth.Location.Y + rightFace.Mouth.Height / 2));
}
#endregion
var pointCloud = new MCvPoint3D64f[leftPoints.Count];
#region Calculate Point Cloud
for (int i = 0; i < leftPoints.Count; i++)
{
var d = rightPoints[i].X - leftPoints[i].X;
var X = leftPoints[i].X * Options.StereoCalibrationOptions.Q[0, 0] + Options.StereoCalibrationOptions.Q[0, 3];
var Y = leftPoints[i].Y * Options.StereoCalibrationOptions.Q[1, 1] + Options.StereoCalibrationOptions.Q[1, 3];
var Z = Options.StereoCalibrationOptions.Q[2, 3];
var W = d * Options.StereoCalibrationOptions.Q[3, 2] + Options.StereoCalibrationOptions.Q[3, 3];
X = X / W;
Y = Y / W;
Z = Z / W;
leftImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, leftPoints[i], new Gray(255));
rightImageR.Draw(string.Format("{0:0.0} {1:0.0} {2:0.0}", X, Y, Z), ref _font, rightPoints[i], new Gray(255));
pointCloud[i] = new MCvPoint3D64f(X, Y, Z);
}
#endregion
if (pointCloud.Length >= 4)
{
var srcPoints = new Matrix<float>(pointCloud.Length, 3);
var dstPoints = new Matrix<float>(pointCloud.Length, 3);
for (int i = 0; i < pointCloud.Length; i++)
{
srcPoints[i, 0] = (float)pointCloud[i].x;
srcPoints[i, 1] = (float)pointCloud[i].y;
srcPoints[i, 2] = (float)pointCloud[i].z;
dstPoints[i, 0] = (float)pointCloud[i].x;
dstPoints[i, 1] = (float)pointCloud[i].y;
dstPoints[i, 2] = 0;
}
var mapMatrix = new Matrix<double>(3,3);
CvInvoke.cvGetPerspectiveTransform(srcPoints.Ptr, dstPoints.Ptr, mapMatrix.Ptr);
try
{
if (_transformed == null)
{
_transformed = new Image<Gray, byte>(leftImageR.Width, leftImageR.Height);
}
CvInvoke.cvPerspectiveTransform(leftImageR.Ptr, _transformed.Ptr, mapMatrix.Ptr);
//_transformed = leftImageR.WarpAffine(mapMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
}
catch(Exception ex)
{
}
//HomographyMatrix homographyMatrix = CameraCalibration.FindHomography(srcPoints, dstPoints, HOMOGRAPHY_METHOD.RANSAC, 2);
//_transformed = leftImageR.WarpPerspective(homographyMatrix, INTER.CV_INTER_CUBIC, WARP.CV_WARP_DEFAULT, new Gray(0));
}
}
var oldLeft = _cameras[0].Image;
var oldRight = _cameras[1].Image;
_cameras[0].Image = leftImageR;
_cameras[1].Image = rightImageR;
oldLeft.Dispose();
oldRight.Dispose();
}
public void DoBundleAdjust()
{
// N = cameras
// M = point count
//public static void BundleAdjust(MCvPoint3D64f[M] points, // Positions of points in global coordinate system (input and output), values will be modified by bundle adjustment
// MCvPoint2D64f[M][N] imagePoints, // Projections of 3d points for every camera
// int[M][N] visibility, // Visibility of 3d points for every camera
// Matrix<double>[N] cameraMatrix, // Intrinsic matrices of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] R, // rotation matrices of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] T, // translation vector of all cameras (input and output), values will be modified by bundle adjustment
// Matrix<double>[N] distCoefficients, // distortion coefficients of all cameras (input and output), values will be modified by bundle adjustment
// MCvTermCriteria termCrit) // Termination criteria, a reasonable value will be (30, 1.0e-12)
_stopwatchGet.Restart();
if (_cameras.Cameras.Count == 0) return;
IEnumerable<CameraModel> orderedCameras = _cameras.Cameras.OrderBy(camera => camera.Calibration.Index);
ObservableCollection<MotionControllerModel> controllers = _cameras.Cameras[0].Controllers;
if (controllers.Count == 0) return;
float radius = CameraCalibrationModel.SPHERE_RADIUS_CM;
int cameraCount = _cameras.Cameras.Count;
int pointCount = 8;
MCvPoint3D64f[] objectPoints = new MCvPoint3D64f[controllers.Count * pointCount];
MCvPoint2D64f[][] imagePoints = new MCvPoint2D64f[cameraCount][];
int[][] visibility = new int[cameraCount][];
Matrix<double>[] cameraMatrix = new Matrix<double>[cameraCount];
Matrix<double>[] R = new Matrix<double>[cameraCount];
Matrix<double>[] T = new Matrix<double>[cameraCount];
Matrix<double>[] distCoefficients = new Matrix<double>[cameraCount];
MCvTermCriteria termCrit = new MCvTermCriteria(30, 1.0e-12);
int visible = 0;
foreach (CameraModel camera in orderedCameras)
{
visibility[camera.Calibration.Index] = new int[controllers.Count * pointCount];
cameraMatrix[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.IntrinsicMatrix.Clone();
distCoefficients[camera.Calibration.Index] = camera.Calibration.IntrinsicParameters.DistortionCoeffs.Clone();
imagePoints[camera.Calibration.Index] = new MCvPoint2D64f[controllers.Count * pointCount];
R[camera.Calibration.Index] = camera.Calibration.RotationToWorld.Clone();
T[camera.Calibration.Index] = camera.Calibration.TranslationToWorld.Clone();
foreach (MotionControllerModel controller in controllers)
{
float x = controller.RawPosition[camera].x;
float y = controller.RawPosition[camera].y;
//if (x == 0 && y == 0) return;
// controller is not visible
if (controller.TrackerStatus[camera] != PSMoveTrackerStatus.Tracking)
{
for (int i = 0; i < pointCount; i++)
{
visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0;
}
}
// controller is visible
else
{
Vector3[] history = controller.PositionHistory[camera];
float avgMagnitude = 0f;
for (int i = 1; i < history.Length; i++)
{
avgMagnitude += history[i].magnitude/(history.Length - 1);
}
// check deviation of newest position
if ((Math.Abs(((history[0].magnitude*100)/avgMagnitude)) - 100) > 5)
{
for (int i = 0; i < pointCount; i++)
{
visibility[camera.Calibration.Index][i + controller.Id * pointCount] = 0;
}
continue;
}
visible++;
//double distance = 0.0;
int startIndex = controller.Id * pointCount;
//MCvPoint3D64f cameraPositionInWorld = new MCvPoint3D64f
//{
// x = camera.Calibration.TranslationToWorld[0, 0],
// y = camera.Calibration.TranslationToWorld[1, 0],
// z = camera.Calibration.TranslationToWorld[2, 0]
//};
// set visibility and calculate distance of the controller relative to the camera
for (int i = startIndex; i < pointCount * controllers.Count; i++)
{
visibility[camera.Calibration.Index][i] = 1;
//double d = CvHelper.GetDistanceToPoint(cameraPositionInWorld,objectPoints[i]);
//distance += d / pointCount;
}
// initialize object's world coordinates
// calculate as the average of each camera's transformed world coordinate
float wx = controller.WorldPosition[camera].x;
float wy = controller.WorldPosition[camera].y;
float wz = controller.WorldPosition[camera].z;
objectPoints[startIndex] += new MCvPoint3D32f(wx - radius, wy - radius, wz - radius);
objectPoints[startIndex + 1] += new MCvPoint3D32f(wx + radius, wy - radius, wz - radius);
objectPoints[startIndex + 2] += new MCvPoint3D32f(wx + radius, wy + radius, wz - radius);
objectPoints[startIndex + 3] += new MCvPoint3D32f(wx - radius, wy + radius, wz - radius);
objectPoints[startIndex + 4] += new MCvPoint3D32f(wx - radius, wy + radius, wz + radius);
objectPoints[startIndex + 5] += new MCvPoint3D32f(wx + radius, wy + radius, wz + radius);
objectPoints[startIndex + 6] += new MCvPoint3D32f(wx + radius, wy - radius, wz + radius);
objectPoints[startIndex + 7] += new MCvPoint3D32f(wx - radius, wy - radius, wz + radius);
//imagePoints[scvm.Camera.Calibration.Index] = Utils.GetImagePoints(mcvm.MotionController.RawPosition[scvm.Camera]);
imagePoints[camera.Calibration.Index] = Array.ConvertAll(camera.Calibration.ObjectPointsProjected, CvHelper.PointFtoPoint2D);
}
} // foreach controller
} // foreach camera
if (visible == 0) return;
// average object points
for (int i = 0; i < objectPoints.Length; i++)
{
objectPoints[i].x /= visible;
objectPoints[i].y /= visible;
objectPoints[i].z /= visible;
}
// calculate object's middle
float prex = 0, prey = 0, prez = 0;
for (int i = 0; i < objectPoints.Length; i++)
{
prex += (float)objectPoints[i].x / objectPoints.Length;
prey += (float)objectPoints[i].y / objectPoints.Length;
prez += (float)objectPoints[i].z / objectPoints.Length;
}
_stopwatchBA.Restart();
//LevMarqSparse.BundleAdjust(objectPoints, imagePoints, visibility, cameraMatrix, R, T, distCoefficients, termCrit);
_stopwatchBA.Stop();
_stopwatchSet.Restart();
// check for calucation error
for (int i = 0; i < objectPoints.Length; i++)
{
if (objectPoints[i].x.ToString().Equals("NaN")) return;
if (objectPoints[i].y.ToString().Equals("NaN")) return;
if (objectPoints[i].z.ToString().Equals("NaN")) return;
}
// save changed matrices
foreach (CameraModel camera in orderedCameras)
{
if (visibility[camera.Calibration.Index][0] == 1)
{
//RotationVector3D rot1 = new RotationVector3D();
//rot1.RotationMatrix = camera.Calibration.RotationToWorld;
//RotationVector3D rot2 = new RotationVector3D();
//rot2.RotationMatrix = R[camera.Calibration.Index];
//Console.WriteLine((int)(rot1[0, 0] * (180 / Math.PI)) + " " + (int)(rot2[0, 0] * (180 / Math.PI)));
//Console.WriteLine((int)(rot1[1, 0] * (180 / Math.PI)) + " " + (int)(rot2[1, 0] * (180 / Math.PI)));
//Console.WriteLine((int)(rot1[2, 0] * (180 / Math.PI)) + " " + (int)(rot2[2, 0] * (180 / Math.PI)) + Environment.NewLine);
//camera.Calibration.IntrinsicParameters.IntrinsicMatrix = cameraMatrix[camera.Calibration.Index];
//camera.Calibration.RotationToWorld = R[camera.Calibration.Index];
//camera.Calibration.TranslationToWorld = T[camera.Calibration.Index];
//camera.Calibration.IntrinsicParameters.DistortionCoeffs = distCoefficients[camera.Calibration.Index];
//camera.Calibration.XAngle = (int)(rot2[0, 0] * (180 / Math.PI));
//camera.Calibration.YAngle = (int)(rot2[1, 0] * (180 / Math.PI));
//camera.Calibration.ZAngle = (int)(rot2[2, 0] * (180 / Math.PI));
}
}
// calculate object's middle
float preCenterX = 0, preCenterY = 0, preCenterZ = 0;
for (int i = 0; i < objectPoints.Length; i++)
{
preCenterX += (float)objectPoints[i].x / objectPoints.Length;
preCenterY += (float)objectPoints[i].y / objectPoints.Length;
preCenterZ += (float)objectPoints[i].z / objectPoints.Length;
}
Vector3 prePosition = new Vector3(preCenterX, -preCenterY, preCenterZ);
if (prePosition != _positionHistory[0])
{
for (int i = _positionHistory.Length - 1; i > 0; --i)
{
_positionHistory[i] = _positionHistory[i - 1];
}
_positionHistory[0] = prePosition;
}
//Vector3 avgPosition = Vector3.zero;
//for (int i = 0; i < _positionHistory.Length; i++)
//{
// avgPosition += _positionHistory[i] / _positionHistory.Length;
//}
// 0 predition, 1 correction / estimated
Matrix<float> kalmanResults = FilterPoints(_kalmanXYZ, prePosition.x, prePosition.y, prePosition.z);
Vector3 kalmanPosition = new Vector3(kalmanResults[1,0], kalmanResults[1,1], kalmanResults[1,2]);
_cameras.Position = kalmanPosition;
_stopwatchGet.Stop();
_stopwatchSet.Stop();
for (int i = 0; i < 4; i++)
{
CameraModel camera = _cameras.Cameras[i];
float xr = controllers[0].RawPosition[camera].x;
float yr = controllers[0].RawPosition[camera].y;
float zr = controllers[0].RawPosition[camera].z;
float xc = controllers[0].CameraPosition[camera].x;
float yc = controllers[0].CameraPosition[camera].y;
float zc = controllers[0].CameraPosition[camera].z;
string str = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}",
iteration,
xr,
yr,
zr,
PsMoveApi.psmove_tracker_distance_from_radius(camera.Handle, controllers[0].RawPosition[camera].z),
xc,
yc,
zc,
Math.Sqrt(xc * xc + yc * yc + zc * zc)
);
if (camera.Calibration.Index == 0)
{
if (csv0.Count > 0 && csv0[csv0.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { }
else csv0.Add(str);
}
else if (camera.Calibration.Index == 1)
{
if (csv1.Count > 0 && csv1[csv1.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { }
else csv1.Add(str);
}
else if (camera.Calibration.Index == 2)
{
if (csv2.Count > 0 && csv2[csv2.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { }
else csv2.Add(str);
}
else if (camera.Calibration.Index == 3)
{
if (csv3.Count > 0 && csv3[csv3.Count - 1].Contains(zr.ToString(new CultureInfo("en-US")))) { }
else csv3.Add(str);
}
}
csvTime.Add(String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3}",
iteration,
_stopwatchGet.ElapsedMilliseconds,
_stopwatchBA.ElapsedMilliseconds,
_stopwatchSet.ElapsedMilliseconds));
string strBA = String.Format(new CultureInfo("en-US"), "{0},{1},{2},{3},{4},{5},{6},{7},{8}",
iteration,
prePosition.x,
prePosition.y,
prePosition.z,
Math.Sqrt(prePosition.x * prePosition.x + prePosition.y * prePosition.y + prePosition.z * prePosition.z),
kalmanPosition.x,
kalmanPosition.y,
kalmanPosition.z,
Math.Sqrt(kalmanPosition.x * kalmanPosition.x + kalmanPosition.y * kalmanPosition.y + kalmanPosition.z * kalmanPosition.z));
if (csvBA.Count > 0 && csvBA[csvBA.Count - 1].Contains(prePosition.x.ToString(new CultureInfo("en-US")))) { }
else csvBA.Add(strBA);
iteration++;
if (csvBA.Count == 100)
{
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\avg_time.csv", csvTime);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance.csv", csvBA);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance0.csv", csv0);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance1.csv", csv1);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance2.csv", csv2);
File.WriteAllLines(@"C:\\Users\\Johannes\\Documents\\GitHub\\Thesis\\Source\\distance3.csv", csv3);
}
}
public static MCvPoint3D64f GetCubeCenter(MCvPoint3D64f[] cubeObjectPoints)
{
double mx = 0, my = 0, mz = 0;
for (int i = 0; i < cubeObjectPoints.Length; i++)
{
mx += cubeObjectPoints[i].x / cubeObjectPoints.Length;
my += cubeObjectPoints[i].y / cubeObjectPoints.Length;
mz += cubeObjectPoints[i].z / cubeObjectPoints.Length;
}
return new MCvPoint3D64f(mx, my, mz);
}
public void TestQuaternionEulerAngleAndRotate()
{
double epsilon = 1.0e-12;
Random r = new Random();
Quaternions q1 = new Quaternions();
double roll1 = r.NextDouble(), pitch1 = r.NextDouble(), yaw1 = r.NextDouble();
double roll2 = 0, pitch2 = 0, yaw2 = 0;
q1.SetEuler(roll1, pitch1, yaw1);
q1.GetEuler(ref roll2, ref pitch2, ref yaw2);
EmguAssert.IsTrue(Math.Abs(roll1 - roll2) < epsilon);
EmguAssert.IsTrue(Math.Abs(pitch1 - pitch2) < epsilon);
EmguAssert.IsTrue(Math.Abs(yaw1 - yaw2) < epsilon);
Quaternions q2 = new Quaternions();
q2.SetEuler(r.NextDouble(), r.NextDouble(), r.NextDouble());
MCvPoint3D64f p = new MCvPoint3D64f(r.NextDouble() * 10, r.NextDouble() * 10, r.NextDouble() * 10);
MCvPoint3D64f delta = (q1 * q2).RotatePoint(p) - q1.RotatePoint(q2.RotatePoint(p));
EmguAssert.IsTrue(delta.X < epsilon);
EmguAssert.IsTrue(delta.Y < epsilon);
EmguAssert.IsTrue(delta.Z < epsilon);
}
public static double GetPoint3dMagnitude(MCvPoint3D64f point)
{
return Math.Sqrt(point.x * point.x + point.y * point.y + point.z * point.z);
}
public void TestAxisAngleCompose()
{
MCvPoint3D64f angle1 = new MCvPoint3D64f(4.1652539565753417e-022, -9.4229054916424228e-022, 5.1619136559035708e-008);
MCvPoint3D64f angle2 = new MCvPoint3D64f(4.3209729769679014e-023, 3.2042397847543764e-023, -6.4083339340765912e-008);
Quaternions q1 = new Quaternions();
q1.AxisAngle = angle1;
Quaternions q2 = new Quaternions();
q2.AxisAngle = angle2;
Quaternions q = q1 * q2;
MCvPoint3D64f angle = q.AxisAngle;
EmguAssert.AreNotEqual(double.NaN, angle.X, "Invalid value x");
EmguAssert.AreNotEqual(double.NaN, angle.Y, "Invalid value y");
EmguAssert.AreNotEqual(double.NaN, angle.Z, "Invalid value z");
}
/// <summary>
/// Compute the minimum and maximum value from the points
/// </summary>
/// <param name="points">The points</param>
/// <param name="min">The minimum x,y,z values</param>
/// <param name="max">The maximum x,y,z values</param>
public static void GetMinMax(IEnumerable<MCvPoint3D64f> points, out MCvPoint3D64f min, out MCvPoint3D64f max)
{
min = new MCvPoint3D64f();
min.x = min.y = min.z = double.MaxValue;
max = new MCvPoint3D64f();
max.x = max.y = max.z = double.MinValue;
foreach (MCvPoint3D64f p in points)
{
min.x = Math.Min(min.x, p.x);
min.y = Math.Min(min.y, p.y);
min.z = Math.Min(min.z, p.z);
max.x = Math.Max(max.x, p.x);
max.y = Math.Max(max.y, p.y);
max.z = Math.Max(max.z, p.z);
}
}
/// <summary>
/// Check if the specific point is in the Cuboid
/// </summary>
/// <param name="point">The point to be checked</param>
/// <returns>True if the point is in the cuboid</returns>
public bool Contains(MCvPoint3D64f point)
{
return(point.x >= Min.x && point.y >= Min.y && point.z >= Min.z &&
point.x <= Max.x && point.y <= Max.y && point.z <= Max.z);
}
/// <summary>
/// Check if the specific point is in the Cuboid
/// </summary>
/// <param name="point">The point to be checked</param>
/// <returns>True if the point is in the cuboid</returns>
public bool Contains(MCvPoint3D64f point)
{
return point.X >= Min.X && point.Y >= Min.Y && point.Z >= Min.Z
&& point.X <= Max.X && point.Y <= Max.Y && point.Z <= Max.Z;
}
public double[][] GetAccMagnetOrientationMatrix(MEMSReadingsSet3f newReadings, bool useAccMagnet, bool useGyroscope, bool useLowpassFilter, bool useAdaptiveFiltering, double accMagnetFilterCoeff, double gyroFilterCoeff)
{
if (useAdaptiveFiltering)
{
this.CalcFilterCoeffs(newReadings, out gyroFilterCoeff, out accMagnetFilterCoeff, accMagnetFilterCoeff, gyroFilterCoeff);
}
double[][] res = new double[3][];
for (int i = 0; i < 3; ++i)
{
res[i] = new double[3];
}
MCvPoint3D64f rawAccPoint = new MCvPoint3D64f(newReadings.AccVector3f.Values[0], newReadings.AccVector3f.Values[1], newReadings.AccVector3f.Values[2]);
MCvPoint3D64f rawMagnetPoint = new MCvPoint3D64f(newReadings.MagnetVector3f.Values[0], newReadings.MagnetVector3f.Values[1], newReadings.MagnetVector3f.Values[2]);
double gyroSpeedMul = 1;
if (useAdaptiveFiltering)
{
if(useAccMagnet)
{
gyroSpeedMul = this.GyroSpeedMul;
}
}
var gyroDiffMatr = this.GetGyroRotationMatrix(newReadings.GyroVector3f, gyroSpeedMul);
MCvPoint3D64f accFilteredPoint;
MCvPoint3D64f magnetFilteredPoint;
if (useLowpassFilter && this.OldAMGFusedAcc != null && this.OldAMGFusedMagnet != null)
{
if (useAccMagnet)
{
//this.FilterAccMagnet(ref resAccPoint, ref resMagnetPoint, accMagnetFilterCoeff);
accFilteredPoint = new MCvPoint3D64f(
rawAccPoint.x * accMagnetFilterCoeff + OldAMGFusedAcc.Value.x * (1 - accMagnetFilterCoeff),
rawAccPoint.y * accMagnetFilterCoeff + OldAMGFusedAcc.Value.y * (1 - accMagnetFilterCoeff),
rawAccPoint.z * accMagnetFilterCoeff + OldAMGFusedAcc.Value.z * (1 - accMagnetFilterCoeff)
);
magnetFilteredPoint = new MCvPoint3D64f(
rawMagnetPoint.x * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.x * (1 - accMagnetFilterCoeff),
rawMagnetPoint.y * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.y * (1 - accMagnetFilterCoeff),
rawMagnetPoint.z * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.z * (1 - accMagnetFilterCoeff)
);
}
else
{
accFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedAcc.Value.x, this.OldAMGFusedAcc.Value.y, this.OldAMGFusedAcc.Value.z);
magnetFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedMagnet.Value.x, this.OldAMGFusedMagnet.Value.y, this.OldAMGFusedMagnet.Value.z);
}
}
else
{
accFilteredPoint = new MCvPoint3D64f(rawAccPoint.x, rawAccPoint.y, rawAccPoint.z);
magnetFilteredPoint = new MCvPoint3D64f(rawMagnetPoint.x, rawMagnetPoint.y, rawMagnetPoint.z);
}
MCvPoint3D64f resAccPoint = new MCvPoint3D64f(0, 9, 0);
MCvPoint3D64f resMagnetPoint = new MCvPoint3D64f(48, 2, 0);
if(this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else
{
if (useGyroscope && useAccMagnet)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(accFilteredPoint).Mul(1 - gyroFilterCoeff)));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(magnetFilteredPoint).Mul(1 - gyroFilterCoeff)));
}
else if (useAccMagnet)
{
resAccPoint = accFilteredPoint;
resMagnetPoint = magnetFilteredPoint;
}
else if(useGyroscope)
{
resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr));
resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr));
}
}
this.OldAMGFusedAcc = resAccPoint;
this.OldAMGFusedMagnet = resMagnetPoint;
this.OldRawAcc = rawAccPoint;
this.OldRawMagnet = rawMagnetPoint;
this.OldFilteredAcc = accFilteredPoint;
this.OldFilteredMagnet = magnetFilteredPoint;
this.OldGyroReadings = newReadings.GyroVector3f;
MCvPoint3D64f x;
MCvPoint3D64f y;
MCvPoint3D64f z;
this.GetOrthoNormalAccMagnetBasis(resAccPoint, resMagnetPoint, out x, out y, out z);
Matrix<double> accMagnetMatrix = new Matrix<double>(3, 3);
accMagnetMatrix[0, 0] = x.x; accMagnetMatrix[0, 1] = x.y; accMagnetMatrix[0, 2] = x.z;
accMagnetMatrix[1, 0] = y.x; accMagnetMatrix[1, 1] = y.y; accMagnetMatrix[1, 2] = y.z;
accMagnetMatrix[2, 0] = z.x; accMagnetMatrix[2, 1] = z.y; accMagnetMatrix[2, 2] = z.z;
//AccMagnetGyro fuse
Matrix<double> newGAMFusedMatrix = null;
//old amg fusion
//if (this.OldGAMFusedMatrix == null)
//{
// newGAMFusedMatrix = accMagnetMatrix;
//}
//else
//{
// var newGyroMatrix = this.OldGAMFusedMatrix.Mul(gyroDiffMatr);
// newGAMFusedMatrix = this.FuseAccMagnetGyro(accMagnetMatrix, newGyroMatrix, gyroFilterCoeff);
//}
newGAMFusedMatrix = accMagnetMatrix;
OldGAMFusedMatrix = newGAMFusedMatrix;
////
var resMatrix = newGAMFusedMatrix.Transpose();
//res = this.ConvertMatrix(m);
//gyro test
//m = this.OldGyroMatrix;
//m = m.Transpose();
//
//auto res set up
//for (int i = 0; i < 3; ++i)
//{
// for (int j = 0; j < 3; ++j)
// {
// res[i][j] = resMatrix[i, j];
// }
//}
////
//manual res set up
res[0][0] = -resMatrix[0, 0]; res[0][1] = -resMatrix[0, 1]; res[0][2] = resMatrix[0, 2];
res[1][0] = -resMatrix[1, 0]; res[1][1] = -resMatrix[1, 1]; res[1][2] = resMatrix[1, 2];
res[2][0] = -resMatrix[2, 0]; res[2][1] = -resMatrix[2, 1]; res[2][2] = resMatrix[2, 2];
////
return res;
}