?? landmarksampleupdater.java
字號:
/** LandmarkSampleUpdate.java * heavily borrowed from scott lenser's code. *//* This code is part of the localize package of TeamBots. * Copyright (c) 1999, 2000 by John Sweeney and Carnegie Mellon University */package EDU.cmu.cs.coral.localize;import java.lang.Math;import EDU.gatech.cc.is.util.Vec2;public class LandmarkSampleUpdater extends GaussianEvaluator { //the marker's location protected Vec2 location; public LandmarkSampleUpdater() { location = new Vec2(); } public LandmarkSampleUpdater(int nv) { super(nv); location = new Vec2(); } public void setLocation(Vec2 loc) { setLocation(loc.x, loc.y); } public void setLocation(double x, double y) { location.setx(x); location.sety(y); } public void updateSample(Sample s) { double dist; double allo_angle; double ego_angle; double x = s.data[Sample.x]; double y = s.data[Sample.y]; double theta = s.data[Sample.t]; double [] in = new double[2]; double [] out; //distance from sample to landmark dist = Math.sqrt( ((location.x - x)*(location.x - x)) + ((location.y - y)*(location.y - y))); //angle between the two positions allo_angle = Math.atan2(location.y-y, location.x-x); if (allo_angle < 0) { allo_angle = -allo_angle; } if (theta < 0) ego_angle = allo_angle +theta; else ego_angle = allo_angle - theta; //ego_angle = allo_angle - theta; while (ego_angle < -Math.PI) ego_angle += 2.0*Math.PI; while (ego_angle >= Math.PI) ego_angle -= 2.0*Math.PI; /* if (theta < 0) theta = -theta; ego_angle = theta+allo_angle; System.out.println("ego_angle = "+ego_angle); */ in[0]=dist; in[1]=ego_angle; // System.out.println("LSU: update: dist = "+dist+" ego_angle = "+ego_angle+" distMean = "+mean[0]+" angleMean = "+mean[1]); out = evaluate(in); // System.out.println("LSU: out[0] = "+out[0] + " out[1] = "+out[1]); s.data[Sample.w] *= out[0]; s.data[Sample.w] *= out[1]; } public double updateSample(double x, double y, double theta, double weight) { double dist; double allo_angle; double ego_angle; double [] in = new double[2]; double [] out; //distance from sample to landmark dist = Math.sqrt( ((location.x - x)*(location.x - x)) + ((location.y - y)*(location.y - y))); //angle between the two positions allo_angle = Math.atan2(location.y-y, location.x-x); if (allo_angle < 0) { allo_angle = -allo_angle; } if (theta < 0) ego_angle = allo_angle +theta; else ego_angle = allo_angle - theta; //ego_angle = allo_angle - theta; while (ego_angle < -Math.PI) ego_angle += 2.0*Math.PI; while (ego_angle >= Math.PI) ego_angle -= 2.0*Math.PI; /* if (theta < 0) theta = -theta; ego_angle = theta+allo_angle; System.out.println("ego_angle = "+ego_angle); */ in[0]=dist; in[1]=ego_angle; // System.out.println("LSU: update: dist = "+dist+" ego_angle = "+ego_angle+" distMean = "+mean[0]+" angleMean = "+mean[1]); out = evaluate(in); // System.out.println("LSU: out[0] = "+out[0] + " out[1] = "+out[1]); weight *= out[0]; weight *= out[1]; return weight; } }
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -