Visual tracking object

Typically: "How do I... ", "How can I... " questions
Post Reply
ghassenouerfelli
Posts: 4
Joined: 08 Mar 2023, 12:05

Visual tracking object

Post by ghassenouerfelli »

Hello coppelia,
I need to track an object (Quadcopter) with vision senor which is related to 2 joints yaw and pitch,
i create a path to make the quadcoter move and also i made the 2 joints and i fix their proprieties but i had a problem to code the vision senor child script(lua) and to make it track the quadcopter and make a bounding box on it.
I'm new with coppelia community and i need your help.
Cordially thanks.

ghassenouerfelli
Posts: 4
Joined: 08 Mar 2023, 12:05

Re: Visual tracking object

Post by ghassenouerfelli »

hello again , i wish can you replay to my post,
so i made a code to track a quadcopter with a vision senor and i need to extarct informations from the vision sensor to work with it (machine learning) so first of all i need to solve my problem
this is the code can you correct it for me
thanks

function sysCall_init()

yawJoint = sim.getObject("/yaw")
pitchJoint = sim.getObject("/pitch")

quadcopter = sim.getObject("/Quadcopter")

camera = sim.getObject(".")
sphereContainer = sim.addDrawingObject(sim.drawing_spherepoints, 0.02, 0, -1, 1000, {1, 0, 0})

Kp = 2
resX = 128
resY = 128
xAngle = math.rad(45)
yAngle = math.rad(45)

end

function sysCall_actuation()

local pos = sim.getObjectPosition(quadcopter, -1)

local camPos = sim.getObjectPosition(camera, -1)

local dx = pos[1] - camPos[1]
local dy = pos[2] - camPos[2]
local dz = pos[3] - camPos[3]
local distance = math.sqrt(dx*dx + dy*dy + dz*dz)
-- Calculer l'angle de rotation en fonction de la position du "quadcopter"
local pitch = math.atan2(dz, math.sqrt(dx*dx + dy*dy))
local yaw = math.atan2(dy, dx) + math.pi
-- Ajouter une correction proportionnelle en fonction de la distance
local pitchCorrection = Kp * (pos[3] - camPos[3])
pitch = pitch + pitchCorrection

sim.setJointPosition(yawJoint, yaw)
sim.setJointPosition(pitchJoint, pitch)
end
-- Variables pour stocker les coordonn?es de la quadcopter
local quadcopterCoords = {0, 0, 0}

function sysCall_sensing()
-- R?cup?rer la matrice de l'objet de la vision sensor
local m=sim.getObjectMatrix(camera,sim.handle_world)
-- Ajouter un objet de dessin pour chaque blob d?tect?
sim.addDrawingObjectItem(sphereContainer,nil)
-- Obtenir les donn?es de la vision sensor
local res,packet1,packet2=sim.handleVisionSensor(camera)
if res>=0 then
local blobCnt=packet2[1]
local valCnt=packet2[2]
if blobCnt~= nil then
for i=0,blobCnt-1,1 do
local blobSize=packet2[2+valCnt*i+1]
local blobOrientation=packet2[2+valCnt*i+2]
local blobPositionX=packet2[2+valCnt*i+3]
local blobPositionY=packet2[2+valCnt*i+4]
local blobWidth=packet2[2+valCnt*i+5]
local blobHeight=packet2[2+valCnt*i+6]
-- Obtenir la profondeur a partir de la vision sensor
local depth=sim.getVisionSensorDepth(camera,1,{1+math.floor(blobPositionX*(resX-0.99)),1+math.floor(blobPositionY*(resY-0.99))},{1,1})
depth=sim.unpackFloatTable(depth)
local coord={0,0,depth[1]}

-- Calculer les coordonnees de la quadcopter a partir de la profondeur et de l'angle de vue
local x=0.5-blobPositionX
local y=blobPositionY-0.5
coord[1]=depth[1]*math.tan(xAngle*0.5)*(0.5-blobPositionX)/0.5
coord[2]=depth[1]*math.tan(yAngle*0.5)*(blobPositionY-0.5)/0.5
coord=sim.multiplyVector(m,coord)
-- Mettre a jour les coordonnees de la quadcopter
quadcopterCoords[1] = coord[1]
quadcopterCoords[2] = coord[2]
quadcopterCoords[3] = coord[3]

sim.auxiliaryConsolePrint(res,string.format("Coordonnees de la quadcopter dans la cam?ra : x=%.2f, y=%.2f, z=%.2f", quadcopterCoords[1], quadcopterCoords[2], quadcopterCoords[3]))
-- Ajouter un objet de dessin pour le blob
sim.addDrawingObjectItem(sphereContainer,coord)
end
end
end
end

ghassenouerfelli
Posts: 4
Joined: 08 Mar 2023, 12:05

Re: Visual tracking object

Post by ghassenouerfelli »

