137 lines
4.0 KiB
C#
137 lines
4.0 KiB
C#
/*
|
|
* Copyright 2018 The Cartographer Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
using CartographerSharp.Transform;
|
|
using RobotNet10.Shared.Numbers;
|
|
|
|
namespace CartographerSharp.Sensor;
|
|
|
|
/// <summary>
|
|
/// Stores 3D position of a point observed by a rangefinder sensor.
|
|
/// </summary>
|
|
public struct RangefinderPoint(Vector3 position)
|
|
{
|
|
public Vector3 Position { get; set; } = position;
|
|
|
|
public static RangefinderPoint operator *(Rigid3f transform, RangefinderPoint point)
|
|
{
|
|
return new RangefinderPoint(transform.TransformPoint(point.Position));
|
|
}
|
|
|
|
public static bool operator ==(RangefinderPoint lhs, RangefinderPoint rhs)
|
|
{
|
|
return lhs.Position == rhs.Position;
|
|
}
|
|
|
|
public static bool operator !=(RangefinderPoint lhs, RangefinderPoint rhs)
|
|
{
|
|
return !(lhs == rhs);
|
|
}
|
|
|
|
public override readonly bool Equals(object? obj)
|
|
{
|
|
return obj is RangefinderPoint other && this == other;
|
|
}
|
|
|
|
public override readonly int GetHashCode()
|
|
{
|
|
return Position.GetHashCode();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts from proto representation.
|
|
/// </summary>
|
|
public static RangefinderPoint FromProto(RangefinderPoint proto)
|
|
{
|
|
return new RangefinderPoint(proto.Position);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts to proto representation.
|
|
/// </summary>
|
|
public readonly RangefinderPoint ToProto()
|
|
{
|
|
return new RangefinderPoint(Position);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stores 3D position of a point with its relative measurement time.
|
|
/// See PointCloud for more details.
|
|
/// </summary>
|
|
public struct TimedRangefinderPoint(Vector3 position, double time)
|
|
{
|
|
public Vector3 Position { get; set; } = position;
|
|
public double Time { get; set; } = time;
|
|
|
|
public static TimedRangefinderPoint operator *(Rigid3f transform, TimedRangefinderPoint point)
|
|
{
|
|
return new TimedRangefinderPoint(transform.TransformPoint(point.Position), point.Time);
|
|
}
|
|
|
|
public static bool operator ==(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
|
|
{
|
|
return lhs.Position == rhs.Position && lhs.Time == rhs.Time;
|
|
}
|
|
|
|
public static bool operator !=(TimedRangefinderPoint lhs, TimedRangefinderPoint rhs)
|
|
{
|
|
return !(lhs == rhs);
|
|
}
|
|
|
|
public override readonly bool Equals(object? obj)
|
|
{
|
|
return obj is TimedRangefinderPoint other && this == other;
|
|
}
|
|
|
|
public override readonly int GetHashCode()
|
|
{
|
|
return HashCode.Combine(Position, Time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts from proto representation.
|
|
/// </summary>
|
|
public static TimedRangefinderPoint FromProto(TimedRangefinderPoint proto)
|
|
{
|
|
return new TimedRangefinderPoint(proto.Position, proto.Time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts to proto representation.
|
|
/// </summary>
|
|
public readonly TimedRangefinderPoint ToProto()
|
|
{
|
|
return new TimedRangefinderPoint(Position, Time);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Converts to RangefinderPoint (drops time information).
|
|
/// </summary>
|
|
public readonly RangefinderPoint ToRangefinderPoint()
|
|
{
|
|
return new RangefinderPoint(Position);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Creates TimedRangefinderPoint from RangefinderPoint with time.
|
|
/// </summary>
|
|
public static TimedRangefinderPoint FromRangefinderPoint(RangefinderPoint point, double time)
|
|
{
|
|
return new TimedRangefinderPoint(point.Position, time);
|
|
}
|
|
}
|