Pepper 机器人：“LandmarkDetection”基于哪个身体框架？

Question

一段时间以来，我一直试图弄清楚“LandmarkDetection”模块基于哪个坐标或身体框架。因为我需要(0, 0, 0)位置进行计算，以实现机器人在“地标检测”的帮助下进行自我定位，所以我认为这已经很重要了…… 我已经让它与NAOqi 2.5.5下的Pepper机器人一起工作，我希望它兼容。

您可以在此处阅读“LandmarkDetection.py”的完整代码：

“LandmarkDetection.py”：

#! /usr/bin/env python
# -*- encoding: UTF-8 -*-

"""示例：演示一种定位机器人的方法 ALLandMarkDetection"""

import qi
import time
import sys
import argparse
import math
import almath


class LandmarkDetector(object):

"""
We first instantiate a proxy to the ALLandMarkDetection module
Note that this module should be loaded on the robot's naoqi.
The module output its results in ALMemory in a variable
called "LandmarkDetected".
We then read this ALMemory value and check whether we get
interesting things.
After that we get the related position of the landmark compared to robot.
"""

def __init__(self, app):
    """
    Initialisation of qi framework and event detection.
    """
    super(LandmarkDetector, self).__init__()

    app.start()
    session = app.session
    # Get the service ALMemory.
    self.memory = session.service("ALMemory")
    # Connect the event callback.

    # Get the services ALMotion & ALRobotPosture.

    self.motion_service = session.service("ALMotion")
    self.posture_service = session.service("ALRobotPosture")

    self.subscriber = self.memory.subscriber("LandmarkDetected")
    print "self.subscriber = self.memory.subscriber(LandmarkDetected)"
    self.subscriber.signal.connect(self.on_landmark_detected)
    print "self.subscriber.signal.connect(self.on_landmark_detected)"
    # Get the services ALTextToSpeech, ALLandMarkDetection and ALMotion.
    self.tts = session.service("ALTextToSpeech")
    self.landmark_detection = session.service("ALLandMarkDetection")
  #  print "self.landmark_detection" is repr(self.landmark_detection)
    self.motion_service = session.service("ALMotion")
    self.landmark_detection.subscribe("LandmarkDetector", 500, 0.0 )
    print "self.landmark_detection.subscribe(LandmarkDetector, 500, 0.0 )"
    self.got_landmark = False
    # Set here the size of the landmark in meters.
    self.landmarkTheoreticalSize = 0.06 #in meters 0  #.05 or 0.06?
    # Set here the current camera ("CameraTop" or "CameraBottom").
    self.currentCamera = "CameraTop"

def on_landmark_detected(self, markData):
    """
    Callback for event LandmarkDetected.
    """
    while markData == [] :  # empty value when the landmark disappears
        self.got_landmark = False
        self.motion_service.moveTo(0, 0, 0.1 * math.pi)

    if not self.got_landmark:  # only speak the first time a landmark appears
        self.got_landmark = True

#stop.motion_service.moveTo

        print "Ich sehe eine Landmarke! "
        self.tts.say("Ich sehe eine Landmarke! ")

        # Retrieve landmark center position in radians.
        wzCamera = markData[1][0][0][1]
        wyCamera = markData[1][0][0][2]

        # Retrieve landmark angular size in radians.
        angularSize = markData[1][0][0][3]

        # Compute distance to landmark.
        distanceFromCameraToLandmark = self.landmarkTheoreticalSize / ( 2 * math.tan( angularSize / 2))

        # Get current camera position in NAO space.
        transform = self.motion_service.getTransform(self.currentCamera, 2, True)
        transformList = almath.vectorFloat(transform)
        robotToCamera = almath.Transform(transformList)

        # Compute the rotation to point towards the landmark.
        cameraToLandmarkRotationTransform = almath.Transform_from3DRotation(0, wyCamera, wzCamera)

        # Compute the translation to reach the landmark.
        cameraToLandmarkTranslationTransform = almath.Transform(distanceFromCameraToLandmark, 0, 0)

        # Combine all transformations to get the landmark position in NAO space.
        robotToLandmark = robotToCamera * cameraToLandmarkRotationTransform *cameraToLandmarkTranslationTransform

#    robotTurnAroundAngle = almath.rotationFromAngleDirection(0, 1, 1, 1)
#        print "robotTurnAroundAngle = ", robotTurnAroundAngle

        print "x " + str(robotToLandmark.r1_c4) + " (in meters)"
        print "y " + str(robotToLandmark.r2_c4) + " (in meters)"
        print "z " + str(robotToLandmark.r3_c4) + " (in meters)"

def run(self):
    """
    Loop on, wait for events until manual interruption.
    """

    # Wake up robot
    self.motion_service.wakeUp()

    # Send robot to Pose Init
    self.posture_service.goToPosture("StandInit", 0.5)

    # Example showing how to get a simplified robot position in world.
    useSensorValues = False
    result = self.motion_service.getRobotPosition(useSensorValues)
    print "Robot Position", result

    # Example showing how to use this information to know the robot's diplacement.
    useSensorValues = False
    #   initRobotPosition = almath.Pose2D(self.motion_service.getRobotPosition(useSensorValues))

    # Make the robot move
    for i in range(1, 12, 1):
        self.motion_service.moveTo(0, 0, 0.1 * math.pi)
        print "self.motion_service.moveTo(0, 0, (0.1)*math.pi)"

    print "Starting LandmarkDetector"
    try:
        while True:
            time.sleep(1)
    except KeyboardInterrupt:
        print "Interrupted by user, stopping LandmarkDetector"
        self.landmark_detection.unsubscribe("LandmarkDetector")
        #stop
        sys.exit(0)


