Obtain global coordinates of a specific point.

[arghya003]

Hello everyone! In my scene, I have 3 joints mounted on a quadcopter that control the Yaw, Pitch and Roll of a vision sensor. I use these joints to track a moving marker. Here you can see the vision sensor.




I am detecting the marker using OpenCV from an external python script. Now, using the python script I can obtain the coordinates of the center of my marker. These coordinates are obviously in image plane. In order to transform these coordinates from the image plane to global coordinates, I went through the model provided at Models/components/sensors/Camera pixels to 3D positions.ttm. Following the method suggested in this model, I have added a non threaded child script to my vision sensor. Here is the code for it -

Code: Select all

-- lua

function sysCall_init()
    sim = require('sim')
    gimbal_camera = sim.getObject('.')
    resX=sim.getObjectInt32Param(gimbal_camera,sim.visionintparam_resolution_x)
    resY=sim.getObjectInt32Param(gimbal_camera,sim.visionintparam_resolution_y)
    
    xAngle=sim.getObjectFloatParam(gimbal_camera,sim.visionfloatparam_perspective_angle)
    yAngle=xAngle
    local ratio=resX/resY
    if resX>resY then
        yAngle=2*math.atan(math.tan(xAngle/2)/ratio)
    else
        xAngle=2*math.atan(math.tan(yAngle/2)/ratio)
    end
end

function sysCall_actuation()
    -- put your actuation code here
end

function sysCall_sensing(coord) -- coord is being supplied by my external python script.
    if coord ~= nil then
        local m=sim.getObjectMatrix(gimbal_camera)
        local depthMap=sim.getVisionSensorDepth(gimbal_camera,1)
        depthMap=sim.unpackFloatTable(depthMap)
    
        -- Obtain depth value information from depth man
        local index = coord[1] * resY + coord[2] + 1
        local depthValue = depthMap[index]
        
        -- Convert coordinates from image plane to camera frame of reference
        local cameraCoord = {0, 0, depthValue}
        cameraCoord[1] = depthValue * math.tan(xAngle * 0.5) * (0.5 - (coord[1] / (resX - 1))) / 0.5
        cameraCoord[2] = depthValue * math.tan(yAngle * 0.5) * ((coord[2] / (resY - 1)) - 0.5) / 0.5
        
        -- Transform 3D Camera Coordinates to Global Coordinates
        local globalCoord = sim.multiplyVector(m, cameraCoord)
        local actualCoord = sim.getObjectPosition(54, sim.handle_world) -- 54 is the handle of the landing surface.
        print(globalCoord, 'transformed coordinates', actualCoord, 'actual coordinates')
    end
end

function sysCall_cleanup()
    -- do some clean-up here
end

-- See the user manual or the available code snippets for additional callback functions and details

However, when I run this code, the results are not accurate. The simulation also lags quite a lot. How do I fix this? Or is there another new method to do what I am doing? You can see the difference in the results below -



Any kind of assistance or guidance will be quite helpful! Thank you in advance!

[coppelia]

Hello,

not sure I understand exactly. So your openCV function is returning the 3D position of the marker? That position would be relative to the vision sensor frame. So from there, all you need to do is multiply that position with the pose/matrix of the vision sensor, e.g.:

Code: Select all

local visionSensorPose = sim.getObjectPose(sensorHandle)
local absolutePosition = sim.multiplyVector(visionSensorPose, positionFromOpenCv)

Cheers

[arghya003]

Thank you for the quick reply! My OpenCV function is returning the 2d coordinates of the landing surface with respect to the image frame of reference.

[coppelia]

If open CV returns 2D coordinates, then you can't extract a point. From that, you can extract a line that passes though all the points the 2D coordinates maps onto.

But you can in a second step extract the depth of the 2D coordinate and then obtain a 3D point from there, like in following example:

Code: Select all

sim=require'sim'

function sysCall_init()
    sensor = sim.getObject('..')
    sphereContainer = sim.addDrawingObject(sim.drawing_spherepts, 0.03, 0, -1, 9999, {1, 0, 1})
    xAngle = sim.getObjectFloatParam(sensor, sim.visionfloatparam_perspective_angle)
    resX = sim.getObjectInt32Param(sensor, sim.visionintparam_resolution_x)
    resY = sim.getObjectInt32Param(sensor, sim.visionintparam_resolution_y)
    yAngle = xAngle
    local ratio = resX / resY
    if resX > resY then
        yAngle = 2* math.atan(math.tan(xAngle / 2) / ratio)
    else
        xAngle = 2 * math.atan(math.tan(yAngle / 2) / ratio)
    end

end

function sysCall_sensing()
    local pose = sim.getObjectPose(sensor)
    sim.addDrawingObjectItem(sphereContainer, nil)
    local pos = {0.0, 0.0} -- vary between 0 and 1
    local depth = sim.getVisionSensorDepth(sensor, 1, {math.floor(pos[1] * (resX - 0.99)), math.floor(pos[2] * (resY - 0.99))}, {1, 1})
    depth = sim.unpackFloatTable(depth)
    depth[1] = depth[1] - 0.05
    local coord = {0, 0, depth[1]}
    coord[1] = depth[1] * math.tan(xAngle * 0.5) * (0.5 - pos[1]) / 0.5
    coord[2] = depth[1] * math.tan(yAngle * 0.5) * (pos[2] - 0.5) / 0.5
    -- Coord is relative to the sensor frame
    coord = sim.multiplyVector(pose, coord)
    -- Coord is now relative to the absolute frame
    sim.addDrawingObjectItem(sphereContainer, coord)
end

function sysCall_cleanup()
    sim.removeDrawingObject(sphereContainer)
end

Cheers

[arghya003]

Hello,

Thank you for your answer! I am really new to CoppeliaSim. I have some doubts which might be quite trivial.

Can you please explain what is the purpose of

sphereContainer = sim.addDrawingObject(sim.drawing_spherepts, 0.03, 0, -1, 9999, {1, 0, 1}) (how are you getting these numbers?),

sim.addDrawingObjectItem(sphereContainer, nil)

and

depth[1] = depth[1] - 0.05

Also, can you please explain from where the following formula is coming

coord[1] = depth[1] * math.tan(xAngle * 0.5) * (0.5 - pos[1]) / 0.5
coord[2] = depth[1] * math.tan(yAngle * 0.5) * (pos[2] - 0.5) / 0.5


I am quite confused about all this. Thank you in advance for your assistance!