Skip to content

Commit

Permalink
Merge branch 'Field-Robotics-Japan:master' into master
Browse files Browse the repository at this point in the history
  • Loading branch information
@RyodoTanaka
RyodoTanaka authored Nov 8, 2023
2 parents e6a00e4 + 745c0ed commit ea49218
Show file tree
Hide file tree
Showing 43 changed files with 1,698 additions and 134 deletions.
55 changes: 55 additions & 0 deletions .vscode/settings.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,55 @@
{
"files.exclude":
{
"**/.DS_Store":true,
"**/.git":true,
"**/.gitmodules":true,
"**/*.booproj":true,
"**/*.pidb":true,
"**/*.suo":true,
"**/*.user":true,
"**/*.userprefs":true,
"**/*.unityproj":true,
"**/*.dll":true,
"**/*.exe":true,
"**/*.pdf":true,
"**/*.mid":true,
"**/*.midi":true,
"**/*.wav":true,
"**/*.gif":true,
"**/*.ico":true,
"**/*.jpg":true,
"**/*.jpeg":true,
"**/*.png":true,
"**/*.psd":true,
"**/*.tga":true,
"**/*.tif":true,
"**/*.tiff":true,
"**/*.3ds":true,
"**/*.3DS":true,
"**/*.fbx":true,
"**/*.FBX":true,
"**/*.lxo":true,
"**/*.LXO":true,
"**/*.ma":true,
"**/*.MA":true,
"**/*.obj":true,
"**/*.OBJ":true,
"**/*.asset":true,
"**/*.cubemap":true,
"**/*.flare":true,
"**/*.mat":true,
"**/*.meta":true,
"**/*.prefab":true,
"**/*.unity":true,
"build/":true,
"Build/":true,
"Library/":true,
"library/":true,
"obj/":true,
"Obj/":true,
"ProjectSettings/":true,
"temp/":true,
"Temp/":true
}
}
97 changes: 83 additions & 14 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Livox/LivoxSensor.cs
View file
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
Expand All @@ -8,6 +9,8 @@
using UnityEngine.Jobs;
using Unity.Jobs;

using Random = Unity.Mathematics.Random;

