import * as THREE from './lib/three.module.js'; import {logger} from "./log.js" import { Quaternion, Vector3 } from "./lib/three.module.js"; import{ml} from "./ml.js"; import {dotproduct, transpose, matmul, euler_angle_to_rotate_matrix_3by3} from "./util.js" function BoxOp(){ console.log("BoxOp called"); this.grow_box_distance_threshold = 0.3; this.init_scale_ratio = {x:2, y:2, z:3}; this.fit_bottom = function(box) { let bottom = box.world.lidar.findBottom(box, {x:2, y:2, z:3}); this.translate_box(box, 'z', bottom + box.scale.z/2); } this.fit_top = function(box) { let top = box.world.lidar.findTop(box, {x:1.2, y:1.2, z:2}); this.translate_box(box, 'z', top - box.scale.z/2); } this.fit_left = function(box) { var extreme = box.world.lidar.grow_box(box, this.grow_box_distance_threshold, this.init_scale_ratio); if (extreme){ this.translate_box(box, 'y', extreme.max.y - box.scale.y/2); } } this.fit_right = function(box) { var extreme = box.world.lidar.grow_box(box, this.grow_box_distance_threshold, this.init_scale_ratio); if (extreme){ this.translate_box(box, 'y', extreme.min.y + box.scale.y/2); } } this.fit_front = function(box) { var extreme = box.world.lidar.grow_box(box, this.grow_box_distance_threshold, this.init_scale_ratio); if (extreme){ this.translate_box(box, 'x', extreme.max.x - box.scale.x/2); } } this.fit_rear = function(box) { var extreme = box.world.lidar.grow_box(box, this.grow_box_distance_threshold, this.init_scale_ratio); if (extreme){ this.translate_box(box, 'x', extreme.min.x + box.scale.x/2); } } this.fit_size = function(box,axies) { this.grow_box(box, this.grow_box_distance_threshold, {x:2, y:2, z:3}, axies); } this.justifyAutoAdjResult = function(orgBox, box) { let distance = Math.sqrt((box.position.x-orgBox.position.x)*(box.position.x-orgBox.position.x) + (box.position.y-orgBox.position.y)*(box.position.y-orgBox.position.y) + (box.position.z-orgBox.position.z)*(box.position.z-orgBox.position.z)); if (distance > Math.sqrt(box.scale.x*box.scale.x + box.scale.y*box.scale.y + box.scale.z*box.scale.z)) { return false; } // if (Math.abs(box.rotation.z - orgBox.rotation.z) > Math.PI/4) // { // return false; // } if (box.scale.x > orgBox.scale.x*3 || box.scale.y > orgBox.scale.y*3 || box.scale.z > orgBox.scale.z*3) { return false; } return true; } this.auto_rotate_xyz= async function(box, callback, apply_mask, on_box_changed, noscaling, rotate_method){ let orgBox = box; box = { position: {x: box.position.x, y: box.position.y, z: box.position.z}, rotation: {x: box.rotation.x, y: box.rotation.y, z: box.rotation.z}, scale: {x: box.scale.x, y: box.scale.y, z: box.scale.z}, world: box.world, }; // auto grow // save scale let grow = (box)=>{ let org_scale = { x: box.scale.x, y: box.scale.y, z: box.scale.z, }; this.grow_box(box, this.grow_box_distance_threshold, {x:2, y:2, z:3}); this.auto_shrink_box(box); // now box has been centered. let points_indices = box.world.lidar.get_points_of_box(box,1.0).index; let extreme = box.world.lidar.get_dimension_of_points(points_indices, box); // restore scale if (noscaling){ box.scale.x = org_scale.x; box.scale.y = org_scale.y; box.scale.z = org_scale.z; } // return extreme; }; //points is N*3 shape let applyRotation = (ret, extreme_after_grow)=>{ let angle = ret.angle; if (!angle){ console.log("prediction not implemented?"); return; } //var points_indices = box.world.get_points_indices_of_box(box); let points_indices = box.world.lidar.get_points_of_box(box,1.0).index; var euler_delta = { x: angle[0], y: angle[1], z: angle[2] }; if (euler_delta.z > Math.PI){ euler_delta.z -= Math.PI*2; }; /* var composite_angel = linalg_std.euler_angle_composite(box.rotation, euler_delta); console.log("orig ", box.rotation.x, box.rotation.y, box.rotation.z); console.log("delt ", euler_delta.x, euler_delta.y, euler_delta.z); console.log("comp ", composite_angel.x, composite_angel.y, composite_angel.z); box.rotation.x = composite_angel.x; box.rotation.y = composite_angel.y; box.rotation.z = composite_angel.z; */ if (apply_mask){ if (apply_mask.x) box.rotation.x = euler_delta.x; if (apply_mask.y) box.rotation.y = euler_delta.y; if (apply_mask.z) box.rotation.z = euler_delta.z; } else{ box.rotation.x = euler_delta.x; box.rotation.y = euler_delta.y; box.rotation.z = euler_delta.z; } // rotation set, now rescaling the box // important: should use original points before rotation set var extreme = box.world.lidar.get_dimension_of_points(points_indices, box); let auto_adj_dimension = []; if (apply_mask){ if (apply_mask.x || apply_mask.y) auto_adj_dimension.push('z'); if (apply_mask.x || apply_mask.z) auto_adj_dimension.push('y'); if (apply_mask.y || apply_mask.z) auto_adj_dimension.push('x'); } else{ auto_adj_dimension = ['x','y','z']; } if (!noscaling){ auto_adj_dimension.forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis] - extreme.min[axis]; }) }else { //anyway, we move the box in a way let trans = euler_angle_to_rotate_matrix_3by3(box.rotation); trans = transpose(trans, 3); // compute the relative position of the origin point,that is, the lidar's position // note the origin point is offseted, we need to restore first. let boxpos = box.position; let orgPoint = [ - boxpos.x, - boxpos.y, - boxpos.z, ]; let orgPointInBoxCoord = matmul(trans, orgPoint, 3); let relativePosition = { x: orgPointInBoxCoord[0], y: orgPointInBoxCoord[1], z: 1, //orgPointInBoxCoord[2], } if (extreme_after_grow) extreme = extreme_after_grow; auto_adj_dimension.forEach((axis)=>{ if (relativePosition[axis]>0){ //stick to max this.translate_box(box, axis, extreme.max[axis] - box.scale[axis]/2); }else{ //stick to min this.translate_box(box, axis, extreme.min[axis] + box.scale[axis]/2); } }) } return box; }; let postProc = (box)=>{ if (this.justifyAutoAdjResult(orgBox, box)) { // copy back orgBox.position.x = box.position.x; orgBox.position.y = box.position.y; orgBox.position.z = box.position.z; orgBox.rotation.x = box.rotation.x; orgBox.rotation.y = box.rotation.y; orgBox.rotation.z = box.rotation.z; orgBox.scale.x = box.scale.x; orgBox.scale.y = box.scale.y; orgBox.scale.z = box.scale.z; } if (on_box_changed) on_box_changed(orgBox); if (callback){ callback(); } return orgBox; }; let extreme_after_grow = grow(box); if (!rotate_method){ let points = box.world.lidar.get_points_relative_coordinates_of_box_wo_rotation(box, 1); //let points = box.world.get_points_relative_coordinates_of_box(box, 1.0); points = points.filter(function(p){ return p[2] > - box.scale.z/2 + 0.3; }) let retBox = await ml.predict_rotation(points) .then(applyRotation) .then(postProc); return retBox; } if (rotate_method == "moving-direction") { let estimatedRot = this.estimate_rotation_by_moving_direciton(box); applyRotation({ angle:[ box.rotation.x, // use original rotation box.rotation.y, // use original rotation estimatedRot? estimatedRot.z : box.rotation.z, // use original rotation ] }, extreme_after_grow); postProc(box); return box; } else{ //dont rotate, or null applyRotation({ angle:[ box.rotation.x, // use original rotation box.rotation.y, // use original rotation box.rotation.z, // use original rotation ] }, extreme_after_grow); postProc(box); return box; } } this.auto_shrink_box= function(box){ var extreme = box.world.lidar.get_points_dimmension_of_box(box); ['x', 'y','z'].forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis]-extreme.min[axis]; }) }; this.estimate_rotation_by_moving_direciton = function(box) { let prevWorld = box.world.data.findWorld(box.world.frameInfo.scene, box.world.frameInfo.frame_index-1); let nextWorld = box.world.data.findWorld(box.world.frameInfo.scene, box.world.frameInfo.frame_index+1); let prevBox = prevWorld?prevWorld.annotation.findBoxByTrackId(box.obj_track_id): null; let nextBox = nextWorld?nextWorld.annotation.findBoxByTrackId(box.obj_track_id): null; if (prevBox && nextBox) { if ((prevBox.annotator && nextBox.annotator) || (!prevBox.annotator && !nextBox.annotator)) { // all annotated by machine or man, it's ok } else { // only one is manually annotated, use this one. if (prevBox.annotator) prevBox = null; if (nextBox.annotator) nextBox = null; } } if (!nextBox && !prevBox){ logger.logcolor("red", "Cannot estimate direction: neither previous nor next frame/box loaded/annotated.") return null; } let currentP = box.world.lidarPosToUtm(box.position); let nextP = nextBox?nextBox.world.lidarPosToUtm(nextBox.position) : null; let prevP = prevBox?prevBox.world.lidarPosToUtm(prevBox.position) : null; if (!prevP) prevP = currentP; if (!nextP) nextP = currentP; let azimuth = Math.atan2(nextP.y-prevP.y, nextP.x-prevP.x) let estimatedRot = box.world.utmRotToLidar(new THREE.Euler(0,0,azimuth, "XYZ")); return estimatedRot; }; this.grow_box= function(box, min_distance, init_scale_ratio, axies){ if (!axies) { axies = ['x','y','z']; } var extreme = box.world.lidar.grow_box(box, min_distance, init_scale_ratio); if (extreme){ axies.forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis] - extreme.min[axis]; }) } }; this.change_rotation_y = function(box, theta, sticky, on_box_changed){ //box.rotation.x += theta; //on_box_changed(box); var points_indices = box.world.lidar.get_points_indices_of_box(box); var _tempQuaternion = new Quaternion(); var rotationAxis = new Vector3(0, 1, 0); // NOTE: the front/end subview is different from top/side view, that we look at the reverse direction of y-axis // it's end view acturally. // we could project front-view, but the translation (left, right) will be in reverse direction of top view. /// that would be frustrating. box.quaternion.multiply( _tempQuaternion.setFromAxisAngle( rotationAxis, -theta ) ).normalize(); if (sticky){ var extreme = box.world.lidar.get_dimension_of_points(points_indices, box); ['x','z'].forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis] - extreme.min[axis]; }) } if (on_box_changed) on_box_changed(box); } this.auto_rotate_y=function(box, on_box_changed){ let points = box.world.lidar.get_points_of_box(box, 2.0); // 1. find surounding points var side_indices = [] var side_points = [] points.position.forEach(function(p, i){ if ((p[0] > box.scale.x/2 || p[0] < -box.scale.x/2) && (p[1] < box.scale.y/2 && p[1] > -box.scale.y/2)){ side_indices.push(points.index[i]); side_points.push(points.position[i]); } }) var end_indices = [] var end_points = [] points.position.forEach(function(p, i){ if ((p[0] < box.scale.x/2 && p[0] > -box.scale.x/2) && (p[1] > box.scale.y/2 || p[1] < -box.scale.y/2)){ end_indices.push(points.index[i]); end_points.push(points.position[i]); } }) // 2. grid by 0.3 by 0.3 // compute slope (derivative) // for side part (pitch/tilt), use y,z axis // for end part (row), use x, z axis // box.world.lidar.set_spec_points_color(side_indices, {x:1,y:0,z:0}); // box.world.lidar.set_spec_points_color(end_indices, {x:0,y:0,z:1}); // box.world.lidar.update_points_color(); var x = end_points.map(function(x){return x[0]}); //var y = side_points.map(function(x){return x[1]}); var z = end_points.map(function(x){return x[2]}); var z_mean = z.reduce(function(x,y){return x+y;}, 0)/z.length; var z = z.map(function(x){return x-z_mean;}); var theta = Math.atan2(dotproduct(x,z), dotproduct(x,x)); console.log(theta); this.change_rotation_y(box, theta, false, on_box_changed); } this.change_rotation_x=function(box, theta, sticky, on_box_changed){ var points_indices = box.world.lidar.get_points_indices_of_box(box); //box.rotation.x += theta; //on_box_changed(box); var _tempQuaternion = new Quaternion(); var rotationAxis = new Vector3(1,0,0); box.quaternion.multiply( _tempQuaternion.setFromAxisAngle( rotationAxis, theta ) ).normalize(); if (sticky){ var extreme = box.world.lidar.get_dimension_of_points(points_indices, box); ['y','z'].forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis] - extreme.min[axis]; }) } if (on_box_changed) on_box_changed(box); }; this.auto_rotate_x=function(box, on_box_changed){ console.log("x auto ratote"); let points = box.world.lidar.get_points_of_box(box, 2.0); // 1. find surounding points var side_indices = [] var side_points = [] points.position.forEach(function(p, i){ if ((p[0] > box.scale.x/2 || p[0] < -box.scale.x/2) && (p[1] < box.scale.y/2 && p[1] > -box.scale.y/2)){ side_indices.push(points.index[i]); side_points.push(points.position[i]); } }) var end_indices = [] var end_points = [] points.position.forEach(function(p, i){ if ((p[0] < box.scale.x/2 && p[0] > -box.scale.x/2) && (p[1] > box.scale.y/2 || p[1] < -box.scale.y/2)){ end_indices.push(points.index[i]); end_points.push(points.position[i]); } }) // 2. grid by 0.3 by 0.3 // compute slope (derivative) // for side part (pitch/tilt), use y,z axis // for end part (row), use x, z axis // box.world.lidar.set_spec_points_color(side_indices, {x:1,y:0,z:0}); // box.world.lidar.set_spec_points_color(end_indices, {x:0,y:0,z:1}); // box.world.lidar.update_points_color(); //render(); var x = side_points.map(function(x){return x[0]}); var y = side_points.map(function(x){return x[1]}); var z = side_points.map(function(x){return x[2]}); var z_mean = z.reduce(function(x,y){return x+y;}, 0)/z.length; var z = z.map(function(x){return x-z_mean;}); var theta = Math.atan2(dotproduct(y,z), dotproduct(y,y)); console.log(theta); this.change_rotation_x(box, theta, false, on_box_changed); }; this.translate_box=function(box, axis, delta){ let t = {x:0, y:0, z:0}; t[axis] = delta; // switch (axis){ // case 'x': // box.position.x += delta*Math.cos(box.rotation.z); // box.position.y += delta*Math.sin(box.rotation.z); // break; // case 'y': // box.position.x += delta*Math.cos(Math.PI/2 + box.rotation.z); // box.position.y += delta*Math.sin(Math.PI/2 + box.rotation.z); // break; // case 'z': // box.position.z += delta; // break; // } let trans = this.translateBoxInBoxCoord(box.rotation, t); box.position.x += trans.x; box.position.y += trans.y; box.position.z += trans.z; }; this.translateBoxInBoxCoord = function(rotation, t) { // euler let euler = new THREE.Euler(rotation.x, rotation.y, rotation.z, "XYZ") let trans = new THREE.Vector3(t.x, t.y, t.z).applyEuler(euler); return trans; }; this.rotate_z=function(box, theta, sticky){ // points indices shall be obtained before rotation. var points_indices = box.world.lidar.get_points_indices_of_box(box); var _tempQuaternion = new Quaternion(); var rotationAxis = new Vector3(0,0,1); box.quaternion.multiply( _tempQuaternion.setFromAxisAngle( rotationAxis, theta ) ).normalize(); if (sticky){ var extreme = box.world.lidar.get_dimension_of_points(points_indices, box); ['x','y'].forEach((axis)=>{ this.translate_box(box, axis, (extreme.max[axis] + extreme.min[axis])/2); box.scale[axis] = extreme.max[axis] - extreme.min[axis]; }) } }, this.interpolate_selected_object= function(sceneName, objTrackId, currentFrame, done){ // var xhr = new XMLHttpRequest(); // // we defined the xhr // xhr.onreadystatechange = function () { // if (this.readyState != 4) // return; // if (this.status == 200) { // var ret = JSON.parse(this.responseText); // console.log(ret); // if (done) // done(sceneName, ret); // } // }; // xhr.open('GET', "/interpolate?scene="+sceneName+"&frame="+currentFrame+"&obj_id="+objTrackId, true); // xhr.send(); }; this.highlightBox = function(box){ if (box){ box.material.color.r=1; box.material.color.g=0; box.material.color.b=1; box.material.opacity=1; } }; this.unhighlightBox = function(box){ if (box){ // box.material.color = new THREE.Color(parseInt("0x"+get_obj_cfg_by_type(box.obj_type).color.slice(1))); // box.material.opacity = box.world.data.cfg.box_opacity; box.world.annotation.color_box(box); } } this.interpolateAsync = async function(worldList, boxList, applyIndList){ // if annotator is not null, it's annotated by us algorithms let anns = boxList.map(b=> (!b || b.annotator)? null : b.world.annotation.ann_to_vector_global(b)); console.log(anns); let ret = await ml.interpolate_annotation(anns); console.log(ret); let refObj = boxList.find(b=>!!b); let obj_type = refObj.obj_type; let obj_track_id = refObj.obj_track_id; let obj_attr = refObj.obj_attr; for (let i = 0; i< boxList.length; i++){ if (!applyIndList[i]) { continue; } // let world = worldList[i]; let ann = world.annotation.vector_global_to_ann(ret[i]); // don't roate x/y if (!pointsGlobalConfig.enableAutoRotateXY) { ann.rotation.x = 0; ann.rotation.y = 0; } // if (world.lidar.get_box_points_number(ann) == 0) // { // continue; // } if (!boxList[i]){ // create new box let newBox = world.annotation.add_box(ann.position, ann.scale, ann.rotation, obj_type, obj_track_id, obj_attr); newBox.annotator="i"; world.annotation.load_box(newBox); world.annotation.setModified(); } else if (boxList[i].annotator) { // modify box attributes let b = ann; boxList[i].position.x = b.position.x; boxList[i].position.y = b.position.y; boxList[i].position.z = b.position.z; boxList[i].scale.x = b.scale.x; boxList[i].scale.y = b.scale.y; boxList[i].scale.z = b.scale.z; boxList[i].rotation.x = b.rotation.x; boxList[i].rotation.y = b.rotation.y; boxList[i].rotation.z = b.rotation.z; boxList[i].annotator = "i"; boxList[i].world.annotation.setModified(); } } }; this.interpolateAndAutoAdjustAsync = async function(worldList, boxList, onFinishOneBoxCB, applyIndList, dontRotate){ // if annotator is not null, it's annotated by us algorithms let anns = boxList.map((b,i)=> { if (!b) return null; if (b.annotator) return null; return b.world.annotation.ann_to_vector_global(b); }); console.log("anns to interpolate", anns); let autoAdjAsync = async (index, newAnn)=>{ //let box = boxList[index]; let world = worldList[index]; let tempBox = world.annotation.vector_global_to_ann(newAnn); tempBox.world = world; // autoadj is timecomsuming // jump this step let rotateThis = dontRotate; if (!applyIndList[index]){ rotateThis = "dontrotate"; } let adjustedBox = await this.auto_rotate_xyz(tempBox, null, null, null, true, rotateThis); return world.annotation.ann_to_vector_global(adjustedBox); }; let refObj = boxList.find(b=>!!b); let obj_type = refObj.obj_type; let obj_track_id = refObj.obj_track_id; let obj_attr = refObj.obj_attr; let onFinishOneBox= (index)=>{ console.log(`auto insert ${index} ${worldList[index].frameInfo.frame}done`); let i = index; if (!applyIndList[i]){ return; } if (!boxList[i]){ // create new box let world = worldList[i]; let ann = world.annotation.vector_global_to_ann(anns[i]); let newBox = world.annotation.add_box(ann.position, ann.scale, ann.rotation, obj_type, obj_track_id, obj_attr); newBox.annotator="a"; world.annotation.load_box(newBox); } else if (boxList[i].annotator) { // modify box attributes let b = boxList[i].world.annotation.vector_global_to_ann(anns[i]); boxList[i].position.x = b.position.x; boxList[i].position.y = b.position.y; boxList[i].position.z = b.position.z; boxList[i].scale.x = b.scale.x; boxList[i].scale.y = b.scale.y; boxList[i].scale.z = b.scale.z; boxList[i].rotation.x = b.rotation.x; boxList[i].rotation.y = b.rotation.y; boxList[i].rotation.z = b.rotation.z; boxList[i].annotator="a"; } if (onFinishOneBoxCB) onFinishOneBoxCB(i); }; let ret = await ml.interpolate_annotation(anns, autoAdjAsync, onFinishOneBox); console.log(ret); // for (let i = 0; i< boxList.length; i++){ // onFinishOneBox(i); // } }; } export {BoxOp}