public static Edge applyRotateShift(Edge edge, Pose2D Changes, Vector2 rotateCenter)
{
Vector2 newP, newT, newH;
newP = MathHelper.TranformVector(edge.point, rotateCenter, Changes);
Line newLine;
List <Line> lines = null;
if (edge.lines == null)
{
throw new Exception("WTF");
}
else
{
lines = new List <Line>();
foreach (Line line in edge.lines)
{
newH = MathHelper.TranformVector(line.head, rotateCenter, Changes); //Point3D.applyRotateShift(line.head, Changes.Rotation, rotateCenter, Changes.Position);
newT = MathHelper.TranformVector(line.tail, rotateCenter, Changes); //Point3D.applyRotateShift(line.tail, Changes.Rotation, rotateCenter, Changes.Position);
newLine = new Line();
newLine.head = newH;
newLine.tail = newT;
lines.Add(newLine);
}
}
return(new Edge(newP, edge.type, lines, edge.AccuracyOfNewObservation));
}
public void updateValues(Pose2D robot, Laser LaserData, int timeCycle)
{
this.LaserData = LaserData;
this.RobotPose = new Pose2D(robot.Position, robot.Rotation);
GenerateLaserVectors();
this.timeCycle = timeCycle;
}
private void btnStep_Click(object sender, EventArgs e)
{
ScanObservation m1 = new ScanObservation(m3DScaleFactor, mPrevLaser);
ScanObservation m2 = new ScanObservation(m3DScaleFactor, mCurrLaser);
MatchResult mR = null;
mSM = new MbIcpScanMatcher();
mEstimated = new Pose2D();
if (chkSeed.Checked)
{
mR = mSM.Match(m1, m2, mSeed);
}
else
{
mR = mSM.Match(m1, m2, new Pose2D());
}
if (mR.Converged)
{
Pose2D es = mR.EstimatedOdometry;
mEstimated = compound(mEstimated, es);
mCurrLaser.posRobot = new Pose2D(mEstimated.Position, mEstimated.Rotation);
mMapPoints.Clear();
ComputeMapPoints(mPrevLaser, 0);
ComputeMapPoints(mCurrLaser, 1);
rwDrawer.Invalidate();
}
}
private void button1_Click(object sender, EventArgs e)
{
IScanMatchers sc = new IdcScanMatcher();
string[] lines = File.ReadAllLines(linkLabel1.Text);
string cmdLaser1 = "SEN {Time 6.42} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174} {FOV 3.1415} {Range " + lines[0] + "}";
string cmdLaser2 = "SEN {Time 6.43} {Type RangeScanner} {Name Scanner1} {Resolution 0.0174} {FOV 3.1415} {Range " + lines[1] + "}";
USARParser p1 = new USARParser(cmdLaser1);
USARParser p2 = new USARParser(cmdLaser2);
Laser m1 = new Laser(p1);
Laser m2 = new Laser(p2);
ScanObservation so1 = new ScanObservation(1000, m1);
ScanObservation so2 = new ScanObservation(1000, m2);
string [] parts = lines[2].Split("/".ToCharArray(), StringSplitOptions.RemoveEmptyEntries);
float[] p = USARParser.parseFloats(parts[0], ",");
float t = float.Parse(parts[1]);
Pose2D pSeed = new Pose2D(p[0], p[1], t);
MatchResult r = sc.Match(so1, so2, pSeed);
textBox1.Text += "My Estm. :" + r.EstimatedOdometry.ToString() + "\r\n";
textBox1.Text += "UvA Estm. :" + lines[3] + "\r\n";
}
public MatchResult(Pose2D rawOdometry, Pose2D estimatedOdometry, Matrix3 covariance, double distance, int numIterations, int numCorrespondencies, int numMilliseconds, bool converged)
{
this._RawOdometry = rawOdometry;
this._EstimatedOdometry = estimatedOdometry;
this._Covariance = covariance;
this._Distance = distance;
this._NumIterations = numIterations;
this._NumCorrespondencies = numCorrespondencies;
this._NumMilliseconds = numMilliseconds;
this._Converged = converged;
if (numCorrespondencies == 0)
{
this._Converged = false;
}
if (System.Math.Abs(covariance[0, 0]) > _CovarianceThreshold ||
System.Math.Abs(covariance[1, 1]) > _CovarianceThreshold ||
System.Math.Abs(covariance[2, 2]) > _CovarianceThreshold)
{
this._Converged = false;
}
this._Converged = !ExceedDelta(estimatedOdometry);
}
// Methods
public MatchResult(Pose2D rawOdometry, Pose2D estimatedOdometry, int numIterations, bool converged)
{
this._RawOdometry = rawOdometry;
this._EstimatedOdometry = estimatedOdometry;
this._NumIterations = numIterations;
this._Converged = converged;
}
public Laser(Laser t)
{
if (t == null)
{
return;
}
fRanges = new List <double>();
fRanges.AddRange(t.fRanges);
fTheta = new List <double>();
fTheta.AddRange(t.fTheta);
bFilters = new List <bool>();
bFilters.AddRange(t.bFilters);
this.rangesString = t.rangesString;
this.fName = t.fName;
this.fAng0 = t.fAng0;
this.fTime = t.fTime;
this.fFov = t.fFov;
this.fResolution = t.fResolution;
this.pos = new Pose2D(t.pos.Position, t.pos.Rotation);
this.posRobot = new Pose2D(t.posRobot.Position, t.posRobot.Rotation);
this.scan_mask = t.scan_mask;
}
private Vector4[] ToPoints(ScanObservation scan, Pose2D q)
{
Matrix2 R = new Matrix2(System.Math.Cos(q.Rotation), -System.Math.Sin(q.Rotation),
System.Math.Sin(q.Rotation), System.Math.Cos(q.Rotation));
List <Vector4> points = new List <Vector4>();
Vector2 tmpC;
Vector2 tmpP;
int len = scan.RangeScanner.Range.Length - 1;
for (int i = 0; i <= len; i++)
{
double dist = scan.RangeScanner.Range[i] * scan.Factor;
double angle = scan.RangeScanner.RangeTheta[i];
if (dist < MaxLaserRange && !scan.RangeScanner.RangeFilters[i])
{
tmpC = new Vector2(System.Math.Cos(angle) * dist, System.Math.Sin(angle) * dist);
tmpC = R * tmpC;
tmpC.X += q.X;
tmpC.Y += q.Y;
tmpP = new Vector2(dist, System.Math.Atan2(tmpC.Y, tmpC.X));
points.Add(new Vector4(tmpC.X, tmpC.Y, tmpP.X, tmpP.Y));
}
}
return(points.ToArray());
}
public override Task <LocalizeRep> getLocalizeResult(LocalizeReq request, ServerCallContext context)
{
LocalizeRep localizeRespone = new LocalizeRep();
double posX = 0;
double posY = 0;
double delta = 0;
int posture = 0;
getLocalizeResultHandler(ref posX, ref posY, ref delta, ref posture);
delta = delta * 180 / Math.PI;
while (delta > 180)
{
delta -= 360;
}
while (delta < -180)
{
delta += 360;
}
delta -= 180; //修改数据
Pose2D result_2D_pos = new Pose2D {
X = posX, Y = posY, Theta = delta
};
localizeRespone.Pose2D = result_2D_pos;
localizeRespone.VisionStatus = posture; //1-工件平放状态 2-工件竖立状态
localizeRespone.OffsetMethod = "P";
return(Task.FromResult(localizeRespone));
}
public override byte[] Serialize(bool partofsomethingelse)
{
int currentIndex = 0, length = 0;
bool hasmetacomponents = false;
byte[] thischunk, scratch1, scratch2;
List <byte[]> pieces = new List <byte[]>();
GCHandle h;
//pose
if (pose == null)
{
pose = new Pose2D();
}
pieces.Add(pose.Serialize(true));
//leg
pieces.Add(new[] { (byte)leg });
//combine every array in pieces into one array and return it
int __a_b__f = pieces.Sum((__a_b__c) => __a_b__c.Length);
int __a_b__e = 0;
byte[] __a_b__d = new byte[__a_b__f];
foreach (var __p__ in pieces)
{
Array.Copy(__p__, 0, __a_b__d, __a_b__e, __p__.Length);
__a_b__e += __p__.Length;
}
return(__a_b__d);
}
public LineMatchResult getCorrelation_NEW(Pose2D Changes, List <Edge> NewEdges, List <Edge> MapEdges, Vector2 RobotPos)
{
List <Edge> newEdges = convertEdges(NewEdges, Changes, RobotPos);
Dictionary <Edge, Edge> matchedEdges = calculateCurrentMatches(newEdges, MapEdges);
float count = matchedEdges.Count;
float P = 0, sigmaP = 0;
float res = 0, dist;
Edge ne, oe;
foreach (KeyValuePair <Edge, Edge> kv in matchedEdges)
{
ne = kv.Key;
oe = kv.Value;
dist = MathHelper.GetDistance(ne.point, oe.point); //ne.point.getDistance2D(oe.point);
//lineAng = Edge.getAngleBetweenLines(ne, oe);
P = (float)System.Math.Sqrt(ne.AccuracyOfNewObservation * oe.AccuracyOfEdgeP);
//P = ne.AccuracyOfNewObservation * bestEdge.AccuracyOfEdgeP;
//res += (dist + lineAng)*P;
res += (dist) * P;
sigmaP += P;
}
res /= ((float)System.Math.Pow(count, 2) * sigmaP);
LineMatchResult lmres = new LineMatchResult(res, matchedEdges, Changes, newEdges);
return(lmres);
}
public void QueueReader()
{
while (!StopQueue)
{
// If the queue is empty, run the delay for checking queue
if (PoseQueue.Count <= 0)
{
Thread.Sleep(QueueCheckDelay);
try
{
ProcessFiles();
}
catch (IOException)
{
Console.WriteLine("IOException. File is probably in-use by another program.");
}
catch (UnauthorizedAccessException)
{
Console.WriteLine("UnauthorizedAccessException. File is probably in-use by another program.");
}
continue;
}
else if (PoseQueue.Count > 20)
{
PoseQueue.RemoveRange(0, PoseQueue.Count - 1);
}
// Parse saved text
JObject parsedPose = JObject.Parse(PoseQueue[0]);
PoseQueue.RemoveAt(0);
// Do JSON stuff to grab keypoints
//Console.WriteLine(parsedPose.ToString());
if (parsedPose["people"].HasValues && parsedPose["people"][0].HasValues)
{
//Console.WriteLine(parsedPose["people"].ToString());
//Console.WriteLine(parsedPose["people"][0].Value<JArray>("pose_keypoints_2d"));
//foreach (double f in parsedPose["people"][0].Value<JArray>("pose_keypoints_2d").Values<double>())
//{
// Console.WriteLine(f);
//}
double[] keypoints = new List <double> (parsedPose["people"][0].Value <JArray>("pose_keypoints_2d").Values <double>()).ToArray();
if (Simulate3D)
{
poseEventHandler.ExecuteHandlers(Pose2D.ParseDoubleArray(keypoints).Simulate3D());
}
else
{
poseEventHandler.ExecuteHandlers(Pose2D.ParseDoubleArray(keypoints));
}
}
}
}
public LineMatchResult(float corrolation, Dictionary <Edge, Edge> matchedEdges, Pose2D changes, List <Edge> newEdges)
{
this.corrolation = corrolation;
this.matchedEdges = matchedEdges;
this.count = matchedEdges.Count;
this.changes = changes;
this.newEdges = newEdges;
}
/*================*\
|* ROS2Custom *|
\*================*/
public static void Pose2DToPose2D(RosPose2D dataOut, Pose2D dataIn)
{
// Set fields
// ----------
dataOut.x = dataIn.z;
dataOut.y = -dataIn.x;
dataOut.theta = -dataIn.theta;
}
/*================*\
|* Custom2ROS *|
\*================*/
public static void Pose2DToPose2D(Pose2D dataOut, RosPose2D dataIn)
{
// Set fields
// ----------
dataOut.z = dataIn.x;
dataOut.x = -dataIn.y;
dataOut.theta = -dataIn.theta;
}
private void CarmenLogParser(string l, ref bool completed)
{
USARParser p = new USARParser(l, "\":", true);
string odo = p.getString("odometry").Replace(" ", "");
odo = "SEN {Type Odometry} {Name Odometry} {Pose " + odo + "}";
mUSARParser.SimulationMessage = odo;
Odometry mOdo = (Odometry)mUSARParser.MessageData;
Pose2D mNewPose = new Pose2D(mOdo.x * m3DScaleFactor, mOdo.y * m3DScaleFactor, mOdo.theta);
mPrevPose = mCurrPose;
mCurrPose = mNewPose;
if (mPrevPose != null)
{
mSeed = new Pose2D(mSeed.Position + (mCurrPose.Position - mPrevPose.Position),
mSeed.Rotation - (mCurrPose.Rotation - mPrevPose.Rotation));
}
else
{
mSeed = new Pose2D();
}
string lsr = p.getString("readings").Replace(" ", "");
string valid = p.getString("valid").Replace(" ", "");
string theta = p.getString("theta").Replace(" ", "");
float nrays = float.Parse(p.getString("nrays"));
float min_theta = float.Parse(p.getString("min_theta"));
float max_theta = float.Parse(p.getString("max_theta"));
float fov = max_theta - min_theta;
float res = fov / nrays;
string range = "SEN {Time 0.0} {Type RangeScanner} {Name Scanner1} {Resolution " + res + "} {FOV " + fov + "} {Range " + lsr + "} {Valid " +
valid + "} {Theta " + theta + "}";
mUSARParser.SimulationMessage = range;
Laser mLaser = (Laser)mUSARParser.MessageData;
if (mPrevLaser == null)
{
mPrevLaser = mLaser;
}
else
{
mCurrLaser = mLaser;
completed = true;
}
}
public Pedestrian(int id, Pose2D pose, Point velocity, Point goalPosition, float radius, float prefSpeed, Policies policy = Policies.RVO, AgentType agentType = AgentType.PEDESTRIAN)
{
this.id = id;
this.pose = pose;
this.velocity = velocity;
this.goalPosition = goalPosition;
this.radius = radius;
this.prefSpeed = prefSpeed;
this.policy = policy;
this.agentType = agentType;
}
private bool isChangeValid(Pose2D Changes)
{
float[] validChanges;
validChanges = getMaxDetectedValidTransformation();
bool changesValid = (MathHelper.VectorLength(Changes.Position) < validChanges[1]) &&
(System.Math.Abs(Changes.GetNormalizedRotation()) < validChanges[0]);
return(changesValid);
}
public static Vector2 TranformVector(Vector2 local, Vector2 center, Pose2D q)
{
Matrix2 R = new Matrix2(System.Math.Cos(q.Rotation), -System.Math.Sin(q.Rotation),
System.Math.Sin(q.Rotation), System.Math.Cos(q.Rotation));
Vector2 tmpL = local - center;
tmpL = R * tmpL + center;
tmpL += q.Position;
return(tmpL);
}
public static List <Edge> convertEdges(List <Edge> Edges, Pose2D Changes, Vector2 RobotPos) // Should Convert Lines
{
Edge nEdge;
List <Edge> res = new List <Edge>();
foreach (Edge edge in Edges)
{
nEdge = Edge.applyRotateShift(edge, Changes, RobotPos);
res.Add(nEdge);
}
return(res);
}
private Pose2D compound(Pose2D t1, Pose2D t2)
{
Pose2D t_ret = new Pose2D();
t_ret.X = t2.X * System.Math.Cos(t1.Rotation) - t2.Y * System.Math.Sin(t1.Rotation) + t1.X;
t_ret.Y = t2.X * System.Math.Sin(t1.Rotation) + t2.Y * System.Math.Cos(t1.Rotation) + t1.Y;
t_ret.Rotation = t1.Rotation + t2.Rotation;
// Make angle [-pi,pi)
t_ret.Rotation = t_ret.GetNormalizedRotation();
return(t_ret);
}
protected virtual Vector2[] ToLocal(Vector2[] points, Pose2D target)
{
TMatrix2D rotmx = target.ToGlobalMatrix();
Vector2[] local = new Vector2[(points.Length - 1) + 1];
int len = points.Length - 1;
for (int i = 0; i <= len; i++)
{
local[i] = (Vector2)(rotmx * points[i]);
}
return(local);
}
private bool ExceedDelta(Pose2D dpose)
{
bool extend = false;
extend = extend || System.Math.Abs(dpose.X) > CHANGE_MAXTRANSLATION;
extend = extend || System.Math.Abs(dpose.Y) > CHANGE_MAXTRANSLATION;
double dradians = dpose.GetNormalizedRotation();
double dangle = dradians / System.Math.PI * 180;
extend = extend || System.Math.Abs(dangle) > CHANGE_MAXROTATION;
return(extend);
}
public static bool ExceedThreshold(Pose2D dpose)
{
bool extend = false;
extend = extend || System.Math.Abs(dpose.X) > MATCH_MAXTRANSLATION;
extend = extend || System.Math.Abs(dpose.Y) > MATCH_MAXTRANSLATION;
double dradians = dpose.GetNormalizedRotation();
double dangle = dradians / System.Math.PI * 180;
extend = extend || System.Math.Abs(dangle) > MATCH_MAXROTATION;
return(extend);
}
public GSLAM()
{
LaserData = null;
NewEdges = new List <Edge>();
MapEdges = new List <Edge>();
NewLines = new List <Line>();
RobotCURState = new Pose2D();
ReservedValidData = false;
isInit = false;
edgeMatcher = new EdgeMatcher();
edgeTrimmer = new EdgeTrimmer();
}
public void updateValues(Laser l, Pose2D curPose)
{
//RobotPos = new Point3D(pos);
//RobotRot = new Point3D(rot);
////Add For KF
//KRobotPos = new Point3D(pos);
//KRobotRot = new Point3D(rot);
RobotCURState = new Pose2D(curPose.Position, curPose.Rotation);
ReservedValidData = (l != null);
LaserData = l;
currentMatch = null;
timeCycle++;
}
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex)
{
int arraylength = -1;
bool hasmetacomponents = false;
object __thing;
int piecesize = 0;
byte[] thischunk, scratch1, scratch2;
IntPtr h;
//pose
pose = new Pose2D(SERIALIZEDSTUFF, ref currentIndex);
//leg
leg = SERIALIZEDSTUFF[currentIndex++];
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//start
start = new Pose2D();
start.Randomize();
//goal
goal = new Pose2D();
goal.Randomize();
}
public override void Randomize()
{
int arraylength = -1;
Random rand = new Random();
int strlength;
byte[] strbuf, myByte;
//pose
pose = new Pose2D();
pose.Randomize();
//leg
myByte = new byte[1];
rand.NextBytes(myByte);
leg = myByte[0];
}
protected override void LoadContent()
{
spriteBatch = new SpriteBatch(GraphicsDevice);
int windowWidth = GraphicsDevice.Viewport.Width;
int windowHeight = GraphicsDevice.Viewport.Height;
// Set up all render targets, the blur map doesn't need a depth buffer
colorMap = new RenderTarget2D(GraphicsDevice, windowWidth, windowHeight, false, SurfaceFormat.Color, DepthFormat.Depth16, 16, RenderTargetUsage.DiscardContents);
lightMap = new RenderTarget2D(GraphicsDevice, windowWidth, windowHeight, false, SurfaceFormat.Color, DepthFormat.Depth16, 16, RenderTargetUsage.DiscardContents);
blurMap = new RenderTarget2D(GraphicsDevice, windowWidth, windowHeight, false, SurfaceFormat.Color, DepthFormat.None, 16, RenderTargetUsage.DiscardContents);
combineEffect = Content.Load <Effect>("Combine");
lightEffect = Content.Load <Effect>("Light");
blurEffect = Content.Load <Effect>("Blur");
quad = new Quad();
Texture2D blockTexture = Content.Load <Texture2D>("Block");
Texture2D blockGlow = Content.Load <Texture2D>("BlockGlow");
blocks = new List <Visual>();
// Add blocks
blocks.Add(new Visual(blockTexture, new Vector2(250, 150), 0, blockGlow));
blocks.Add(new Visual(blockTexture, new Vector2(windowWidth - 250, 150), 0, blockGlow));
blocks.Add(new Visual(blockTexture, new Vector2(250, windowHeight - 150), 0.0f, blockGlow));
blocks.Add(new Visual(blockTexture, new Vector2(windowWidth - 250, windowHeight - 150), 0.0f, blockGlow));
for (int i = 0; i < 10; i++)
{
Vector2 position = new Vector2(150 + (10 * (i + 1)) * i, windowHeight / 2.0f);
Pose2D p = new Pose2D(position, (MathHelper.PiOver2 / 10) * i, (i + 1) * 0.15f);
blocks.Add(new Visual(blockTexture, p, blockGlow));
}
// Add lights
lights = new List <PointLight>();
lights.Add(new PointLight(lightEffect, new Vector2(300, 300), 500, Color.White, 1.0f));
//lights.Add(new PointLight(lightEffect, new Vector2(100, 300), 500, Color.Red, 1.0f));
//lights.Add(new PointLight(lightEffect, new Vector2(800, 300), 500, Color.Green, 1.0f));
//lights.Add(new PointLight(lightEffect, new Vector2(100, 600), 500, Color.Blue, 1.0f));
//lights.Add(new PointLight(lightEffect, new Vector2(800, 600), 500, Color.Yellow, 1.0f));
}
///<exclude/>
public bool Equals(Pose2D other)
{
if (ReferenceEquals(null, other)) return false;
if (ReferenceEquals(this, other)) return true;
return other._Position.Equals(_Position) && other._Heading.Equals(_Heading);
}
