/// <summary> /// Construct a PHDNavigator using the indicated vehicle as a reference. /// </summary> /// <param name="vehicle">Vehicle to track.</param> /// <param name="onlymapping">If true, don't do SLAM, but mapping /// (i.e. the localization is assumed known). Currently not used.</param> public ISAM2Navigator(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle, bool onlymapping = false) : base(vehicle, onlymapping) { int nmeasurement = vehicle.MeasurementCovariance.Length; int nmotion = vehicle.MotionCovariance.Length; double[] measurementnoise = new double[nmeasurement]; double[] motionnoise = new double[nmotion]; double[] mparams = vehicle.Measurer.ToLinear(); // NOTE the iSAM2 interface assumes independent noise in each component, // discarding every entry that's not in the diagonal; // although it is possible to define a complete matrix, // it doesn't seem to be worth it when comparing performance for (int i = 0; i < nmeasurement; ++i) { measurementnoise[i] = Math.Sqrt(vehicle.MeasurementCovariance[i][i]); } for (int i = 0; i < nmotion; ++i) { motionnoise[i] = MotionDt * Math.Sqrt(vehicle.MotionCovariance[i][i]); } bestestimate = new TrackVehicle <MeasurerT, PoseT, MeasurementT>(); MapModel = new IndexedMap(3); plmodel = new IndexedMap(3); CandidateMapModel = new IndexedMap(3); BestEstimate.Pose = vehicle.Pose.DClone(); handle = NewNavigator(vehicle.Pose.State, measurementnoise, motionnoise, mparams); nextlabel = 0; }
TrackClone(TrackVehicle <KinectMeasurer, Pose3D, PixelRangeMeasurement> vehicle, bool copytrajectory = false) { if (vehicle is KinectTrackVehicle) { return(new KinectTrackVehicle((KinectTrackVehicle)vehicle, copytrajectory)); } else { throw new ArgumentException("A Kinect vehicle should be tracked with KinectTrackVehicles"); } }
/// <summary> /// Resample the vehicle particles with their corresponding weights and map models /// using systematic resampling (aka Wheel resampling). /// The weights are transferred into the unit range (0, 1] and random number is /// generated in the range (0, 1/n], adding 1/n to iterate through the unit range /// obtaining appropiate samples. /// This method is fast and simple although it loses the important property of /// giving independent samples, but in practice works very well. /// </summary> public void ResampleParticles() { var particles = new TrackVehicle <MeasurerT, PoseT, MeasurementT> [VehicleParticles.Length]; double random = (double)Util.Uniform.Next() / VehicleWeights.Length; double[] weights = new double[VehicleWeights.Length]; Map[] models = new Map[MapModels.Length]; double maxweight = 0; for (int i = 0, k = 0; i < weights.Length; i++) { // k should never be out of range, but because of floating point arithmetic, // the probabilities may not add up to one. If the random number hits this tiny difference // it will throw an exception. In such rare case, just expand the last guy's probability for (; random > 0 && k < VehicleWeights.Length; k++) { random -= VehicleWeights[k]; } particles[i] = RefVehicle.TrackClone(VehicleParticles[k - 1], true); models [i] = new Map(MapModels[k - 1]); weights [i] = 1.0 / weights.Length; random += 1.0 / weights.Length; if (VehicleWeights[k - 1] > maxweight) { maxweight = VehicleWeights[k - 1]; BestParticle = i; } } // debug: force the first particle to remain there // use with "make the first particle the real particle" on Update() // to also carry its map model //particles[0] = VehicleParticles[0]; //models[0] = MapModels[0]; VehicleParticles = particles; VehicleWeights = weights; MapModels = models; }
/// <summary> /// Reset the state of the navigator to set of equal particles. /// </summary> /// <param name="vehicle">New vehicle pose.</param> /// <param name="model">New map model estimate.</param> /// <param name="explore">New queued measurements to explore.</param> /// <param name="particlecount">Number of particles for the Montecarlo filter.</param> private void reset(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle, Map model, List <double[]> explore, int particlecount) { // do a deep copy, so no real info flows // into the navigator, only estimates, in SLAM // (this avoid bugs where the user unknowingly uses // the groundtruth to estimate the groundtruth itself) VehicleParticles = new TrackVehicle <MeasurerT, PoseT, MeasurementT> [particlecount]; MapModels = new Map [particlecount]; VehicleWeights = new double [particlecount]; toexplore = new List <double[]> [particlecount]; for (int i = 0; i < particlecount; i++) { VehicleParticles[i] = vehicle.TrackClone(MotionCovarianceMultiplier, MeasurementCovarianceMultiplier, PD, ClutterDensity, true); MapModels [i] = new Map(model); VehicleWeights [i] = 1.0 / particlecount; toexplore [i] = new List <double[]>(explore); } BestParticle = 0; }
TrackClone(TrackVehicle <MeasurerT, PoseT, MeasurementT> vehicle, bool copytrajectory = false) { return(new TrackVehicle <MeasurerT, PoseT, MeasurementT>(vehicle, copytrajectory)); }
/// <summary> /// Construct a OdometryNavigator using the indicated vehicle as a reference. /// </summary> /// <param name="vehicle">Vehicle to track.</param> public OdometryNavigator(Vehicle <MeasurerT, PoseT, MeasurementT> vehicle) : base(vehicle) { bestestimate = new TrackVehicle <MeasurerT, PoseT, MeasurementT>(vehicle, 1, 1, 1, 0); bestmapmodel = new Map(3); }