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(___) возвращает недавно добавленное ребро и ID ребра с помощью любого из предыдущих синтаксисов.

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

свернуть все

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

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

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

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

Примечание

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

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

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

Для poseGraph (2D), каждая информационная матрица является вектором с шестью элементами. Значением по умолчанию является [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 узел.

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

свернуть все

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

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

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

Введенный в R2019b