The robot performed 5 angle tilts: 12.3, 6, 0, -7.4, -13.9 with the ToF sensor being 3.38" above the ground. There were 100 samples per angle (50 on either side of Y axis green) resulting in the mesh above. It  is not accurate due to the ToF sensor shifting as part of the robot's geometry.

The robot on the bottom right is not to scale. The blue squares are 1 ft so robot is 1.5' away from the box. Robot is 8.5" cubed roughly.

I used the multi-color panels because just a solid color it's hard to tell without shading the depth.

I will admit this took me way too long to do. I had the basics down, how to plot a line (3 points) and a plane (4 points). But this was super problematic as you can see below in my initial tries.

Oh yeah the bottom left of the plot, I believe that's the ToF sensor hitting the robot's own leg. So that would be an outlier to take out.

I had to start simple and use the lines/plane samples and check that the points lined up.

There is also a lot of data to organize/sort into something that makes sense from the IMU/ToF measurements.

This is a lot of data, it's 500 points which is 1500 coordinates. A lot with regard to the ESP-01 sending that data by Serial/Websocket. So I will simplify the "real time telemetry" as cubes (bounding boxes) and their location in 3D space with respect to the robot's initial 0, 0, 0 position and where it's been (linear accel/rotation velocity sum tracking).

There is unfortunately a lot of sources of error with this project. I have messed around with for example magnetometer calibration but have not calibrated the accelerometer. The sweep movement needs to include the outer legs, currently it sways around. I fixed the pitch because there it was very bad.

I could not get the pitch angle from the gyro x or NED (harder math) so I just used external visual measurements using a camera/SketchUp to measure the pitch angles.

Also when the robot moves the ToF sensor also moves in 3D offset, so the measurements above are not showing a straight face of the box, it has a slant.

Anyway it's progress. I got side tracked with another software project and also my day job but I want to finish this within February because I will have received the Pinephone Pro hopefully sometime then and that'll be the next thing I work on.

I will keep working on robots I have a new one in mind (think the Star Wars homing spider droid). I just thought about it when I was messing with a tripod for my camera, those could be it is legs and uses a pinion rack type gear to raise/lower them. Mainly though it would be big, as in be able to move at speed/cover ground and definitely overcome human obstacles eg. laundry on the floor, random plastic bags, boxes, etc... it also would feature better parts/better math. I will dump some time into the IMU stuff to really get that down.

But this also means a lot of money as I'm done buying cheap parts.

Anyway I will keep working on this, there's still some stuff to do, this is just ThreeJS visual stuff, the navigation will actually occur in the robot eg. the Teensy.

This math is still useful/I'll translate it into C++ from JS and use it for the mesh collision math (that's what I think at this time anyway).