// Extract and convert Rotation and Center data to matrix
private AARServo ExtractServoInformation(JToken _jobj, string _servoName)
{
var servo = _jobj[_servoName];
var LtP = servo["LocalToPVCamera"];
// Local to PV camera transform
Matrix4x4 T = new Matrix4x4();
for (int i = 0; i < 3; ++i)
{
JToken v = LtP[i];
Vector4 vec = new Vector4(
v[0].ToObject <float>(),
v[1].ToObject <float>(),
v[2].ToObject <float>(),
v[3].ToObject <float>() / 1000.0f);
T.SetRow(i, vec);
}
T.m33 = 1;
// Range of servo in degrees
var rangeJson = servo["RangeDegrees"];
AARServo.Range range = new AARServo.Range(
rangeJson[0].ToObject <float>(),
rangeJson[1].ToObject <float>());
// Hard limits of servo in degrees
var limitJson = servo["LimitDegrees"];
AARServo.Range limit = new AARServo.Range(
limitJson[0].ToObject <float>(),
limitJson[1].ToObject <float>());
// The axis of rotation for the servo
var axisJson = servo["AxisOfRotation"];
Vector3 axis = new Vector3(
axisJson[0].ToObject <float>(),
axisJson[1].ToObject <float>(),
axisJson[2].ToObject <float>());
// Get ID used for the Arduino
int servoID = servo["ID"].ToObject <int>();
// Calibrated center for servo at rest (i.e. facing forward parallel with the hololens)
float calibratedCenter = servo["CalibratedCenter"].ToObject <float>();
// Get the ticks used in calibration and calcualted degrees
float servoTicks = servo["ServoTick"].ToObject <float>();
float calibratedRotationDegree = servo["CalibratedRotationDegree"].ToObject <float>();
var s = new AARServo(
_servoName,
servoID,
axis,
T,
range,
limit,
calibratedCenter,
servoTicks,
calibratedRotationDegree);
return(s);
}
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
///
/// PARSING METHODS
///
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////////////
#region CALIB_PARSING
private void ParseAllCalibrationData()
{
//////////////////////////////////////////////////////////////////////////////////////////////////
// Build All Hololens Sensors (VLC and PV)\
//////////////////////////////////////////////////////////////////////////////////////////////////
{
TextAsset jText = Resources.Load(HololensSensorCalibFile) as TextAsset;
JObject sensorCalibJObj = JObject.Parse(jText.text);
foreach (var name in m_sensorNames)
{
var sensor = ExtractSensorInformation(sensorCalibJObj, name);
HololensSensorList.Add(name, sensor);
}
}
// Visualize structure;
m_sensorStructure = new GameObject();
m_sensorStructure.name = "CameraSensorStructure";
m_sensorStructure.transform.parent = m_AARCameraProjectorRig.transform;
m_sensorStructure.transform.localPosition = Vector3.zero;
foreach (var sensor in HololensSensorList)
{
GameObject obj = (SensorModel == null) ?
GameObject.CreatePrimitive(PrimitiveType.Sphere) :
GameObject.Instantiate(SensorModel);
obj.name = sensor.Key.ToString();
obj.transform.parent = m_sensorStructure.transform;
obj.transform.localPosition = Vector3.zero;
obj.transform.localPosition = sensor.Value.GetPositionHololensFrameOfReference();
obj.transform.rotation = sensor.Value.GetRotationHololensFrameOfReference();
sensor.Value.AttachGameObject(obj);
}
//////////////////////////////////////////////////////////////////////////////////////////////////
// Build servo and projector
//////////////////////////////////////////////////////////////////////////////////////////////////
// TODO: Attach the servo and projector scripts directly to these game objects
// TODO: Create projector script that can take in the json format
{
TextAsset jText = Resources.Load(ServoProjectorCalibFile) as TextAsset;
JObject servoProCalib = JObject.Parse(jText.text);
// Servo
var servos = servoProCalib["Servos"];
foreach (var servoName in ServoNames)
{
var servo = ExtractServoInformation(servos, servoName);
ServoList.Add(servoName, servo);
}
// Projector
var projectors = servoProCalib["ProjectorViews"];
foreach (var projectorName in ProjectorNames)
{
var projector = ExtractProjectorInformation(projectors, projectorName);
ProjectorList.Add(projectorName, projector);
}
}
// Create structure on hololens
m_servoProjectorStructure = new GameObject();
m_servoProjectorStructure.name = "ServoProjectorStructure";
m_servoProjectorStructure.transform.parent = m_AARCameraProjectorRig.transform;
m_servoProjectorStructure.transform.localPosition = Vector3.zero;
// Servos (ordering matters!)
AARServo lastAddedServo = null;
foreach (var servoName in ServoNames)
{
AARServo servo = ServoList[servoName];
// Servo Object
GameObject servoObj = new GameObject();
servoObj.name = servo.GetName();
servoObj.transform.parent = m_servoProjectorStructure.transform;
servoObj.transform.localPosition = Vector3.zero;
// Set positio and rotation based on calibration
var pvCamera = HololensSensorList[
Util.SensorTypeToString(HololensSensorType.AAR_SENSOR_PVCAMERA)
];
Matrix4x4 coordinateChange = Matrix4x4.Rotate(Quaternion.Euler(0, 0, 180));
Vector4 p = new Vector4(0, 0, 0, 1);
Matrix4x4 servoToHololens =
coordinateChange *
pvCamera.GetLocalToHololensFrameOfReferenceMatrix() *
servo.GetLocalToPVCameraMatrix() *
coordinateChange;
servoObj.transform.localPosition = servoToHololens * p;
servoObj.transform.localRotation = servoToHololens.rotation;
// Check if there is a previous servo for linking
if (lastAddedServo != null)
{
GameObject previousServo = lastAddedServo.GetRotationGameObject();
servoObj.transform.parent = previousServo.transform;
}
servo.AttachGameObject(servoObj);
lastAddedServo = servo;
}
// Projectors
foreach (var projector in ProjectorList)
{
GameObject projectoObj = new GameObject();
projectoObj.name = projector.Key.ToString();
projectoObj.transform.parent = m_servoProjectorStructure.transform;
projectoObj.transform.localPosition = Vector3.zero;
// Set positon and rotation based on calibration
var pvCamera = HololensSensorList[
Util.SensorTypeToString(HololensSensorType.AAR_SENSOR_PVCAMERA)
];
Vector4 p = new Vector4(0, 0, 0, 1);
Matrix4x4 coordinateChange = Matrix4x4.Rotate(Quaternion.Euler(0, 0, 180));
Matrix4x4 servoToHololens =
coordinateChange *
pvCamera.GetLocalToHololensFrameOfReferenceMatrix() *
projector.Value.GetLocalToPVCameraMatrix(); // * coordinateChange;
// Set local positions
projectoObj.transform.localPosition = servoToHololens * p;
projectoObj.transform.localRotation = servoToHololens.rotation;
// Attach to last seen servo
if (lastAddedServo != null)
{
projectoObj.transform.parent = lastAddedServo.GetRotationGameObject().transform;
}
projector.Value.AttachGameObject(projectoObj);
// Model
GameObject model = (ProjectorModel == null) ?
GameObject.CreatePrimitive(PrimitiveType.Sphere) :
GameObject.Instantiate(ProjectorModel);
projector.Value.SetModel(model);
}
}
