public void TestEmptyList()
{
List <CameraToCameraPoint> dataSet = new List <CameraToCameraPoint>();
KabschSolver solver = new KabschSolver();
solver.Solve(dataSet);
}
public void TestAffine()
{
Matrix expected = Matrix.Translation(0.2f, 5.0f, 8.0f) * Matrix.RotationYawPitchRoll(0.2f, -3.0f, 1.6f);
KabschSolver solver = new KabschSolver();
List <CameraToCameraPoint> dataSet = new List <CameraToCameraPoint>();
Vector3 o1 = new Vector3(0.2f, 7.0f, 3.0f); Vector3 d1 = Vector3.TransformCoordinate(o1, expected);
Vector3 o2 = new Vector3(0.2f, 0.12f, 3.0f); Vector3 d2 = Vector3.TransformCoordinate(o2, expected);
Vector3 o3 = new Vector3(0.2f, 0.25f, 3.0f); Vector3 d3 = Vector3.TransformCoordinate(o3, expected);
Vector3 o4 = new Vector3(0.4f, 0.50f, 3.0f); Vector3 d4 = Vector3.TransformCoordinate(o4, expected);
Vector3 o5 = new Vector3(0.2f, 7.0f, 30.0f); Vector3 d5 = Vector3.TransformCoordinate(o5, expected);
Vector3 o6 = new Vector3(0.8f, 70.0f, 30.0f); Vector3 d6 = Vector3.TransformCoordinate(o6, expected);
dataSet.Add(new CameraToCameraPoint(o1, d1));
dataSet.Add(new CameraToCameraPoint(o2, d2));
dataSet.Add(new CameraToCameraPoint(o3, d3));
dataSet.Add(new CameraToCameraPoint(o4, d4));
dataSet.Add(new CameraToCameraPoint(o5, d5));
dataSet.Add(new CameraToCameraPoint(o6, d6));
Matrix actual = Matrix.Invert(solver.Solve(dataSet));
Assert.IsTrue(NearEqual(actual, expected));
}
void Update()
{
solver = new KabschSolver();
for (int iteration = 0; iteration < 2; iteration++)
{
float currentCost = 0f;
for (int i = 0; i < features.Length; i++)
{
refPoints[i] = features[i].position;
inPoints[i] = Constraints.ConstrainToSegment(features[i].position, aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i] * 6f));
currentCost += Vector3.Distance(refPoints[i], inPoints[i]);
Debug.DrawLine(aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i]));
Debug.DrawLine(features[i].position, inPoints[i], Color.red);
}
Matrix4x4 iterationStep = solver.SolveKabsch(inPoints, refPoints, true);
Matrix4x4 steppedTransform = iterationStep * aligningCamera.localToWorldMatrix;
if (useLinearEstimate)
{
float stepScale = ((previousCost - currentCost) > 0.0000001f && iteration > 0) ? (previousCost / (previousCost - currentCost)) * 0.8f : 1f;
aligningCamera.position += (steppedTransform.GetVector3() - aligningCamera.position) * stepScale;
aligningCamera.rotation = Quaternion.SlerpUnclamped(Quaternion.identity, iterationStep.GetQuaternion(), stepScale) * aligningCamera.rotation;
}
else
{
aligningCamera.position = steppedTransform.GetVector3();
aligningCamera.rotation = steppedTransform.GetQuaternion();
}
previousCost = currentCost;
}
}
public void TestAffine()
{
Matrix expected = Matrix.Translation(0.2f, 5.0f, 8.0f) * Matrix.RotationYawPitchRoll(0.2f, -3.0f, 1.6f);
KabschSolver solver = new KabschSolver();
List<CameraToCameraPoint> dataSet = new List<CameraToCameraPoint>();
Vector3 o1 = new Vector3(0.2f, 7.0f, 3.0f); Vector3 d1 = Vector3.TransformCoordinate(o1, expected);
Vector3 o2 = new Vector3(0.2f, 0.12f, 3.0f); Vector3 d2 = Vector3.TransformCoordinate(o2, expected);
Vector3 o3 = new Vector3(0.2f, 0.25f, 3.0f); Vector3 d3 = Vector3.TransformCoordinate(o3, expected);
Vector3 o4 = new Vector3(0.4f, 0.50f, 3.0f); Vector3 d4 = Vector3.TransformCoordinate(o4, expected);
Vector3 o5 = new Vector3(0.2f, 7.0f, 30.0f); Vector3 d5 = Vector3.TransformCoordinate(o5, expected);
Vector3 o6 = new Vector3(0.8f, 70.0f, 30.0f); Vector3 d6 = Vector3.TransformCoordinate(o6, expected);
dataSet.Add(new CameraToCameraPoint(o1, d1));
dataSet.Add(new CameraToCameraPoint(o2, d2));
dataSet.Add(new CameraToCameraPoint(o3, d3));
dataSet.Add(new CameraToCameraPoint(o4, d4));
dataSet.Add(new CameraToCameraPoint(o5, d5));
dataSet.Add(new CameraToCameraPoint(o6, d6));
Matrix actual = Matrix.Invert(solver.Solve(dataSet));
Assert.IsTrue(NearEqual(actual, expected));
}
public static Quaternion GetRotation(List <Vector3> a, List <Vector3> b)
{
var closest = ComputePointMapping(a, b);
var c = (from i in closest.Values select b[i]).ToList();
KabschSolver solver = new KabschSolver();
var matrix = solver.SolveKabsch(a, c);
return(matrix.GetQuaternion());
}
public void TestNull()
{
KabschSolver solver = new KabschSolver();
solver.Solve(null);
}
public void TestNull()
{
KabschSolver solver = new KabschSolver();
solver.Solve(null);
}
public void TestEmptyList()
{
List<CameraToCameraPoint> dataSet = new List<CameraToCameraPoint>();
KabschSolver solver = new KabschSolver();
solver.Solve(dataSet);
}
// Update is called once per frame
void Update()
{
if (devices.Length > 1)
{
// Add the set of joints to device-specific lists
if (Input.GetKeyUp(takeCalibrationSampleKey))
{
bool handInAllFrames = true;
for (int i = 0; i < devices.Length; i++)
{
Hand rightHand = devices[i].deviceProvider.CurrentFrame.Get(Chirality.Right);
if (rightHand != null)
{
devices[i].currentHand = rightHand;
}
else
{
handInAllFrames = false;
}
}
if (handInAllFrames)
{
for (int i = 0; i < devices.Length; i++)
{
if (devices[i].handPoints == null)
{
devices[i].handPoints = new List <Vector3>();
}
for (int j = 0; j < 5; j++)
{
for (int k = 0; k < 4; k++)
{
devices[i].handPoints.Add(devices[i].currentHand.Fingers[j].bones[k].Center.ToVector3());
}
}
}
}
}
// Moves subsidiary devices to be in alignment with the device at the 0 index
if (Input.GetKeyUp(solveForRelativeTransformKey))
{
if (devices[0].handPoints.Count > 3)
{
for (int i = 1; i < devices.Length; i++)
{
KabschSolver solver = new KabschSolver();
Matrix4x4 deviceToOriginDeviceMatrix =
solver.SolveKabsch(devices[i].handPoints, devices[0].handPoints, 200);
devices[i].deviceProvider.transform.Transform(deviceToOriginDeviceMatrix);
devices[i].handPoints.Clear();
}
devices[0].handPoints.Clear();
}
}
}
}
IEnumerator CalibrateScreenLocation()
{
//Reset some stuff
HyperMegaStuff.HyperMegaLines.drawer.dontClear = false;
monitorPattern.SetActive(false);
headsetPattern.SetActive(false);
for (int i = 0; i < calibrationDevices.Length; i++)
{
if (calibrationDevices[i].isConnected)
{
calibrationDevices[i].resetMasks();
}
calibrationDevices[i].undistortImage = false; // Stop undistorting the image because it's slow
}
whiteCircle.gameObject.SetActive(false);
//1) Take a background subtraction shot of a black screen
yield return(new WaitForSeconds(standardDelay));
float startTime = Time.unscaledTime;
while (Time.unscaledTime < startTime + standardDelay * 10f)
{
//Take the max of the current image against the current subtraction image
updateSubtractionBackgrounds();
yield return(null);
}
yield return(new WaitForSeconds(standardDelay));
//2) Unhide a white object
monitorWhiteness.SetActive(true);
yield return(new WaitForSeconds(standardDelay));
//3) Hit "Create Mask"
// Create the binary masks from the subtracted images
createBinaryMasks(monitorMaskThreshold);
yield return(new WaitForSeconds(standardDelay));
monitorWhiteness.SetActive(false);
//4) Start Moving the Circle Around
whiteCircle.SetActive(true);
yield return(new WaitForSeconds(standardDelay / 2f));
HyperMegaStuff.HyperMegaLines drawer = HyperMegaStuff.HyperMegaLines.drawer;
drawer.dontClear = true;
for (int i = 0; i < _realDots.Count; i++)
{
whiteCircle.transform.position = calibrationDotsParent.GetChild(i).position;
int foundDots = 0;
while (foundDots == 0)
{
yield return(new WaitForSeconds(standardDelay));
foundDots = 0;
for (int j = 0; j < calibrationDevices.Length; j++)
{
Vector3 triangulatedDot = triangulate(j, drawer);
if (triangulatedDot != Vector3.zero)
{
calibrationDevices[j].triangulatedDots[i] = triangulatedDot;
foundDots++;
}
}
}
}
whiteCircle.SetActive(false);
drawer.dontClear = false;
//Kabsch the Dots, move the screen!
KabschSolver solver = new KabschSolver();
//Place the monitor based off of one of the webcams
List <Vector3> _triangulatedDots = new List <Vector3>(), _monitorDots = new List <Vector3>();
for (int j = 0; j < _realDots.Count; j++)
{
if (calibrationDevices[0].triangulatedDots[j] != Vector3.zero)
{
_monitorDots.Add(calibrationDotsParent.GetChild(j).position);
_triangulatedDots.Add(calibrationDevices[0].triangulatedDots[j]);
}
if (calibrationDevices[1].triangulatedDots[j] != Vector3.zero)
{
_monitorDots.Add(calibrationDotsParent.GetChild(j).position);
_triangulatedDots.Add(calibrationDevices[1].triangulatedDots[j]);
}
}
//エラー発見
// CalibrationMonitor.Transform(solver.SolveKabsch(_monitorDots, _triangulatedDots, 200));
for (int j = 0; j < _realDots.Count; j++)
{
_realDots[j] = calibrationDotsParent.GetChild(j).position;
}
monitorPattern.SetActive(true);
headsetPattern.SetActive(true);
}
