Мы уже подсвечивали пеленги Wi-Fi точек в дополненной реальности, сегодня поговорим об их местоопределении.
Кому интересны технические подробности и при чём тут котелок, добро пожаловать под кат.
Также вашему вниманию предлагается фото- и видеоотчет о том, что получилось.
Постановка задачи
Есть пеленгатор Wi-Fi точек доступа, описанный здесь, и устройство дополненной реальности. Требуется подсветить местоположение заданной Wi-Fi точки.
Кратко напомню, что пеленгатор работает по принципу холодно-горячо: показывает насколько хорошо мы нацелились на объект.
Проблема
Для того, чтобы определить местоположение источника сигнала, требуется пересечь несколько лучей-пеленгов, желательно из разных позиций. И тут начинаются проблемы. При чём, проблемы начинаются даже в двумерном случае (когда мы хотим местоопределять на карте): три и больше лучей ну никак не хотят пересекаться в одной точке.
А в нашем случае даже два луча не хотят пересекаться.
Решение
Итак, точного решения у нас не получится. Придётся искать приближённое.
Базовый алгоритм основан на разбиении пространства на кубики. Для каждого кубика мы будем хоронить хранить, сколько лучей через него прошло. Несколько самых популярных кубиков нарисуем. Мне больше нравятся шары, поэтому я буду рисовать описанный вокруг куба шар.
Так как у нас лучей относительно мало, то предвидятся сильно разреженные данные, поэтому будем их хранить в хеш-таблице. В качестве ключа у нас будет пара координат кубика, а в качестве значения — число прошедших лучей.
Каждый пеленг имеет координаты начала (местоположение пеленгатора) и направление. Ввиду ограничений суровой реальности, мы, разумеется, пересекаем не лучи, а отрезки. Мало того, что мы ограничены памятью компьютера, так ещё и не можем засечь Wi-Fi точки на бесконечном удалении от нас :(
Для того, чтобы отметить кубики, в компьютерной графике есть алгоритм Брезенхэма. Правда, экраны сейчас пока только двумерные, поэтому везде описан алгоритм Брезенхэма на плоскости, но ничего, адаптируем для 3D.
Прежде, чем читать про реализацию, посмотрите видео, как это работает:
Камера, к сожалению, не хотела фокусироваться на телефоне, но при желании можно увидеть, что название точки, которую мы ищем и найденной совпадают.
Реализация
Легенда гласит, что когда яхта Петра I впервые подошла к берегам острова Котлин, охранявшие его шведские солдаты дали стрекача так быстро, что оставили после себя горящий костёр, на котором в котле готовилась еда. И именно из-за этого происшествия остров получил своё название.
Среди массовых устройств дополненной реальности, по прежнему, самые точные и доступные — это устройства на базе Google Tango. Поэтому, будем использовать их.
Подготовка данных
На вход нам подаётся текущая "поза" Tango устройства и событие, что в данный момент устройство с некоторой точностью направлено на источник сигнала. Для алгоритма Брезенхэма требуется получить начало и конец отрезка.
Но перед тем, как начать искать конец отрезка, посмотрим, получится ли найти заранее неизвестную Wi-Fi точку?
Для того, чтобы получить конец отрезка, требуется к z-компоненте точки начала прибавить его длину, а затем повернуть. Почему к z? Потому что в данной системе координат z — это "вперёд".
private float[] calcSagittaEnd(float[] start, float[] rotateMatrix) {
float[] end = new float[4];
System.arraycopy(start, 0, end, 0, 3);
end[3] = 0f;
end[2] += BEARING_LENGTH;
Matrix.multiplyMV(end, 0, rotateMatrix, 0, end, 0);
return end;
}Матрицу поворота мы берём из библиотеки Tango:
public boolean addBearing() {
/// area -> camera
TangoPoseData startPose = TangoSupport.getPoseAtTime(
mRgbTimestampGlThread,
TangoPoseData.COORDINATE_FRAME_AREA_DESCRIPTION,
TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR,
TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL,
0);
if (startPose.statusCode == TangoPoseData.POSE_VALID) {
float[] end = new float[4];
float[] start = new float[4];
start[0] = (float) startPose.translation[0];
start[1] = (float) startPose.translation[1];
start[2] = (float) startPose.translation[2];
start[3] = 0.0f;
TangoPointCloudData pointCloud =
mPointCloudManager.getLatestPointCloud();
if (pointCloud == null) {
return true; //Пеленга не будет: устройство плохо понимает, где находится.
}
// Мы должны посчитать преобразование между цветной камерой в момент получения пеленга и последними данными камеры глубины
TangoPoseData colorTdepthPose =
TangoSupport.calculateRelativePose(
mRgbTimestampGlThread,
TangoPoseData.COORDINATE_FRAME_CAMERA_COLOR,
pointCloud.timestamp,
TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH);
if (colorTdepthPose.statusCode == TangoPoseData.POSE_VALID) {
// Переходим к матрицам в OpenGL сцене
TangoSupport.TangoMatrixTransformData depthTarea =
TangoSupport.getMatrixTransformAtTime(
pointCloud.timestamp,
TangoPoseData.COORDINATE_FRAME_START_OF_SERVICE,
TangoPoseData.COORDINATE_FRAME_CAMERA_DEPTH,
TangoSupport.TANGO_SUPPORT_ENGINE_OPENGL,
TangoSupport.TANGO_SUPPORT_ENGINE_TANGO,
0);
if (depthTarea.statusCode == TangoPoseData.POSE_VALID) {
float[] depthTOpenGl = depthTarea.matrix;
mRenderer.addBearing(start, depthTOpenGl);
return true;
}
}
}
return false; //Пеленга не будет: устройство плохо понимает, где находится.
}Самое время посмотреть, как мы находим Wi-Fi точку кафе Шахерезада:
Заполнение кубиков
Назначим координатой x ту, на которую самая длинная проекция отрезка нашего пеленга.
В алгоритме Брезенхэма каждый шаг идёт инкремент координаты x, а координата y увеличивается только если набежала достаточная ошибка. Мы же будем делать то же самое ещё и с координатой z.
enum class LengthCoord {x, y, z}
val space = hashMapOf<Coordinate, Int>()
private fun markCubes(start: Coordinate, end: Coordinate, lengthCoord: LengthCoord) {
//Назначим координатой x ту, проекция на которую самая длинная.
fun getX(vertex: Coordinate) = when(lengthCoord) {
LengthCoord.x -> vertex.x
LengthCoord.y -> vertex.y
LengthCoord.z -> vertex.z
}
fun getY(vertex: Coordinate) = when(lengthCoord) {
LengthCoord.x -> vertex.y
LengthCoord.y -> vertex.x
LengthCoord.z -> vertex.y
}
fun getZ(vertex: Coordinate) = when(lengthCoord) {
LengthCoord.x -> vertex.z
LengthCoord.y -> vertex.z
LengthCoord.z -> vertex.x
}
fun setXYZ(x: Int, y: Int, z: Int, lengthCoord: LengthCoord): Coordinate =
when(lengthCoord) {
LengthCoord.x -> Coordinate(x, y, z)
LengthCoord.y -> Coordinate(y, x, z)
LengthCoord.z -> Coordinate(z, y, x)
}
val dx = when {
getX(end) > getX(start) -> 1
getX(end) < getX(start) -> -1
else -> 0
}
val dy = when {
getY(end) > getY(start) -> 1
getY(end) < getY(start) -> -1
else -> 0
}
val dz = when {
getZ(end) > getZ(start) -> 1
getZ(end) < getZ(start) -> -1
else -> 0
}
val d = arrayOf(Math.abs(getX(start) - getX(end)), Math.abs(getY(start) - getY(end)), Math.abs(getZ(start) - getZ(end)))
var x = getX(start)
var y = getY(start)
var z = getZ(start)
var errY = d[0] / 2
var errZ = d[0] / 2
while (x != getX(end)) {
x += dx
errY -= d[1]
errZ -= d[2]
if (errY < 0) {
y += dy
errY += d[0]
}
if (errZ < 0) {
z += dz
errZ += d[0]
}
addPoint(setXYZ(x, y, z, lengthCoord)) //Мы не хотим добавлять самый первый куб
}
addPoint(setXYZ(x, y, z, lengthCoord))
}Функция addPoint, которая увеличивает счётчик проходящих через куб лучей:
private fun addPoint(coord: Coordinate) {
lock.withLock{
var v = space[coord]
if (v != null) {
v++
space[coord] = v.toInt()
} else {
space[coord] = 1
}
}
}Выбираем самые популярные кубики: сортируем, а затем берём первые несколько. В текущей реализации не более трёх.
fun getIntersection(): List<Array<Float>> {
val resCandidates = ArrayList<Pair<Array<Float>, Int>>()
lock.withLock {
// Выбираем все кубики, через которые проходили лучи.
for ((coord, counter) in space) {
if (counter >= showThreshold) {
resCandidates.add(Pair(scaleCoordinate(arrayOf(
coord.x.toFloat(),
coord.y.toFloat(),
coord.z.toFloat())),
counter))
}
}
}
resCandidates.sortBy { item -> -item.second } // Сортируем по убыванию.
// Выбираем первые несколько элементов
return resCandidates.subList(0, minOf(
amountCellsToShow,
resCandidates.size)
).map { elem -> elem.first }
}Заключение
Был рассмотрен подход к решению задачи пространственного местоопределения объекта по заданным пеленгам и предложена его реализация.
Весь исходный код доступен на GitHub.
Конструктивная критика от мастеров Котлина (и не только Котлина) приветствуется.
Спасибо за внимание!
Автор: Михаил Лукин