pleaaaase help me coppelia :(

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: Visual tracking object

Post by coppelia »

Hello,

visual tracking is not that easy, and we cannot debug your code. Best is to start from something that is working, and to expand from there.
In Models/components/sensors/, you can find a few relevant models, e.g.:
  • Blob to 3D position.ttm
  • Camera pixels to 3D positions.ttm
Have a look at how those models operate

Cheers

ghassenouerfelli
Posts: 4
Joined: 08 Mar 2023, 12:05

Re: Visual tracking object

Post by ghassenouerfelli »

Hi again,
I made this code and it is debugging, but the problem here is i need to detect the object first of all and then make the vision sensor track it, so i need to extrat the cordinate of the blob and track it and make the object in the center of the cam.
And can you help me how to make a bounding box for the object tracked.

Code: Select all

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
    yawJoint = sim.getObject("/yaw")
    pitchJoint = sim.getObject("/pitch")
    quadcopter = sim.getObject("/Quadcopter")
    Kp=1
    graph=sim.getObject('/Graph')
    joint1Vel=sim.addGraphStream(graph,'Yaw velocity','deg/s',0,{1,0,0})
    joint2Vel=sim.addGraphStream(graph,'Pitch velocity','deg/s',0,{0,1,0})
end

function sysCall_vision(inData)
    local retVal={}
    retVal.trigger=false
    retVal.packedPackets={}
    simVision.sensorImgToWorkImg(inData.handle)
    local trig,packedPacket=simVision.blobDetectionOnWorkImg(inData.handle,0.100000,0.000000,true) if trig then retVal.trigger=true end if packedPacket then retVal.packedPackets[#retVal.packedPackets+1]=packedPacket end
    simVision.workImgToSensorImg(inData.handle)
    return retVal
end


function sysCall_sensing()
    local m=sim.getObjectMatrix(sensor,sim.handle_world)
    sim.addDrawingObjectItem(sphereContainer,nil)
    local res,packet1,packet2=sim.handleVisionSensor(sensor)
    if res>=0 then
        local blobCnt=packet2[1]
        local valCnt=packet2[2]
        for i=0,blobCnt-1,1 do
            local blobSize=packet2[2+valCnt*i+1]
            local blobOrientation=packet2[2+valCnt*i+2]
            local blobPositionX=packet2[2+valCnt*i+3]
            local blobPositionY=packet2[2+valCnt*i+4]
            local blobWidth=packet2[2+valCnt*i+5]
            local blobHeight=packet2[2+valCnt*i+6]
            local depth=sim.getVisionSensorDepth(sensor,1,{1+math.floor(blobPositionX*(resX-0.99)),1+math.floor(blobPositionY*(resY-0.99))},{1,1})
            depth=sim.unpackFloatTable(depth)
            local coord={0,0,depth[1]}
            local x=0.5-blobPositionX
            local y=blobPositionY-0.5
            coord[1]=depth[1]*math.tan(xAngle*0.5)*(0.5-blobPositionX)/0.5
            coord[2]=depth[1]*math.tan(yAngle*0.5)*(blobPositionY-0.5)/0.5
            coord=sim.multiplyVector(m,coord)
            -- coord now contains the position of the blob, in world coordinates
            sim.addDrawingObjectItem(sphereContainer,coord)
            -- Calculate the 2D coordinates of the blob in the vision sensor's image
            local xCoord = math.floor(blobPositionX * resX)
            local yCoord = math.floor(blobPositionY * resY)
    
            -- Print the 2D coordinates of the blob
            print('Blob ' .. i .. ' coordinates: (' .. xCoord .. ', ' .. yCoord .. ')')
        end
    end
      sim.setGraphStreamValue(graph,joint1Vel,180*sim.getJointVelocity(yawJoint)/math.pi)
    sim.setGraphStreamValue(graph,joint2Vel,180*sim.getJointVelocity(pitchJoint)/math.pi)
end
function sysCall_actuation()
   
    local pos = sim.getObjectPosition(quadcopter, -1)
 
    local camPos = sim.getObjectPosition(sensor, -1)
    
    local dx = pos[1] - camPos[1]
    local dy = pos[2] - camPos[2]
    local dz = pos[3] - camPos[3]
    --local distance = math.sqrt(dx*dx + dy*dy + dz*dz)
    -- Calculer l'angle de rotation en fonction de la position du "objet"
    local pitch = math.atan2(dz, math.sqrt(dx*dx + dy*dy))
    local yaw = math.atan2(dy, dx) + math.pi
    -- Ajouter une correction proportionnelle en fonction de la distance
    local pitchCorrection = Kp * (0.5 - camPos[3])
    local yawCorrection = Kp * (0.5 - camPos[2])
    pitch = pitch + pitchCorrection
    yaw   = yaw + yawCorrection

    sim.setJointPosition(yawJoint, yaw)
    sim.setJointPosition(pitchJoint, pitch)
end

function sysCall_cleanup()
    sim.removeDrawingObject(sphereContainer)
end

Post Reply