namespace UnitySensors
{
public class LivoxSensor : Sensor
Expand All @@ -19,9 +22,13 @@ public class LivoxSensor : Sensor
private int _scanSeparation = 40;

[SerializeField]
private float _minRange = 0.1f;
private float _minDistance = 0.1f;
[SerializeField]
private float _maxDistance = 100.0f;
[SerializeField]
private float _maxIntensity = 255.0f;
[SerializeField]
private float _maxRange = 100.0f;
private float _gaussianNoiseSigma = 0.0f;

[SerializeField]
private int _resolution = 100;
Expand All @@ -36,11 +43,16 @@ public class LivoxSensor : Sensor
private Texture2D _texture;

private JobHandle _handle;
private TextureToPointsJob _job;
private TextureToPointsJob _textureToPointsJob;
private UpdateGaussianNoisesJob _updateGaussianNoisesJob;
private Random _random;
public NativeArray<Vector3> points;
public NativeArray<float> intensities;
private NativeArray<Vector3> _directions;
private NativeArray<int> _pixelIndices;
private NativeArray<float> _noises;

private uint _randomSeed;
private int _pointsNum;
public uint pointsNum { get=>(uint)(_pointsNum);}

Expand Down Expand Up @@ -80,8 +92,8 @@ private void SetupCamera()

_cam.targetTexture = _rt;
_cam.fieldOfView = fov;
_cam.nearClipPlane = _minRange;
_cam.farClipPlane = _maxRange;
_cam.nearClipPlane = _minDistance;
_cam.farClipPlane = _maxDistance;
_cam.gameObject.AddComponent<DepthCamera>();
_cam.clearFlags = CameraClearFlags.SolidColor;

Expand Down Expand Up @@ -109,23 +121,44 @@ private void SetupIndicesAndDirections()
private void SetupJob()
{
points = new NativeArray<Vector3>(_pointsNum, Allocator.Persistent);
_job = new TextureToPointsJob()
intensities = new NativeArray<float>(_pointsNum, Allocator.Persistent);
_randomSeed = (uint)Environment.TickCount;
_random = new Random(_randomSeed);

_noises = new NativeArray<float>(_pointsNum, Allocator.Persistent);

_updateGaussianNoisesJob = new UpdateGaussianNoisesJob()
{
sigma = _gaussianNoiseSigma,
random = _random,
noises = _noises
};

_textureToPointsJob = new TextureToPointsJob()
{
far = _maxRange,
scanSeparation = _scanSeparation,
separationCounter = 0,
minDistance = _minDistance,
minDistance_sqr = _minDistance * _minDistance,
maxDistance = _maxDistance,
maxIntensity = _maxIntensity,
pixelIndices = _pixelIndices,
directions = _directions,
pixels = _texture.GetPixelData<Color>(0),
points = points
noises = _noises,
points = points,
intensities = intensities
};
}

protected override void UpdateSensor()
{
_handle.Complete();
_job.separationCounter++;
if (_job.separationCounter >= _scanSeparation) _job.separationCounter = 0;
if (_randomSeed++ == 0) _randomSeed = 1;
_updateGaussianNoisesJob.random.InitState(_randomSeed);

_textureToPointsJob.separationCounter++;
if (_textureToPointsJob.separationCounter >= _scanSeparation) _textureToPointsJob.separationCounter = 0;

AsyncGPUReadback.Request(_rt, 0, request => {
if (request.hasError)
Expand All @@ -140,7 +173,8 @@ protected override void UpdateSensor()
}
});

_handle = _job.Schedule(_pointsNum, 1);
JobHandle updateGaussianNoisesJobHandle = _updateGaussianNoisesJob.Schedule(_pointsNum, 1);
_handle = _textureToPointsJob.Schedule(_pointsNum, 1, updateGaussianNoisesJobHandle);

JobHandle.ScheduleBatchedJobs();
}
Expand All @@ -153,36 +187,71 @@ public void CompleteJob()
private void OnDestroy()
{
_handle.Complete();
_noises.Dispose();
_pixelIndices.Dispose();
_directions.Dispose();
points.Dispose();
intensities.Dispose();

_rt.Release();
}

[BurstCompile]
private struct UpdateGaussianNoisesJob : IJobParallelFor
{
public float sigma;
public Random random;
public NativeArray<float> noises;

public void Execute(int index)
{
var rand2 = random.NextFloat();
var rand3 = random.NextFloat();
float normrand =
(float)Math.Sqrt(-2.0f * Math.Log(rand2)) *
(float)Math.Cos(2.0f * Math.PI * rand3);
noises[index] = sigma * normrand;
}
}

[BurstCompile]
private struct TextureToPointsJob : IJobParallelFor
{
public float far;
public int scanSeparation;
public int separationCounter;

[ReadOnly]
public float minDistance;
[ReadOnly]
public float minDistance_sqr;
[ReadOnly]
public float maxDistance;
[ReadOnly]
public float maxIntensity;

[ReadOnly]
public NativeArray<int> pixelIndices;
[ReadOnly]
public NativeArray<Vector3> directions;

[ReadOnly]
public NativeArray<Color> pixels;
[ReadOnly]
public NativeArray<float> noises;

public NativeArray<Vector3> points;
public NativeArray<float> intensities;

public void Execute(int index)
{
int offset = points.Length * separationCounter / scanSeparation;
int pixelIndex = pixelIndices.AsReadOnly()[index + offset];
float distance = pixels.AsReadOnly()[pixelIndex].r;
points[index] = directions.AsReadOnly()[index + offset] * far * Mathf.Clamp01(1.0f - distance);
float distance = maxDistance * Mathf.Clamp01(1.0f - pixels.AsReadOnly()[pixelIndex].r) + noises[index];
bool isValid = (minDistance <= distance && distance <= maxDistance);
if (!isValid) distance = 0;
points[index] = directions.AsReadOnly()[index + offset] * distance;
float distance_sqr = distance * distance;
intensities[index] = isValid ? maxIntensity * minDistance_sqr / distance_sqr : 0;
}
}
}
Expand Down
41 changes: 39 additions & 2 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/LiDAR/Velodyne/VelodyneSensor.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,12 @@ public class VelodyneSensor : Sensor
[SerializeField]
private RotatingLiDARScanPattern _scanPattern;
[SerializeField]
private float _minDistance = 0.0f;
[SerializeField]
private float _maxDistance = 100.0f;
[SerializeField]
private float _maxIntensity = 255.0f;
[SerializeField]
private float _gaussianNoiseSigma = 0.0f;

