pointcloud2: buffergeometry, subsampling

RobotWebTools · Apr 4, 2018 · d63760b · d63760b
1 parent 654c2b8
commit d63760b
Show file tree

Hide file tree

Showing 9 changed files with 414 additions and 413 deletions.
diff --git a/build/ros3d.js b/build/ros3d.js
diff --git a/build/ros3d.min.js b/build/ros3d.min.js
diff --git a/examples/kitti.html b/examples/kitti.html
@@ -0,0 +1,66 @@
+<!DOCTYPE html>
+<html>
+<head>
+<meta charset="utf-8" />
+
+<script src="https://static.robotwebtools.org/threejs/current/three.js"></script>
+<script src="https://static.robotwebtools.org/EventEmitter2/current/eventemitter2.min.js"></script>
+<script src="https://static.robotwebtools.org/roslibjs/current/roslib.js"></script>
+<script src="../build/ros3d.js"></script>
+
+<script>
+  /**
+   * Setup all visualization elements when the page is loaded.
+   */
+  function init() {
+    // Connect to ROS.
+    var ros = new ROSLIB.Ros({
+      url : 'ws://localhost:9090'
+    });
+
+    // Create the main viewer.
+    var viewer = new ROS3D.Viewer({
+      divID : 'viewer',
+      width : 800,
+      height : 600,
+      antialias : true
+    });
+
+    // Setup a client to listen to TFs.
+    var tfClient = new ROSLIB.TFClient({
+      ros : ros,
+      angularThres : 0.01,
+      transThres : 0.01,
+      rate : 10.0,
+      fixedFrame : 'velo_link'
+    });
+
+    var cloudClient = new ROS3D.PointCloud2({
+        ros: ros,
+        tfClient: tfClient,
+        rootObject: viewer.scene,
+        topic: '/kitti/velo/pointcloud',
+        max_pts: 200000,
+        pointRatio: 2,
+        messageRatio: 1,
+        material: { size: 3, color: new THREE.Color(0xffffff), sizeAttenuation: false },
+        //colorsrc: 'i', colormap: function(i) { return new THREE.Color(3*i,0,1-3*i); }
+        colorsrc: 'z', colormap: function(z) { z=z+2; return new THREE.Color(z,0,1-z); }
+    });
+  }
+</script>
+</head>
+
+<body onload="init()">
+  <h1><a href="http://www.cvlibs.net/datasets/kitti">Kitti</a> PointCloud2 Example</h1>
+  <p>Run the following commands in the terminal then refresh the page.</p>
+  <ol>
+    <li><tt>roslaunch ros3djs.launch</tt></li>
+    <li><tt>roslaunch rosbridge_server rosbridge_websocket.launch</tt></li>
+    <li><tt>rosrun tf2_web_republisher tf2_web_republisher</tt></li>
+    <li><tt>rosparam set use_sim_time true</tt></li>
+    <li><tt>rosbag play -l --clock <a href="https://github.com/tomas789/kitti2bag">kitti_2011_09_26_drive_0002_synced.bag</a></tt></li>
+  </ol>
+  <div id="viewer"></div>
+</body>
+</html>
diff --git a/examples/pointcloud2.html b/examples/pointcloud2.html
@@ -39,7 +39,8 @@
         ros: ros,
         tfClient: tfClient,
         rootObject: viewer.scene,
-        topic: '/camera/depth_registered/points'
+        topic: '/camera/depth_registered/points',
+        material: { size: 0.05 }
     });
   }
 </script>

