76 lines
3.1 KiB
C#
76 lines
3.1 KiB
C#
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
|
|
|
|
/// <summary>
|
|
/// Converts point cloud (x,y) to MCL scan format (angle_min, angle_max, angle_increment, range_min, range_max, ranges[]).
|
|
/// Used when feeding TimedPointCloudData to MCL (bin by angle, take min range per bin).
|
|
/// </summary>
|
|
public static class MclScanHelper
|
|
{
|
|
/// <summary>
|
|
/// Transforms points from base_link frame to sensor (lidar) frame.
|
|
/// baseFromLidar: p_base = R(yaw)*p_lidar + (tx, ty). So p_lidar = R(-yaw)*(p_base - (tx, ty)).
|
|
/// </summary>
|
|
/// <param name="pointsBase">Points (x, y) in base_link frame</param>
|
|
/// <param name="tx">X of lidar origin in base_link (baseFromLidar translation)</param>
|
|
/// <param name="ty">Y of lidar origin in base_link</param>
|
|
/// <param name="yawRad">Yaw of lidar frame in base_link (radians)</param>
|
|
/// <returns>Points (x, y) in sensor/lidar frame</returns>
|
|
public static IEnumerable<(double x, double y)> BaseToSensorFrame(
|
|
IEnumerable<(double x, double y)> pointsBase,
|
|
double tx, double ty, double yawRad)
|
|
{
|
|
double c = Math.Cos(-yawRad);
|
|
double s = Math.Sin(-yawRad);
|
|
foreach (var (xb, yb) in pointsBase)
|
|
{
|
|
double dx = xb - tx;
|
|
double dy = yb - ty;
|
|
yield return (c * dx - s * dy, s * dx + c * dy);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Bins points by angle and returns ranges (min range per bin). Angles in radians; ranges in meters.
|
|
/// Points should be in sensor (lidar) frame so that angles are relative to sensor (required by MCL likelihood).
|
|
/// </summary>
|
|
public static (double angleMin, double angleMax, double angleIncrement, double rangeMin, double rangeMax, double[] ranges) ConvertToScan(
|
|
IEnumerable<Vector2> points,
|
|
double angleMinDeg,
|
|
double angleMaxDeg,
|
|
int numBins,
|
|
double rangeMin,
|
|
double rangeMax)
|
|
{
|
|
double angleMin = angleMinDeg * Math.PI / 180.0;
|
|
double angleMax = angleMaxDeg * Math.PI / 180.0;
|
|
double angleIncrement = (angleMax - angleMin) / Math.Max(1, numBins);
|
|
var bins = new List<double>[numBins];
|
|
for (int i = 0; i < numBins; i++)
|
|
bins[i] = [];
|
|
|
|
foreach (var point in points)
|
|
{
|
|
var range = Math.Sqrt(point.X * point.X + point.Y * point.Y);
|
|
if (range < 1e-6) continue;
|
|
var angle = Math.Atan2(point.Y, point.X);
|
|
int bin = (int)Math.Floor((angle - angleMin) / angleIncrement);
|
|
if (bin < 0) bin = 0;
|
|
if (bin >= numBins) bin = numBins - 1;
|
|
bins[bin].Add(range);
|
|
}
|
|
|
|
var ranges = new double[numBins];
|
|
for (int i = 0; i < numBins; i++)
|
|
{
|
|
if (bins[i].Count == 0)
|
|
ranges[i] = rangeMax;
|
|
else
|
|
ranges[i] = bins[i].Min();
|
|
}
|
|
return (angleMin, angleMax, angleIncrement, rangeMin, rangeMax, ranges);
|
|
}
|
|
}
|