You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
643 lines
18 KiB
JavaScript
643 lines
18 KiB
JavaScript
import * as THREE from "./lib/three.module.js";
|
|
|
|
import { RadarManager } from "./radar.js";
|
|
import { AuxLidarManager } from "./aux_lidar.js";
|
|
import { Lidar } from "./lidar.js";
|
|
import { Annotation } from "./annotation.js";
|
|
import { EgoPose } from "./ego_pose.js";
|
|
import { logger } from "./log.js";
|
|
import request from "./request.js";
|
|
import {
|
|
euler_angle_to_rotate_matrix,
|
|
euler_angle_to_rotate_matrix_3by3,
|
|
matmul,
|
|
matmul2,
|
|
mat,
|
|
} from "./util.js";
|
|
|
|
function FrameInfo(data, sceneMeta, sceneName, frame) {
|
|
this.data = data;
|
|
this.sceneMeta = sceneMeta;
|
|
this.dir = "";
|
|
this.scene = sceneName;
|
|
this.frame = frame;
|
|
this.pcd_ext = "";
|
|
(this.frame_index = this.sceneMeta.frames.findIndex(function (x) {
|
|
return x == frame;
|
|
})),
|
|
(this.transform_matrix = this.sceneMeta.point_transform_matrix),
|
|
(this.annotation_format = this.sceneMeta.boxtype), //xyz(24 number), csr(center, scale, rotation, 9 number)
|
|
// this.set = function(scene, frame_index, frame, transform_matrix, annotation_format){
|
|
// this.scene = scene;
|
|
// this.frame = frame;
|
|
// this.frame_index = frame_index;
|
|
// this.transform_matrix = transform_matrix;
|
|
// this.annotation_format = annotation_format;
|
|
// };
|
|
|
|
(this.get_pcd_path = function () {
|
|
return (
|
|
request + "/data/" + this.scene + "/lidar/" + this.frame + this.sceneMeta.lidar_ext
|
|
);
|
|
});
|
|
this.get_radar_path = function (name) {
|
|
return `${request}/data/${this.scene}/radar/${name}/${this.frame}${this.sceneMeta.radar_ext}`;
|
|
};
|
|
this.get_aux_lidar_path = function (name) {
|
|
return `${request}/data/${this.scene}/aux_lidar/${name}/${this.frame}${this.sceneMeta.radar_ext}`;
|
|
};
|
|
|
|
this.get_anno_path = function () {
|
|
if (this.annotation_format == "psr") {
|
|
return request + "/data/" + this.scene + "/label/" + this.frame + ".json";
|
|
} else {
|
|
return request + "/data/" + this.scene + "/bbox.xyz/" + this.frame + ".bbox.txt";
|
|
}
|
|
};
|
|
|
|
this.anno_to_boxes = function (text) {
|
|
var _self = this;
|
|
if (this.annotation_format == "psr") {
|
|
var boxes = JSON.parse(text);
|
|
|
|
return boxes;
|
|
} else return this.python_xyz_to_psr(text);
|
|
};
|
|
this.transform_point = function (m, x, y, z) {
|
|
var rx = x * m[0] + y * m[1] + z * m[2];
|
|
var ry = x * m[3] + y * m[4] + z * m[5];
|
|
var rz = x * m[6] + y * m[7] + z * m[8];
|
|
|
|
return [rx, ry, rz];
|
|
};
|
|
|
|
/*
|
|
input is coordinates of 8 vertices
|
|
bottom-left-front, bottom-right-front, bottom-right-back, bottom-left-back
|
|
top-left-front, top-right-front, top-right-back, top-left-back
|
|
|
|
this format is what SECOND/PointRcnn save their results.
|
|
*/
|
|
this.python_xyz_to_psr = function (text) {
|
|
var _self = this;
|
|
|
|
var points_array = text
|
|
.split("\n")
|
|
.filter(function (x) {
|
|
return x;
|
|
})
|
|
.map(function (x) {
|
|
return x.split(" ").map(function (x) {
|
|
return parseFloat(x);
|
|
});
|
|
});
|
|
|
|
var boxes = points_array.map(function (ps) {
|
|
for (var i = 0; i < 8; i++) {
|
|
var p = _self.transform_point(
|
|
_self.transform_matrix,
|
|
ps[3 * i + 0],
|
|
ps[3 * i + 1],
|
|
ps[3 * i + 2]
|
|
);
|
|
ps[i * 3 + 0] = p[0];
|
|
ps[i * 3 + 1] = p[1];
|
|
ps[i * 3 + 2] = p[2];
|
|
}
|
|
return ps;
|
|
});
|
|
|
|
var boxes_ann = boxes.map(this.xyz_to_psr);
|
|
|
|
return boxes_ann; //, boxes];
|
|
};
|
|
|
|
this.xyz_to_psr = function (ann_input) {
|
|
var ann = [];
|
|
if (ann_input.length == 24) ann = ann_input;
|
|
else
|
|
for (var i = 0; i < ann_input.length; i++) {
|
|
if ((i + 1) % 4 != 0) {
|
|
ann.push(ann_input[i]);
|
|
}
|
|
}
|
|
|
|
var pos = { x: 0, y: 0, z: 0 };
|
|
for (var i = 0; i < 8; i++) {
|
|
pos.x += ann[i * 3];
|
|
pos.y += ann[i * 3 + 1];
|
|
pos.z += ann[i * 3 + 2];
|
|
}
|
|
pos.x /= 8;
|
|
pos.y /= 8;
|
|
pos.z /= 8;
|
|
|
|
var scale = {
|
|
x: Math.sqrt(
|
|
(ann[0] - ann[3]) * (ann[0] - ann[3]) +
|
|
(ann[1] - ann[4]) * (ann[1] - ann[4])
|
|
),
|
|
y: Math.sqrt(
|
|
(ann[0] - ann[9]) * (ann[0] - ann[9]) +
|
|
(ann[1] - ann[10]) * (ann[1] - ann[10])
|
|
),
|
|
z: ann[14] - ann[2],
|
|
};
|
|
|
|
/*
|
|
1. atan2(y,x), not x,y
|
|
2. point order in xy plane
|
|
0 1
|
|
3 2
|
|
*/
|
|
|
|
var angle = Math.atan2(
|
|
ann[4] + ann[7] - 2 * pos.y,
|
|
ann[3] + ann[6] - 2 * pos.x
|
|
);
|
|
|
|
return {
|
|
position: pos,
|
|
scale: scale,
|
|
rotation: { x: 0, y: 0, z: angle },
|
|
};
|
|
};
|
|
}
|
|
|
|
function Images(sceneMeta, sceneName, frame) {
|
|
this.loaded = function () {
|
|
for (var n in this.names) {
|
|
if (!this.loaded_flag[this.names[n]]) return false;
|
|
}
|
|
|
|
return true;
|
|
};
|
|
|
|
this.names = sceneMeta.camera; //["image","left","right"],
|
|
this.loaded_flag = {};
|
|
// this.active_name = "";
|
|
// this.active_image = function(){
|
|
// return this.content[this.active_name];
|
|
// };
|
|
this.getImageByName = function (name) {
|
|
return this.content[name];
|
|
};
|
|
|
|
// this.activate = function(name){
|
|
// this.active_name = name;
|
|
// };
|
|
|
|
this.content = {};
|
|
this.on_all_loaded = null;
|
|
|
|
(this.load = function (on_all_loaded, active_name) {
|
|
this.on_all_loaded = on_all_loaded;
|
|
|
|
// if global camera not set, use first camera as default.
|
|
// if (active_name.length > 0)
|
|
// this.active_name = active_name;
|
|
// else if (this.names && this.names.length>0)
|
|
// this.active_name = this.names[0];
|
|
|
|
var _self = this;
|
|
|
|
if (this.names) {
|
|
this.names.forEach(function (cam) {
|
|
_self.content[cam] = new Image();
|
|
_self.content[cam].onload = function () {
|
|
_self.loaded_flag[cam] = true;
|
|
_self.on_image_loaded();
|
|
};
|
|
_self.content[cam].onerror = function () {
|
|
_self.loaded_flag[cam] = true;
|
|
_self.on_image_loaded();
|
|
};
|
|
|
|
_self.content[cam].src =
|
|
"data/" +
|
|
sceneName +
|
|
"/camera/" +
|
|
cam +
|
|
"/" +
|
|
frame +
|
|
sceneMeta.camera_ext;
|
|
console.log("image set");
|
|
});
|
|
}
|
|
}),
|
|
(this.on_image_loaded = function () {
|
|
if (this.loaded()) {
|
|
this.on_all_loaded();
|
|
}
|
|
});
|
|
}
|
|
|
|
function World(data, sceneName, frame, coordinatesOffset, on_preload_finished) {
|
|
this.data = data;
|
|
this.sceneMeta = this.data.getMetaBySceneName(sceneName);
|
|
this.frameInfo = new FrameInfo(this.data, this.sceneMeta, sceneName, frame);
|
|
|
|
this.coordinatesOffset = coordinatesOffset;
|
|
|
|
this.toString = function () {
|
|
return this.frameInfo.scene + "," + this.frameInfo.frame;
|
|
};
|
|
//points_backup: null, //for restore from highlight
|
|
|
|
this.cameras = new Images(this.sceneMeta, sceneName, frame);
|
|
this.radars = new RadarManager(this.sceneMeta, this, this.frameInfo);
|
|
this.lidar = new Lidar(this.sceneMeta, this, this.frameInfo);
|
|
this.annotation = new Annotation(this.sceneMeta, this, this.frameInfo);
|
|
this.aux_lidars = new AuxLidarManager(this.sceneMeta, this, this.frameInfo);
|
|
this.egoPose = new EgoPose(this.sceneMeta, this, this.FrameInfo);
|
|
|
|
// todo: state of world could be put in a variable
|
|
// but still need mulitple flags.
|
|
|
|
(this.points_loaded = false),
|
|
(this.preloaded = function () {
|
|
return (
|
|
this.lidar.preloaded &&
|
|
this.annotation.preloaded &&
|
|
//this.cameras.loaded() &&
|
|
this.aux_lidars.preloaded() &&
|
|
this.radars.preloaded() &&
|
|
this.egoPose.preloaded
|
|
);
|
|
});
|
|
|
|
this.create_time = 0;
|
|
this.finish_time = 0;
|
|
this.on_preload_finished = null;
|
|
|
|
this.on_subitem_preload_finished = function (on_preload_finished) {
|
|
if (this.preloaded()) {
|
|
logger.log(
|
|
`finished preloading ${this.frameInfo.scene} ${this.frameInfo.frame}`
|
|
);
|
|
|
|
this.calcTransformMatrix();
|
|
|
|
if (this.on_preload_finished) {
|
|
this.on_preload_finished(this);
|
|
}
|
|
|
|
if (this.active) {
|
|
this.go();
|
|
}
|
|
}
|
|
};
|
|
|
|
this.calcTransformMatrix = function () {
|
|
if (this.egoPose.egoPose) {
|
|
let thisPose = this.egoPose.egoPose;
|
|
let refPose = this.data.getRefEgoPose(this.frameInfo.scene, thisPose);
|
|
|
|
let thisRot = {
|
|
x: (thisPose.pitch * Math.PI) / 180.0,
|
|
y: (thisPose.roll * Math.PI) / 180.0,
|
|
z: (-thisPose.azimuth * Math.PI) / 180.0,
|
|
};
|
|
|
|
let posDelta = {
|
|
x: thisPose.x - refPose.x,
|
|
y: thisPose.y - refPose.y,
|
|
z: thisPose.z - refPose.z,
|
|
};
|
|
|
|
//console.log("pose", thisPose, refPose, delta);
|
|
|
|
//let theta = delta.rotation.z*Math.PI/180.0;
|
|
|
|
// https://docs.novatel.com/OEM7/Content/SPAN_Operation/SPAN_Translations_Rotations.htm
|
|
//let trans_utm_ego = euler_angle_to_rotate_matrix_3by3({x: refPose.pitch*Math.PI/180.0, y: refPose.roll*Math.PI/180.0, z: refPose.azimuth*Math.PI/180.0}, "ZXY");
|
|
|
|
// this should be a calib matrix
|
|
//let trans_lidar_ego = euler_angle_to_rotate_matrix({x: 0, y: 0, z: Math.PI}, {x:0, y:0, z:0.4});
|
|
|
|
let trans_lidar_ego = new THREE.Matrix4()
|
|
.makeRotationFromEuler(new THREE.Euler(0, 0, Math.PI, "ZYX"))
|
|
.setPosition(0, 0, 0.4);
|
|
|
|
//let trans_ego_utm = euler_angle_to_rotate_matrix(thisRot, posDelta, "ZXY");
|
|
let trans_ego_utm = new THREE.Matrix4()
|
|
.makeRotationFromEuler(
|
|
new THREE.Euler(thisRot.x, thisRot.y, thisRot.z, "ZXY")
|
|
)
|
|
.setPosition(posDelta.x, posDelta.y, posDelta.z);
|
|
|
|
let trans_utm_scene = new THREE.Matrix4()
|
|
.identity()
|
|
.setPosition(
|
|
this.coordinatesOffset[0],
|
|
this.coordinatesOffset[1],
|
|
this.coordinatesOffset[2]
|
|
);
|
|
// let offset_ego = matmul(trans_utm_ego, [delta.position.x, delta.position.y, delta.position.z], 3);
|
|
// let offset_lidar = matmul(trans_ego_lidar, offset_ego, 3);
|
|
|
|
// let trans_lidar = euler_angle_to_rotate_matrix({x: - delta.rotation.x*Math.PI/180.0, y: -delta.rotation.y*Math.PI/180.0, z: - delta.rotation.z*Math.PI/180.0},
|
|
// {x:offset_lidar[0], y:offset_lidar[1], z:offset_lidar[2]},
|
|
// "ZXY");
|
|
|
|
// let R = matmul2(trans_ego_utm, trans_lidar_ego, 4);
|
|
// let inv = [
|
|
// mat(R,4,0,0), mat(R,4,1,0), mat(R,4,2,0), -mat(R,4,0,3),
|
|
// mat(R,4,0,1), mat(R,4,1,1), mat(R,4,2,1), -mat(R,4,1,3),
|
|
// mat(R,4,0,2), mat(R,4,1,2), mat(R,4,2,2), -mat(R,4,2,3),
|
|
// 0, 0, 0, 1,
|
|
// ];
|
|
|
|
this.trans_lidar_utm = new THREE.Matrix4().multiplyMatrices(
|
|
trans_ego_utm,
|
|
trans_lidar_ego
|
|
);
|
|
|
|
if (this.data.cfg.coordinateSystem == "utm")
|
|
this.trans_lidar_scene = new THREE.Matrix4().multiplyMatrices(
|
|
trans_utm_scene,
|
|
this.trans_lidar_utm
|
|
);
|
|
else this.trans_lidar_scene = trans_utm_scene; //only offset.
|
|
|
|
this.trans_utm_lidar = new THREE.Matrix4()
|
|
.copy(this.trans_lidar_utm)
|
|
.invert();
|
|
this.trans_scene_lidar = new THREE.Matrix4()
|
|
.copy(this.trans_lidar_scene)
|
|
.invert();
|
|
} else {
|
|
let trans_utm_scene = new THREE.Matrix4()
|
|
.identity()
|
|
.setPosition(
|
|
this.coordinatesOffset[0],
|
|
this.coordinatesOffset[1],
|
|
this.coordinatesOffset[2]
|
|
);
|
|
let id = new THREE.Matrix4().identity();
|
|
|
|
this.trans_lidar_utm = id;
|
|
this.trans_lidar_scene = trans_utm_scene;
|
|
|
|
this.trans_utm_lidar = new THREE.Matrix4()
|
|
.copy(this.trans_lidar_utm)
|
|
.invert();
|
|
this.trans_scene_lidar = new THREE.Matrix4()
|
|
.copy(this.trans_lidar_scene)
|
|
.invert();
|
|
}
|
|
|
|
this.webglGroup.matrix.copy(this.trans_lidar_scene);
|
|
this.webglGroup.matrixAutoUpdate = false;
|
|
};
|
|
|
|
// global scene
|
|
this.scenePosToLidar = function (pos) {
|
|
let tp = new THREE.Vector4(pos.x, pos.y, pos.z, 1).applyMatrix4(
|
|
this.trans_scene_lidar
|
|
);
|
|
|
|
return tp;
|
|
};
|
|
|
|
// global scene
|
|
this.lidarPosToScene = function (pos) {
|
|
let tp = new THREE.Vector3(pos.x, pos.y, pos.z).applyMatrix4(
|
|
this.trans_lidar_scene
|
|
);
|
|
|
|
return tp;
|
|
};
|
|
|
|
// global scene
|
|
this.lidarPosToUtm = function (pos) {
|
|
let tp = new THREE.Vector3(pos.x, pos.y, pos.z).applyMatrix4(
|
|
this.trans_lidar_utm
|
|
);
|
|
|
|
return tp;
|
|
};
|
|
|
|
this.sceneRotToLidar = function (rotEuler) {
|
|
if (!rotEuler.isEuler) {
|
|
rotEuler = new THREE.Euler(rotEuler.x, rotEuler.y, rotEuler.z, "XYZ");
|
|
}
|
|
|
|
let rotG = new THREE.Quaternion().setFromEuler(rotEuler);
|
|
let GlobalToLocalRot = new THREE.Quaternion().setFromRotationMatrix(
|
|
this.trans_scene_lidar
|
|
);
|
|
|
|
let retQ = rotG.multiply(GlobalToLocalRot);
|
|
|
|
let retEuler = new THREE.Euler().setFromQuaternion(retQ, rotEuler.order);
|
|
|
|
return retEuler;
|
|
};
|
|
|
|
this.lidarRotToScene = function (rotEuler) {
|
|
if (!rotEuler.isEuler) {
|
|
rotEuler = new THREE.Euler(rotEuler.x, rotEuler.y, rotEuler.z, "XYZ");
|
|
}
|
|
|
|
let rotL = new THREE.Quaternion().setFromEuler(rotEuler);
|
|
let localToGlobalRot = new THREE.Quaternion().setFromRotationMatrix(
|
|
this.trans_lidar_scene
|
|
);
|
|
|
|
let retQ = rotL.multiply(localToGlobalRot);
|
|
|
|
let retEuler = new THREE.Euler().setFromQuaternion(retQ, rotEuler.order);
|
|
|
|
return retEuler;
|
|
};
|
|
|
|
this.lidarRotToUtm = function (rotEuler) {
|
|
if (!rotEuler.isEuler) {
|
|
rotEuler = new THREE.Euler(rotEuler.x, rotEuler.y, rotEuler.z, "XYZ");
|
|
}
|
|
|
|
let rotL = new THREE.Quaternion().setFromEuler(rotEuler);
|
|
let localToGlobalRot = new THREE.Quaternion().setFromRotationMatrix(
|
|
this.trans_lidar_utm
|
|
);
|
|
|
|
let retQ = rotL.multiply(localToGlobalRot);
|
|
|
|
let retEuler = new THREE.Euler().setFromQuaternion(retQ, rotEuler.order);
|
|
|
|
return retEuler;
|
|
};
|
|
|
|
this.utmRotToLidar = function (rotEuler) {
|
|
if (!rotEuler.isEuler) {
|
|
rotEuler = new THREE.Euler(rotEuler.x, rotEuler.y, rotEuler.z, "XYZ");
|
|
}
|
|
|
|
let rot = new THREE.Quaternion().setFromEuler(rotEuler);
|
|
let trans = new THREE.Quaternion().setFromRotationMatrix(
|
|
this.trans_utm_lidar
|
|
);
|
|
|
|
let retQ = rot.multiply(trans);
|
|
|
|
let retEuler = new THREE.Euler().setFromQuaternion(retQ, rotEuler.order);
|
|
|
|
return retEuler;
|
|
};
|
|
|
|
this.preload = function (on_preload_finished) {
|
|
this.create_time = new Date().getTime();
|
|
console.log(this.create_time, sceneName, frame, "start");
|
|
|
|
this.webglGroup = new THREE.Group();
|
|
this.webglGroup.name = "world";
|
|
|
|
let _preload_cb = () =>
|
|
this.on_subitem_preload_finished(on_preload_finished);
|
|
|
|
this.lidar.preload(_preload_cb);
|
|
this.annotation.preload(_preload_cb);
|
|
this.radars.preload(_preload_cb);
|
|
this.cameras.load(_preload_cb, this.data.active_camera_name);
|
|
this.aux_lidars.preload(_preload_cb);
|
|
this.egoPose.preload(_preload_cb);
|
|
};
|
|
|
|
(this.scene = null),
|
|
(this.destroy_old_world = null), //todo, this can be a boolean
|
|
(this.on_finished = null),
|
|
(this.activate = function (scene, destroy_old_world, on_finished) {
|
|
this.scene = scene;
|
|
this.active = true;
|
|
this.destroy_old_world = destroy_old_world;
|
|
this.on_finished = on_finished;
|
|
if (this.preloaded()) {
|
|
this.go();
|
|
}
|
|
});
|
|
|
|
(this.active = false), (this.everythingDone = false);
|
|
|
|
this.go = function () {
|
|
if (this.everythingDone) {
|
|
//console.error("re-activate world?");
|
|
|
|
//however we still call on_finished
|
|
if (this.on_finished) {
|
|
this.on_finished();
|
|
}
|
|
return;
|
|
}
|
|
|
|
if (this.preloaded()) {
|
|
//this.points.material.size = data.cfg.point_size;
|
|
|
|
if (this.destroy_old_world) {
|
|
this.destroy_old_world();
|
|
}
|
|
|
|
if (this.destroyed) {
|
|
console.log("go after destroyed.");
|
|
this.unload();
|
|
return;
|
|
}
|
|
|
|
this.scene.add(this.webglGroup);
|
|
|
|
this.lidar.go(this.scene);
|
|
this.annotation.go(this.scene);
|
|
this.radars.go(this.scene);
|
|
this.aux_lidars.go(this.scene);
|
|
|
|
this.finish_time = new Date().getTime();
|
|
console.log(
|
|
this.finish_time,
|
|
sceneName,
|
|
frame,
|
|
"loaded in ",
|
|
this.finish_time - this.create_time,
|
|
"ms"
|
|
);
|
|
|
|
// render is called in on_finished() callback
|
|
if (this.on_finished) {
|
|
this.on_finished();
|
|
}
|
|
|
|
this.everythingDone = true;
|
|
}
|
|
};
|
|
|
|
this.add_line = function (start, end, color) {
|
|
var line = this.new_line(start, end, color);
|
|
this.scene.add(line);
|
|
};
|
|
|
|
this.new_line = function (start, end, color) {
|
|
var vertex = start.concat(end);
|
|
this.world.data.dbg.alloc();
|
|
var line = new THREE.BufferGeometry();
|
|
line.addAttribute("position", new THREE.Float32BufferAttribute(vertex, 3));
|
|
|
|
if (!color) {
|
|
color = 0x00ff00;
|
|
}
|
|
|
|
var material = new THREE.LineBasicMaterial({
|
|
color: color,
|
|
linewidth: 1,
|
|
opacity: this.data.cfg.box_opacity,
|
|
transparent: true,
|
|
});
|
|
return new THREE.LineSegments(line, material);
|
|
};
|
|
|
|
this.destroyed = false;
|
|
|
|
// todo, Image resource to be released?
|
|
|
|
this.unload = function () {
|
|
if (this.everythingDone) {
|
|
//unload all from scene, but don't destroy elements
|
|
this.lidar.unload();
|
|
this.radars.unload();
|
|
this.aux_lidars.unload();
|
|
this.annotation.unload();
|
|
|
|
this.scene.remove(this.webglGroup);
|
|
|
|
this.active = false;
|
|
this.everythingDone = false;
|
|
}
|
|
};
|
|
|
|
this.deleteAll = function () {
|
|
var _self = this;
|
|
|
|
logger.log(`delete world ${this.frameInfo.scene},${this.frameInfo.frame}`);
|
|
|
|
if (this.everythingDone) {
|
|
this.unload();
|
|
}
|
|
|
|
// todo, check if all objects are removed from webgl scene.
|
|
if (this.destroyed) {
|
|
console.log("destroy destroyed world!");
|
|
}
|
|
|
|
this.lidar.deleteAll();
|
|
this.radars.deleteAll();
|
|
this.aux_lidars.deleteAll();
|
|
this.annotation.deleteAll();
|
|
|
|
this.destroyed = true;
|
|
console.log(this.frameInfo.scene, this.frameInfo.frame, "destroyed");
|
|
// remove me from buffer
|
|
};
|
|
|
|
this.preload(on_preload_finished);
|
|
}
|
|
|
|
export { World };
|