3D point cloud map alignment with open street map for outdoor 6D localization on mobile platforms
A method of mapping and localization is disclosed that includes, reconstructing a point cloud and a camera pose based on VSLAM, synchronizing the camera pose and a GPS timestamp at a first set of GPS coordinate points and transforming the first set of GPS coordinate points corresponding to the GPS timestamp into a first set of ECEF coordinate points. The method also includes determining a translation and a rotation between the camera pose and the first set of ECEF coordinate points, transforming the point cloud and the camera pose into a second set of ECEF coordinates based on the translation and the rotation and transforming the point cloud and the camera pose into a second set of GPS coordinate points. The method further includes constructing and storing a key-frame image, a key-frame timestamp and a key-frame GPS based on the second set of GPS coordinate points.
1. A method of mapping and localization, comprising:
synchronizing a set of timestamps of a set of sensor data including a set of GPS coordinates;
assigning a closest GPS timestamp corresponding to the set of GPS coordinates to the set of sensor data;
fusing the set of sensor data based on a Kalman filter to synthesize a set of high-frequency location data;
reconstructing a point cloud and a camera pose based on VSLAM;
rescaling the point cloud and the camera pose;
transforming the rescaled point cloud and camera pose into an IMU point cloud and an IMU camera pose;
transforming the IMU point cloud and the IMU camera pose into a NED point cloud and a NED camera pose;
transforming the NED point cloud and the NED camera pose into a GPS point cloud and a GPS camera pose;
determining a range of key-frame images based on a current set of GPS coordinates associated with the closest GPS timestamp;
constructing a key-frame GPS based on the set of GPS coordinates; and
storing the GPS point cloud, the GPS camera pose, the key-frame GPS and the closest GPS timestamp.
2. The method of claim 1 , further comprising selecting a current image as a candidate key-frame image within the range of key-frame images.
3. A method of mapping and localization, comprising:
synchronizing a set of timestamps of a set of sensor data including a set of GPS coordinates;
assigning a closest GPS timestamp corresponding to the set of GPS coordinates to the set of sensor data;
fusing the set of sensor data based on a Kalman filter to synthesize a set of high-frequency location data;
reconstructing a point cloud and a camera pose based on VSLAM;
rescaling the point cloud and the camera pose;
transforming the resealed point cloud and camera pose into an IMU point cloud and an IMU camera pose;
transforming the IMU point cloud and the IMU camera pose into a NED point cloud and a NED camera pose;
transforming the NED point cloud and the NED camera pose into a GPS point cloud and GPS camera pose;
determining a range of key-frame images based on the set of GPS coordinates associated with the closest GPS timestamp;
selecting a current image as a candidate key-frame image within the range of key-frame images;
constructing a key-frame GPS based on the set of GPS coordinates and the key-frame candidate image;
constructing a key-frame image based on the key-frame candidate image; and
storing the GPS point cloud, the GPS camera pose, the key-frame image, the key-frame GPS and the closest timestamp.
4. The method of claim 3 , further comprising further localizing based on the GPS point cloud, the GPS camera pose, the key-frame image, the key-frame GPS and the closest timestamp.
5. The method of claim 3 , wherein the camera pose is six dimensional.
6. The method of claim 3 , wherein the transforming of the IMU point cloud and the IMU camera pose into the NED point cloud and the NED camera pose is based in part on a set of magnetometer data.