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.

922 lines
24 KiB
JavaScript

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 };