More work on calibration

clients/TrackerSimulator/camera.lua

@@ -32,7 +32,7 @@ function camera:update(trackers)
 
 	if self.client then
 		for i, tracker in ipairs(self.seenTrackers) do
-			self.client:send("CAM", tracker.id, {pos={x=tracker.x/400*2-1, y=tracker.y/240*2-1}})
+			self.client:send("CAM", tracker.id, {pos={x=(tracker.x/400-0.5)*self.fov, y=(tracker.y/240-0.5)*self.fov}})
 		end
 		local commands = self.client:update()
 		for i, v in ipairs(commands) do

clients/TrackerSimulator/main.lua

@@ -80,12 +80,21 @@ function love.load()
 
 	trackerControls:add(layout.text("Tracker Controls:"))
 	trackerControls:add(layout.spacer())
-	trackerControls:add(layout.text("Calibration hold time:"))
-	local caliHold = trackerControls:add(layout.slider())
-	trackerControls:add(layout.text("Calibration positions:"))
-	local caliPos = trackerControls:add(layout.slider(10))
+	local caliHoldText = trackerControls:add(layout.text("Calibration hold time: 0.5s"))
+	local caliHold = trackerControls:add(layout.slider(46, function(pos)
+		caliHoldText.text = "Calibration hold time: " .. tostring(pos*4.5+0.5) .. "s"
+	end))
+	local caliPosText = trackerControls:add(layout.text("Calibration positions: 3"))
+	local caliPos = trackerControls:add(layout.slider(10, function(pos)
+		caliPosText.text = "Calibration positions: " .. tostring(math.floor(pos*9)+1)
+	end))
+	caliPos.position = 2/9
 	trackerControls:add(layout.button("Run calibration", function()
-		configClient:send("CAL", -1, {settings={x=caliHold.position*10, y=math.floor(caliPos.position*9)+1, z=0}})
+		local calibrationSettings = {x=(caliHold.position*4.5+0.5), y=math.floor(caliPos.position*9)+1, z=0}
+		for i, tk in ipairs(trackers) do
+			tk:doCalibration(calibrationSettings)
+		end
+		configClient:send("CAL", -1, {settings=calibrationSettings})
 	end))
 
 	layout:addPane("simulate", simulationControls)

clients/TrackerSimulator/tracker.lua

@@ -7,14 +7,27 @@ function tracker.new(id, colour)
 		x = 0, y = 0, z = 0,
 		id = id, colour = colour,
 		time = 0, mode = "noise",
+		calibrationStart = 0,
+		calibrationSteps = 0,
+		calibrationHold = 0,
 	}, tracker.mt)
 end
 
 function tracker:update(delta)
-	self.time = self.time + delta/10
-	self.x = love.math.noise(self.time, self.id*4/3+15/7, 12.5)*4-2
-	self.y = love.math.noise(self.time, self.id*4/3+23/7, 12.5)
-	self.z = love.math.noise(self.time, self.id*4/3+8/7, 12.5)*4-2
+	self.time = self.time + delta
+	if self.mode == "noise" then
+		self.x = love.math.noise(self.time/10, self.id*4/3+15/7, 12.5)*4-2
+		self.y = love.math.noise(self.time/10, self.id*4/3+23/7, 12.5)
+		self.z = love.math.noise(self.time/10, self.id*4/3+8/7, 12.5)*4-2
+	elseif self.mode == "calibration" then
+		local idx = math.floor((self.time-self.calibrationStart)/self.calibrationHold)-(self.calibrationSteps-1)
+		self.x = love.math.noise(idx, self.id*4/3+15/7, 12.5)*4-2
+		self.y = love.math.noise(idx, self.id*4/3+23/7, 12.5)
+		self.z = love.math.noise(idx, self.id*4/3+8/7, 12.5)*4-2
+		if idx > 0 then
+			self.mode = "noise"
+		end
+	end
 
 	if self.client then
 		self.client:send("IMU", self.id, {rot={w=1, x=0, y=0, z=0}, accel={x=0, y=0, z=0}})
@@ -37,4 +50,11 @@ function tracker:disconnect()
 	self.client:disconnect()
 end
 
+function tracker:doCalibration(settings)
+	self.mode = "calibration"
+	self.calibrationStart = self.time
+	self.calibrationHold = settings.x*1.5
+	self.calibrationSteps = settings.y
+end
+
 return tracker

servers/SteamVRInputReciever/SteamVRInputRecieverDriver/ENetServer.cpp

@@ -18,7 +18,7 @@ ENetServer::~ENetServer() {
 	enet_host_destroy(this->server);
 }
 
-std::vector<Message> ENetServer::update() {
+std::vector<Message> ENetServer::update(float deltaTime) {
 	std::vector<Message> messages;
 	ENetEvent event;
 
@@ -48,5 +48,6 @@ std::vector<Message> ENetServer::update() {
 			event.peer->data = NULL;
 		}
 	}
-	return messages;
+
+
 }

