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/PointCloud ... ration.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()
```