private Transform _transform;
Expand All @@ -30,11 +36,16 @@ public class VelodyneSensor : Sensor
private NativeArray<RaycastHit> _raycastHits;
private Random _random;
private NativeArray<float> _noises;

public NativeArray<float> distances;
public NativeArray<Vector3> points;
public NativeArray<float> intensities;

private uint _randomSeed;
private int _pointsNum;
public uint pointsNum { get => (uint)_pointsNum; }
public int layersNum { get => _scanPattern.numOfLayer; }
public int azimuthResolution { get => _scanPattern.azimuthResolution; }

protected override void Init()
{
Expand All @@ -56,7 +67,9 @@ private void SetupDirections()

private void SetupJobs()
{
distances = new NativeArray<float>(_pointsNum, Allocator.Persistent);
points = new NativeArray<Vector3>(_pointsNum, Allocator.Persistent);
intensities = new NativeArray<float>(_pointsNum, Allocator.Persistent);
_raycastCommands = new NativeArray<RaycastCommand>(_pointsNum, Allocator.Persistent);
_raycastHits = new NativeArray<RaycastHit>(_pointsNum, Allocator.Persistent);

Expand All @@ -82,10 +95,16 @@ private void SetupJobs()

_raycastHitsToPointsJob = new RaycastHitsToPointsJob()
{
minDistance = _minDistance,
minDistance_sqr = _minDistance * _minDistance,
maxDistance = _maxDistance,
maxIntensity = _maxIntensity,
directions = _directions,
raycastHits = _raycastHits,
noises = _noises,
points = points
distances = distances,
points = points,
intensities = intensities
};
}

Expand Down Expand Up @@ -119,7 +138,9 @@ private void OnDestroy()
_directions.Dispose();
_raycastCommands.Dispose();
_raycastHits.Dispose();
distances.Dispose();
points.Dispose();
intensities.Dispose();
}

[BurstCompile]
Expand Down Expand Up @@ -161,18 +182,34 @@ public void Execute(int index)
[BurstCompile]
private struct RaycastHitsToPointsJob : IJobParallelFor
{
[ReadOnly]
public float minDistance;
[ReadOnly]
public float minDistance_sqr;
[ReadOnly]
public float maxDistance;
[ReadOnly]
public float maxIntensity;
[ReadOnly]
public NativeArray<Vector3> directions;
[ReadOnly]
public NativeArray<RaycastHit> raycastHits;
[ReadOnly]
public NativeArray<float> noises;

public NativeArray<float> distances;
public NativeArray<Vector3> points;
public NativeArray<float> intensities;

public void Execute(int index)
{
points[index] = directions[index] * (raycastHits[index].distance + noises[index]);
float distance = raycastHits[index].distance + noises[index];
bool isValid = (minDistance <= distance && distance <= maxDistance);
if (!isValid) distance = 0;
distances[index] = distance;
points[index] = directions[index] * distance;
float distance_sqr = distance * distance;
intensities[index] = isValid ? maxIntensity * minDistance_sqr / distance_sqr : 0;
}
}
}
Expand Down
8 changes: 8 additions & 0 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/TF.meta
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

14 changes: 14 additions & 0 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,14 @@
using UnityEngine;
using UnitySensors;

public class TFSensor : Sensor {

[ReadOnly]
private Vector3 _position;

[ReadOnly]
private Quaternion _rotation;

public Vector3 position { get => _position; }
public Quaternion rotation { get => _rotation; }
}
11 changes: 11 additions & 0 deletions Assets/UnitySensors/Runtime/Scripts/Sensors/TF/TFSensor.cs.meta
View file

Some generated files are not rendered by default. Learn more about how customized files appear on GitHub.

Loading

0 comments on commit ea49218

Please sign in to comment.