addRelativePose

Добавьте относительное положение в график положения

Описание

addRelativePose(poseGraph,measurement) создает узел на основе входа measurement который соединяется с последним узлом положения в графике положения. Чтобы добавить знаковые узлы, смотрите addPointLandmark функция.

addRelativePose(poseGraph,measurement,infoMat) также задает информационную матрицу как часть ограничения ребра, которое представляет неопределенность измерения положения.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID) создает новый узел положения и соединяет его с определенным узлом, заданным fromNodeID.

addRelativePose(poseGraph,measurement,infoMat,fromNodeID,toNodeID) создает ребро путем определения относительного измерения положения между существующими узлами, заданными fromNodeID и toNodeID. Это ребро называется loop closure. Если замыкание цикла уже существует, функция добавляет новое измерение. Вызов optimizePoseGraph Функция объединяет несколько добавленных измерений в одно ребро. Этот синтаксис не поддерживает добавление ребер к знаковому узлу.

[nodePair,edgeID] = addRelativePose(___) возвращает добавленные идентификатор ребра и ребра с помощью любого из предыдущих синтаксисов.

Входные параметры

свернуть все

График положения, заданный как poseGraph или poseGraph3D объект.

Относительное положение между узлами, заданное как одно из следующего:

Для poseGraph (2-D), положение является [x y theta] вектор, который задает xy положение и угол ориентации, theta.

Для poseGraph3D, положение является [x y z qw qx qy qz] вектор, который определяется xyz ориентацией -position и quaternion, [qw qx qy qz]

Примечание

Многие другие источники для 3-D графиков положения, включая .g2o форматы, задайте ориентацию кватерниона в другом порядке, например [qx qy qz qw]. Проверьте источник данных графика положения перед добавлением узлов к poseGraph3D объект.

Информационные матрицы, заданные в компактном виде как вектор с 6 элементами или вектор с 21 элементами.

Каждая строка является верхним треугольником квадратной информационной матрицы. Информационная матрица представляет неопределенность измерения. Матрица вычисляется как обратная ковариации. Если измерение является [x y theta] вектор, ковариационная матрица является 3 на 3 парных ковариационных вычислений. Обычно неопределенность определяется моделью датчика.

Для poseGraph (2-D), каждая информационная матрица является вектором с шестью элементами. Значение по умолчанию является [1 0 0 1 0 1]. Для знаковых узлов последние три элемента возвращаются следующим NaN.

Для poseGraph3Dкаждая информационная матрица является вектором с 21 элементом. Значение по умолчанию является [1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1].

Узел для присоединения, заданный как положительное целое число. Это целое число соответствует идентификатору узла в poseGraph. При указании без toNodeID, addRelativePose создает новый узел и добавляет ребро между новым узлом и fromNodeID узел.

Узел для присоединения, заданный как положительное целое число. Это целое число соответствует идентификатору узла в poseGraph. addRelativePose добавляет ребра между этим узлом и fromNodeID узел.

Выходные аргументы

свернуть все

Ребро пар узлов в графике положения, возвращенный как двухэлементный вектор, который приводит идентификаторы двух узлов, которые соединяет каждое ребро. Между одной и той же парой узлов может существовать несколько ребер.

Идентификатор добавленного ребра, возвращенный как положительное целое число.

Расширенные возможности

.
Введенный в R2019b