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.
1432 lines
46 KiB
JavaScript
1432 lines
46 KiB
JavaScript
|
|
import * as THREE from './lib/three.module.js';
|
|
import { matmul, euler_angle_to_rotate_matrix, transpose, psr_to_xyz, array_as_vector_range, array_as_vector_index_range, vector_range, euler_angle_to_rotate_matrix_3by3} from "./util.js"
|
|
import { PCDLoader } from './lib/PCDLoader.js';
|
|
import {globalObjectCategory} from './obj_cfg.js';
|
|
|
|
|
|
import {settings} from "./settings.js"
|
|
|
|
function Lidar(sceneMeta, world, frameInfo){
|
|
this.world = world;
|
|
this.data = world.data;
|
|
this.frameInfo = frameInfo;
|
|
this.sceneMeta = sceneMeta;
|
|
|
|
this.points = null;
|
|
this.points_load_time = 0;
|
|
|
|
this.remove_high_ponts = function(pcd, z){
|
|
let position = [];
|
|
let color = [];
|
|
let normal = [];
|
|
let intensity = [];
|
|
//3, 3, 3, 1
|
|
|
|
for (let i = 0; i < pcd.position.length/3; i++){
|
|
if (pcd.position[i*3+2] < z){
|
|
position.push(pcd.position[i*3+0]);
|
|
position.push(pcd.position[i*3+1]);
|
|
position.push(pcd.position[i*3+2]);
|
|
|
|
if (pcd.color.length>0){
|
|
color.push(pcd.color[i*3+0]);
|
|
color.push(pcd.color[i*3+1]);
|
|
color.push(pcd.color[i*3+2]);
|
|
}
|
|
|
|
if (pcd.normal.length>0){
|
|
normal.push(pcd.normal[i*3+0]);
|
|
normal.push(pcd.normal[i*3+1]);
|
|
normal.push(pcd.normal[i*3+2]);
|
|
}
|
|
|
|
if (pcd.intensity){
|
|
intensity.push(pcd.intensity[i]);
|
|
}
|
|
}
|
|
}
|
|
|
|
pcd.position = position;
|
|
pcd.intensity = intensity;
|
|
pcd.color = color;
|
|
pcd.normal = normal;
|
|
|
|
return pcd;
|
|
};
|
|
|
|
|
|
|
|
this.preload=function(on_preload_finished){
|
|
this.on_preload_finished = on_preload_finished;
|
|
|
|
var loader = new PCDLoader();
|
|
|
|
var _self = this;
|
|
loader.load( this.frameInfo.get_pcd_path(),
|
|
//ok
|
|
function ( pcd ) {
|
|
|
|
|
|
|
|
|
|
_self.points_parse_time = new Date().getTime();
|
|
console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "parse pionts ", _self.points_parse_time - _self.create_time, "ms");
|
|
|
|
// if (_self.frameInfo.transform_matrix){
|
|
|
|
// var arr = position;
|
|
// var num = position.length;
|
|
// var ni = 3;
|
|
|
|
// for (var i=0; i<num/ni; i++){
|
|
// var np = _self.frameInfo.transform_point(_self.frameInfo.transform_matrix, arr[i*ni+0], arr[i*ni+1], arr[i*ni+2]);
|
|
// arr[i*ni+0]=np[0];
|
|
// arr[i*ni+1]=np[1];
|
|
// arr[i*ni+2]=np[2];
|
|
// }
|
|
|
|
// //points.geometry.computeBoundingSphere();
|
|
// }
|
|
|
|
|
|
if (_self.data.cfg.enableFilterPoints)// do some filtering work here
|
|
{
|
|
pcd = _self.remove_high_ponts(pcd, _self.data.cfg.filterPointsZ);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
let position = pcd.position;
|
|
|
|
|
|
// build geometry
|
|
_self.world.data.dbg.alloc();
|
|
var geometry = new THREE.BufferGeometry();
|
|
if ( position.length > 0 )
|
|
geometry.setAttribute( 'position', new THREE.Float32BufferAttribute( position, 3 ) );
|
|
|
|
let normal = pcd.normal;
|
|
// normal and colore are note used in av scenes.
|
|
if ( normal.length > 0 )
|
|
geometry.setAttribute( 'normal', new THREE.Float32BufferAttribute( normal, 3 ) );
|
|
|
|
let color = pcd.color;
|
|
if ( color.length == 0 ) {
|
|
color = []
|
|
|
|
// by default we set all points to same color
|
|
for (let i =0; i< position.length; ++i){
|
|
color.push(_self.data.cfg.point_brightness);
|
|
}
|
|
|
|
|
|
// if enabled intensity we color points by intensity.
|
|
if (_self.data.cfg.color_points=="intensity" && pcd.intensity.length>0){
|
|
// map intensity to color
|
|
for (var i =0; i< pcd.intensity.length; ++i){
|
|
let intensity = pcd.intensity[i];
|
|
intensity *= 8;
|
|
|
|
if (intensity > 1)
|
|
intensity = 1.0;
|
|
|
|
|
|
//color.push( 2 * Math.abs(0.5-intensity));
|
|
|
|
color[i*3] = intensity;
|
|
color[i*3+1] = intensity;
|
|
color[i*3+2] = 1 - intensity;
|
|
}
|
|
}
|
|
|
|
// save color, in case color needs to be restored.
|
|
pcd.color = color;
|
|
}
|
|
|
|
geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(color, 3 ) );
|
|
|
|
geometry.computeBoundingSphere();
|
|
// build material
|
|
|
|
var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );
|
|
|
|
/*
|
|
|
|
if ( color.length > 0 ) {
|
|
material.vertexColors = color;
|
|
} else {
|
|
//material.color.setHex(0xffffff);
|
|
material.color.r = 0.6;
|
|
material.color.g = 0.6;
|
|
material.color.b = 0.6;
|
|
}
|
|
*/
|
|
|
|
//material.size = 2;
|
|
material.sizeAttenuation = false;
|
|
|
|
// build mesh
|
|
|
|
var mesh = new THREE.Points( geometry, material );
|
|
mesh.name = "pcd";
|
|
|
|
//return mesh;
|
|
// add to parent.
|
|
_self.world.webglGroup.add(mesh);
|
|
|
|
_self.points = mesh;
|
|
_self.pcd = pcd;
|
|
//_self.points_backup = mesh;
|
|
|
|
_self.build_points_index();
|
|
_self.points_load_time = new Date().getTime();
|
|
|
|
console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "loaded pionts ", _self.points_load_time - _self.create_time, "ms");
|
|
|
|
_self._afterPreload();
|
|
},
|
|
|
|
// on progress,
|
|
function(){
|
|
|
|
},
|
|
|
|
// on error
|
|
function(){
|
|
//error
|
|
console.log("load pcd failed.");
|
|
_self._afterPreload();
|
|
},
|
|
|
|
// on file loaded
|
|
function(){
|
|
_self.points_readfile_time = new Date().getTime();
|
|
console.log(_self.points_load_time, _self.frameInfo.scene, _self.frameInfo.frame, "read file ", _self.points_readfile_time - _self.create_time, "ms");
|
|
}
|
|
);
|
|
};
|
|
|
|
this.deleteAll = function(){
|
|
return this.remove_all_points();
|
|
}
|
|
|
|
this._afterPreload = function(){
|
|
this.preloaded = true;
|
|
console.log("lidar preloaded");
|
|
//go ahead, may load picture
|
|
if (this.on_preload_finished){
|
|
this.on_preload_finished();
|
|
}
|
|
if (this.go_cmd_received){
|
|
this.go(this.webglScene, this.on_go_finished);
|
|
}
|
|
};
|
|
|
|
|
|
this.loaded = false;
|
|
this.webglScene = null;
|
|
this.go_cmd_received = false;
|
|
this.on_go_finished = null;
|
|
|
|
this.go = function(webglScene, on_go_finished){
|
|
this.webglScene = webglScene;
|
|
|
|
if (this.preloaded){
|
|
|
|
if (!this.world.data.cfg.show_background){
|
|
this.hide_background();
|
|
}
|
|
|
|
|
|
if (this.data.cfg.color_obj != "no"){
|
|
this.color_objects();
|
|
}
|
|
|
|
if (on_go_finished)
|
|
on_go_finished();
|
|
} else {
|
|
this.go_cmd_received = true;
|
|
this.on_go_finished = on_go_finished;
|
|
}
|
|
};
|
|
|
|
this.unload = function(){
|
|
this.cancel_highlight();
|
|
|
|
if (this.points){
|
|
// this.world.webglGroup.remove(this.points);
|
|
|
|
// if (this.points.points_backup){
|
|
// let backup = this.points.points_backup;
|
|
// this.points.points_backup = null;
|
|
// this.remove_all_points();
|
|
// this.points = backup;
|
|
|
|
// }
|
|
}
|
|
};
|
|
|
|
this.deleteAll = function(){
|
|
this.remove_all_points();
|
|
}
|
|
|
|
this.set_point_size=function(v){
|
|
if (this.points){
|
|
this.points.material.size = v;
|
|
|
|
// this could happen if the points are still loading
|
|
if (this.points.points_backup){
|
|
this.points.points_backup.material.size = v;
|
|
|
|
if (this.points.points_backup.points_backup){
|
|
this.points.points_backup.points_backup.material.size = v;
|
|
}
|
|
}
|
|
}
|
|
|
|
};
|
|
|
|
this.color_objects = function(){
|
|
if (this.data.cfg.color_obj != "no"){
|
|
this.world.annotation.boxes.map((b)=>{
|
|
if (!b.annotator)
|
|
this.set_box_points_color(b);
|
|
})
|
|
}
|
|
};
|
|
|
|
// color points according to object category
|
|
this.color_points=function(){
|
|
// color all points inside these boxes
|
|
let color = this.points.geometry.getAttribute("color").array;
|
|
|
|
// step 1, color all points.
|
|
if (this.data.cfg.color_points=="intensity" && this.pcd.intensity.length>0){
|
|
// by intensity
|
|
for (var i =0; i< this.pcd.intensity.length; ++i){
|
|
let intensity = this.pcd.intensity[i];
|
|
intensity *= 8;
|
|
|
|
if (intensity > 1)
|
|
intensity = 1.0;
|
|
|
|
|
|
//color.push( 2 * Math.abs(0.5-intensity));
|
|
|
|
color[i*3] = intensity;
|
|
color[i*3+1] = intensity;
|
|
color[i*3+2] = 1 - intensity;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// mono color
|
|
for (let i =0; i< this.pcd.position.length; ++i){
|
|
color[i] = this.data.cfg.point_brightness;
|
|
}
|
|
}
|
|
|
|
// step 2 color objects
|
|
this.color_objects();
|
|
|
|
//this.update_points_color();
|
|
};
|
|
|
|
this.transformPointsByEgoPose = function(points){
|
|
|
|
if (!this.world.transLidar)
|
|
return points;
|
|
|
|
|
|
let newPoints=[];
|
|
for (let i=0; i<points.length; i+=3)
|
|
{
|
|
let p = matmul(this.world.transLidar, [points[i], points[i+1], points[i+2], 1], 4);
|
|
newPoints.push(p[0]);
|
|
newPoints.push(p[1]);
|
|
newPoints.push(p[2]);
|
|
}
|
|
return newPoints;
|
|
}
|
|
|
|
|
|
this.get_all_pionts=function(){
|
|
return this.points.geometry.getAttribute("position");
|
|
};
|
|
|
|
this.computeCenter = function(){
|
|
|
|
if (! this.center)
|
|
{
|
|
let position = this.points.geometry.getAttribute("position");
|
|
// computer center position
|
|
let center = {x:0,y:0,z:0};
|
|
for (let i = 0; i<position.count; i++)
|
|
{
|
|
center.x += position.array[i*3];
|
|
center.y += position.array[i*3+1];
|
|
center.z += position.array[i*3+2];
|
|
}
|
|
|
|
center.x /= position.count;
|
|
center.y /= position.count;
|
|
center.z /= position.count;
|
|
|
|
this.center = center;
|
|
}
|
|
|
|
return this.center;
|
|
};
|
|
|
|
this.build_points_index=function(){
|
|
var ps = this.points.geometry.getAttribute("position");
|
|
var points_index = {};
|
|
|
|
if (ps){ // points may be empty
|
|
for (var i = 0; i<ps.count; i++){
|
|
var k = this.get_position_key(ps.array[i*3], ps.array[i*3+1], ps.array[i*3+2]);
|
|
k = this.key_to_str(k);
|
|
|
|
if (points_index[k]){
|
|
points_index[k].push(i);
|
|
} else {
|
|
points_index[k]=[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
this.points.points_index = points_index;
|
|
};
|
|
|
|
this.points_index_grid_size= 1,
|
|
|
|
this.get_position_key=function(x,y,z){
|
|
return [Math.floor(x/this.points_index_grid_size),
|
|
Math.floor(y/this.points_index_grid_size),
|
|
Math.floor(z/this.points_index_grid_size),];
|
|
};
|
|
this.key_to_str=function(k){
|
|
return k[0]+","+k[1]+","+k[2];
|
|
};
|
|
|
|
// candidate pionts, covering the box(center, scale), but larger.
|
|
this.get_covering_position_indices=function(points, center, scale, rotation, scale_ratio){
|
|
/*
|
|
var ck = this.get_position_key(center.x, center.y, center.z);
|
|
var radius = Math.sqrt(scale.x*scale.x + scale.y*scale.y + scale.z*scale.z)/2;
|
|
var radius_grid = Math.ceil(radius/this.points_index_grid_size);// + 1;
|
|
|
|
var indices = [];
|
|
for(var x = -radius_grid; x <= radius_grid; x++){
|
|
for(var y = -radius_grid; y <= radius_grid; y++){
|
|
for(var z = -radius_grid; z <= radius_grid; z++){
|
|
var temp = points.points_index[this.key_to_str([ck[0]+x, ck[1]+y, ck[2]+z])];
|
|
if (temp)
|
|
indices = indices.concat(temp);
|
|
}
|
|
}
|
|
}
|
|
|
|
console.log("found indices 1: " + indices.length);
|
|
//return indices;
|
|
*/
|
|
|
|
if (typeof(scale_ratio) == "number"){
|
|
scale_ratio = {
|
|
x: scale_ratio,
|
|
y: scale_ratio,
|
|
z: scale_ratio,
|
|
};
|
|
};
|
|
|
|
var indices=[];
|
|
|
|
var scaled_scale = {
|
|
x: scale.x*scale_ratio.x,
|
|
y: scale.y*scale_ratio.y,
|
|
z: scale.z*scale_ratio.z,
|
|
}
|
|
|
|
var box_corners = psr_to_xyz(center, scaled_scale, rotation);
|
|
var extreme = array_as_vector_range(box_corners, 4);
|
|
|
|
var indices = [];
|
|
for(var x = Math.floor(extreme.min[0]/this.points_index_grid_size); x <= Math.floor(extreme.max[0]/this.points_index_grid_size); x++){
|
|
for(var y = Math.floor(extreme.min[1]/this.points_index_grid_size); y <= Math.floor(extreme.max[1]/this.points_index_grid_size); y++){
|
|
for(var z = Math.floor(extreme.min[2]/this.points_index_grid_size); z <= Math.floor(extreme.max[2]/this.points_index_grid_size); z++){
|
|
var temp = points.points_index[this.key_to_str([x, y, z])];
|
|
if (temp)
|
|
indices = indices.concat(temp);
|
|
}
|
|
}
|
|
}
|
|
|
|
//console.log("found indices 2: " + indices.length);
|
|
return indices;
|
|
};
|
|
|
|
this.toggle_background=function(){
|
|
if (this.points.points_backup){ // cannot differentiate highlighted-scene and no-background-whole-scene
|
|
this.cancel_highlight();
|
|
return;
|
|
}
|
|
else{
|
|
this.hide_background();
|
|
}
|
|
};
|
|
|
|
// hide all points not inside any box
|
|
this.hide_background=function(){
|
|
if (this.points.points_backup){
|
|
//already hidden, or in highlight mode
|
|
return;
|
|
}
|
|
|
|
var _self = this;
|
|
var pos = this.points.geometry.getAttribute("position");
|
|
var color = this.points.geometry.getAttribute("color");
|
|
|
|
var hl_point=[];
|
|
var hl_color=[];
|
|
var highlight_point_indices = [];
|
|
this.world.annotation.boxes.forEach(function(box){
|
|
var indices= _self._get_points_index_of_box(_self.points, box, 1);
|
|
|
|
indices.forEach(function(i){
|
|
hl_point.push(pos.array[i*3]);
|
|
hl_point.push(pos.array[i*3+1]);
|
|
hl_point.push(pos.array[i*3+2]);
|
|
|
|
hl_color.push(color.array[i*3]);
|
|
hl_color.push(color.array[i*3+1]);
|
|
hl_color.push(color.array[i*3+2]);
|
|
})
|
|
|
|
highlight_point_indices = highlight_point_indices.concat(indices);
|
|
});
|
|
|
|
|
|
// build new geometry
|
|
this.world.data.dbg.alloc();
|
|
var geometry = new THREE.BufferGeometry();
|
|
|
|
if (hl_point.length > 0 ) {
|
|
geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(hl_point, 3 ) );
|
|
geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(hl_color, 3 ) );
|
|
}
|
|
|
|
|
|
geometry.computeBoundingSphere();
|
|
|
|
var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );
|
|
|
|
material.sizeAttenuation = false;
|
|
|
|
var mesh = new THREE.Points( geometry, material );
|
|
mesh.name = "pcd";
|
|
mesh.points_backup = this.points;
|
|
mesh.highlight_point_indices = highlight_point_indices;
|
|
|
|
//swith geometry
|
|
this.world.webglGroup.remove(this.points);
|
|
|
|
this.points = mesh;
|
|
this.build_points_index();
|
|
this.world.webglGroup.add(mesh);
|
|
};
|
|
|
|
this.cancel_highlight=function(box){
|
|
if (this.points && this.points.points_backup){
|
|
|
|
this.world.annotation.set_box_opacity(this.data.cfg.box_opacity);
|
|
|
|
//copy colors, maybe changed.
|
|
if (this.data.cfg.color_obj != "no"){
|
|
var highlight_point_color = this.points.geometry.getAttribute("color");
|
|
var backup_point_color = this.points.points_backup.geometry.getAttribute("color");
|
|
|
|
this.points.highlight_point_indices.forEach(function(n, i){
|
|
backup_point_color.array[n*3] = highlight_point_color.array[i*3];
|
|
backup_point_color.array[n*3+1] = highlight_point_color.array[i*3+1];
|
|
backup_point_color.array[n*3+2] = highlight_point_color.array[i*3+2];
|
|
});
|
|
}
|
|
|
|
|
|
//switch
|
|
var points_backup = this.points.points_backup;
|
|
this.points.points_backup = null;
|
|
|
|
this.world.webglGroup.remove(this.points);
|
|
this.remove_all_points(); //this.points is null now
|
|
this.points = points_backup;
|
|
|
|
if (box){
|
|
// in highlighted mode, the box my be moved outof the highlighted area, so
|
|
// we need to color them again.
|
|
if (this.data.cfg.color_obj != "no")
|
|
this.set_box_points_color(box);
|
|
}
|
|
|
|
if (this.data.cfg.color_obj != "no")
|
|
this.update_points_color();
|
|
|
|
this.world.webglGroup.add(this.points);
|
|
}
|
|
};
|
|
|
|
this.reset_points=function(points){ // coordinates of points
|
|
|
|
|
|
this.world.data.dbg.alloc();
|
|
var geometry = new THREE.BufferGeometry();
|
|
|
|
|
|
geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(points, 3 ) );
|
|
geometry.computeBoundingSphere();
|
|
|
|
var material = new THREE.PointsMaterial( { size: this.data.cfg.point_size} );
|
|
|
|
material.sizeAttenuation = false;
|
|
|
|
var mesh = new THREE.Points( geometry, material );
|
|
mesh.name = "pcd";
|
|
|
|
//swith geometry
|
|
this.world.webglGroup.remove(this.points);
|
|
this.remove_all_points();
|
|
|
|
this.points = mesh;
|
|
this.world.webglGroup.add(mesh);
|
|
};
|
|
|
|
this.highlight_box_points=function(box){
|
|
if (this.points.highlighted_box){
|
|
//already highlighted.
|
|
return;
|
|
}
|
|
|
|
|
|
// hide all other boxes
|
|
this.world.annotation.set_box_opacity(0);
|
|
|
|
// keep myself
|
|
box.material.opacity = 1;
|
|
|
|
|
|
var _self = this;
|
|
var pos = this.points.geometry.getAttribute("position");
|
|
var color = this.points.geometry.getAttribute("color");
|
|
|
|
|
|
|
|
|
|
var hl_point=[];
|
|
var hl_color=[];
|
|
|
|
var highlight_point_indices= this._get_points_index_of_box(this.points, box, 3);
|
|
|
|
highlight_point_indices.forEach(function(i){
|
|
hl_point.push(pos.array[i*3]);
|
|
hl_point.push(pos.array[i*3+1]);
|
|
hl_point.push(pos.array[i*3+2]);
|
|
|
|
hl_color.push(color.array[i*3]);
|
|
hl_color.push(color.array[i*3+1]);
|
|
hl_color.push(color.array[i*3+2]);
|
|
})
|
|
|
|
|
|
// build new geometry
|
|
this.world.data.dbg.alloc();
|
|
var geometry = new THREE.BufferGeometry();
|
|
|
|
if (hl_point.length > 0 ) {
|
|
geometry.setAttribute( 'position', new THREE.Float32BufferAttribute(hl_point, 3 ) );
|
|
geometry.setAttribute( 'color', new THREE.Float32BufferAttribute(hl_color, 3 ) );
|
|
}
|
|
|
|
|
|
geometry.computeBoundingSphere();
|
|
|
|
var material = new THREE.PointsMaterial( { size: _self.data.cfg.point_size, vertexColors: THREE.VertexColors } );
|
|
|
|
material.sizeAttenuation = false;
|
|
|
|
var mesh = new THREE.Points( geometry, material );
|
|
mesh.name = "highlighted_pcd";
|
|
|
|
//swith geometry
|
|
this.world.webglGroup.remove(this.points);
|
|
|
|
mesh.points_backup = this.points;
|
|
mesh.highlight_point_indices = highlight_point_indices;
|
|
mesh.highlighted_box = box;
|
|
|
|
this.points = mesh;
|
|
this.build_points_index();
|
|
this.world.webglGroup.add(mesh);
|
|
};
|
|
|
|
this.get_points_indices_of_box=function(box){
|
|
return this._get_points_of_box(this.points, box, 1).index;
|
|
};
|
|
|
|
this.get_points_of_box_in_box_coord=function(box){
|
|
return this._get_points_of_box(this.points, box, 1).position;
|
|
};
|
|
|
|
// IMPORTANT
|
|
// ground plane affects auto-adjustment
|
|
// we don't count in the ponits of lowest part to reduce the affection.
|
|
// note how the 'lower part' is defined, we count
|
|
// lowest_part_type has two options: lowest_point, or lowest_box
|
|
this.get_points_dimmension_of_box=function(box, use_box_bottom_as_limit){
|
|
var p = this._get_points_of_box(this.points, box, 1).position; //position is relative to box coordinates
|
|
|
|
var lowest_limit = - box.scale.z/2;
|
|
|
|
if (!use_box_bottom_as_limit){
|
|
var extreme1 = vector_range(p, 3);
|
|
lowest_limit = extreme1.min[2];
|
|
}
|
|
|
|
|
|
//filter out lowest part
|
|
var p = p.filter(function(x){
|
|
return x[2] - settings.ground_filter_height > lowest_limit;
|
|
})
|
|
|
|
//compute range again.
|
|
var extreme2 = vector_range(p, 3);
|
|
|
|
return {
|
|
max:{
|
|
x: extreme2.max[0],
|
|
y: extreme2.max[1],
|
|
z: extreme2.max[2],
|
|
},
|
|
min:{
|
|
x: extreme2.min[0],
|
|
y: extreme2.min[1],
|
|
z: lowest_limit,
|
|
}
|
|
}
|
|
};
|
|
|
|
// given points and box, calculate new box scale
|
|
this.get_dimension_of_points=function(indices, box){
|
|
var p = this._get_points_of_box(this.points, box, 1, indices).position;
|
|
var extreme1 = vector_range(p, 3);
|
|
|
|
//filter out lowest part, to calculate x-y size.
|
|
var p = p.filter(function(x){
|
|
return x[2] - settings.ground_filter_height > extreme1.min[2];
|
|
})
|
|
|
|
//compute range again.
|
|
var extreme2 = vector_range(p, 3);
|
|
|
|
return {
|
|
max:{
|
|
x: extreme2.max[0],
|
|
y: extreme2.max[1],
|
|
z: extreme1.max[2], // orignal extreme.
|
|
},
|
|
min:{
|
|
x: extreme2.min[0],
|
|
y: extreme2.min[1],
|
|
z: extreme1.min[2],
|
|
}
|
|
}
|
|
};
|
|
|
|
//centered, but without rotation
|
|
this.get_points_relative_coordinates_of_box_wo_rotation=function(box, scale_ratio){
|
|
return this._get_points_of_box(this.points, box, scale_ratio).position_wo_rotation;
|
|
};
|
|
|
|
|
|
this.get_points_of_box=function(box, scale_ratio){
|
|
return this._get_points_of_box(this.points, box, scale_ratio);
|
|
};
|
|
|
|
|
|
this.get_points_relative_coordinates_of_box=function(box, scale_ratio){
|
|
var ret = this._get_points_of_box(this.points, box, scale_ratio);
|
|
return ret.position;
|
|
};
|
|
|
|
|
|
this._get_points_index_of_box=function(points, box, scale_ratio){
|
|
return this._get_points_of_box(points, box, scale_ratio).index;
|
|
};
|
|
|
|
// this
|
|
this._get_points_of_box=function(points, box, scale_ratio, point_indices){
|
|
|
|
if (!scale_ratio){
|
|
scale_ratio = 1;
|
|
}
|
|
var pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
|
|
var relative_position = [];
|
|
var relative_position_wo_rotation = [];
|
|
|
|
var r = box.rotation;
|
|
var trans = transpose(euler_angle_to_rotate_matrix(r, {x:0, y:0, z:0}), 4);
|
|
|
|
var indices=[];
|
|
var cand_point_indices = point_indices;
|
|
if (!point_indices)
|
|
{
|
|
cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, scale_ratio);
|
|
}
|
|
|
|
cand_point_indices.forEach(function(i){
|
|
//for (var i = 0; i < pos.count; i++){
|
|
var x = pos_array[i*3];
|
|
var y = pos_array[i*3+1];
|
|
var z = pos_array[i*3+2];
|
|
|
|
var p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
|
|
|
|
var tp = matmul(trans, p, 4);
|
|
|
|
if (!point_indices){
|
|
// if indices is provided by caller, don't filter
|
|
if ((Math.abs(tp[0]) > box.scale.x/2 * scale_ratio+0.01)
|
|
|| (Math.abs(tp[1]) > box.scale.y/2 * scale_ratio+0.01)
|
|
|| (Math.abs(tp[2]) > box.scale.z/2 *scale_ratio+0.01) ){
|
|
return;
|
|
}
|
|
|
|
indices.push(i);
|
|
}
|
|
|
|
relative_position.push([tp[0],tp[1],tp[2]]);
|
|
relative_position_wo_rotation.push([p[0], p[1], p[2]])
|
|
|
|
});
|
|
|
|
//console.log("found indices: " + indices.length);
|
|
|
|
return {
|
|
index: indices,
|
|
position: relative_position,
|
|
position_wo_rotation: relative_position_wo_rotation,
|
|
}
|
|
};
|
|
|
|
this.findTop = function(box, init_scale_ratio){
|
|
var points = this.points;
|
|
var pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);
|
|
|
|
|
|
var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
|
|
// all cand points are translated into box coordinates
|
|
|
|
let translated_cand_points = cand_point_indices.map(function(i){
|
|
let x = pos_array[i*3];
|
|
let y = pos_array[i*3+1];
|
|
let z = pos_array[i*3+2];
|
|
|
|
let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
|
|
let tp = matmul(trans, p, 4);
|
|
return tp;
|
|
});
|
|
|
|
|
|
let maxZ = -1000;
|
|
|
|
|
|
translated_cand_points.forEach((tp, i)=>{
|
|
if (Math.abs(tp[0]) < box.scale.x * init_scale_ratio.x/2 &&
|
|
Math.abs(tp[1]) < box.scale.y * init_scale_ratio.y/2 &&
|
|
Math.abs(tp[2]) < box.scale.z * init_scale_ratio.z/2)
|
|
{
|
|
if (tp[2] > maxZ)
|
|
maxZ = tp[2];
|
|
}
|
|
});
|
|
|
|
return maxZ;
|
|
}
|
|
|
|
// find bottom and top points, in range of init_scale_ratio
|
|
this.findBottom = function(box, init_scale_ratio){
|
|
|
|
var points = this.points;
|
|
var pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);
|
|
|
|
|
|
var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
|
|
// all cand points are translated into box coordinates
|
|
|
|
let translated_cand_points = cand_point_indices.map(function(i){
|
|
let x = pos_array[i*3];
|
|
let y = pos_array[i*3+1];
|
|
let z = pos_array[i*3+2];
|
|
|
|
let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
|
|
let tp = matmul(trans, p, 4);
|
|
return tp;
|
|
});
|
|
|
|
|
|
let minZ = 1000;
|
|
|
|
|
|
translated_cand_points.forEach((tp, i)=>{
|
|
if (Math.abs(tp[0]) < box.scale.x * init_scale_ratio.x/2 &&
|
|
Math.abs(tp[1]) < box.scale.y * init_scale_ratio.y/2 &&
|
|
Math.abs(tp[2]) < box.scale.z * init_scale_ratio.z/2)
|
|
{
|
|
if (tp[2] < minZ)
|
|
minZ = tp[2];
|
|
}
|
|
});
|
|
|
|
return minZ;
|
|
};
|
|
|
|
this.grow_box = function(box, min_distance, init_scale_ratio){
|
|
console.log("grow box, min_distance", min_distance, box.scale, init_scale_ratio);
|
|
let start_time = new Date().getTime();
|
|
var points = this.points;
|
|
var pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
var trans = transpose(euler_angle_to_rotate_matrix(box.rotation, {x:0, y:0, z:0}), 4);
|
|
|
|
|
|
var cand_point_indices = this.get_covering_position_indices(points, box.position, box.scale, box.rotation, init_scale_ratio);
|
|
|
|
//todo: different definition.
|
|
let groundLevel = 0.3;
|
|
|
|
if (this.data.cfg.enableDynamicGroundLevel)
|
|
{
|
|
groundLevel = Math.min(box.scale.z/3, Math.max(0.2, box.scale.x/10, box.scale.y/10));
|
|
console.log('ground level', groundLevel, box.scale);
|
|
}
|
|
|
|
|
|
// all cand points are translated into box coordinates
|
|
|
|
let translated_cand_points = cand_point_indices.map(function(i){
|
|
let x = pos_array[i*3];
|
|
let y = pos_array[i*3+1];
|
|
let z = pos_array[i*3+2];
|
|
|
|
let p = [x-box.position.x, y-box.position.y, z-box.position.z, 1];
|
|
let tp = matmul(trans, p, 4);
|
|
return tp;
|
|
});
|
|
|
|
|
|
|
|
let extreme= {
|
|
max: {
|
|
x:-100000,
|
|
y:-100000,
|
|
z:-100000,
|
|
},
|
|
|
|
min: {
|
|
x:1000000,
|
|
y:1000000,
|
|
z:1000000,
|
|
},
|
|
};
|
|
|
|
|
|
let inside_points = 0;
|
|
translated_cand_points.forEach((tp, i)=>{
|
|
if ((Math.abs(tp[0]) > box.scale.x/2+0.01)
|
|
|| (Math.abs(tp[1]) > box.scale.y/2+0.01)
|
|
|| (Math.abs(tp[2]) > box.scale.z/2+0.01) ){
|
|
|
|
|
|
return;
|
|
} else{
|
|
|
|
if ((box.scale.z < 0.6) || ((box.scale.z > 0.6) && (tp[2] > -box.scale.z/2 + groundLevel)))
|
|
{
|
|
inside_points += 1;
|
|
|
|
if (tp[0] > extreme.max.x) {
|
|
extreme.max.x = tp[0];
|
|
}
|
|
|
|
if (tp[0] < extreme.min.x){
|
|
extreme.min.x = tp[0];
|
|
}
|
|
|
|
if (tp[1] > extreme.max.y){
|
|
extreme.max.y = tp[1];
|
|
}
|
|
|
|
if (tp[1] < extreme.min.y){
|
|
extreme.min.y = tp[1];
|
|
}
|
|
}
|
|
|
|
if (tp[2] > extreme.max.z){
|
|
extreme.max.z = tp[2];
|
|
}
|
|
|
|
if (tp[2] < extreme.min.z){
|
|
extreme.min.z = tp[2];
|
|
}
|
|
|
|
}
|
|
});
|
|
|
|
if (inside_points < 10) //too few points, give up.
|
|
{
|
|
return {
|
|
max:{
|
|
x: box.scale.x/2,
|
|
y: box.scale.y/2,
|
|
z: box.scale.z/2,
|
|
},
|
|
min:{
|
|
x: -box.scale.x/2,
|
|
y: -box.scale.y/2,
|
|
z: -box.scale.z/2,
|
|
}
|
|
};
|
|
}
|
|
|
|
//let translated_cand_points_with_ground = translated_cand_points;
|
|
|
|
// filter ground points
|
|
// translated_cand_points = translated_cand_points.filter(function(tp, i){
|
|
// return tp[2] > -box.scale.z/2 + groundLevel;
|
|
// });
|
|
|
|
|
|
let extreme_adjusted = true;
|
|
let loop_count = 0;
|
|
while (extreme_adjusted){
|
|
loop_count++;
|
|
if (loop_count > 100000)
|
|
{
|
|
console.log("deep loops in grow_box");
|
|
break;
|
|
}
|
|
|
|
extreme_adjusted = false;
|
|
|
|
// x+
|
|
let find_point = translated_cand_points.find(tp=>{
|
|
return tp[0] > extreme.max.x && tp[0] < extreme.max.x + min_distance/2 &&
|
|
tp[1] < extreme.max.y && tp[1] > extreme.min.y &&
|
|
tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.max.x += min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
// x -
|
|
find_point = translated_cand_points.find(tp=>{
|
|
return tp[0] < extreme.min.x && tp[0] > extreme.min.x - min_distance/2 &&
|
|
tp[1] < extreme.max.y && tp[1] > extreme.min.y &&
|
|
tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.min.x -= min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
// y+
|
|
find_point = translated_cand_points.find(tp=>{
|
|
return tp[1] > extreme.max.y && tp[1] < extreme.max.y + min_distance/2 &&
|
|
tp[0] < extreme.max.x && tp[0] > extreme.min.x &&
|
|
tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.max.y += min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
// y -
|
|
find_point = translated_cand_points.find(tp=>{
|
|
return tp[1] < extreme.min.y && tp[1] > extreme.min.y - min_distance/2 &&
|
|
tp[0] < extreme.max.x && tp[0] > extreme.min.x &&
|
|
tp[2] < extreme.max.z && tp[2] > extreme.min.z + groundLevel;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.min.y -= min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
|
|
// z+
|
|
find_point = translated_cand_points.find(tp=>{
|
|
return tp[0] < extreme.max.x && tp[0] > extreme.min.x &&
|
|
tp[1] < extreme.max.y && tp[1] > extreme.min.y &&
|
|
tp[2] > extreme.max.z && tp[2] < extreme.max.z + min_distance/2;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.max.z += min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
// z-
|
|
find_point = translated_cand_points.find(tp=>{
|
|
return tp[0] < extreme.max.x && tp[0] > extreme.min.x &&
|
|
tp[1] < extreme.max.y && tp[1] > extreme.min.y &&
|
|
tp[2] < extreme.min.z && tp[2] > extreme.min.z - min_distance/2;
|
|
});
|
|
|
|
if (find_point){
|
|
extreme.min.z -= min_distance/2;
|
|
extreme_adjusted = true;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
// refine extreme values
|
|
//1 set initial value
|
|
let refined_extreme= {
|
|
max: {
|
|
x: extreme.max.x - min_distance/2,
|
|
y: extreme.max.y - min_distance/2,
|
|
z: extreme.max.z - min_distance/2,
|
|
},
|
|
|
|
min: {
|
|
x: extreme.min.x + min_distance/2,
|
|
y: extreme.min.y + min_distance/2,
|
|
z: extreme.min.z + min_distance/2,
|
|
},
|
|
};
|
|
|
|
|
|
//2 find refined values.
|
|
translated_cand_points.forEach(tp=>{
|
|
if (tp[0] > extreme.max.x || tp[0] < extreme.min.x ||
|
|
tp[1] > extreme.max.y || tp[1] < extreme.min.y ||
|
|
tp[2] > extreme.max.z || tp[2] < extreme.min.z)
|
|
{
|
|
|
|
}
|
|
else{
|
|
if (tp[0] > refined_extreme.max.x && tp[2] > extreme.min.z + groundLevel) {
|
|
refined_extreme.max.x = tp[0];
|
|
}
|
|
|
|
if (tp[0] < refined_extreme.min.x && tp[2] > extreme.min.z + groundLevel){
|
|
refined_extreme.min.x = tp[0];
|
|
}
|
|
|
|
if (tp[1] > refined_extreme.max.y && tp[2] > extreme.min.z + groundLevel){
|
|
refined_extreme.max.y = tp[1];
|
|
}
|
|
|
|
if (tp[1] < refined_extreme.min.y && tp[2] > extreme.min.z + groundLevel){
|
|
refined_extreme.min.y = tp[1];
|
|
}
|
|
|
|
if (tp[2] > refined_extreme.max.z){
|
|
refined_extreme.max.z = tp[2];
|
|
}
|
|
|
|
if (tp[2] < refined_extreme.min.z){
|
|
refined_extreme.min.z = tp[2];
|
|
}
|
|
|
|
}
|
|
});
|
|
|
|
refined_extreme.min.z -= groundLevel;
|
|
console.log("refined extreme", JSON.stringify(refined_extreme));
|
|
return refined_extreme;
|
|
}
|
|
|
|
|
|
this.get_box_points_number=function(box){
|
|
var indices = this._get_points_index_of_box(this.points, box, 1.0);
|
|
return indices.length;
|
|
};
|
|
|
|
this.reset_box_points_color = function(box){
|
|
let color = this.points.geometry.getAttribute("color").array;
|
|
let indices = this._get_points_index_of_box(this.points, box, 1.0);
|
|
if (this.data.cfg.color_points=="intensity")
|
|
{
|
|
|
|
indices.forEach((i)=>{
|
|
let intensity = this.pcd.intensity[i];
|
|
intensity *= 8;
|
|
|
|
if (intensity > 1)
|
|
intensity = 1.0;
|
|
|
|
color[i*3] = intensity;
|
|
color[i*3+1] = intensity;
|
|
color[i*3+2] = 1 - intensity;
|
|
});
|
|
|
|
}
|
|
else
|
|
{
|
|
indices.forEach((i)=>{
|
|
color[i*3] = this.data.cfg.point_brightness;
|
|
color[i*3+1] = this.data.cfg.point_brightness;
|
|
color[i*3+2] = this.data.cfg.point_brightness;
|
|
});
|
|
}
|
|
};
|
|
|
|
|
|
this.set_box_points_color=function(box, target_color){
|
|
//var pos = this.points.geometry.getAttribute("position");
|
|
var color = this.points.geometry.getAttribute("color");
|
|
|
|
if (!target_color){
|
|
if (this.data.cfg.color_obj == "category")
|
|
{
|
|
target_color = globalObjectCategory.get_color_by_category(box.obj_type);
|
|
}
|
|
else if (this.data.cfg.color_obj == "id")// by id
|
|
{
|
|
let idx = (box.obj_track_id)?parseInt(box.obj_track_id): box.obj_local_id;
|
|
target_color = globalObjectCategory.get_color_by_id(idx);
|
|
}
|
|
else // no color
|
|
{
|
|
|
|
}
|
|
}
|
|
|
|
if (target_color)
|
|
{
|
|
|
|
var indices = this._get_points_index_of_box(this.points, box, 1.0);
|
|
indices.forEach(function(i){
|
|
color.array[i*3] = target_color.x;
|
|
color.array[i*3+1] = target_color.y;
|
|
color.array[i*3+2] = target_color.z;
|
|
});
|
|
}
|
|
|
|
};
|
|
|
|
this.set_spec_points_color=function(point_indices, target_color){
|
|
//var pos = this.points.geometry.getAttribute("position");
|
|
var color = this.points.geometry.getAttribute("color");
|
|
|
|
point_indices.forEach(function(i){
|
|
color.array[i*3] = target_color.x;
|
|
color.array[i*3+1] = target_color.y;
|
|
color.array[i*3+2] = target_color.z;
|
|
});
|
|
};
|
|
|
|
// this is used when pointbrightness is updated.
|
|
this.recolor_all_points = function(){
|
|
this.set_points_color({
|
|
x: this.data.cfg.point_brightness,
|
|
y: this.data.cfg.point_brightness,
|
|
z: this.data.cfg.point_brightness,
|
|
});
|
|
this.color_points();
|
|
this.update_points_color();
|
|
};
|
|
|
|
// set all points to specified color
|
|
this.set_points_color=function(target_color){
|
|
var color = this.points.geometry.getAttribute("color");
|
|
for (var i = 0; i<color.count; i++){
|
|
color.array[i*3] = target_color.x;
|
|
color.array[i*3+1] = target_color.y;
|
|
color.array[i*3+2] = target_color.z;
|
|
}
|
|
};
|
|
|
|
|
|
this.update_points_color=function(){
|
|
if (this.points){ //some time points may fail to load.
|
|
this.points.geometry.getAttribute("color").needsUpdate = true;
|
|
//this.points.geometry.removeAttribute("color");
|
|
//this.points.geometry.setAttribute("color", new THREE.Float32BufferAttribute(color.array, 3 ));
|
|
}
|
|
};
|
|
|
|
|
|
this.remove_all_points=function(){
|
|
if (this.points){
|
|
this.world.data.dbg.free();
|
|
this.points.geometry.dispose();
|
|
this.points.material.dispose();
|
|
|
|
|
|
if (this.points.points_backup){
|
|
this.world.data.dbg.free();
|
|
this.points.points_backup.geometry.dispose();
|
|
this.points.points_backup.material.dispose();
|
|
|
|
if (this.points.points_backup.points_backup){
|
|
this.world.data.dbg.free();
|
|
this.points.points_backup.points_backup.geometry.dispose();
|
|
this.points.points_backup.points_backup.material.dispose();
|
|
this.points.points_backup.points_backup = null;
|
|
}
|
|
|
|
this.points.points_backup = null;
|
|
}
|
|
|
|
|
|
this.points = null;
|
|
}else {
|
|
console.error("destroy empty world!");
|
|
}
|
|
};
|
|
|
|
|
|
|
|
this.select_points_by_view_rect=function(x,y,w,h, camera){
|
|
var points = this.points;
|
|
var pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
var indices = [];
|
|
var points = [];
|
|
var p = new THREE.Vector3();
|
|
|
|
for (var i=0; i< pos_array.length/3; i++){
|
|
p.set(pos_array[i*3], pos_array[i*3+1], pos_array[i*3+2]);
|
|
p = this.world.lidarPosToScene(p);
|
|
p.project(camera);
|
|
//p.x = p.x/p.z;
|
|
//p.y = p.y/p.z;
|
|
//console.log(p);
|
|
if ((p.x >= x) && (p.x <= x+w) && (p.y>=y) && (p.y<=y+h) && (p.z>0)){
|
|
indices.push(i);
|
|
points.push([pos_array[i*3], pos_array[i*3+1], pos_array[i*3+2]]);
|
|
}
|
|
}
|
|
|
|
console.log("select rect points", indices.length);
|
|
|
|
//this.set_spec_points_color(indices, {x:1,y:0,z:0});
|
|
//this.update_points_color();
|
|
|
|
return points;
|
|
};
|
|
|
|
this.get_centroid = function(point_indices){
|
|
let points = this.points;
|
|
let pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
let center ={
|
|
x:0,y:0,z:0
|
|
};
|
|
|
|
point_indices.forEach(i=>{
|
|
center.x += pos_array[i*3];
|
|
center.y += pos_array[i*3+1];
|
|
center.z += pos_array[i*3+2];
|
|
});
|
|
|
|
center.x /= point_indices.length;
|
|
center.y /= point_indices.length;
|
|
center.z /= point_indices.length;
|
|
|
|
return center;
|
|
|
|
};
|
|
|
|
this.create_box_by_points = function(point_indices, camera){
|
|
|
|
|
|
let indices = point_indices;
|
|
let points = this.points;
|
|
let pos_array = points.geometry.getAttribute("position").array;
|
|
|
|
// todo: copied the following code from next function. refactor it!
|
|
console.log("select rect points", indices.length);
|
|
|
|
//compute center, no need to tranform to box coordinates, and can't do it in this stage.
|
|
/*
|
|
var extreme = array_as_vector_index_range(pos_array, 3, indices);
|
|
|
|
var center = {
|
|
x: (extreme.max[0]+extreme.min[0])/2,
|
|
y: (extreme.max[1]+extreme.min[1])/2,
|
|
z: (extreme.max[2]+extreme.min[2])/2,
|
|
};
|
|
*/
|
|
var rotation_z = camera.rotation.z + Math.PI/2;
|
|
var trans = transpose(euler_angle_to_rotate_matrix({x:0,y:0,z:rotation_z}, {x:0, y:0, z:0}), 4);
|
|
|
|
let center ={
|
|
x:0,y:0,z:0
|
|
};
|
|
|
|
point_indices.forEach(i=>{
|
|
center.x += pos_array[i*3];
|
|
center.y += pos_array[i*3+1];
|
|
center.z += pos_array[i*3+2];
|
|
});
|
|
|
|
center.x /= point_indices.length;
|
|
center.y /= point_indices.length;
|
|
center.z /= point_indices.length;
|
|
center.z = 0;
|
|
|
|
|
|
var relative_position = [];
|
|
indices.forEach(function(i){
|
|
//for (var i = 0; i < pos.count; i++){
|
|
var x = pos_array[i*3];
|
|
var y = pos_array[i*3+1];
|
|
var z = pos_array[i*3+2];
|
|
var p = [x-center.x, y-center.y, z-center.z, 1];
|
|
var tp = matmul(trans, p, 4);
|
|
relative_position.push([tp[0],tp[1],tp[2]]);
|
|
});
|
|
|
|
var relative_extreme = vector_range(relative_position);
|
|
var scale = {
|
|
x: relative_extreme.max[0] - relative_extreme.min[0],
|
|
y: relative_extreme.max[1] - relative_extreme.min[1],
|
|
z: relative_extreme.max[2] - relative_extreme.min[2],
|
|
};
|
|
|
|
// enlarge scale a little
|
|
|
|
|
|
// adjust center
|
|
this.world.annotation.translate_box_position(center, rotation_z, "x", relative_extreme.min[0] + scale.x/2);
|
|
this.world.annotation.translate_box_position(center, rotation_z, "y", relative_extreme.min[1] + scale.y/2);
|
|
this.world.annotation.translate_box_position(center, rotation_z, "z", relative_extreme.min[2] + scale.z/2);
|
|
|
|
|
|
scale.x += 0.02;
|
|
scale.y += 0.02;
|
|
scale.z += 0.02;
|
|
|
|
return this.world.annotation.add_box(center, scale, {x:0,y:0,z:rotation_z}, "Unknown", "");
|
|
};
|
|
|
|
}
|
|
|
|
export{Lidar}
|