diff --git a/examples/ros3djs.launch b/examples/ros3djs.launch
@@ -0,0 +1,4 @@
+<launch>
+	<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" />
+	<node pkg="tf2_web_republisher" type="tf2_web_republisher" name="tf2_web_republisher" />
+</launch>
diff --git a/package.json b/package.json
@@ -12,7 +12,7 @@
   },
   "devDependencies": {
     "chai": "^3.5.0",
-    "grunt": "^1.0.1",
+    "grunt": "^1.0.2",
     "grunt-contrib-clean": "^1.0.0",
     "grunt-contrib-concat": "^1.0.1",
     "grunt-contrib-jshint": "^1.1.0",

diff --git a/src/sensors/LaserScan.js b/src/sensors/LaserScan.js
@@ -9,22 +9,19 @@
  * @param options - object with following keys:
  *
  *  * ros - the ROSLIB.Ros connection handle
- *  * topic - the marker topic to listen to
+ *  * topic - the marker topic to listen to (default '/scan')
  *  * tfClient - the TF client handle to use
- *  * color - (optional) color of the points (default 0xFFA500)
- *  * texture - (optional) Image url for a texture to use for the points. Defaults to a single white pixel.
- *  * rootObject (optional) - the root object to add this marker to
- *  * size (optional) - size to draw each point (default 0.05)
- *  * max_pts (optional) - number of points to draw (default 100)
+ *  * rootObject (optional) - the root object to add this marker to use for the points.
+ *  * max_pts (optional) - number of points to draw (default: 10000)
+ *  * pointRatio (optional) - point subsampling ratio (default: 1, no subsampling)
+ *  * messageRatio (optional) - message subsampling ratio (default: 1, no subsampling)
+ *  * material (optional) - a material object or an option to construct a PointsMaterial.
  */
 ROS3D.LaserScan = function(options) {
   options = options || {};
   this.ros = options.ros;
   this.topicName = options.topic || '/scan';
-  this.color = options.color || 0xFFA500;
-
   this.particles = new ROS3D.Particles(options);
-
   this.rosTopic = undefined;
   this.subscribe();
 
@@ -51,20 +48,19 @@ ROS3D.LaserScan.prototype.subscribe = function(){
 };
 
 ROS3D.LaserScan.prototype.processMessage = function(message){
-  setFrame(this.particles, message.header.frame_id);
-
+  if(!this.particles.setup(message.header.frame_id)) {
+      return;
+  }
   var n = message.ranges.length;
-  for(var i=0;i<n;i++){
+  var j = 0;
+  for(var i=0;i<n;i+=this.particles.pointRatio){
     var range = message.ranges[i];
-    if(range < message.range_min || range > message.range_max){
-      this.particles.alpha[i] = 0.0;
-    }else{
+    if(range >= message.range_min && range <= message.range_max){
         var angle = message.angle_min + i * message.angle_increment;
-        this.particles.points[i] = new THREE.Vector3( range * Math.cos(angle), range * Math.sin(angle), 0.0 );
-        this.particles.alpha[i] = 1.0;
+        this.particles.point.array[j++] = range * Math.cos(angle);
+        this.particles.point.array[j++] = range * Math.sin(angle);
+        this.particles.point.array[j++] = 0.0;
     }
-    this.particles.colors[ i ] = new THREE.Color( this.color );
   }
-
-  finishedUpdate(this.particles, n);
+  this.particles.update(j/3);
 };
diff --git a/src/sensors/Particles.js b/src/sensors/Particles.js
@@ -1,5 +1,6 @@
 /**
  * @author David V. Lu!! - davidvlu@gmail.com
+ * @author Mathieu Bredif - mathieu.bredif@ign.fr
  */
 
 /**
@@ -9,143 +10,103 @@
  * @param options - object with following keys:
  *
  *  * tfClient - the TF client handle to use
- *  * texture - (optional) Image url for a texture to use for the points. Defaults to a single white pixel.
- *  * rootObject (optional) - the root object to add this marker to
- *  * size (optional) - size to draw each point (default 0.05)
- *  * max_pts (optional) - number of points to draw (default 100)
+ *  * rootObject (optional) - the root object to add this marker to use for the points.
+ *  * max_pts (optional) - number of points to draw (default: 10000)
+ *  * pointRatio (optional) - point subsampling ratio (default: 1, no subsampling)
+ *  * messageRatio (optional) - message subsampling ratio (default: 1, no subsampling)
+ *  * material (optional) - a material object or an option to construct a PointsMaterial.
+ *  * colorsrc (optional) - the field to be used for coloring (default: 'rgb')
+ *  * colormap (optional) - function that turns the colorsrc field value to a color
  */
 ROS3D.Particles = function(options) {
   options = options || {};
   this.tfClient = options.tfClient;
-  var texture = options.texture || 'https://upload.wikimedia.org/wikipedia/commons/a/a2/Pixel-white.png';
-  var size = options.size || 0.05;
-  this.max_pts = options.max_pts || 10000;
-  this.first_size = null;
-  this.prev_pts = 0;
   this.rootObject = options.rootObject || new THREE.Object3D();
-  var that = this;
+  this.max_pts = options.max_pts || 10000;
+  this.pointRatio = options.pointRatio || 1;
+  this.messageRatio = options.messageRatio || 1;
+  this.messageCount = 0;
+  this.material = options.material || {};
+  this.colorsrc = options.colorsrc;
+  this.colormap = options.colormap;
   THREE.Object3D.call(this);
 
-  this.vertex_shader = [
-    'attribute vec3 customColor;',
-    'attribute float alpha;',
-    'varying vec3 vColor;',
-    'varying float falpha;',
-    'void main() ',
-    '{',
-    '    vColor = customColor; // set color associated to vertex; use later in fragment shader',
-    '    vec4 mvPosition = modelViewMatrix * vec4( position, 1.0 );',
-    '    falpha = alpha; ',
-    '',
-    '    // option (1): draw particles at constant size on screen',
-    '    // gl_PointSize = size;',
-    '    // option (2): scale particles as objects in 3D space',
-    '    gl_PointSize = ', size, '* ( 300.0 / length( mvPosition.xyz ) );',
-    '    gl_Position = projectionMatrix * mvPosition;',
-    '}'
-    ].join('\n');
-
-  this.fragment_shader = [
-    'uniform sampler2D texture;',
-    'varying vec3 vColor; // colors associated to vertices; assigned by vertex shader',
-    'varying float falpha;',
-    'void main() ',
-    '{',
-    '    // THREE.Material.alphaTest is not evaluated for ShaderMaterial, so we',
-    '    // have to take care of this ourselves.',
-    '    if (falpha < 0.5) discard;',
-    '    // calculates a color for the particle',
-    '    gl_FragColor = vec4( vColor, falpha );',
-    '    // sets particle texture to desired color',
-    '    gl_FragColor = gl_FragColor * texture2D( texture, gl_PointCoord );',
-    '}'
-    ].join('\n');
-
-    var customUniforms =
-    {
-		texture:   { type: 't', value: new THREE.TextureLoader().load( texture ) }
-        //texture:   { type: 't', value: THREE.ImageUtils.loadTexture( texture ) },
-    };
-
-
-    /*this.attribs =
-    {
-        customColor:   { type: 'c', value: [] },
-        alpha:         { type: 'f', value: [] }
-    };*/
-
-    this.shaderMaterial = new THREE.ShaderMaterial(
-    {
-        uniforms:          customUniforms,
-        vertexShader:      this.vertex_shader,
-        fragmentShader:    this.fragment_shader,
-        transparent: true,
-    });
-
-    this.geom = new THREE.BufferGeometry();
-
-    var positions = [];
-    var customColor = [];
-    var alpha = [];
-
-    for(var i = 0; i < this.max_pts; i++){
-
-		//positions.push(new THREE.Vector3( ));
-        //this.geom.vertices.push(new THREE.Vector3( ));
-        positions.push(0);
-        positions.push(0);
-        positions.push(0);
-    }
-
-    this.geom.addAttribute( 'points', new THREE.Float32BufferAttribute( positions, 3 ) );
-    this.geom.addAttribute( 'colors', new THREE.Float32BufferAttribute( customColor, 3 ) );
-    this.geom.addAttribute( 'alpha', new THREE.Float32BufferAttribute( alpha, 1 ) );
-
-
-    this.ps = new THREE.Points( this.geom, this.shaderMaterial );
-    this.sn = null;
-
-    this.points = this.geom.attributes.points;
-    //this.colors = this.attribs.customColor.value;
-    this.colors = this.geom.attributes.colors;
-    //this.alpha =  this.attribs.alpha.value;
-    this.alpha = this.geom.attributes.alpha;
-
+  this.sn = null;
+  this.buffer = null;
 };
 
-function setFrame(particles, frame)
+ROS3D.Particles.prototype.setup = function(frame, point_step, fields)
 {
-    if(particles.sn === null){
-        particles.sn = new ROS3D.SceneNode({
+    if(this.sn===null){
+        // scratch space to decode base64 buffers
+        if(point_step) {
+            this.buffer = new Uint8Array( this.max_pts * point_step );
+        }
+        // turn fields to a map
+        fields = fields || [];
+        this.fields = {};
+        for(var i=0; i<fields.length; i++) {
+            this.fields[fields[i].name] = fields[i];
+        }
+        this.geom = new THREE.BufferGeometry();
+
+        this.point = new THREE.BufferAttribute( new Float32Array( this.max_pts * 3), 3, false );
+        this.geom.addAttribute( 'position', this.point.setDynamic(true) );
+
+        if(!this.colorsrc && this.fields.rgb) {
+            this.colorsrc = 'rgb';
+        }
+        if(this.colorsrc) {
+            var field = this.fields[this.colorsrc];
+            if (field) {
+                this.color = new THREE.BufferAttribute( new Float32Array( this.max_pts * 3), 3, false );
+                this.geom.addAttribute( 'color', this.color.setDynamic(true) );
+                var offset = field.offset;
+                this.getColor = [
+                    function(dv,base,le){return dv.getInt8(base+offset,le);},
+                    function(dv,base,le){return dv.getUint8(base+offset,le);},
+                    function(dv,base,le){return dv.getInt16(base+offset,le);},
+                    function(dv,base,le){return dv.getUint16(base+offset,le);},
+                    function(dv,base,le){return dv.getInt32(base+offset,le);},
+                    function(dv,base,le){return dv.getUint32(base+offset,le);},
+                    function(dv,base,le){return dv.getFloat32(base+offset,le);},
+                    function(dv,base,le){return dv.getFloat64(base+offset,le);}
+                ][field.datatype-1];
+                this.colormap = this.colormap || function(x){return new THREE.Color(x);};
+            } else {
+                console.warn('unavailable field "' + this.colorsrc + '" for coloring.');
+            }
+        }
+
+        if(!this.material.isMaterial) { // if it is an option, apply defaults and pass it to a PointsMaterial
+            if(this.color && this.material.vertexColors === undefined) {
+                this.material.vertexColors = THREE.VertexColors;
+            }
+            this.material = new THREE.PointsMaterial(this.material);      
+        }
+
+        this.ps = new THREE.Points( this.geom, this.material );
+
+        this.sn = new ROS3D.SceneNode({
             frameID : frame,
-            tfClient : particles.tfClient,
-            object : particles.ps
+            tfClient : this.tfClient,
+            object : this.ps
         });
 
-        particles.rootObject.add(particles.sn);
+        this.rootObject.add(this.sn);
     }
-}
+    return (this.messageCount++ % this.messageRatio) === 0;
+};
 
-function finishedUpdate(particles, n)
+ROS3D.Particles.prototype.update = function(n)
 {
-    if(particles.first_size === null){
-        particles.first_size = n;
-        particles.max_pts = Math.max(particles.max_pts, n);
-    }
+  this.geom.setDrawRange(0,n);
 
-    for(var i=n; i<particles.prev_pts; i++){
-        particles.alpha[i] = 0.0;
-    }
-    particles.prev_pts = n;
-
-    particles.geom.verticesNeedUpdate = true;
-    particles.points.needsUpdate = true;
-    //particles.attribs.customColor.needsUpdate = true;
-    particles.colors.needsUpdate = true;
-    //particles.attribs.alpha.needsUpdate = true;
-    particles.alpha.needsUpdate = true;
+  this.point.needsUpdate = true;
+  this.point.updateRange.count = n * this.point.itemSize;
 
-    if(n>particles.max_pts){
-        console.error('Attempted to draw more points than max_pts allows');
-    }
-}
+  if (this.color) {
+    this.color.needsUpdate = true;
+    this.color.updateRange.count = n * this.color.itemSize;
+  }
+};