servers/SteamVRInputReciever/SteamVRInputRecieverDriver/ENetServer.hpp

@@ -27,12 +27,20 @@ struct Message {
 	};
 };
 
+struct CalibrationPoint {
+	int camera, tracker;
+	glm::vec2 minPos, maxPos;
+	float steadyTime;
+};
+
 class ENetServer {
 public:
 	ENetServer();
 	~ENetServer();
 
-	std::vector<Message> update();
+	std::vector<Message> update(float deltaTime);
 private:
 	ENetHost* server;
+	bool inCalibration;
+	
 };

servers/SteamVRInputReciever/SteamVRInputRecieverDriver/TrackerManager.cpp

@@ -29,23 +29,21 @@ static inline PositionDistance posFromTwoLines(glm::vec3 p0, glm::vec3 v0, glm::
 }
 
 static inline glm::vec3 directionFromPixel(glm::vec2 p) {
-
+	return glm::vec3(
+		sinf(p.x) * cosf(p.y),
+		sinf(p.y),
+		cosf(p.x) * cosf(p.y)
+	);
 }
 
 static struct CalibrationCamera {
 	int id;
 	std::vector<CameraPoint*>* points;
 	glm::vec3 position;
+	glm::vec2 rotation;
 };
 
-static PositionDistance positionFromPoints(std::vector<CameraPoint> points) {
-	std::unordered_map<int, CalibrationCamera> sortedPoints;
-	for (CameraPoint& point : points) {
-		if (!sortedPoints.count(point.camera)) {
-			sortedPoints[point.camera] = { point.camera, new std::vector<CameraPoint*>(), glm::vec3(0.f, 0.f, 0.f) };
-		}
-		sortedPoints[point.camera].points->push_back(&point);
-	}
+static PositionDistance positionFromPoints(std::unordered_map<int, CalibrationCamera> sortedPoints) {
 	glm::vec3 totalPos;
 	float totalDist = 0;
 	int pointCount = 0;
@@ -55,7 +53,7 @@ static PositionDistance positionFromPoints(std::vector<CameraPoint> points) {
 				if (points.second.id != points2.second.id) {
 					for (CameraPoint* point2 : *points2.second.points) {
 						if (point->id == point2->id) {
-							PositionDistance posDst = posFromTwoLines(points.second.position, directionFromPixel(point->pos), points2.second.position, directionFromPixel(point->pos));
+							PositionDistance posDst = posFromTwoLines(points.second.position, directionFromPixel(points.second.rotation + point->pos), points2.second.position, directionFromPixel(points.second.rotation + point->pos));
 							totalPos += posDst.position;
 							totalDist += posDst.distance;
 							pointCount++;
@@ -69,8 +67,54 @@ static PositionDistance positionFromPoints(std::vector<CameraPoint> points) {
 	return { totalPos, totalDist };
 }
 
-void TrackerManager::calibrateCameras(std::vector<CameraPoint>) {
+static struct CameraParameter {
+	int id;
+	int32_t posx, posy, posz, roty, rotx;
+};
+
+static struct CamerasSpecimen {
+	double fitness;
+	std::vector<CameraParameter>* cameras;
+};
+
+static float toFloat(int32_t num) {
+	return ((float)num) / 536870912.f;
+}
+
+static void evaluateSpecimen(std::unordered_map<int, CalibrationCamera> points, CamerasSpecimen& cameras) {
+	for (CameraParameter& camera : *cameras.cameras) {
+		CalibrationCamera& cam = points[camera.id];
+		cam.position.x = toFloat(camera.posx);
+		cam.position.y = toFloat(camera.posy);
+		cam.position.z = toFloat(camera.posz);
+		cam.rotation.x = toFloat(camera.rotx);
+		cam.rotation.y = toFloat(camera.roty);
+	}
+	PositionDistance posDst = positionFromPoints(points);
+	cameras.fitness = posDst.distance;
+}
 
+void TrackerManager::calibrateCameras(std::vector<CameraPoint> points) {
+	std::unordered_map<int, CalibrationCamera> sortedPoints;
+	for (CameraPoint& point : points) {
+		if (!sortedPoints.count(point.camera)) {
+			sortedPoints[point.camera] = { point.camera, new std::vector<CameraPoint*>(), glm::vec3(0.f, 0.f, 0.f) };
+		}
+		sortedPoints[point.camera].points->push_back(&point);
+	}
+	std::vector<CamerasSpecimen> population;
+	for (int i = 0; i < 100; i++) {
+		std::vector<CameraParameter>* cameras = new std::vector<CameraParameter>();
+		for (auto& calibrationCamera : sortedPoints) {
+			cameras->push_back({ calibrationCamera.first, rand(), rand(), rand(), rand(), rand() });
+		}
+		population.push_back({0, cameras});
+	}
+	for (int gen = 0; gen < 1000; gen++) {
+		for (auto& spec : population) {
+			evaluateSpecimen(sortedPoints, spec);
+		}
+	}
 }
 
 void TrackerManager::setPoint(CameraPoint& point) {