I have a puzzling error when manually creating a point cloud, which causes it to be distorted.

I made a 3D laser scanner in V-REP, using a proximity sensor that can rotate from left to right and from top to bottom. Sweeping over a certain range of two angles (phi and theta), I record the distance measurements returned by the proximity sensor.

From the angles and the distance information I compute a point in 3D space, relative to the proximity sensor's origin (see sphereCoordsToCartesianCoords function in attached code) and add it to a point cloud object.

To validate my results I also created a xyz file that can be imported into MeshLab. V-REP and MeshLab agree on the result/visualization, but as I said in the introduction, the result is distorted.

Please see the link below for a simple example scene where I scan a couple of cubes, and dynamically display the result in a point cloud.

As can be seen, the scanned surfaces in the point cloud are not straight, but concave (left and right surfaces/walls) or convex (top wall/surface).

If you got any idea where I made a mistake, maybe in converting to carthesian coordinates, or somewhere else, I'd be glad for a hint. I am out of ideas.

Here is the simple project (couldn't find a way to attach it directly): http://mh-nexus.de/downloads/PointCloudGeneration%20for%20calibration.ttt

In case the code is enough to get an idea, here it is:

Code: Select all

`sphereCoordsToCartesianCoords=function(yawAngle, pitchAngle, distance)`

local theta = yawAngle

local phi = pitchAngle

local r = distance

-- the following spherical coordinate transformation is only valid given these

-- assertions hold

assert(r >= 0)

assert((theta >= 0) and (theta <= math.pi))

assert((phi >= 0) and (phi < 2*math.pi))

x = r * math.sin(theta) * math.cos(phi)

y = r * math.sin(theta) * math.sin(phi)

z = r * math.cos(theta)

return x, y, z

end

threadFunction=function()

while simGetSimulationState()~=sim_simulation_advancing_abouttostop do

simSetJointTargetPosition(yawJoint, yaw_angle)

simSetJointTargetPosition(pitchJoint, pitch_angle)

simWait(0.001)

local result, dist=simReadProximitySensor(proxSensor)

if (result>0) then

local x, y, z = sphereCoordsToCartesianCoords(yaw_angle - min_yaw_angle, pitch_angle - min_pitch_angle, dist)

xyzfile:write(x .. " " .. y .. " " .. z .. "\n")

simInsertPointsIntoPointCloud(pointCloud, 1, {x, y, z})

-- logfile:write(yaw_angle .. " " .. pitch_angle .. " " .. distance .. "\n")

end

if (yaw_angle >= max_yaw_angle) then

yaw_angle = min_yaw_angle

pitch_angle = pitch_angle + angle_step

else

yaw_angle = yaw_angle + angle_step

end

if (pitch_angle > max_pitch_angle) then

break

end

-- Now don't waste time in this loop if the simulation time hasn't changed! This also synchronizes this thread with the main script

simSwitchThread() -- This thread will resume just before the main script is called again

end

end

-- Initialization:

simSetThreadSwitchTiming(200) -- We want to manually switch for synchronization purposes (and also not to waste processing time!)

xyzfile = io.open("proximityReadings.xyz", "w")

-- Get some handles first:

yawJoint=simGetObjectHandle("proxJointYaw")

pitchJoint=simGetObjectHandle("proxJointPitch")

proxSensor=simGetObjectHandle("proxSensor")

pointCloud=simGetObjectHandle("Point_cloud")

-- range 0 .. pi

min_yaw_angle = (-3 * math.pi) / 2

max_yaw_angle = -math.pi / 2

yaw_angle = min_yaw_angle

-- range 0 .. 0.75*pi

min_pitch_angle = (-5 * math.pi) / 4

max_pitch_angle = (-2 * math.pi) / 4

pitch_angle = min_pitch_angle

angle_step = 0.05

simSetJointTargetPosition(yawJoint, min_yaw_angle)

simSetJointTargetPosition(pitchJoint, min_pitch_angle)

simWait(3)

-- Execute the thread function:

res,err=xpcall(threadFunction,function(err) return debug.traceback(err) end)

if not res then

simAddStatusbarMessage('Lua runtime error: '..err)

end

-- Clean-up:

xyzfile:close()