if __name__ == "__main__":


    parser = argparse.ArgumentParser()
    parser.add_argument("--ip", type=str, default="10.0.0.10",
                    help="Robot IP address. On robot or Local Naoqi: use 
'10.0.0.10'.")
    parser.add_argument("--port", type=int, default=9559,
                    help="Naoqi port number")

    args = parser.parse_args()
    try:
        # Initialize qi framework.
        connection_url = "tcp://" + args.ip + ":" + str(args.port)
        app = qi.Application(["LandmarkDetector", "--qi-url=" + connection_url])
    except RuntimeError:
        print ("Can't connect to Naoqi at ip \"" + args.ip + "\" on port " + str(args.port) +".\n"
               "Please check your script arguments. Run with -h option for help.")
        sys.exit(1)
    landmark_detector = LandmarkDetector(app)
    landmark_detector.run()

在第 69 行，如图所示，它显示 NAO 空间，但在谷歌搜索并在 Aledebaran 官方网站上查看此概念后，我没有找到它。 在“transform = self.motion_service.getTransform(self.currentCamera, 2, True)”的函数“getTransform()”的第70行，结合Aldebaran网站的信息：Cartesian control API：参数“ 2”可以代表“FRAME_ROBOT”。 由于官方文档，“FRAME_ROBOT”（我认为是 NAO）是 ALMotion 使用的 3 个空间参考之一。这是围绕垂直 z 轴投影的两只脚位置的平均值。这个空间很有用，因为 x 轴总是向前的，所以提供了一个自然的以自我为中心的参考。 但以下身体框架中的哪一个是 Pepper 的 FRAME 坐标？ Body frames - Aldebaran

基本上传感器值应该基于这张图片中的坐标：

我刚刚跟踪了转换的值：

('wzCamera = markData[1][0][0][1] = :', 0.20727014541625977)
('wyCamera = markData[1][0][0][2] = :', 0.13570936024188995)
robotToCamera = [[0.984479, 0.00163947, -0.175494, 0.0191678]
 [-0.00241521, 0.999988, -0.00420686, -0.00231434]
 [0.175485, 0.00456542, 0.984471, 1.16607]]
cameraToLandmarkRotationTransform =[[0.969599, -0.205789, 0.132397, 0]
 [0.203897, 0.978596, 0.0278419, 0]
 [-0.135293, 0, 0.990806, 0]]
cameraToLandmarkTranslationTransform=[[1, 0, 0, 1.32545]
 [0, 1, 0, 0]
 [0, 0, 1, 0]]
robotToLandmark = [[0.978627, -0.200991, -0.0434926, 1.31629]
 [0.202122, 0.979082, 0.0233536, 0.265588]
 [0.0378889, -0.0316453, 0.998781, 1.21629]]

结合下图的信息

来自视频：Robotics: 3D World to First Person Transformation

我真的很想知道“robotToCamera”的“robot”是否意味着原始cemara点...... 有人知道如何理解“robotToCamera”中的这些矩阵值吗？

观察“robotTocamera”的值，当机器人旋转自己（全身+头部，他不同时点头）寻找墙壁上的地标时，它会发生一点变化：

1.  robotToCamera =
 [[0.979136, 0.0719852, -0.190025, 0.0157459]
 [-0.0700786, 0.997401, 0.0167426, -0.00282726]
 [0.190737, -0.00307653, 0.981636, 1.16688]]
2.  robotToCamera = 
[[0.981156, 0.0583434, -0.184198, 0.0168686]
 [-0.057959, 0.998291, 0.00747475, -0.00518941]
 [0.184319, 0.00334202, 0.982861, 1.16652]]
3.  robotToCamera = 
[[0.981628, 0.0598889, -0.18116, 0.0196037]
[-0.0594456, 0.9982, 0.0078808, -0.00525115]
 [0.181306, 0.00303314, 0.983422, 1.16649]]
4.  robotToCamera = 
[[0.982268, 0.0583533, -0.178169, 0.0191384]
 [-0.0579817, 0.998291, 0.0072969, -0.00522032]
 [0.17829, 0.00316301, 0.983973, 1.16627] ]
5.  robotToCamera = 
[[0.984, 0.021, -0.178, 0.016]
 [-0.021, 1.00, 0.0027, -0.0019]
 [0.178, 0.001, 0.984, 1.17] ]

这是我的几个问题：

我们可以通过“robotToCamera”来计算“Robot”的位置吗？

我们能否使用“robotToCamera”的值来获得“CameraToRobot”的值，例如计算逆矩阵或伴随矩阵？

对于“robotToCamera”中的第五个矩阵，如果我们计算零件的逆矩阵

[[0.984, 0.021, -0.178]
 [-0.021, 1.00, 0.0027,]
 [0.178, 0.001, 0.984] ]

应该是

[[0.984, -0.021,-0.178]
 [0.021, 1.00, -0.001,]
 [-0.178, 0.0027, 0.984] ]

等于它的伴随矩阵。那么它应该是由于维基百科的正交矩阵。

我可以从中获得什么信息？

视频6点10分，老师继续讲解镜头：

在我的情况下，不需要学习lens(L)，或者？

Answer 1

地标检测是用其顶部摄像头检测 NAOMark。该文档解释说您在事件值中获得“形状信息”，例如：

ShapeInfo = [ 1, alpha, beta, sizeX, sizeY, heading]

alpha 和 beta 代表 Naomark 在相机方面的位置 角度

sizeX 和 sizeY 是标记的相机角度大小

航向角描述了 Naomark 如何围绕 相对于机器人头部的垂直轴。

然后代码调用 getTransform，它将为您提供相机的方向，因此您可以将检测到的标记角度与机器人前方的实际方向 (X,Y,Z)（成为机器人框架）相关联。

如果需要，标记在这里：http://doc.aldebaran.com/2-5/naoqi/vision/allandmarkdetection.html