label-3d-points/public/js/aux_lidar.js

import * as THREE from "./lib/three.module.js";
import { PCDLoader } from "./lib/PCDLoader.js";
import { matmul, euler_angle_to_rotate_matrix_3by3 } from "./util.js";

//todo: clean arrows

function AuxLidar(sceneMeta, world, frameInfo, auxLidarName) {
  this.world = world;
  this.frameInfo = frameInfo;
  this.name = auxLidarName;
  this.sceneMeta = sceneMeta;
  this.coordinatesOffset = world.coordinatesOffset;

  this.showPointsOnly = true;
  this.showCalibBox = false;
  //this.cssStyleSelector = this.sceneMeta.calib.aux_lidar[this.name].cssstyleselector;
  this.color = this.sceneMeta.calib.aux_lidar[this.name].color;

  if (!this.color) {
    this.color = [
      this.world.data.cfg.point_brightness,
      this.world.data.cfg.point_brightness,
      this.world.data.cfg.point_brightness,
    ];
  }

  this.lidar_points = null; // read from file, centered at 0
  this.elements = null; // geometry points

  this.preloaded = false;
  this.loaded = false;

  this.go_cmd_received = false;
  this.webglScene = null;
  this.on_go_finished = null;
  this.go = function (webglScene, on_go_finished) {
    this.webglScene = webglScene;

    if (this.preloaded) {
      if (this.elements) {
        this.webglScene.add(this.elements.points);

        if (this.showCalibBox) this.webglScene.add(this.calib_box);
      }

      this.loaded = true;
      if (on_go_finished) on_go_finished();
    }

    //anyway we save go cmd
    {
      this.go_cmd_received = true;
      this.on_go_finished = on_go_finished;
    }
  };

  this.showCalibBox = function () {
    this.showCalibBox = true;
    this.webglScene.add(this.calib_box);
  };

  this.hideCalibBox = function () {
    this.showCalibBox = false;
    this.webglScene.remove(this.calib_box);
  };

  this.get_unoffset_lidar_points = function () {
    if (this.elements) {
      let pts = this.elements.points.geometry.getAttribute("position").array;
      return pts.map((p, i) => p - this.world.coordinatesOffset[i % 3]);
    } else {
      return [];
    }
  };

  // todo: what if it's not preloaded yet
  this.unload = function (keep_box) {
    if (this.elements) {
      this.webglScene.remove(this.elements.points);
      if (!this.showPointsOnly)
        this.elements.arrows.forEach((a) => this.webglScene.remove(a));

      if (!keep_box) this.webglScene.remove(this.calib_box);
    }
    this.loaded = false;
  };

  // todo: its possible to remove points before preloading,
  this.deleteAll = function (keep_box) {
    if (this.loaded) {
      this.unload();
    }

    if (this.elements) {
      //this.scene.remove(this.points);
      this.world.data.dbg.free();

      if (this.elements.points) {
        this.elements.points.geometry.dispose();
        this.elements.points.material.dispose();
      }

      if (this.elements.arrows) {
        this.elements.arrows.forEach((a) => {
          this.world.data.dbg.free();
          a.geometry.dispose();
          a.material.dispose();
        });
      }

      this.elements = null;
    }

    if (!keep_box && this.calib_box) {
      this.world.data.dbg.free();
      this.calib_box.geometry.dispose();
      this.calib_box.material.dispose();
      this.calib_box = null;
    }
  };

  this.filterPoints = function (position) {
    let filtered_position = [];

    if (pointsGlobalConfig.enableFilterPoints) {
      for (let i = 0; i <= position.length; i += 3) {
        if (position[i + 2] <= pointsGlobalConfig.filterPointsZ) {
          filtered_position.push(position[i]);
          filtered_position.push(position[i + 1]);
          filtered_position.push(position[i + 2]);
        }
      }
    }

    return filtered_position;
  };

  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_aux_lidar_path(this.name),
      //ok
      function (pcd) {
        var position = pcd.position;

        //_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");
        _self.lidar_points = position;

        // add one box to calibrate lidar with lidar
        _self.calib_box = _self.createCalibBox();

        // install callback for box changing
        _self.calib_box.on_box_changed = () => {
          _self.move_lidar(_self.calib_box);
        };

        //position = _self.transformPointsByOffset(position);
        position = _self.move_points(_self.calib_box);

        let elements = _self.buildGeometry(position);

        _self.elements = elements;
        //_self.points_backup = mesh;

        _self._afterPreload();
      },

      // on progress,
      function () {},

      // on error
      function () {
        //error
        console.log("load lidar 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");
      }
    );
  };

  // internal funcs below
  this._afterPreload = function () {
    this.preloaded = true;
    console.log(`lidar ${this.auxLidarName} preloaded`);
    if (this.on_preload_finished) {
      this.on_preload_finished();
    }
    if (this.go_cmd_received) {
      this.go(this.webglScene, this.on_go_finished);
    }
  };

  this.createCalibBox = function () {
    if (
      this.sceneMeta.calib.aux_lidar &&
      this.sceneMeta.calib.aux_lidar[this.name]
    ) {
      return this.world.annotation.createCuboid(
        {
          x:
            this.sceneMeta.calib.aux_lidar[this.name].translation[0] +
            this.coordinatesOffset[0],
          y:
            this.sceneMeta.calib.aux_lidar[this.name].translation[1] +
            this.coordinatesOffset[1],
          z:
            this.sceneMeta.calib.aux_lidar[this.name].translation[2] +
            this.coordinatesOffset[2],
        },
        { x: 0.5, y: 0.5, z: 0.5 },
        {
          x: this.sceneMeta.calib.aux_lidar[this.name].rotation[0],
          y: this.sceneMeta.calib.aux_lidar[this.name].rotation[1],
          z: this.sceneMeta.calib.aux_lidar[this.name].rotation[2],
        },
        "lidar",
        this.name
      );
    } else {
      return this.world.annotation.createCuboid(
        {
          x: this.coordinatesOffset[0],
          y: this.coordinatesOffset[1],
          z: this.coordinatesOffset[2],
        },
        { x: 0.5, y: 0.5, z: 0.5 },
        { x: 0, y: 0, z: 0 },
        "lidar",
        this.name
      );
    }
  };

  this.buildPoints = function (position) {
    // build geometry
    this.world.data.dbg.alloc();
    let geometry = new THREE.BufferGeometry();
    if (position.length > 0)
      geometry.addAttribute(
        "position",
        new THREE.Float32BufferAttribute(position, 3)
      );

    let pointColor = this.color;
    let color = [];
    for (var i = 0; i < position.length; i += 3) {
      color.push(pointColor[0]);
      color.push(pointColor[1]);
      color.push(pointColor[2]);
    }

    geometry.addAttribute("color", new THREE.Float32BufferAttribute(color, 3));

    geometry.computeBoundingSphere();

    // build material
    let pointSize = this.sceneMeta.calib.aux_lidar[this.name].point_size;
    if (!pointSize) pointSize = 1;

    let material = new THREE.PointsMaterial({
      size: pointSize,
      vertexColors: THREE.VertexColors,
    });
    //material.size = 2;
    material.sizeAttenuation = false;

    // build mesh
    let mesh = new THREE.Points(geometry, material);
    mesh.name = "lidar";

    return mesh;
  };

  this.buildGeometry = function (position) {
    let points = this.buildPoints(position);

    return {
      points: points,
    };
  };

  this.move_points = function (box) {
    let points = this.lidar_points;
    let trans = euler_angle_to_rotate_matrix_3by3(box.rotation);
    let rotated_points = matmul(trans, points, 3);
    let translation = [box.position.x, box.position.y, box.position.z];
    let translated_points = rotated_points.map((p, i) => {
      return p + translation[i % 3];
    });

    let filtered_position = this.filterPoints(translated_points);
    return filtered_position;
  };

  this.move_lidar = function (box) {
    let translated_points = this.move_points(box);

    let elements = this.buildGeometry(translated_points);

    // remove old points
    this.unload(true);
    this.deleteAll(true);

    this.elements = elements;
    //_self.points_backup = mesh;
    if (this.go_cmd_received) {
      // this should be always true
      this.webglScene.add(this.elements.points);
      if (!this.showPointsOnly)
        this.elements.arrows.forEach((a) => this.webglScene.add(a));
    }
  };
}

