public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public LidarProxy()
{
lidarSensorPose = new SensorPose(0, 0, 0.5, 0, 0, 0, 0);
addressProvider = new HardcodedAddressProvider();
lidar = new SickLMS(addressProvider.GetAddressByName("Sick"), lidarSensorPose);
lidar.ScanReceived += new EventHandler <ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > >(lidar_ScanReceived);
}
public LidarFilterPackageMessage(int robotID, PoseFilterState state, SensorPose sensorPose, ILidarScan <ILidar2DPoint> scan)
{
this.robotID = robotID;
this.state = state;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
{
pose = null;
this.useHokuyo = useHokuyo;
this.sickPose = sickPose;
sickDCM = Matrix4.FromPose(this.sickPose);
if (this.useHokuyo)
{
this.hokuyoPose = hokuyoPose;
hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
}
sickHistory = new List <Vector2[]>();
hokuyoHistory = new List <Vector2[]>();
this.angularResolution = angularResolution;
numBuckets = (int)Math.Round(360 / this.angularResolution);
trapped = false;
kThreshold = (int)Math.Round(160 / this.angularResolution);
rHistogram = new Double[numBuckets];
}
public LidarPosePackageMessage(int robotID, RobotPose pose, SensorPose sensorPose, ILidarScan<ILidar2DPoint> scan)
{
this.robotID = robotID;
this.pose = pose;
this.lidarScan = scan;
this.sensorPose = sensorPose;
}
public LidarRenderer(string name, int sensorID, int robotID, SensorPose laserToBody)
{
this.name = name;
this.sensorID = sensorID;
this.robotID = robotID;
this.laserToBody = laserToBody;
}
public SimLidar(SensorPose toBodyTransform, int id)
{
INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
simLidarClient = new GenericMulticastClient<SimMessage<ILidarScan<ILidar2DPoint>>>(addressProvider.GetAddressByName("SimLidar"), new CSharpMulticastSerializer<SimMessage<ILidarScan<ILidar2DPoint>>>(true));
simLidarClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<ILidarScan<ILidar2DPoint>>>>(simLidarClient_MsgReceived);
this.toBodyTransform = toBodyTransform;
robotID = id;
}
public SimLidar(SensorPose toBodyTransform, int id)
{
INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
simLidarClient = new GenericMulticastClient <SimMessage <ILidarScan <ILidar2DPoint> > >(addressProvider.GetAddressByName("SimLidar"), new CSharpMulticastSerializer <SimMessage <ILidarScan <ILidar2DPoint> > >(true));
simLidarClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <SimMessage <ILidarScan <ILidar2DPoint> > > >(simLidarClient_MsgReceived);
this.toBodyTransform = toBodyTransform;
robotID = id;
}
public SeptentrioSerial(string comPort, int baud, SensorPose sensorPose, ITimingProvider timingProvider)
{
rxSerial = new SerialPort(comPort, baud);
foreach (string p in SerialPort.GetPortNames())
{
Console.WriteLine(p);
}
rxSerial.DataReceived += new SerialDataReceivedEventHandler(rxSerial_DataReceived);
this.sensorPose = sensorPose;
this.timingProvider = timingProvider;
}
public SensorCoverage(double fovDegrees, double range, SensorPose location, Color color, string id)
{
this.fov = fovDegrees * (Math.PI / 180.0);
this.range = range;
this.location = location;
this.renderColor = color;
this.id = id;
//get the option by name
//Type coverageOptionsType = typeof(SensorCoverageRenderOptions);
//optionProperties = coverageOptionsType.GetProperties();
}
public void SetSensorPose(SensorPose laserPose, bool forCamera)
{
laserToRobot = Matrix4.FromPose(laserPose);
if (forCamera)
{
laserToRobot[2, 3] = 0;
}
//laserToRobot[0, 2] = laserPose.x;
//laserToRobot[1, 2] = laserPose.y;
//laserToRobot[2, 2] = 0;
//laserToRobot[3, 3] = 1.0;
}
public void UpdateSensorPose(SensorPose sensorPose)
{
Matrix4 laser2RobotDCM = Matrix4.FromPose(sensorPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
}
public RangerSensorParkingSonar(string name, SensorPose pose, IAbstractRobotHardware brick, int frequency)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.0d; // shows 0cm all right
this.MaxDistanceMeters = 2.5d; // shows 2.55m at infinity
this.parkingSonar = brick.produceParkingSonar(frequency);
parkingSonar.DistanceChanged += new HardwareComponentEventHandler(psonar_DistanceChanged);
}
public RangerSensorParkingSonar(string name, SensorPose pose, IAbstractRobotHardware brick, int frequency)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.0d; // shows 0cm all right
this.MaxDistanceMeters = 2.5d; // shows 2.55m at infinity
this.parkingSonar = brick.produceParkingSonar(frequency);
parkingSonar.DistanceChanged += new HardwareComponentEventHandler(psonar_DistanceChanged);
}
public RangerSensorSonar(string name, SensorPose pose, IAbstractRobotHardware brick, GpioPinId triggerPin, GpioPinId outputPin, int frequency, double threshold)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.04d; // shows 3cm all right
this.MaxDistanceMeters = 2.5d; // shows 2.55m at infinity
this.srf04 = brick.produceSonarSRF04(triggerPin, outputPin, frequency, threshold);
srf04.DistanceChanged += new HardwareComponentEventHandler(ir_DistanceChanged);
}
public LidarTargetDetector(double thresholdHeight, SensorPose laserPose, bool forCamera)
{
this.thresholdHeight = thresholdHeight;
laserToRobot = Matrix4.FromPose(laserPose);
if (forCamera)
{
laserToRobot[2, 3] = 0;
}
//laserToRobot[0, 2] = laserPose.x;
//laserToRobot[1, 2] = laserPose.y;
//laserToRobot[2, 2] = laserPose.z;
//laserToRobot[3, 3] = 1.0;
}
public RangerSensorIR20_150(string name, SensorPose pose, IAbstractRobotHardware brick, AnalogPinId pinId, int frequency, double threshold)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.26d; // shows 0.22 when too close
this.MaxDistanceMeters = 1.52d; // shows 1.60 at infinity
this.gp2d12 = brick.produceSharpGP2D12(pinId, frequency, threshold);
gp2d12.DistanceChanged += new HardwareComponentEventHandler(ir_DistanceChanged);
}
public RangerSensorSonar(string name, SensorPose pose, IAbstractRobotHardware brick, GpioPinId triggerPin, GpioPinId outputPin, int frequency, double threshold)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.04d; // shows 3cm all right
this.MaxDistanceMeters = 2.5d; // shows 2.55m at infinity
this.srf04 = brick.produceSonarSRF04(triggerPin, outputPin, frequency, threshold);
srf04.DistanceChanged += new HardwareComponentEventHandler(ir_DistanceChanged);
}
public RangerSensorIR10_80(string name, SensorPose pose, IAbstractRobotHardware brick, AnalogPinId pinId, int frequency, double threshold)
{
this.Name = name;
this.Pose = pose;
// reliably measured range:
this.MinDistanceMeters = 0.11d; // shows 10cm when 10 cm or closer
this.MaxDistanceMeters = 0.62d; // shows 0.64 at infinity
this.gp2d12 = brick.produceSharpGP2D12(pinId, frequency, threshold);
gp2d12.DistanceChanged += new HardwareComponentEventHandler(ir_DistanceChanged);
}
public HokuyoURG(string port, int scannerId, bool useHokuyoTimestamp)
{
this.port = port;
this.scannerId = scannerId;
this.sensorPose = new SensorPose(0, 0, 0, 0, 0, 0, 0);
this.useHokuyoTimestamp = useHokuyoTimestamp;
if (!useHokuyoTimestamp)
{
INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
segway = new SegwayRMP50(addressProvider.GetAddressByName("SegwayFeedback"), addressProvider.GetAddressByName("SegwayControl"), false, 1);
segway.Start();
segway.WheelPositionUpdate += new EventHandler <TimestampedEventArgs <Magic.Common.Robots.IRobotTwoWheelStatus> >(segway_WheelPositionUpdate);
}
}
public GlobalGaussianMixMap(OccupancyGrid2D globalOcGrid)
{
addressProvider = new HardcodedAddressProvider();
//robotPoseClient = new GenericMulticastClient<RobotPoseMessage>(addressProvider.GetAddressByName("RobotPose"), new CSharpMulticastSerializer<RobotPoseMessage>(true));
//lidarScanClient = new GenericMulticastClient<LidarScanMessage>(addressProvider.GetAddressByName("LidarScan"), new CSharpMulticastSerializer<LidarScanMessage>(true));
//sensorPoseClient = new GenericMulticastClient<SensorPoseMessage>(addressProvider.GetAddressByName("SensorPose"), new CSharpMulticastSerializer<SensorPoseMessage>(true));
lidarPosePackageClient = new GenericMulticastClient <LidarPosePackageMessage>(addressProvider.GetAddressByName("LidarPosePackage"), new CSharpMulticastSerializer <LidarPosePackageMessage>(true));
simLidarPosePackageClient = new GenericMulticastClient <SimMessage <LidarPosePackageMessage> >(addressProvider.GetAddressByName("SimLidarPosePackage"), new CSharpMulticastSerializer <SimMessage <LidarPosePackageMessage> >(true));
globalGaussianMixMapServer = new GenericMulticastServer <GlobalMapCell>(addressProvider.GetAddressByName("GlobalGaussianMixMap"), new CSharpMulticastSerializer <GlobalMapCell>(true));
globalGaussianMixMapServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
robotIDToPose = new Dictionary <int, RobotPose>();
robotIDToScan = new Dictionary <int, ILidarScan <ILidar2DPoint> >();
robotIDToSensorPose = new Dictionary <int, SensorPose>();
robotIDToTimestamp = new Dictionary <int, double>();
robotIDToPastTimestamp = new Dictionary <int, double>();
globalOcGridByEachRobot = new Dictionary <int, OccupancyGrid2D>();
robotIDToDynamicObstacles = new Dictionary <int, List <Polygon> >();
this.globalOcGrid = new OccupancyGrid2D(globalOcGrid);
laserToRobot = new SensorPose(0, 0, 0.5, 0, 0 / 180.0, 0, 0);
gaussianMixMapAlgorithm = new GaussianMixMapping(globalOcGrid, laserToRobot);
globalOcGridByEachRobotAlgorithm = new Dictionary <int, GaussianMixMapping>();
lidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
simLidarPosePackageClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
lidarPosePackageClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <LidarPosePackageMessage> >(lidarPosePackageClient_MsgReceived);
simLidarPosePackageClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <SimMessage <LidarPosePackageMessage> > >(simLidarPosePackageClient_MsgReceived);
// local map request from robots
localMapRequestClient = new GenericMulticastClient <LocalMapRequestMessage>(addressProvider.GetAddressByName("LocalMapRequest"), new CSharpMulticastSerializer <LocalMapRequestMessage>(true));
localMapRequestClient.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
localMapRequestClient.MsgReceived += new EventHandler <MsgReceivedEventArgs <LocalMapRequestMessage> >(localMapRequestClient_MsgReceived);
// local map update sender
localMapReponseServer = new GenericMulticastServer <UpdateMapDataMessage>(addressProvider.GetAddressByName("LocalMapResponse"), new CSharpMulticastSerializer <UpdateMapDataMessage>(true));
localMapReponseServer.Start(NetworkAddress.GetBindingAddressByType(NetworkAddress.BindingType.Wired));
Thread t = new Thread(new ParameterizedThreadStart(UpdateGlobalMap));
t.Start();
//Thread t2 = new Thread(new ParameterizedThreadStart(SendGlobalUpdate));
//t2.Start();
}
public HokuyoURG(string port, int scannerId, SensorPose sensorPose, bool useHokuyoTimestamp)
{
this.port = port;
this.scannerId = scannerId;
this.sensorPose = sensorPose;
this.useHokuyoTimestamp = useHokuyoTimestamp;
if (!useHokuyoTimestamp)
{
INetworkAddressProvider addressProvider = new HardcodedAddressProvider();
//segway = new SegwayRMP50(addressProvider.GetAddressByName("SegwayFeedback"), addressProvider.GetAddressByName("SegwayControl"), false, 1);
//segway.Start();
//segway.WheelPositionUpdate += new EventHandler<TimestampedEventArgs<Magic.Common.Robots.IRobotTwoWheelStatus>>(segway_WheelPositionUpdate);
ILidar2D lidar = new SickLMS(addressProvider.GetAddressByName("Sick"), new SensorPose(), true);
lidar.ScanReceived += new EventHandler <ILidarScanEventArgs <ILidarScan <ILidar2DPoint> > >(lidar_ScanReceived);
lidar.Start();
}
}
/// <summary>
/// we can create sensors here, but cannot send commands before bridge_CommunicationStarted is called in PluckyTheRobot
/// for example, encoder.Clear() will hang.
/// </summary>
public async Task InitSensors(CancellationTokenSource cts)
{
// see C:\Projects\Serializer_3_0\ExampleApps\AnalogSensorExample\AnalogSensorExample\Form1.cs
// Note: the Element board communicates at 19200 Baud, which is roughly 1.5 kbytes/sec
// Comm link is busy with motor commands and has to be responsive to encoders, for odometry to work.
// Sensors must carefully adjust their demands by setting UpdateFrequency and Enabled properties.
// *********************** Parking Sonar:
SensorPose spParkingSonar = new SensorPose()
{
XMeters = 0.0d, YMeters = 0.0d, ThetaRadians = 0.0d
};
ParkingSonar = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorParkingSonar, "ParkingSonar", spParkingSonar,
hardwareBrick, rangersSamplingIntervalMs);
ParkingSonar.distanceChangedEvent += new EventHandler <RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(ParkingSonar.Name, ParkingSonar);
// *********************** wheel encoders - roughly XXX ticks per wheel revolution, XXX ticks per meter.
encoderLeft = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder2, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
encoderRight = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder1, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
Ahrs = hardwareBrick.produceAhrs(0x00, AhrsSamplingIntervalMs, AhrsSensitivityThreshold);
Ahrs.ValuesChanged += new HardwareComponentEventHandler(Ahrs_ValuesChanged);
await RPiCameraSensor.Open(cts);
RPiCameraSensor.TargetingCameraTargetsChanged += RPiCameraSensor_TargetsChanged;
batteryVoltage = CreateBatteryVoltageMeter(hardwareBrick, batterySamplingIntervalMs, batterySensitivityThresholdVolts);
batteryVoltage.Enabled = true; // slow update rate, leave it turned on
SonarsEnabled = true;
EncodersEnabled = true;
CompassEnabled = true;
RPiCameraEnabled = true;
currentSensorsData = new SensorsDataPlucky()
{
RangerSensors = this.RangerSensors
};
}
public static IRangerSensor produceRangerSensor(RangerSensorFactoryProducts productType, string name, SensorPose pose, IAbstractRobotHardware brick, params Object[] args)
{
switch (productType)
{
case RangerSensorFactoryProducts.RangerSensorIR10_80:
return new RangerSensorIR10_80(name, pose, brick, (AnalogPinId)args[0], (int)args[1], (double)args[2]);
case RangerSensorFactoryProducts.RangerSensorIR20_150:
return new RangerSensorIR20_150(name, pose, brick, (AnalogPinId)args[0], (int)args[1], (double)args[2]);
case RangerSensorFactoryProducts.RangerSensorSonar:
return new RangerSensorSonar(name, pose, brick, (GpioPinId)args[0], (GpioPinId)args[1], (int)args[2], (double)args[3]);
case RangerSensorFactoryProducts.RangerSensorParkingSonar:
return new RangerSensorParkingSonar(name, pose, brick, (int)args[0]);
default:
throw new NotImplementedException("Error: RangerSensorFactory cannot produce ranger type " + productType);
}
}
/// <summary>
/// we can create sensors here, but cannot send commands before bridge_CommunicationStarted is called in PluckyTheRobot
/// for example, encoder.Clear() will hang.
/// </summary>
public async Task InitSensors(CancellationTokenSource cts)
{
// see C:\Projects\Serializer_3_0\ExampleApps\AnalogSensorExample\AnalogSensorExample\Form1.cs
// Note: the Element board communicates at 19200 Baud, which is roughly 1.5 kbytes/sec
// Comm link is busy with motor commands and has to be responsive to encoders, for odometry to work.
// Sensors must carefully adjust their demands by setting UpdateFrequency and Enabled properties.
// *********************** Parking Sonar:
SensorPose spParkingSonar = new SensorPose() { XMeters = 0.0d, YMeters = 0.0d, ThetaRadians = 0.0d };
ParkingSonar = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorParkingSonar, "ParkingSonar", spParkingSonar,
hardwareBrick, rangersSamplingIntervalMs);
ParkingSonar.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(ParkingSonar.Name, ParkingSonar);
// *********************** wheel encoders - roughly XXX ticks per wheel revolution, XXX ticks per meter.
encoderLeft = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder2, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
encoderRight = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder1, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
Ahrs = hardwareBrick.produceAhrs(0x00, AhrsSamplingIntervalMs, AhrsSensitivityThreshold);
Ahrs.ValuesChanged += new HardwareComponentEventHandler(Ahrs_ValuesChanged);
await RPiCameraSensor.Open(cts);
RPiCameraSensor.TargetingCameraTargetsChanged += RPiCameraSensor_TargetsChanged;
batteryVoltage = CreateBatteryVoltageMeter(hardwareBrick, batterySamplingIntervalMs, batterySensitivityThresholdVolts);
batteryVoltage.Enabled = true; // slow update rate, leave it turned on
SonarsEnabled = true;
EncodersEnabled = true;
CompassEnabled = true;
RPiCameraEnabled = true;
currentSensorsData = new SensorsData() { RangerSensors = this.RangerSensors };
}
public SickLMS(NetworkAddress na, SensorPose toBodyTransform, bool upsideDown)
{
this.ip = na.Address; this.port = na.Port; this.upsideDown = upsideDown;
this.toBodyTransform = toBodyTransform;
}
/// <summary>
/// we can create sensors here, but cannot send commands before bridge_CommunicationStarted is called in PluckyTheRobot
/// for example, encoder.Clear() will hang.
/// </summary>
public async Task InitSensors(CancellationTokenSource cts)
{
// see C:\Projects\Serializer_3_0\ExampleApps\AnalogSensorExample\AnalogSensorExample\Form1.cs
// Note: the Element board communicates at 19200 Baud, which is roughly 1.5 kbytes/sec
// Comm link is busy with motor commands and has to be responsive to encoders, for odometry to work.
// Sensors must carefully adjust their demands by setting UpdateFrequency and Enabled properties.
// *********************** infrared rangers:
SensorPose spIrLeft = new SensorPose() { XMeters = 0.0d, YMeters = 0.1d, ThetaRadians = Math.PI / 2.0d };
RangerIrLeft = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrLeft", spIrLeft,
hardwareBrick, AnalogPinId.A1, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrLeft.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrLeft.Name, RangerIrLeft);
SensorPose spIrRight = new SensorPose() { XMeters = 0.0d, YMeters = -0.1d, ThetaRadians = -Math.PI / 2.0d };
RangerIrRight = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrRight", spIrRight,
hardwareBrick, AnalogPinId.A0, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrRight.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrRight.Name, RangerIrRight);
SensorPose spIrFront = new SensorPose() { XMeters = 0.1d, YMeters = 0.0d, ThetaRadians = 0.0d };
RangerIrFront = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrFront", spIrFront,
hardwareBrick, AnalogPinId.A3, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrFront.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrFront.Name, RangerIrFront);
SensorPose spIrRear = new SensorPose() { XMeters = -0.1d, YMeters = 0.0d, ThetaRadians = Math.PI };
RangerIrRear = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorIR10_80, "IrRear", spIrRear,
hardwareBrick, AnalogPinId.A2, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerIrRear.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerIrRear.Name, RangerIrRear);
// *********************** ultrasonic rangers:
SensorPose spSonarLeft = new SensorPose() { XMeters = 0.1d, YMeters = 0.05d, ThetaRadians = Math.PI / 6.0d };
RangerSonarLeft = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorSonar, "SonarLeft", spSonarLeft,
hardwareBrick, GpioPinId.Pin4, GpioPinId.Pin5, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerSonarLeft.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerSonarLeft.Name, RangerSonarLeft);
SensorPose spSonarRight = new SensorPose() { XMeters = 0.1d, YMeters = -0.05d, ThetaRadians = -Math.PI / 6.0d };
RangerSonarRight = RangerSensorFactory.produceRangerSensor(RangerSensorFactoryProducts.RangerSensorSonar, "SonarRight", spSonarRight,
hardwareBrick, GpioPinId.Pin2, GpioPinId.Pin3, rangersSamplingIntervalMs, rangersSensitivityThresholdCm);
RangerSonarRight.distanceChangedEvent += new EventHandler<RangerSensorEventArgs>(RangerDistanceChangedEvent);
RangerSensors.Add(RangerSonarRight.Name, RangerSonarRight);
// *********************** wheel encoders - roughly 500 ticks per wheel revolution, 1200 ticks per meter.
encoderLeft = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder2, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
encoderRight = CreateWheelEncoder(hardwareBrick, WheelEncoderId.Encoder1, (int)mainLoopCycleMs, encodersSensitivityThresholdTicks);
Compass = hardwareBrick.produceCompassCMPS03(0x60, CompassSamplingIntervalMs, CompassSensitivityThreshold);
Compass.HeadingChanged += new HardwareComponentEventHandler(Compass_HeadingChanged);
// arduino based AHRS - PWM DAC to pin 4:
Ahrs = hardwareBrick.produceAnalogSensor(AnalogPinId.A4, CompassSamplingIntervalMs, CompassSensitivityThreshold);
Ahrs.AnalogValueChanged += new HardwareComponentEventHandler(Ahrs_ValueChanged);
// arduino based AHRS - I2C does not connect:
//Ahrs = new I2CDevice(hardwareBrick);
//Ahrs.I2CAddress = 173; // The Wire library uses 7 bit addresses throughout.
// // If you have a datasheet or sample code that uses 8 bit address, you'll want to drop the low bit (i.e. shift the value one bit to the right),
// // yielding an address between 0 and 127. However the addresses from 0 to 7 are not used because are reserved so the first address that can be used is 8.
//Ahrs.WriteCommand = "234";
//Ahrs.ReadCommand = "6";
await PixyCameraSensor.Open(cts);
PixyCameraSensor.TargetingCameraTargetsChanged += PixyCameraSensor_PixyCameraBlocksChanged;
batteryVoltage = CreateBatteryVoltageMeter(hardwareBrick, batterySamplingIntervalMs, batterySensitivityThresholdVolts);
batteryVoltage.Enabled = true; // slow update rate, leave it turned on
IrRangersEnabled = true; // if false, do not sample or update
SonarsEnabled = false; // will enable sonars with an interval 50ms to avoid interference
EncodersEnabled = true;
//CompassEnabled = true; // compass no good inside concrete buildings, use gyro instead
CompassEnabled = false;
Ahrs.Enabled = true;
PixyCameraSensor.Enabled = true;
currentSensorsData = new SensorsData() { RangerSensors = this.RangerSensors };
}
public static IRangerSensor produceRangerSensor(RangerSensorFactoryProducts productType, string name, SensorPose pose, IAbstractRobotHardware brick, params Object[] args)
{
switch (productType)
{
case RangerSensorFactoryProducts.RangerSensorIR10_80:
return(new RangerSensorIR10_80(name, pose, brick, (AnalogPinId)args[0], (int)args[1], (double)args[2]));
case RangerSensorFactoryProducts.RangerSensorIR20_150:
return(new RangerSensorIR20_150(name, pose, brick, (AnalogPinId)args[0], (int)args[1], (double)args[2]));
case RangerSensorFactoryProducts.RangerSensorSonar:
return(new RangerSensorSonar(name, pose, brick, (GpioPinId)args[0], (GpioPinId)args[1], (int)args[2], (double)args[3]));
case RangerSensorFactoryProducts.RangerSensorParkingSonar:
return(new RangerSensorParkingSonar(name, pose, brick, (int)args[0]));
default:
throw new NotImplementedException("Error: RangerSensorFactory cannot produce ranger type " + productType);
}
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover, int cellToUpdate)
{
//currentLaserPoseToRobot = laserRelativePositionToRover;
this.rangeToApply = cellToUpdate;
Matrix4 laser2RobotDCM = Matrix4.FromPose(laserRelativePositionToRover);
laserToRobotDCM = new UMatrix(4, 4);
robotToGlocalDCM = new UMatrix(4, 4);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
covRobotPose = new UMatrix(6, 6);
covLaserPose = new UMatrix(6, 6);
covLaserScan = new UMatrix(2, 2);
pij_sum = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
pu_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
psig_u_hat_square = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
uhatGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
sigSqrGM = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
laserHit = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
thresholdedHeightMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
resetCountMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
indexMap = new OccupancyGrid2D((OccupancyGrid2D)ocToUpdate);
this.numCellX = ocToUpdate.NumCellX;
this.numCellY = ocToUpdate.NumCellY;
#region Initial setup
double[] Qp_robot = new double[36] {
0.1, 0, 0, 0, 0, 0,
0, 0.1, 0, 0, 0, 0,
0, 0, 0.1, 0, 0, 0,
0, 0, 0, 0.01, 0, 0,
0, 0, 0, 0, 0.01, 0,
0, 0, 0, 0, 0, 0.01
};
double[] Qp_laser = new double[36] {
0.001, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0,
0, 0, 0, 0.0001, 0, 0,
0, 0, 0, 0, 0.0001, 0,
0, 0, 0, 0, 0, 0.0001
};
double[] Qr = new double[4] {
.01 * .01, 0, 0, (0.1 * Math.PI / 180.0) * (0.1 * Math.PI / 180.0)
};
// assign to our UMatrix
for (int i = 0; i < 6; i++)
{
for (int j = 0; j < 6; j++)
{
covRobotPose[j, i] = Qp_robot[j * 6 + i]; // *1e-6;
covLaserPose[j, i] = Qp_laser[j * 6 + i]; // *1e-6;
}
}
covLaserScan[0, 0] = Qr[0];
covLaserScan[0, 1] = 0;
covLaserScan[1, 0] = 0;
covLaserScan[1, 1] = Qr[3];
#endregion
int k = 0;
sinLookupHokuyo = new double[682];
cosLookupHokuyo = new double[682];
for (double i = -120; i <= 120; i += 0.35190615835777126099706744868035)
{
sinLookupHokuyo[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupHokuyo[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleHokuyo = 120;
MAXRANGEHokuyo = 5.0;
MINRANGEHokuyo = 0.5;
k = 0;
sinLookupSick = new double[361];
cosLookupSick = new double[361];
for (double i = -90; i <= 90; i += .5)
{
sinLookupSick[k] = (Math.Sin(i * Math.PI / 180.0));
cosLookupSick[k] = (Math.Cos(i * Math.PI / 180.0));
k++;
}
laserHalfAngleSick = 90;
MAXRANGESick = 30.0;
MINRANGESick = 0.5;
hokuyoStartIdx = 100;
hokuyoEndIdx = 500;
indicesList = new List <Index>();
heightList = new List <float>();
covList = new List <float>();
pijSumList = new List <float>();
laser2RobotTransMatrixDictionary = new Dictionary <int, Dictionary <int, UMatrix> >();
jacobianLaserPoseDictionary = new Dictionary <int, Dictionary <int, UMatrix> >();
indicesDictionary = new Dictionary <Index, int>();
}
/// <summary>
/// Update OccupancyGrid based on lidarScan and robotPose received
/// </summary>
/// <param name="lidarScan"></param>
/// <param name="currentRobotPose"></param>
public void UpdateOccupancyGrid(ILidarScan <ILidar2DPoint> lidarScan, int robotID, int scannerID, PoseFilterState currentRobotPose, SensorPose lidarPose, List <Polygon> dynamicObstacles)
{
if (robotID == 1)
{
MAXRANGESick = 7.0;
}
else if (robotID == 3)
{
MAXRANGESick = 30.0;
}
if (lidarPose == null)
{
lidarPose = new SensorPose(0, 0, 0.5, 0, 0 * Math.PI / 180.0, 0, 0);
}
if (laser2RobotTransMatrixDictionary.ContainsKey(robotID))
{
if (laser2RobotTransMatrixDictionary[robotID].ContainsKey(scannerID))
{
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
laserToRobotDCM = laser2RobotTransMatrixDictionary[robotID][scannerID];
}
else
{
Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
laser2RobotTransMatrixDictionary[robotID].Add(scannerID, laserToRobotDCM);
jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
}
}
else
{
laser2RobotTransMatrixDictionary.Add(robotID, new Dictionary <int, UMatrix>());
jacobianLaserPoseDictionary.Add(robotID, new Dictionary <int, UMatrix>());
Matrix4 laser2RobotDCM = Matrix4.FromPose(lidarPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
laserToRobotDCM[i, j] = laser2RobotDCM[i, j];
}
}
laser2RobotTransMatrixDictionary[robotID].Add(scannerID, new UMatrix(laserToRobotDCM));
jacobianLaserPoseDictionary[robotID].Add(scannerID, ComputeJacobian(lidarPose.yaw, lidarPose.pitch, lidarPose.roll));
JTpl = jacobianLaserPoseDictionary[robotID][scannerID];
}
// calculate robot2global transformation matrix
if (currentRobotPose == null)
{
return;
}
Matrix4 robot2GlocalDCM = Matrix4.FromPose(currentRobotPose);
for (int i = 0; i < 4; i++)
{
for (int j = 0; j < 4; j++)
{
robotToGlocalDCM[i, j] = robot2GlocalDCM[i, j];
}
}
if (lidarScan == null)
{
return;
}
UMatrix JTpr = ComputeJacobianQ(currentRobotPose.q1, currentRobotPose.q2, currentRobotPose.q3, currentRobotPose.q4);
List <UMatrix> JfPrCubixLaserToRobotDCM = new List <UMatrix>(6);
List <UMatrix> RobotToGlocalDCMJfPlCubix = new List <UMatrix>(7);
for (int i = 0; i < 7; i++)
{
//derivative of the robot transform matrtix w.r.t. some element of the robot psoe
UMatrix j = new UMatrix(4, 4);
j[0, 0] = JTpr[0, i]; j[1, 0] = JTpr[1, i]; j[2, 0] = JTpr[2, i]; j[3, 0] = JTpr[3, i];
j[0, 1] = JTpr[4, i]; j[1, 1] = JTpr[5, i]; j[2, 1] = JTpr[6, i]; j[3, 1] = JTpr[7, i];
j[0, 2] = JTpr[8, i]; j[1, 2] = JTpr[9, i]; j[2, 2] = JTpr[10, i]; j[3, 2] = JTpr[11, i];
j[0, 3] = JTpr[12, i]; j[1, 3] = JTpr[13, i]; j[2, 3] = JTpr[14, i]; j[3, 3] = JTpr[15, i];
JfPrCubixLaserToRobotDCM.Add(j * laserToRobotDCM);
if (i == 7)
{
continue; // same as break
}
UMatrix tempJacobianPl = new UMatrix(4, 4);
tempJacobianPl[0, 0] = JTpl[0, i]; tempJacobianPl[1, 0] = JTpl[1, i]; tempJacobianPl[2, 0] = JTpl[2, i]; tempJacobianPl[3, 0] = JTpl[3, i];
tempJacobianPl[0, 1] = JTpl[4, i]; tempJacobianPl[1, 1] = JTpl[5, i]; tempJacobianPl[2, 1] = JTpl[6, i]; tempJacobianPl[3, 1] = JTpl[7, i];
tempJacobianPl[0, 2] = JTpl[8, i]; tempJacobianPl[1, 2] = JTpl[9, i]; tempJacobianPl[2, 2] = JTpl[10, i]; tempJacobianPl[3, 2] = JTpl[11, i];
tempJacobianPl[0, 3] = JTpl[12, i]; tempJacobianPl[1, 3] = JTpl[13, i]; tempJacobianPl[2, 3] = JTpl[14, i]; tempJacobianPl[3, 3] = JTpl[15, i];
RobotToGlocalDCMJfPlCubix.Add(robotToGlocalDCM * tempJacobianPl);
}
UMatrix laserToENU = robotToGlocalDCM * laserToRobotDCM;
//UMatrix pijCell = new UMatrix(rangeToApply * 2 + 1, rangeToApply * 2 + 1);
// update covariance
UpdateCovarianceQ(currentRobotPose.Covariance);
//SickPoint p = new SickPoint(new RThetaCoordinate(1.0f, 0.0f));
for (int laserIndex = 0; laserIndex < lidarScan.Points.Count; laserIndex++)
{
lock (locker)
{
ILidar2DPoint p = lidarScan.Points[laserIndex];
if (scannerID == 1)
{
if (p.RThetaPoint.R >= MAXRANGESick || p.RThetaPoint.R <= MINRANGESick)
{
continue;
}
}
else if (scannerID == 2)
{
if (p.RThetaPoint.R >= MAXRANGEHokuyo || p.RThetaPoint.R <= MINRANGEHokuyo || laserIndex < hokuyoStartIdx || laserIndex > hokuyoEndIdx)
{
continue;
}
}
bool hitDynamicObstacles = false;
// figure out if this lidar point is hitting other robot
// find laser points in 3D space
// first define 2D point on the laser plane
UMatrix laserPoint = new UMatrix(4, 1);
double deg = (p.RThetaPoint.theta * 180.0 / Math.PI);
int thetaDegIndex = 0;
if (scannerID == 2) // hokuyo
{
thetaDegIndex = (int)Math.Round((deg + laserHalfAngleHokuyo) * 2.84); // % 360;
}
else if (scannerID == 1) // sick
{
thetaDegIndex = (int)Math.Round((deg + laserHalfAngleSick) * 2) % 360;
}
double cosTheta = 0, sinTheta = 0;
if (scannerID == 1)
{
cosTheta = cosLookupSick[thetaDegIndex];
sinTheta = sinLookupSick[thetaDegIndex];
}
else if (scannerID == 2)
{
cosTheta = cosLookupHokuyo[thetaDegIndex];
sinTheta = sinLookupHokuyo[thetaDegIndex];
}
// initial laser points
laserPoint[0, 0] = p.RThetaPoint.R * cosTheta;
laserPoint[1, 0] = p.RThetaPoint.R * sinTheta;
laserPoint[2, 0] = 0;
laserPoint[3, 0] = 1;
//calculate r_hat_ENU
UMatrix r_hat_ENU = laserToENU * laserPoint;
foreach (Polygon dp in dynamicObstacles)
{
if (dp.IsInside(new Vector2(r_hat_ENU[0, 0], r_hat_ENU[1, 0])))
{
hitDynamicObstacles = true;
break;
}
}
if (hitDynamicObstacles)
{
continue;
}
//-------------------------------//
// COVARIANCE UMatrix CALCULATION //
//-------------------------------//
UMatrix JRr = new UMatrix(4, 2);
JRr.Zero();
JRr[0, 0] = cosTheta;
JRr[0, 1] = -p.RThetaPoint.R * sinTheta;
JRr[1, 0] = sinTheta;
JRr[1, 1] = p.RThetaPoint.R * cosTheta;
UMatrix Jfr = new UMatrix(3, 2); // 3x2
Jfr = (laserToENU * JRr).Submatrix(0, 2, 0, 1); // 4x4 * 4x4 * 4x2
UMatrix Jfpr = new UMatrix(3, 7);
UMatrix Jfpl = new UMatrix(3, 6);
for (int i = 0; i < 7; i++) //for each state var (i.e. x,y,z,y,p,r)
{
UMatrix JfprTemp = (JfPrCubixLaserToRobotDCM[i]) * laserPoint; // 4 by 1 UMatrix
Jfpr[0, i] = JfprTemp[0, 0];
Jfpr[1, i] = JfprTemp[1, 0];
Jfpr[2, i] = JfprTemp[2, 0];
//UMatrix JfplTemp = (RobotToGlocalDCMJfPlCubix[i]) * laserPoint; // 4 by 1 UMatrix
//Jfpl[0, i] = JfplTemp[0, 0];
//Jfpl[1, i] = JfplTemp[1, 0];
//Jfpl[2, i] = JfplTemp[2, 0];
}
UMatrix JfprQprJfprT = new UMatrix(3, 3);
UMatrix JfplQplJfplT = new UMatrix(3, 3);
UMatrix JfrQrJfrT = new UMatrix(3, 3);
JfprQprJfprT = (Jfpr * covRobotPoseQ) * Jfpr.Transpose();
//JfplQplJfplT = (Jfpl * covLaserPose) * Jfpl.Transpose(); // not doing because covariance of laser point is so small
JfrQrJfrT = (Jfr * covLaserScan) * Jfr.Transpose();
// add above variables together and get the covariance
UMatrix covRHatENU = JfprQprJfprT + /*JfplQplJfplT +*/ JfrQrJfrT; // 3x3 UMatrix
//-----------------------------//
// FIND WHICH CELLS TO COMPUTE //
//-----------------------------//
// find out cells around this laser point
int laserPointIndexX = 0;
int laserPointIndexY = 0;
//this is used just to do the transformation from reaal to grid and visa versa
psig_u_hat_square.GetIndicies(r_hat_ENU[0, 0], r_hat_ENU[1, 0], out laserPointIndexX, out laserPointIndexY); // get cell (r_hat_ENU_X, r_hat_ENU_y)
if ((laserPointIndexX < 0 || laserPointIndexX >= numCellX) || (laserPointIndexY < 0 || laserPointIndexY >= numCellY))
{
continue;
}
int rangeToApplyX = (int)Math.Round(Math.Sqrt(covRHatENU[0, 0]) / (pij_sum.ResolutionX * 2)) * 2;
int rangeToApplyY = (int)Math.Round(Math.Sqrt(covRHatENU[1, 1]) / (pij_sum.ResolutionY * 2)) * 2;
//-----------------------------------------//
// COMPUTE THE DISTRIBUTION OF UNCERTAINTY //
//-----------------------------------------//
UMatrix pijCell = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
double sigX = Math.Sqrt(covRHatENU[0, 0]);
double sigY = Math.Sqrt(covRHatENU[1, 1]);
double rho = covRHatENU[1, 0] / (sigX * sigY);
double logTerm = Math.Log(2 * Math.PI * sigX * sigY * Math.Sqrt(1 - (rho * rho)));
double xTermDenom = (1 - (rho * rho));
//for (int i = -rangeToApply; i <= rangeToApply; i++) // row
for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
{
//for (int j = -rangeToApplyX; j <= rangeToApplyX; j++) // column
for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
{
if (laserPointIndexX + i < 0 || laserPointIndexX + i >= numCellX || laserPointIndexY + j < 0 || laserPointIndexY + j >= numCellY)
{
continue;
}
// estimate using Bivariate Normal Distribution
double posX = 0; double posY = 0;
psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
posX += psig_u_hat_square.ResolutionX / 2;
posY += psig_u_hat_square.ResolutionY / 2;
double x = posX - r_hat_ENU[0, 0];
double y = posY - r_hat_ENU[1, 0];
double z = (x * x) / (sigX * sigX) -
(2 * rho * x * y / (sigX * sigY)) +
(y * y) / (sigY * sigY);
double xTerm = -0.5 * z / xTermDenom;
// chi2 test
//if ((2 * (1 - (rho * rho))) * ((x * x) / (sigX * sigX) + (y * y) / (sigY * sigY) - (2 * rho * x * y) / (sigX * sigY)) > 15.2)
// continue;
pijCell[j + rangeToApplyY, i + rangeToApplyX] = Math.Exp(xTerm - logTerm) * psig_u_hat_square.ResolutionX * psig_u_hat_square.ResolutionY;
laserHit.SetCellByIdx(laserPointIndexX + i, laserPointIndexY + j, laserHit.GetCellByIdxUnsafe(laserPointIndexX + i, laserPointIndexY + j) + 1);
}
}
//---------------------------//
// COMPUTE HEIGHT ESTIMATION //
//---------------------------//
UMatrix PEN = covRHatENU.Submatrix(0, 1, 0, 1);
UMatrix PENInv = PEN.Inverse2x2;
UMatrix PuEN = new UMatrix(1, 2);
UMatrix PENu = new UMatrix(2, 1);
UMatrix PuENPENInv = PuEN * PENInv;
UMatrix uHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
UMatrix sigUHatMatrix = new UMatrix(rangeToApplyY * 2 + 1, rangeToApplyX * 2 + 1);
PuEN[0, 0] = covRHatENU[2, 0];
PuEN[0, 1] = covRHatENU[2, 1];
PENu[0, 0] = covRHatENU[0, 2];
PENu[1, 0] = covRHatENU[1, 2];
double sig_u_hat_product = (PuENPENInv * PENu)[0, 0]; // output = 1x1 UMatrix
for (int i = -rangeToApplyX; i <= rangeToApplyX; i++) // row
{
for (int j = -rangeToApplyY; j <= rangeToApplyY; j++) // column
{
UMatrix ENmr_EN = new UMatrix(2, 1);
double posX = 0; double posY = 0;
psig_u_hat_square.GetReals(laserPointIndexX + i, laserPointIndexY + j, out posX, out posY);
ENmr_EN[0, 0] = posX - r_hat_ENU[0, 0];
ENmr_EN[1, 0] = posY - r_hat_ENU[1, 0];
double u_hat_product = (PuENPENInv * (ENmr_EN))[0, 0]; // output = 1x1 UMatrix
uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = r_hat_ENU[2, 0] + u_hat_product;
sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] = covRHatENU[2, 2] - sig_u_hat_product;
}
}
//-------------------------------------------//
// ASSIGN FINAL VALUES TO THE OCCUPANCY GRID //
//-------------------------------------------//
for (int i = -rangeToApplyX; i <= rangeToApplyX; i++)
{
for (int j = -rangeToApplyY; j <= rangeToApplyY; j++)
{
int indexXToUpdate = laserPointIndexX + i;
int indexYToUpdate = laserPointIndexY + j;
// if the cell to update is out of range, continue
if (!psig_u_hat_square.CheckValidIdx(indexXToUpdate, indexYToUpdate))
{
continue;
}
pij_sum.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] + pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
pu_hat.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
pu_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApplyY, i + rangeToApplyX] * uHatMatrix[j + rangeToApply, i + rangeToApply] + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
psig_u_hat_square.SetCellByIdx(indexXToUpdate, indexYToUpdate,
pijCell[j + rangeToApplyY, i + rangeToApplyX] * sigUHatMatrix[j + rangeToApplyY, i + rangeToApplyX] + psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate,
(pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)));
double largeU = (pu_hat.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
double largeSig = (psig_u_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) + pu_hat_square.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate)) / pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) - largeU * largeU;
if (pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) > 1)
{
thresholdedHeightMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU); //pij_sum.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) / laserHit.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate));
}
uhatGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU);
//sigSqrGM.SetCellByIdx(indexXToUpdate, indexYToUpdate, largeU + 2 * Math.Sqrt(largeSig));
if (indexMap.GetCellByIdxUnsafe(indexXToUpdate, indexYToUpdate) != 1.0)
{
Index index = new Index(indexXToUpdate, indexYToUpdate);
indicesDictionary.Add(index, indicesDictionary.Count);
indexMap.SetCellByIdx(indexXToUpdate, indexYToUpdate, 1.0);
}
}
}
} // end foreach
//Console.WriteLine("1: " + sw1.ElapsedMilliseconds +
// " 2: " + sw2.ElapsedMilliseconds +
// " 3: " + sw3.ElapsedMilliseconds +
// " 4: " + sw4.ElapsedMilliseconds +
// " 5: " + sw5.ElapsedMilliseconds +
// " 6: " + sw6.ElapsedMilliseconds +
// " TOTAL: " + (sw1.ElapsedMilliseconds + sw2.ElapsedMilliseconds + sw3.ElapsedMilliseconds + sw4.ElapsedMilliseconds + sw5.ElapsedMilliseconds + sw6.ElapsedMilliseconds).ToString());
} // end function
}
public GaussianMixMappingQ(IOccupancyGrid2D ocToUpdate, SensorPose laserRelativePositionToRover)
: this(ocToUpdate, laserRelativePositionToRover, 10)
{
}
/// <summary>
/// Returns lidar scan projection into a image plane
/// </summary>
/// <param name="lidarScan"></param>
/// <param name="cameraSize">"320x240" or "640x480"</param>
/// <param name="camType">"Fire-i" or "FireFly"</param>
/// <param name="camPose">Your camera pose relative to the lidar</param>
/// <returns>List of pixel values for each lidar point</returns>
static public List <Vector2> FindLidarProjection(ILidarScan <ILidar2DPoint> lidarScan, string cameraSize, string camType, SensorPose camPose)
{
Matrix DCM4D = new Matrix(4, 4, 1.0);
double[] fc = new double[2];
double[] cc = new double[2];
if (cameraSize.Equals("320x240"))
{
//for 320 x 240 image with Unibrain Fire-i camera
if (camType.Equals("Fire-i"))
{
fc[0] = 384.4507; fc[1] = 384.1266;
cc[0] = 155.1999; cc[1] = 101.5641;
}
// Fire-Fly MV
else if (camType.Equals("FireFly"))
{
fc[0] = 345.26498; fc[1] = 344.99438;
cc[0] = 159.36854; cc[1] = 118.26944;
}
}
else if (cameraSize.Equals("640x480"))
{
// for 640 x 480 image with Unibrain Fire-i camera
if (camType.Equals("Fire-i"))
{
fc[0] = 763.5805; fc[1] = 763.8337;
cc[0] = 303.0963; cc[1] = 215.9287;
}
// for Fire-Fly MV (Point Gray)
else if (camType.Equals("FireFly"))
{
fc[0] = 691.09778; fc[1] = 690.70187;
cc[0] = 324.07388; cc[1] = 234.22204;
}
}
double alpha_c = 0;
// camera matrix
Matrix KK = new Matrix(fc[0], alpha_c * fc[0], cc[0], 0, fc[1], cc[1], 0, 0, 1);
// update DCM for point transformation
DCM4D[0, 3] = camPose.x; DCM4D[1, 3] = camPose.y; DCM4D[2, 3] = camPose.z;
DCM4D[0, 0] = Math.Cos(camPose.yaw); DCM4D[1, 1] = Math.Cos(camPose.yaw);
DCM4D[0, 1] = Math.Sin(camPose.yaw); DCM4D[1, 0] = -Math.Sin(camPose.yaw);
List <Vector2> pixelList = new List <Vector2>(lidarScan.Points.Count);
foreach (ILidar2DPoint pt in lidarScan.Points)
{
Matrix point = new Matrix(4, 1);
point[0, 0] = -pt.RThetaPoint.ToVector4().Y;
point[1, 0] = pt.RThetaPoint.ToVector4().X;
point[2, 0] = pt.RThetaPoint.ToVector4().Z;
point[3, 0] = 1;
Matrix transPt = DCM4D * point;
Matrix ptImgPlane = new Matrix(3, 1);
ptImgPlane[0, 0] = transPt[0, 0] / transPt[1, 0];
ptImgPlane[1, 0] = -transPt[2, 0] / transPt[1, 0];
ptImgPlane[2, 0] = transPt[1, 0] / transPt[1, 0];
ptImgPlane = KK * ptImgPlane;
pixelList.Add(new Vector2(ptImgPlane[0, 0], ptImgPlane[1, 0]));
}
return(pixelList);
}
/// <summary>
/// Update tracker with given input. It will do simple data association with given data (comparing distance between existing targets).
/// </summary>
/// <param name="uPixel">x-pixel in a camera coordinate</param>
/// <param name="vPixel">y-pixel in a camera coordinate</param>
/// <param name="range">laser range reading</param>
/// <param name="currentPose"></param>
/// <returns>target index = targetID</returns>
public int Update(double uPixel, double vPixel, RobotPose currentPose, SensorPose lidarPose, TargetTypes type, ILidar2DPoint lidarPt, string cameraType, int numCamera)
{
Matrix zSCR, Xx2;
double distance = 0;
int targetIdx = 0;
lock (locker)
{
// Compute X and Y position of the target based on the passed lidar point
//Vector2 xyPos = TargetTrackingImpl.FindPosCoord(screenSize, range, uPixel, vPixel, currentPose, null);
// this X Y coordinate = E N
double yaw = currentPose.yaw - Math.PI / 2;
double pitch = currentPose.pitch; double roll = currentPose.roll;
Matrix R_ENU2R = new Matrix(Math.Cos(yaw), Math.Sin(yaw), 0, -Math.Sin(yaw), Math.Cos(yaw), 0, 0, 0, 1) *
new Matrix(1, 0, 0, 0, Math.Cos(pitch), -Math.Sin(pitch), 0, Math.Sin(pitch), Math.Cos(pitch)) *
new Matrix(Math.Cos(roll), 0, Math.Sin(roll), 0, 1, 0, -Math.Sin(roll), 0, Math.Cos(roll));
Matrix localPt = new Matrix(3, 1); localPt[0, 0] = -lidarPt.RThetaPoint.ToVector2().Y - lidarPose.y; localPt[1, 0] = lidarPt.RThetaPoint.ToVector2().X + lidarPose.x;
Matrix globalPt = R_ENU2R.Inverse * localPt;
Vector2 xyPos = new Vector2(globalPt[0, 0] + currentPose.x, globalPt[1, 0] + currentPose.y);
this.xyPosList.Add(xyPos);
#region debugging - go away
// find the closest target
//if (currentTargetPose.Count == 0)
//{
// distance = double.MaxValue;
// targetIdx = 0;
//}
//else
//{
// targetIdx = FindClosestTarget(xyPos, ref distance, type);
//}
//if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
//{
// if (distance > staticDistanceThreshold) // if the distance is larger than the threshold, add as a new target
// {
// if (currentTargetPose.Count == currentTargetPose.Capacity)
// currentTargetPose.RemoveAt(0);
// currentTargetPose.Add(xyPos);
// implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
// targetIdx = currentTargetPose.Count - 1;
// }
//}
//else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
//{
// if (distance > dynamicDistanceThreshold) // if the distance is larger than the threshold, add as a new target
// {
// if (currentTargetPose.Count == currentTargetPose.Capacity)
// currentTargetPose.RemoveAt(0);
// currentTargetPose.Add(xyPos);
// implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
// targetIdx = currentTargetPose.Count - 1;
// }
//}
#endregion
zSCR = new Matrix(3, 1);
zSCR[0, 0] = uPixel; zSCR[1, 0] = vPixel; zSCR[2, 0] = lidarPt.RThetaPoint.R;
Xx2 = new Matrix(9, 1);
Xx2[0, 0] = currentPose.x; Xx2[1, 0] = currentPose.y; /* Xx2[2, 0] = currentPose.z; */ Xx2[2, 0] = 0;
Xx2[3, 0] = currentPose.roll; Xx2[4, 0] = currentPose.pitch; Xx2[5, 0] = currentPose.yaw;
//Xx2[3, 0] = 0; Xx2[4, 0] = 0; Xx2[5, 0] = currentPose.yaw;
Xx2[6, 0] = 0; Xx2[7, 0] = 0; Xx2[8, 0] = 0;
// modification for 3 cameras
if (numCamera == 3)
{
if (uPixel > 0 && uPixel <= 320)
{
Xx2[6, 0] = 50 * Math.PI / 180;
}
else if (uPixel > 320 && uPixel <= 320 * 2)
{
zSCR[0, 0] = uPixel - 320; zSCR[1, 0] = vPixel;
Xx2[6, 0] = 1.5 * Math.PI / 180;
}
else if (uPixel > 320 * 2)
{
zSCR[0, 0] = uPixel - 320 * 2; zSCR[1, 0] = vPixel;
Xx2[6, 0] = -47 * Math.PI / 180;
}
}
// find the most associated target
Matrix Sxx2 = new Matrix(9, 9);
Sxx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
Sxx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
Sxx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
Sxx2[3, 3] = Math.Sqrt(0.01); // roll
Sxx2[4, 4] = Math.Sqrt(0.01); // pitch
Sxx2[5, 5] = Math.Sqrt(0.03); // yaw
Sxx2[6, 6] = Math.Sqrt(0.01); // pan
Sxx2[7, 7] = Math.Sqrt(0.01); // tilt
Sxx2[8, 8] = Math.Sqrt(0.01); // scan
//targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCR);
Matrix zSCRForTest = new Matrix(2, 1); zSCRForTest[0, 0] = lidarPt.RThetaPoint.R; zSCRForTest[1, 0] = lidarPt.RThetaPoint.theta;
targetIdx = FindMostAssociatedTarget(xyPos, type, Xx2, Sxx2, zSCRForTest);
if (type == TargetTypes.PotentialSOOI || type == TargetTypes.ConfirmedSOOI)
{
if (targetIdx == -1) // if no good associated target found, then add a new one
{
implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
targetIdx = implList.Count - 1;
}
}
else if (type == TargetTypes.PotentialMOOI || type == TargetTypes.ConfirmedMOOI)
{
if (targetIdx == -1)
{
implList.Add(new TargetTrackingImpl(sigma_f, dT, screenSize, cameraType));
targetIdx = implList.Count - 1;
}
}
Matrix xhatPOI = new Matrix(7, 1); xhatPOI.Zero();
xhatPOI[0, 0] = xyPos.X; xhatPOI[1, 0] = xyPos.Y; xhatPOI[2, 0] = 0;
if (!implList[targetIdx].IsInitialized)
{
implList[targetIdx].SetInitialPOIInfo(xhatPOI);
}
}
Matrix Sx2 = new Matrix(9, 9);
Sx2[0, 0] = Math.Sqrt(Math.Abs(currentPose.covariance[0, 0]));
Sx2[1, 1] = Math.Sqrt(Math.Abs(currentPose.covariance[1, 1]));
Sx2[2, 2] = Math.Sqrt(Math.Abs(currentPose.covariance[2, 2]));
//Sx2[0, 0] = Math.Sqrt(0.1);
//Sx2[1, 1] = Math.Sqrt(0.1);
//Sx2[2, 2] = Math.Sqrt(0.01);
Sx2[3, 3] = Math.Sqrt(0.01); // roll
Sx2[4, 4] = Math.Sqrt(0.01); // pitch
Sx2[5, 5] = Math.Sqrt(0.01); // yaw
Sx2[6, 6] = Math.Sqrt(0.01); // pan
Sx2[7, 7] = Math.Sqrt(0.01); // tilt
Sx2[8, 8] = Math.Sqrt(0.01); // scan
// update with correct target
implList[targetIdx].UpdateZSCR(zSCR);
implList[targetIdx].UpdateVehicleState(Xx2, currentPose.timestamp);
implList[targetIdx].UpdateSx2(Sx2);
if (implList[targetIdx].Type != TargetTypes.ConfirmedSOOI && implList[targetIdx].Type != TargetTypes.ConfirmedMOOI)
{
implList[targetIdx].SetTargetType(type);
}
implList[targetIdx].Update();
return(targetIdx);
}
public void UpdateSensorPose(SensorPose p)
{
curLidarToBody = new SensorPose(p);
}
public LidarPolyExtractor(double threshold, RobotPose curRobotPose, SensorPose curLidarToBody)
{
this.curLidarToBody = curLidarToBody;
this.curRobotPose = curRobotPose;
this.threshold = threshold;
}
public void SetLidarPose(SensorPose p)
{
lock (dataLocker) { curLidarToBody = p; }
}
public static List <Vector2> ExtractGlobalFeature(ILidarScan <ILidar2DPoint> scan, SensorPose sensorPose, RobotPose currentPose, double wheelRadius)
{
Stopwatch sw = new Stopwatch();
sw.Start();
List <Vector2> features = new List <Vector2>();
List <ILidar2DPoint> filteredScan = new List <ILidar2DPoint>();
//foreach (ILidar2DPoint pt in scan.Points)
for (int i = 0; i < scan.Points.Count; i++)
{
if (scan.Points[i].RThetaPoint.R > 0.20)
{
filteredScan.Add(scan.Points[i]);
}
}
Matrix4 laser2robotDCM = new Matrix4(1, 0, 0, 0,
0, Math.Cos(sensorPose.pitch), Math.Sin(sensorPose.pitch), 0,
0, -Math.Sin(sensorPose.pitch), Math.Cos(sensorPose.pitch), sensorPose.z,
0, 0, 0, 1);
int neighbor = 3;
for (int i = neighbor; i < filteredScan.Count - neighbor; i++)
{
// AGAIN, these codes are written respect to ENU coordinate frame
double currentX = -filteredScan[i].RThetaPoint.R * Math.Sin(filteredScan[i].RThetaPoint.theta) - sensorPose.y;
double currentY = filteredScan[i].RThetaPoint.R * Math.Cos(filteredScan[i].RThetaPoint.theta) + sensorPose.x;
double lastX = -filteredScan[i - neighbor].RThetaPoint.R * Math.Sin(filteredScan[i - neighbor].RThetaPoint.theta) - sensorPose.y;
double lastY = filteredScan[i - neighbor].RThetaPoint.R * Math.Cos(filteredScan[i - neighbor].RThetaPoint.theta) + sensorPose.x;
double nextX = -filteredScan[i + neighbor].RThetaPoint.R * Math.Sin(filteredScan[i + neighbor].RThetaPoint.theta) - sensorPose.y;
double nextY = filteredScan[i + neighbor].RThetaPoint.R * Math.Cos(filteredScan[i + neighbor].RThetaPoint.theta) + sensorPose.x;
Matrix localPt = new Matrix(4, 1), lastLocalPt = new Matrix(4, 1), nextLocalPt = new Matrix(4, 1);
localPt[0, 0] = currentX; localPt[1, 0] = currentY; localPt[3, 0] = 1;
lastLocalPt[0, 0] = lastX; lastLocalPt[1, 0] = lastY; lastLocalPt[3, 0] = 1;
nextLocalPt[0, 0] = nextX; nextLocalPt[1, 0] = nextY; nextLocalPt[3, 0] = 1;
Vector4 currentPt = laser2robotDCM * localPt;
Vector4 lastPt = laser2robotDCM * lastLocalPt;
Vector4 nextPt = laser2robotDCM * nextLocalPt;
// check if the z position of the laser return is higher than 10 cm of current z-state
//if (currentPt.Z > currentPose.z + 0.1)
// features.Add(new Vector2(currentPt.X, currentPt.Y));
// height comparison
Vector2 neighborPt = FindNeighborToCompare(filteredScan, sensorPose, i, 0.1);
Matrix neighbotPtM = new Matrix(4, 1);
neighbotPtM[0, 0] = neighborPt.X; neighbotPtM[1, 0] = neighborPt.Y; neighbotPtM[3, 0] = 1;
Vector4 neighborPtV = laser2robotDCM * neighbotPtM;
double yaw = currentPose.yaw - Math.PI / 2;
double heightDiff = neighborPtV.Z - currentPt.Z;
if (heightDiff > 0.1)
{
features.Add(new Vector2(neighborPtV.X * Math.Cos(yaw) - neighborPtV.Y * Math.Sin(yaw) + currentPose.x,
neighborPtV.X * Math.Sin(yaw) + neighborPtV.Y * Math.Cos(yaw) + currentPose.y));
}
else if (heightDiff < -0.10)
{
features.Add(new Vector2(currentPt.X * Math.Cos(yaw) - currentPt.Y * Math.Sin(yaw) + currentPose.x,
currentPt.X * Math.Sin(yaw) + currentPt.Y * Math.Cos(yaw) + currentPose.y));
}
}
Console.WriteLine("Jump(?) Extraction Time:" + sw.ElapsedMilliseconds);
return(features);
}
private static Vector2 FindNeighborToCompare(List <ILidar2DPoint> filteredScan, SensorPose lidarPose, int initialIdx, double threshold)
{
double currentX = -filteredScan[initialIdx].RThetaPoint.R * Math.Sin(filteredScan[initialIdx].RThetaPoint.theta) - lidarPose.y;
double currentY = filteredScan[initialIdx].RThetaPoint.R * Math.Cos(filteredScan[initialIdx].RThetaPoint.theta) + lidarPose.x;
double neighborX = -filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Sin(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) - lidarPose.y;
double neighborY = filteredScan[filteredScan.Count - 1].RThetaPoint.R * Math.Cos(filteredScan[filteredScan.Count - 1].RThetaPoint.theta) + lidarPose.x;
Vector2 currentPt = new Vector2(currentX, currentY);
for (int k = initialIdx; k < filteredScan.Count; k++)
{
neighborX = -filteredScan[k].RThetaPoint.R * Math.Sin(filteredScan[k].RThetaPoint.theta) - lidarPose.y;
neighborY = filteredScan[k].RThetaPoint.R * Math.Cos(filteredScan[k].RThetaPoint.theta) + lidarPose.x;
Vector2 neighborPt = new Vector2(neighborX, neighborY);
if (currentPt.DistanceTo(neighborPt) > threshold)
{
return(neighborPt);
}
}
return(new Vector2(neighborX, neighborY));
}
public SimIMU(NetworkAddress na, SensorPose sp)
{
simIMUClient = new GenericMulticastClient<SimMessage<IMUData>>(na, new CSharpMulticastSerializer<SimMessage<IMUData>>(false));
simIMUClient.MsgReceived += new EventHandler<MsgReceivedEventArgs<SimMessage<IMUData>>>(simIMUClient_MsgReceived);
toBodyTransform = sp;
}
