Мы уже подсвечивали пеленги 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.


Реализация алгоритма Брезенхэма в 3D
    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, которая увеличивает счётчик проходящих через куб лучей:


fun 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.


Конструктивная критика от мастеров Котлина (и не только Котлина) приветствуется.


Спасибо за внимание!

Комментарии (5)


  1. aamonster
    09.12.2017 08:34

    Описание луча прямой — не самая светлая идея. Как минимум конус (с большим весом вдоль оси, чем по краям).


    Впрочем, если лучей не очень много — можно обойтись без воксельного пространства вообще. "Пересечения" ищутся просто — достаточно искать не точку пересечения, а две ближайшие точки прямых. Проходим по всем парам прямых, считаем, оставляем только те, где расстояние мало (сложность: "малость" расстояния зависит от расстояния до пеленгатора). Потом каждую точку проверяем на близость к остальным лучам (или просто прогоняем по серединам отрезков какой-то алгоритм поиска кластеров).
    Но с вокселями код всяко проще отладить.


    1. Ktator Автор
      09.12.2017 11:51

      Проходим по всем парам прямых ...

      Да, этот вариант, думаю, будет поточнее, но тут есть тонкость: мы получаем кубическую ассимптотику, и при большом числе лучей производительность просядет.

      А с конусами интересно, надо будет попробовать.


  1. aamonster
    09.12.2017 13:49

    Меньше, чем кубическую: первый проход — по парам прямых O(nn), из nn точек оставляем m — второй проход O(m*n).
    Но всё равно воксельная модель проще (в смысле, в её реализации труднее накосячить).


    1. Ktator Автор
      09.12.2017 15:08

      Но в плохом случае m = O(n*n), в результате получаем второй проход O(m*n) = O(n*n*n).


      1. aamonster
        09.12.2017 16:27

        Нет-нет, такой плохой случай можно исключить сразу. Если у нас получилось n*n равноправных точек — они или все ложные, или все истинные (и тогда можно просто выбрать из них часть более-менее произвольно — например, по 2 точки на прямую).


        Впрочем, это всё неважно — у вас уже есть более простой работоспособный алгоритм, позволяющий модификации "малой кровью".