An overview of how LIDAR data is converted to world coordinates.
The data sources and how they work
GPS coordinate systems
The Novatel ProPak LB GPS returns coordinates in "earth centered, earth fixed" (ECEF) coordinates. This is a coordinate system centered at the center of the earth, with the Z axis through the North Pole and the X and Y axes through the equator. The GPSINS program transforms ECEF coordinates into coordinates relative to a plane tangent to the earth's surface at a specified point. Currently, in "gpsins", the tangent point is fixed, and it is centered on a point in the driveway of the Overbot shop. In "fusednav", which will replace "gpsins" next week, the first waypoint is used to determine the tangent point. The tangent point needs to be within a few hundred kilometers of the vehicle position, since the tangent plane assumes a locally flat earth.
This transformation is done with a standard function (ECEF_dist in Nav.cc in gc/src/qnx/common/lib) from an autopilot software library, and yields "north, east, down" (NED) coordinates. The "north, east, down" coordinates are placed in GPSINS messages by the GPSINS program, and appear in the pos field of the GPSINS_MSG records (defined in gpsins_messaging.h from /gc/src/qnx/common/include). Those are the position values in both GPS logs and messages from the GPSINS server.
Note that these positions are measured from the GPS antenna on the back bed of the vehicle.
This data appears, at present, to be generally valid. GPS position is accurate to about 15 cm in OMNISTAR_HP mode, to about 1m in OMNISTAR mode, and to tens of meters in other modes.
With "fusednav", the position in pos is adjusted using dead-reckoning and filtering, and, rather than looking at the posStat and posType fields to determine the GPS mode, the "uncertainty" field (unc) is checked. The position reported should be within the circle defined by max(unc, unc) almost all the time.
AHRS coordinate systems
The Crossbow Attitude and Heading Reference System (AHRS 400) provides heading and attitude information. It consists of three accelerometers, three rate gyros, a flux-gate compass, and a digital signal processor to process the data and output it on a serial port. It outputs records providing roll, pitch, and yaw, expressed as signed 16-bit values in the range -32768 .. 32767. The GPSINS program (in AHRS.cc in gc/src/qnx/common/nav/gpsins) converts these to degrees and puts them in the rpy (roll, pitch, yaw) field of the GPSINS records. Yaw is corrected using the compass and inertial data. Pitch and roll are entirely from inertial and gravity data.
The coordinate system of the AHRS unit is defined by Crossbow as follows:
Note that roll, pitch, and yaw are all zero when the vehicle is flat on the ground and pointed north.
The roll, pitch, and yaw values from the AHRS appear to be generally correct at this point.
GPSINS data is updated 20 times per second. AHRS data is updated 50 times per second. There may be trouble if the GPS timestamp is being updated on AHRS updates.
LIDAR data is scanned at 75 scan lines per second, synchronized with tilt head position, and transmitted to the MAP program with a tilt angle that should be valid for the scan line. Tilt is in radians, with 0 pointing down and positive values pointing forward.
How this data is transformed
The MAP program queries the GPSINS (or fusednav) server and receives GPSINS messages as defined in gpsins_messaging.h. It must then synchronize this information with the LIDAR scan lines and tilt data.
The GPSINS messages are converted to 4x4 transformation matrices in posefromfix in geocoords.cc in gc/src/qnx/nav/map. (All pathse here refer to CVS). This is a likely source of trouble. The key code reads as follows.
The call to Eul_ToHMatrix converts a set of Euler angles to a 4x4 transformation matrix. There are 24 standard ways to do this, depending on the order in which the axes are coupled. The constant EulOrdXYZr selects one of these. See eulerangle.cc in the same directory. The order, XYZ, matches the Crossbow manual. The "r" suffix means that the axes are "rotating", as opposed to "static". This may be wrong.
All the general-purpose matrix math, including the operations on vec3, and mat4 types, is in algebra3.h in gc/src/qnx/common/include.
All this gets us the vehicle position and orientation, usually referred to in the code as "vehpose". The "vehpose" matrix should transform a point in vehicle coordinates to world coordinates.
GPSINS records are timestamped, and are normally 50ms apart. We need a valid position for each LIDAR scan line, so we must do some interpolation. The actual interpolation is in interpolateposedumb in geocoords.cc. This converts pose matrices to angles, interpolates the angles, and converts them back. This uses more trig functions than necessary and has a singularity at attitudes the vehicle can never reach (i.e. pointed straight up), which is why it is called "dumb". It should be looked at. The proper solution uses quaternions and an algorithm called SLERP.
Vehicle pose management, which keeps the last few timestamped vehicle poses and will interpolate a position for any recent time, is in vehiclepose.cc. This calls interpolateposedumb.
LIDAR data is synchronized with GPS data in LMShandleLidarData in LMSmapupdate.cc. This obtains an interpolated vehpose matrix that matches the timestamp of the scan line.
The synchronized vehicle pose matrix and scan line are passed to LMShandlePosedLidarData. The tilt data has not yet been combined with the vehicle position data. The "tilt corrector" in LMStiltcorrect.cc is then called with the scan line data and the tilt information, and has an opportunity to modify the tilt. (As of August 17th, tilt correction is not turned on).
LMStiltPose is called to convert the tilt angle to a 4x4 transformation matrix.
This computes the position and orientation of the LIDAR scan head in world space. Note that two transformation matrices have been combined by multiplying them. This seems to be correct, but note that this multiplication is not commutative.
LMStiltpose is worth a look.
The tilt pose matrix should transform a point in scanner coordinates to vehicle coordinates. And scannerpose should transform a point in scanner coordinates to world coordinates.
The actual scan data consists of arrays of distances, in integer centimeters, from the scanner. Ranges are first computed to floating point, and meters, in LMSupdateScanlinePair.
The computations involving k_scan_table convert LIDAR ranges into points in the plane of the scan, using a precomputed table of radial vectors. X is ahead and Y is to the right.
Those last multiplications transform points in scanner space into points in world space, which is where everything comes together.
Somewhere this is wrong when the vehicle is not on flat terrain. Where?