Hello,
are your extracted points distorted? I guess that you simply forgot to multiply the points with the vision sensor frame transformation, i.e. each point should be multiplied with the matrix of the vision sensor in order to obtain absolute coordinates. Maybe try something like (not tested):
Code: Select all
function sysCall_vision(inData)
simVision.sensorImgToWorkImg(inData.handle)
trig,packedPacket=simVision.coordinatesFromWorkImg(inData.handle,{512,512},false)
local ptsX=sim.unpackFloatTable(packedPacket,0,1)
local ptsY=sim.unpackFloatTable(packedPacket,1,1)
local pts=sim.unpackFloatTable(packedPacket,2)
local correctedPacket={}
local m=sim.getObjectMatrix(inData.handle,-1)
for i=0,(ptsX*ptsY)-1,1 do
local v={pts[4*i+1],pts[4*i+2],pts[4*i+3]}
sim.multiplyVector(m,v)
pts[4*i+1]=v[1]
pts[4*i+2]=v[2]
pts[4*i+3]=v[3]
end
sim.setStringSignal("camera_d_msg",correctedPacket)
simVision.workImgToSensorImg(inData.handle,false)
end
Above might eb quite slow. In next release (V4.1.0, out within 1-2 weeks), you'll be able to do:
Code: Select all
trig,packedPacket=simVision.coordinatesFromWorkImg(inData.handle+sim.handleflag_abscoords,{512,512},false)
to retrieve points relative to the absolute reference frame, which will be much faster than above.
Cheers