function AuxLidarManager(sceneMeta, world, frameInfo) {
  this.lidarList = [];

  if (world.data.cfg.enableAuxLidar && sceneMeta.aux_lidar) {
    let lidars = [];

    for (let r in sceneMeta.calib.aux_lidar) {
      if (!sceneMeta.calib.aux_lidar[r].disable) lidars.push(r);
    }

    this.lidarList = lidars.map((name) => {
      return new AuxLidar(sceneMeta, world, frameInfo, name);
    });
  }

  this.getAllBoxes = function () {
    if (this.showCalibBox) {
      return this.lidarList.map((r) => r.calib_box);
    } else {
      return [];
    }
  };

  this.preloaded = function () {
    for (let r in this.lidarList) {
      if (!this.lidarList[r].preloaded) return false;
    }
    return true;
  };

  this.go = function (webglScene, on_go_finished) {
    this.lidarList.forEach((r) => r.go(webglScene, on_go_finished));
  };

  this.preload = function (on_preload_finished) {
    this.lidarList.forEach((r) => r.preload(on_preload_finished));
  };

  this.unload = function () {
    this.lidarList.forEach((r) => r.unload());
  };

  this.deleteAll = function () {
    this.lidarList.forEach((r) => r.deleteAll());
  };

  this.getOperableObjects = function () {
    return this.lidarList.flatMap((r) => r.getOperableObjects());
  };

  this.showCalibBox = false;
  this.showCalibBox = function () {
    this.showCalibBox = true;
    this.lidarList.forEach((r) => r.showCalibBox());
  };

  this.hideCalibBox = function () {
    this.showCalibBox = false;
    this.lidarList.forEach((r) => r.hideCalibBox());
  };
}

export { AuxLidarManager };