/// <summary>
/// 他のマーカとの距離を返す
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public double GetDistance(MarkPoint B)
{
double dx = (X - B.X);
double dy = (Y - B.Y);
return(Math.Sqrt((dx * dx) + (dy * dy)));
}
/// <summary>
/// 位置補正執行
/// </summary>
/// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param>
public string LocalizeRevision(bool bRevisionFromGPS)
{
// 補正ログメッセージ
string revisionMsg = "";
// 補正後座標
MarkPoint rev = new MarkPoint(V1);
revisionMsg += "LocRevision:Issue / 座標補正を実行\n";
revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n";
// GPSが有効なら、GPSの値を元にPF補正
if (bEnableGPS)
{
// 補正基準 位置,向き
double RivLocX = G1.X;
double RivLocY = G1.Y;
double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う
if (bActiveCompass)
{
// コンパスを使った
revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n";
}
if (bRevisionFromGPS)
{
// GPSの値をそのまま使う
rev.Set(RivLocX, RivLocY, RivDir);
// GPSを使った
revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n";
}
else
{
// 補正基準位置からパーティクルフィルター補正
rev.Set(RivLocX, RivLocY, RivDir);
// V1ローパスフィルターリセット
ResetLPF_V1(rev);
// 自己位置推定演算 10回
CalcLocalize(V1, true, 10);
// PF
revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
}
else
{
// V1の値をそのまま使う
revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
// 結果を反映
R1.Set(rev);
E1.Set(rev);
oldE1.Set(rev);
return(revisionMsg);
}
/// <summary>
/// マーカーと同じか比較
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public bool IsEqual(MarkPoint B)
{
if (X == B.X && Y == B.Y && Theta == B.Theta)
{
return(true);
}
return(false);
}
/// <summary>
/// 自己位置推定計算
/// </summary>
/// <param name="LRF_Data">ＬＲＦ実取得データ</param>
/// <param name="MRF">MAPデータ</param>
/// <param name="mkp">想定ロボット位置(計算基点)</param>
/// <param name="Particles">パーティクル作業バッファ</param>
public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List<Particle> Particles)
{
double sum = 0;
if (null == LRF_Data)
{
Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData");
return;
}
// パーティクルをばらまく
//for (int i = 0; i < Particles.Count; ++i)
int numParticles = Particles.Count;
Parallel.For(0, numParticles, i =>
{
// 散らばらせる
MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location);
// 散らばり％ = w ?
// マップデータとLRFのデータを比べる
Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location));
sum += Particles[i].W;
});
// W順に並べ替え
Particles.Sort((a, b) => (int)((a.W - b.W)*1000.0));
// パーティクルから 整合性の高いデータを取り出す？(リサンプリング)
for (int n = 0; n < ResamplingNumber; ++n)
{
double selectVal = Rand.NextDouble() * sum;
double subsum = 0;
for (int i = 0; i < Particles.Count; ++i)
{
subsum += Particles[i].W;
if (selectVal < subsum)
{
ResamplingSet[n] = Particles[i].Location;
break;
}
}
}
// パーティクルの平均値から、仮想ロボットの位置を補正
double newX = 0;
double newY = 0;
double newTheta = 0;
for (int n = 0; n < ResamplingNumber; ++n) {
newX += ResamplingSet[n].X;
newY += ResamplingSet[n].Y;
newTheta += ResamplingSet[n].Theta;
}
mkp.X = newX / ResamplingNumber;
mkp.Y = newY / ResamplingNumber;
mkp.Theta = newTheta / ResamplingNumber;
}
/// <summary>
/// パーティクル生成
/// </summary>
/// <param name="v">基点</param>
/// <param name="sigma">散らばりのフレ幅[pixel]</param>
/// <param name="sigmaAng">散らばりのフレ幅[角度]</param>
/// <param name="p">格納先</param>
private void MakeParticle(MarkPoint v, double sigma, double sigmaAng, MarkPoint p)
{
var dx = NormalDistribution(sigma); // m単位
var dy = NormalDistribution(sigma);
var dtheta = NormalDistribution(sigmaAng);
// 座標、回転角をふれ幅内で生成
p.X = v.X + dx;
p.Y = v.Y + dy;
p.Theta = v.Theta + dtheta;
}
/// <summary>
/// GPS 現在位置取得
/// 差分を計算し現在位置更新
/// </summary>
private MarkPoint updateMkp_FromGPS()
{
MarkPoint resMkp = new MarkPoint(R1);
// 自己位置更新
// ロータリーエンコーダの移動量差分を加えて、自己位置を更新
resMkp.X += (G1.X - oldG1.X);
resMkp.Y += (G1.Y - oldG1.Y);
resMkp.Theta = G1.Theta; // REの向き
oldG1.Set(G1);
return(resMkp);
}
/// <summary>
/// 自己位置推定 開始位置セット
/// </summary>
/// <param name="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
startMapX = stWldX;
startMapY = stWldY;
startMapDir = stDir;
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
A1 = new MarkPoint(0, 0, 0); // AMCL
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0); // サンプリング数、パーティクルのレンジ
Particles = new List <Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0, 0, 0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}
private List<Particle> Particles; // �p�[�e�B�N�����X�g
#endregion Fields
#region Methods
/// <summary>
/// �C�Ӎ��W�ł̂o�e�
/// </summary>
/// <param name="numPF"></param>
public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1)
{
// �p�[�e�B�N���t�B���^�[���Z
if (useLowFilter)
{
KalmanFilter.SimpleLPF lpfX = null;
KalmanFilter.SimpleLPF lpfY = null;
KalmanFilter.SimpleLPF lpTheta = null;
for (int i = 0; i < numPF; i++)
{
MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(mkp.X), worldMap.GetAreaY(mkp.Y), mkp.Theta);
ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);
locMkp.X = worldMap.GetWorldX(locMkp.X);
locMkp.Y = worldMap.GetWorldY(locMkp.Y);
// ���ʂɃ��[�p�X�t�B���^�[�������
if (null == lpfX)
{
// �ŏ��̒l������l�ɂ���
lpfX = new KalmanFilter.SimpleLPF(locMkp.X);
lpfY = new KalmanFilter.SimpleLPF(locMkp.Y);
lpTheta = new KalmanFilter.SimpleLPF(locMkp.Theta);
}
else
{
lpfX.update(locMkp.X);
lpfY.update(locMkp.Y);
lpTheta.update(locMkp.Theta);
}
}
if (null != lpfX)
{
mkp.X = lpfX.value();
mkp.Y = lpfY.value();
mkp.Theta = lpTheta.value();
}
}
else
{
ParticleFilter.Localize(LRF.getData(), MRF, mkp, Particles);
}
}
/// <summary>
/// 任意座標でのＰＦ補正
/// </summary>
/// <param name="numPF"></param>
public void CalcLocalize(MarkPoint mkp, bool useLowFilter, int numPF = 1)
{
// パーティクルフィルター演算
if (useLowFilter)
{
KalmanFilter.SimpleLPF lpfX = null;
KalmanFilter.SimpleLPF lpfY = null;
KalmanFilter.SimpleLPF lpTheta = null;
for (int i = 0; i < numPF; i++)
{
MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(mkp.X), worldMap.GetAreaY(mkp.Y), mkp.Theta);
ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);
locMkp.X = worldMap.GetWorldX(locMkp.X);
locMkp.Y = worldMap.GetWorldY(locMkp.Y);
// 結果にローパスフィルターをかける
if (null == lpfX)
{
// 最初の値を初期値にする
lpfX = new KalmanFilter.SimpleLPF(locMkp.X);
lpfY = new KalmanFilter.SimpleLPF(locMkp.Y);
lpTheta = new KalmanFilter.SimpleLPF(locMkp.Theta);
}
else
{
lpfX.update(locMkp.X);
lpfY.update(locMkp.Y);
lpTheta.update(locMkp.Theta);
}
}
if (null != lpfX)
{
mkp.X = lpfX.value();
mkp.Y = lpfY.value();
mkp.Theta = lpTheta.value();
}
}
else
{
ParticleFilter.Localize(LRF.getData(), MRF, mkp, Particles);
}
}
/// <summary>
/// マーカー描画
/// </summary>
/// <param name="g"></param>
/// <param name="robot"></param>
/// <param name="brush"></param>
/// <param name="size"></param>
private void DrawMaker_Area(Graphics g, float fScale, MarkPoint robot, Brush brush, int size)
{
double mkX = worldMap.GetAreaX(robot.X) * fScale;
double mkY = worldMap.GetAreaY(robot.Y) * fScale;
double mkDir = robot.Theta - 90.0;
var P1 = new PointF(
(float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0)));
var P2 = new PointF(
(float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0)));
var P3 = new PointF(
(float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0)));
g.FillPolygon(brush, new PointF[] { P1, P2, P3 });
}
/// <summary>
/// マーカー描画
/// </summary>
/// <param name="g"></param>
/// <param name="fScale"></param>
/// <param name="robot"></param>
/// <param name="brush"></param>
/// <param name="size"></param>
static public void DrawMaker(Graphics g, float fScale, MarkPoint robot, Brush brush, int size)
{
double mkX = robot.X * fScale;
double mkY = robot.Y * fScale;
double mkDir = robot.Theta - 90.0;
var P1 = new PointF(
(float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0)));
var P2 = new PointF(
(float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0)));
var P3 = new PointF(
(float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0)));
g.FillPolygon(brush, new PointF[] { P1, P2, P3 });
}
/// <summary>
/// マーカー描画
/// </summary>
/// <param name="g"></param>
/// <param name="fScale"></param>
/// <param name="robot"></param>
/// <param name="brush"></param>
/// <param name="size"></param>
public static void DrawMaker(Graphics g, float fScale, MarkPoint robot, Brush brush, int size)
{
double mkX = robot.X * fScale;
double mkY = robot.Y * fScale;
double mkDir = robot.Theta - 90.0;
var P1 = new PointF(
(float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0)));
var P2 = new PointF(
(float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0)));
var P3 = new PointF(
(float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0)));
g.FillPolygon(brush, new PointF[] { P1, P2, P3 });
}
/// <summary>
/// PF自己位置計算 継続
/// </summary>
/// <param name="mkp"></param>
/// <param name="useLowFilter"></param>
/// <param name="numPF"></param>
public void ParticleFilterLocalize(bool useLPF)
{
MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(V1.X), worldMap.GetAreaY(V1.Y), V1.Theta);
// パーティクルフィルター演算
ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);
if (useLPF)
{
// 結果にローパスフィルターをかける
V1.X = lpfV1X.update(worldMap.GetWorldX(locMkp.X));
V1.Y = lpfV1Y.update(worldMap.GetWorldY(locMkp.Y));
V1.Theta = lpfV1Theta.update(locMkp.Theta);
}
else
{
V1.X = worldMap.GetWorldX(locMkp.X);
V1.Y = worldMap.GetWorldY(locMkp.Y);
V1.Theta = locMkp.Theta;
}
}
/// <summary>
/// ローパスフィルター初期化
/// </summary>
/// <param name="mkp"></param>
public void ResetLPF_V1(MarkPoint mkp)
{
lpfV1X = new KalmanFilter.SimpleLPF(mkp.X);
lpfV1Y = new KalmanFilter.SimpleLPF(mkp.Y);
lpfV1Theta = new KalmanFilter.SimpleLPF(mkp.Theta);
}
/// <summary>
/// 他のマーカとの距離を返す
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public double GetDistance(MarkPoint B)
{
double dx = (X - B.X);
double dy = (Y - B.Y);
return Math.Sqrt((dx*dx) + (dy*dy));
}
/// <summary>
/// ���O�ۑ��p�@���[���h�}�b�v�ւ̋O�Չ摜����
/// </summary>
/// <returns></returns>
public Bitmap MakeMakerLogBmp(bool bPointOn, MarkPoint marker)
{
if (R1Log.Count <= 0) return null; // �f�[�^�Ȃ�
Bitmap logBmp = new Bitmap(worldMap.mapBmp);
Graphics g = Graphics.FromImage(logBmp);
// �O�Օ`��
// ���Ȉʒu
DrawMakerLogLine_World(g, R1Log, Color.Red.R, Color.Red.G, Color.Red.B);
// �p�[�e�B�N���t�B���^�[ ���Ȉʒu����
DrawMakerLogLine_World(g, V1Log, Color.Cyan.R, Color.Cyan.G, Color.Cyan.B);
// ���[�^���[�G���R�[�_���W
DrawMakerLogLine_World(g, E1Log, Color.Purple.R, Color.Purple.G, Color.Purple.B);
// GPS���W
DrawMakerLogLine_World(g, G1Log, Color.Green.R, Color.Green.G, Color.Green.B);
// �ŏI�n�_�Ƀ}�[�J�\��
if (R1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, R1Log[R1Log.Count - 1], Brushes.Red, 4);
}
if (V1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, V1Log[V1Log.Count - 1], Brushes.Cyan, 4);
}
if (E1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, E1Log[E1Log.Count - 1], Brushes.Purple, 4);
}
if (G1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, G1Log[G1Log.Count - 1], Brushes.Green, 4);
}
if (null != marker)
{
DrawMaker_Area(g, 1.0f, marker, Brushes.GreenYellow, 4);
}
// �����Ԃ��Ƃ̈ʒu�ƌ���
if (bPointOn)
{
// ���Ȉʒu
//foreach (var p in R1Log)
for (int i = 0; i < R1Log.Count; i++)
{
if ((i % 30) != 0) continue;
DrawMaker_Area(g, 1.0f, R1Log[i], Brushes.Red, 4);
}
// LRF �p�[�e�B�N���t�B���^�[
//foreach (var p in V1Log)
for (int i = 0; i < V1Log.Count; i++)
{
if ((i % 30) != 0) continue;
DrawMaker_Area(g, 1.0f, V1Log[i], Brushes.Cyan, 3);
}
// ���[�^���[�G���R�[�_���W
//foreach (var p in E1Log)
for (int i = 0; i < E1Log.Count; i++)
{
if ((i % 30) != 0) continue;
DrawMaker_Area(g, 1.0f, E1Log[i], Brushes.Purple, 4);
}
}
g.Dispose();
return logBmp;
}
/// <summary>
/// �}�[�J�[�`��
/// </summary>
/// <param name="g"></param>
/// <param name="robot"></param>
/// <param name="brush"></param>
/// <param name="size"></param>
private void DrawMaker_Area(Graphics g, float fScale, MarkPoint robot, Brush brush, int size)
{
double mkX = worldMap.GetAreaX(robot.X) * fScale;
double mkY = worldMap.GetAreaY(robot.Y) * fScale;
double mkDir = robot.Theta - 90.0;
var P1 = new PointF(
(float)(mkX + size * Math.Cos(mkDir * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin(mkDir * Math.PI / 180.0)));
var P2 = new PointF(
(float)(mkX + size * Math.Cos((mkDir - 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir - 150) * Math.PI / 180.0)));
var P3 = new PointF(
(float)(mkX + size * Math.Cos((mkDir + 150) * Math.PI / 180.0)),
(float)(mkY + size * Math.Sin((mkDir + 150) * Math.PI / 180.0)));
g.FillPolygon(brush, new PointF[] { P1, P2, P3 });
}
/// <summary>
/// マップイメージログ出力
/// </summary>
/// <param name="BrainCtrl"></param>
/// <returns></returns>
public bool Output_ImageLog(ref Brain BrainCtrl )
{
try
{
// 軌跡ログ出力
if (!string.IsNullOrEmpty(saveLogFname))
{
MarkPoint tgtMaker = null;
// 次の目的地取得
{
int tgtPosX = 0;
int tgtPosY = 0;
double dir = 0;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
tgtMaker = new MarkPoint(tgtPosX, tgtPosY, dir);
}
{
Bitmap bmp = BrainCtrl.LocSys.MakeMakerLogBmp(false, tgtMaker);
if (null != bmp)
{
// 画像ファイル保存
string saveImageLogFname = Path.ChangeExtension(saveLogFname, "png");
bmp.Save(saveImageLogFname, System.Drawing.Imaging.ImageFormat.Png);
}
}
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message + System.Environment.NewLine + ex.StackTrace);
return false;
}
return true;
}
/// <summary>
/// PF���Ȉʒu�v�Z �p��
/// </summary>
/// <param name="mkp"></param>
/// <param name="useLowFilter"></param>
/// <param name="numPF"></param>
public void ParticleFilterLocalize(bool useLPF)
{
MarkPoint locMkp = new MarkPoint(worldMap.GetAreaX(V1.X), worldMap.GetAreaY(V1.Y), V1.Theta);
// �p�[�e�B�N���t�B���^�[���Z
ParticleFilter.Localize(LRF.getData(), MRF, locMkp, Particles);
if (useLPF)
{
// ���ʂɃ��[�p�X�t�B���^�[�������
V1.X = lpfV1X.update(worldMap.GetWorldX(locMkp.X));
V1.Y = lpfV1Y.update(worldMap.GetWorldY(locMkp.Y));
V1.Theta = lpfV1Theta.update(locMkp.Theta);
}
else
{
V1.X = worldMap.GetWorldX(locMkp.X);
V1.Y = worldMap.GetWorldY(locMkp.Y);
V1.Theta = locMkp.Theta;
}
}
/// <summary>
/// 指定位置のマップ障害物情報をLRFの値として返す
/// </summary>
/// <param name="mapX"></param>
/// <param name="mapY"></param>
/// <param name="ang"></param>
/// <returns></returns>
public double[] GetMapLRF( int mapX, int mapY, double ang )
{
MarkPoint mkp = new MarkPoint(worldMap.GetAreaX(mapX), worldMap.GetAreaY(mapY), ang);
return MRF.Sense(mkp);
}
//static int areaMapDrawCnt = 0;
public void AreaMap_Draw_WorldMap(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
{
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// 全体マップ描画
float viewScale;
g.FillRectangle(Brushes.Black, 0, 0, worldMapBmp.Width, worldMapBmp.Height);
if (((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width) < ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height))
{
viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.h / (float)worldMapBmp.Height));
}
else
{
viewScale = (float)(1.0 / ((float)LocSys.worldMap.WorldSize.w / (float)worldMapBmp.Width));
}
//g.ResetTransform();
//g.TranslateTransform(-ctrX, -ctrY, MatrixOrder.Append);
//g.RotateTransform((float)layer.wAng, MatrixOrder.Append);
//g.ScaleTransform(viewScale, viewScale, MatrixOrder.Append);
if (null != worldMapBmp)
{
g.DrawImage(worldMapBmp, 0, 0);
}
//g.ResetTransform();
// 各マーカーの位置を描画
LocSys.DrawWorldMap(g, viewScale);
// ターゲット描画
if (null != CersioCt)
{
int tgtPosX, tgtPosY;
double dir = 0;
tgtPosX = tgtPosY = 0;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
MarkPoint tgtMk = new MarkPoint(tgtPosX, tgtPosY, dir + 180);
DrawMaker(g, viewScale, tgtMk, Brushes.GreenYellow, 8);
// ターゲットまでのライン
DrawMakerLine(g, viewScale,
LocSys.R1,
tgtMk,
Pens.Olive, 1);
}
}
public void CarInit(MarkPoint _mkp)
{
CarInit(_mkp.X, _mkp.Y, _mkp.Theta);
mkp.Set(_mkp);
}
public Grid GetMapInfo(MarkPoint mkp)
{
return(worldMap.GetAreaMapInfo(mkp.X, mkp.Y));
}
/// <summary>
/// エリアマップ描画
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void AreaMap_Draw_Area(Graphics g, ref CersioCtrl CersioCt, ref Brain BrainCtrl)
{
// 書き換えＢＭＰ（追加障害物）描画
LocPreSumpSystem LocSys = BrainCtrl.LocSys;
// エリアマップ描画
//g.FillRectangle(Brushes.Black, 0, 0, picbox_AreaMap.Width, picbox_AreaMap.Height);
g.DrawImage(LocSys.AreaOverlayBmp, 0, 0);
// ターゲット描画
if (null != CersioCt)
{
int tgtPosX, tgtPosY;
double dir = 0;
tgtPosX = tgtPosY = 0;
float olScale = (float)LocSys.AreaOverlayBmp.Width / (float)LocSys.AreaBmp.Width;
BrainCtrl.RTS.getNowTarget(ref tgtPosX, ref tgtPosY);
BrainCtrl.RTS.getNowTargetDir(ref dir);
MarkPoint tgtMk = new MarkPoint(LocSys.worldMap.GetAreaX(tgtPosX), LocSys.worldMap.GetAreaY(tgtPosY), dir);
DrawMaker(g, olScale, tgtMk, Brushes.GreenYellow, 8);
// ターゲットまでのライン
DrawMakerLine(g, olScale,
new MarkPoint(LocSys.worldMap.GetAreaX(LocSys.R1.X), LocSys.worldMap.GetAreaY(LocSys.R1.Y), 0),
tgtMk,
Pens.Olive, 1);
}
}
public double[] GetMapLRF(MarkPoint mkp)
{
return(GetMapLRF((int)(mkp.X + 0.5), (int)(mkp.Y + 0.5), mkp.Theta));
}
/// <summary>
/// 指定位置のマップ障害物情報をLRFの値として返す
/// </summary>
/// <param name="mapX"></param>
/// <param name="mapY"></param>
/// <param name="ang"></param>
/// <returns></returns>
public double[] GetMapLRF(int mapX, int mapY, double ang)
{
MarkPoint mkp = new MarkPoint(worldMap.GetAreaX(mapX), worldMap.GetAreaY(mapY), ang);
return(MRF.Sense(mkp));
}
/// <summary>
/// ロータリーエンコーダ 現在位置取得
/// 差分を計算し現在位置更新
/// </summary>
private MarkPoint updateMkp_FromREPlot()
{
MarkPoint resMkp = new MarkPoint(R1);
// 自己位置更新
// ロータリーエンコーダの移動量差分を加えて、自己位置を更新
resMkp.X += (E1.X - oldE1.X);
resMkp.Y += (E1.Y - oldE1.Y);
resMkp.Theta = E1.Theta; // REの向き
oldE1.Set(E1);
return resMkp;
}
/// <summary>
/// 自己位置推定計算
/// </summary>
/// <param name="LRF_Data">ＬＲＦ実取得データ</param>
/// <param name="MRF">MAPデータ</param>
/// <param name="mkp">想定ロボット位置(計算基点)</param>
/// <param name="Particles">パーティクル作業バッファ</param>
public void Localize(double[] LRF_Data, MapRangeFinder MRF, MarkPoint mkp, List <Particle> Particles)
{
double sum = 0;
if (null == LRF_Data)
{
Console.WriteLine("ParticleFilter.cs Localize() : Error! null LrfData");
return;
}
// パーティクルをばらまく
//for (int i = 0; i < Particles.Count; ++i)
int numParticles = Particles.Count;
Parallel.For(0, numParticles, i =>
{
// 散らばらせる
MakeParticle(mkp, PtclRange, PtclDirRange, Particles[i].Location);
// 散らばり％ = w ?
// マップデータとLRFのデータを比べる
Particles[i].W = Likefood(LRF_Data, MRF.Sense(Particles[i].Location));
sum += Particles[i].W;
});
// W順に並べ替え
Particles.Sort((a, b) => (int)((a.W - b.W) * 1000.0));
// パーティクルから 整合性の高いデータを取り出す？(リサンプリング)
for (int n = 0; n < ResamplingNumber; ++n)
{
double selectVal = Rand.NextDouble() * sum;
double subsum = 0;
for (int i = 0; i < Particles.Count; ++i)
{
subsum += Particles[i].W;
if (selectVal < subsum)
{
ResamplingSet[n] = Particles[i].Location;
break;
}
}
}
// パーティクルの平均値から、仮想ロボットの位置を補正
double newX = 0;
double newY = 0;
double newTheta = 0;
for (int n = 0; n < ResamplingNumber; ++n)
{
newX += ResamplingSet[n].X;
newY += ResamplingSet[n].Y;
newTheta += ResamplingSet[n].Theta;
}
mkp.X = newX / ResamplingNumber;
mkp.Y = newY / ResamplingNumber;
mkp.Theta = newTheta / ResamplingNumber;
}
public double[] Sense(MarkPoint mkp)
{
return(Sense(Map, mkp.X, mkp.Y, mkp.Theta));
}
public double[] GetMapLRF(MarkPoint mkp)
{
return GetMapLRF((int)(mkp.X+0.5), (int)(mkp.Y+0.5), mkp.Theta);
}
public Particle(MarkPoint r, double w)
{
Location = r;
W = w;
}
/// <summary>
/// パーティクル生成
/// </summary>
/// <param name="v">基点</param>
/// <param name="sigma">散らばりのフレ幅[pixel]</param>
/// <param name="sigmaAng">散らばりのフレ幅[角度]</param>
/// <param name="p">格納先</param>
private void MakeParticle(MarkPoint v, double sigma, double sigmaAng, MarkPoint p)
{
var dx = NormalDistribution(sigma); // m単位
var dy = NormalDistribution(sigma);
var dtheta = NormalDistribution(sigmaAng);
// 座標、回転角をふれ幅内で生成
p.X = v.X + dx;
p.Y = v.Y + dy;
p.Theta = v.Theta + dtheta;
}
public void Set(MarkPoint B)
{
Set(B.X, B.Y, B.Theta);
}
public Grid GetMapInfo(MarkPoint mkp)
{
return worldMap.GetAreaMapInfo(mkp.X, mkp.Y);
}
public MarkPoint(MarkPoint mkp) : this(mkp.X, mkp.Y, mkp.Theta)
{
}
/// <summary>
/// ログ保存用 ワールドマップへの軌跡画像生成
/// </summary>
/// <returns></returns>
public Bitmap MakeMakerLogBmp(bool bPointOn, MarkPoint marker)
{
if (R1Log.Count <= 0)
{
return(null); // データなし
}
Bitmap logBmp = new Bitmap(worldMap.mapBmp);
Graphics g = Graphics.FromImage(logBmp);
// 軌跡描画
// 自己位置
DrawMakerLogLine_World(g, R1Log, Color.Red.R, Color.Red.G, Color.Red.B);
// パーティクルフィルター 自己位置推定
DrawMakerLogLine_World(g, V1Log, Color.Cyan.R, Color.Cyan.G, Color.Cyan.B);
// ロータリーエンコーダ座標
DrawMakerLogLine_World(g, E1Log, Color.Purple.R, Color.Purple.G, Color.Purple.B);
// GPS座標
DrawMakerLogLine_World(g, G1Log, Color.Green.R, Color.Green.G, Color.Green.B);
// 最終地点にマーカ表示
if (R1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, R1Log[R1Log.Count - 1], Brushes.Red, 4);
}
if (V1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, V1Log[V1Log.Count - 1], Brushes.Cyan, 4);
}
if (E1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, E1Log[E1Log.Count - 1], Brushes.Purple, 4);
}
if (G1Log.Count > 0)
{
DrawMaker_Area(g, 1.0f, G1Log[G1Log.Count - 1], Brushes.Green, 4);
}
if (null != marker)
{
DrawMaker_Area(g, 1.0f, marker, Brushes.GreenYellow, 4);
}
// 一定期間ごとの位置と向き
if (bPointOn)
{
// 自己位置
//foreach (var p in R1Log)
for (int i = 0; i < R1Log.Count; i++)
{
if ((i % 30) != 0)
{
continue;
}
DrawMaker_Area(g, 1.0f, R1Log[i], Brushes.Red, 4);
}
// LRF パーティクルフィルター
//foreach (var p in V1Log)
for (int i = 0; i < V1Log.Count; i++)
{
if ((i % 30) != 0)
{
continue;
}
DrawMaker_Area(g, 1.0f, V1Log[i], Brushes.Cyan, 3);
}
// ロータリーエンコーダ座標
//foreach (var p in E1Log)
for (int i = 0; i < E1Log.Count; i++)
{
if ((i % 30) != 0)
{
continue;
}
DrawMaker_Area(g, 1.0f, E1Log[i], Brushes.Purple, 4);
}
}
g.Dispose();
return(logBmp);
}
/// <summary>
/// 自己位置推定 開始位置セット
/// </summary>
/// <param name="stWldX">マップ座標X</param>
/// <param name="stWldY">マップ座標Y</param>
/// <param name="stDir">角度</param>
public void SetStartPostion(int stWldX, int stWldY, double stDir)
{
MRF = new MapRangeFinder(LRF_Ctrl.LRFmaxRange_mm / MapToRealScale); // 仮想Map用 LRFクラス
worldMap.UpdateAreaCenter(stWldX, stWldY);
AreaBmp = worldMap.AreaGridMap.UpdateBitmap();
//bAreaMapUpdateReqest = false;
MRF.SetMap(worldMap.AreaGridMap);
// ロボットマーカ設定
R1 = new MarkPoint(stWldX, stWldY, stDir); // 実ロボット位置 R.E,Compass,GPSでの位置+ LRF補正
E1 = new MarkPoint(stWldX, stWldY, stDir); // R.Encoderの位置
oldE1 = new MarkPoint(stWldX, stWldY, stDir); //
E2 = new MarkPoint(stWldX, stWldY, stDir); // R.E Pluse & Compus の位置
V1 = new MarkPoint(stWldX, stWldY, stDir); // 推定位置ロボット
C1 = new MarkPoint(0, 0, stDir);
G1 = new MarkPoint(0, 0, 0); // GPS
oldG1 = new MarkPoint(stWldX, stWldY, stDir); //
// パーティクルフィルター初期化
{
// サンプル数(ParticleSizeと同じで、すべてのパーティクルを対象とする)
// LRFの有効距離を半径に変換(/2.0)、20%の距離で散らばる
// +-5度の範囲
ParticleFilter = new ParticleFilter(ParticleSize, (((LRF_Ctrl.LRFmaxRange_mm / 2.0) * 0.20) / MapToRealScale), 30.0 ); // サンプリング数、パーティクルのレンジ
Particles = new List<Particle>();
for (int i = 0; i < ParticleSize; ++i)
{
// パーティクルマーカー
Particles.Add(new Particle(new MarkPoint(0,0,0), 0));
}
}
// ローパスフィルター初期化
ResetLPF_V1(V1);
}
public void Set(MarkPoint B)
{
Set(B.X, B.Y, B.Theta);
}
/// <summary>
/// ローパスフィルター初期化
/// </summary>
/// <param name="mkp"></param>
public void ResetLPF_V1(MarkPoint mkp)
{
lpfV1X = new KalmanFilter.SimpleLPF(mkp.X);
lpfV1Y = new KalmanFilter.SimpleLPF(mkp.Y);
lpfV1Theta = new KalmanFilter.SimpleLPF(mkp.Theta);
}
/// <summary>
/// マーカーと同じか比較
/// </summary>
/// <param name="B"></param>
/// <returns></returns>
public bool IsEqual(MarkPoint B)
{
if (X == B.X && Y == B.Y && Theta == B.Theta) return true;
return false;
}
/// <summary>
/// 位置補正執行
/// </summary>
/// <param name="bRevisionFromGPS">GPSの値をそのまま使う</param>
public string LocalizeRevision( bool bRevisionFromGPS )
{
// 補正ログメッセージ
string revisionMsg = "";
// 補正後座標
MarkPoint rev = new MarkPoint(V1);
revisionMsg += "LocRevision:Issue / 座標補正を実行\n";
revisionMsg += "LocRevision: beforR1 R1.X" + R1.X.ToString("f2") + "/R1.Y " + R1.Y.ToString("f2") + "/R1.Dir " + R1.Theta.ToString("f2") + "\n";
// GPSが有効なら、GPSの値を元にPF補正
if (bEnableGPS)
{
// 補正基準 位置,向き
double RivLocX = G1.X;
double RivLocY = G1.Y;
double RivDir = (bActiveCompass ? C1.Theta : R1.Theta); // コンパスが使えるなら、コンパスの値を使う
if (bActiveCompass)
{
// コンパスを使った
revisionMsg += "LocRevision:useCompass C1.Dir " + C1.Theta.ToString("f2") + "\n";
}
if (bRevisionFromGPS)
{
// GPSの値をそのまま使う
rev.Set(RivLocX, RivLocY, RivDir);
// GPSを使った
revisionMsg += "LocRevision: useGPS G1.X" + G1.X.ToString("f2") + "/G1.Y " + G1.Y.ToString("f2") + "/G1.Dir " + G1.Theta.ToString("f2") + "\n";
}
else
{
// 補正基準位置からパーティクルフィルター補正
rev.Set(RivLocX, RivLocY, RivDir);
// V1ローパスフィルターリセット
ResetLPF_V1(rev);
// 自己位置推定演算 10回
CalcLocalize(V1, true,10);
// PF
revisionMsg += "LocRevision: usePF V1.X" + V1.X.ToString("f2") + "/V1.Y " + V1.Y.ToString("f2") + "/V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
}
else
{
// V1の値をそのまま使う
revisionMsg += "LocRevision: V1toR1 V1.X" + V1.X.ToString("f2") + "/ V1.Y " + V1.Y.ToString("f2") + "/ V1.Dir " + V1.Theta.ToString("f2") + "\n";
}
// 結果を反映
R1.Set(rev);
E1.Set(rev);
oldE1.Set(rev);
return revisionMsg;
}
private void DrawMakerLine(Graphics g, float fScale, MarkPoint robotA, MarkPoint robotB, Pen pen, int size)
{
var P1 = new PointF(
(float)(robotA.X * fScale),
(float)(robotA.Y * fScale));
var P2 = new PointF(
(float)(robotB.X * fScale),
(float)(robotB.Y * fScale));
g.DrawLine(pen, P1, P2);
}
/// <summary>
/// 現在位置更新
/// 各センサー情報から、座標、向きを選択
/// </summary>
public void update_NowLocation()
{
// R.E.Plotの座標を計算・取得
MarkPoint RePlotMkp = updateMkp_FromREPlot();
// GPS座標を計算・取得
MarkPoint GpsMkp = updateMkp_FromGPS();
// 移動情報 更新
if (Setting.bMoveSrcRePlot)
{
// R.E Plot
R1.X = RePlotMkp.X;
R1.Y = RePlotMkp.Y;
}
else if (Setting.bMoveSrcGPS)
{
// GPS
R1.X = GpsMkp.X;
R1.Y = GpsMkp.Y;
}
else if (Setting.bMoveSrcSVO)
{
// Semi-direct Monocular Visual Odometry
// ※未実装
}
else if (Setting.bMoveSrcReCompus)
{
// RE Pulse値(移動量) & Compusの向きでの座標
R1.X = E2.X;
R1.Y = E2.Y;
}
else if (Setting.bMoveSrcPF)
{
// RE Pulse値(移動量) & PFの向きでの座標
// ※未実装
}
else if (Setting.bMoveAMCL)
{
R1.X = A1.X;
R1.Y = A1.Y;
}
// 向き情報 更新
if (Setting.bDirSrcRePlot)
{
R1.Theta = RePlotMkp.Theta;
}
else if (Setting.bDirSrcGPS)
{
R1.Theta = GpsMkp.Theta;
}
else if (Setting.bDirSrcSVO)
{
// Semi-direct Monocular Visual Odometry
// ※未実装
}
else if (Setting.bDirSrcCompus)
{
if (bActiveCompass)
{
R1.Theta = C1.Theta;
}
}
else if (Setting.bDirAMCL)
{
R1.Theta = A1.Theta;
}
// 更新結果ログ保存
UpdateLogData();
}
public double[] Sense(MarkPoint mkp )
{
return Sense(Map, mkp.X, mkp.Y, mkp.Theta);
}
public MarkPoint(MarkPoint mkp)
: this(mkp.X, mkp.Y, mkp.Theta)
{
}
public Particle(MarkPoint r, double w)
{
Location = r;
W = w;
}
/// <summary>
///
/// </summary>
private void SimCarInit()
{
// クルマの配置の初期状態
MarkPoint carInitPos = new MarkPoint(mapFileData.startPosition.x * ScalePixelToReal,
mapFileData.startPosition.y * ScalePixelToReal,
mapFileData.startDir);
// シムカー生成
carPos = new MarkPoint(carInitPos.X, carInitPos.Y, carInitPos.Theta);
carSim = new CarSim(ScalePixelToReal);
carSim.CarInit(carInitPos);
carSim.MapInit(MapBmp);
}
