public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
Debug.Assert(num >= 4);
double err0 = 0, err1;
NyARIcpUtils.U u = this.__u;
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__E = new double[num];
this.__E2 = new double[num];
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
double[] E = this.__E;
double[] E2 = this.__E2;
NyARDoublePoint2d[] du = this.__du;
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
int inlierNum = (int)(num * this.getInlierProbability()) - 1;
if (inlierNum < 3)
{
inlierNum = 3;
}
o_matxw2xc.setValue(initMatXw2Xc);
for (int i = 0;; i++) {
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return false;
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
du[j].x = dx;
du[j].y = dy;
E[j] = E2[j] = dx * dx + dy * dy;
}
Array.Sort(E2, 0, num);// qsort(E2, data->num, sizeof(ARdouble), compE);
double K2 = E2[inlierNum] * K2_FACTOR;
if (K2 < 16.0)
{
K2 = 16.0;
}
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (E2[j] > K2)
{
err1 += K2 / 6.0;
}
else
{
err1 += K2 / 6.0 * (1.0 - (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2) * (1.0 - E2[j] / K2));
}
}
err1 /= num;
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < this.breakLoopErrorThresh2 && err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this._maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (E[j] <= K2)
{
double W = (1.0 - E[j] / K2) * (1.0 - E[j] / K2);
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],W))
{
return false;
}
}
}
if (jus.getLength() < 3)
{
return false;
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if(o_result_param!=null){
o_result_param.last_error=err1;
}
return true;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
double err0 = 0, err1;
System.Diagnostics.Debug.Assert(num >= 4);
NyARIcpUtils.DeltaS dS = this.__dS;
NyARIcpUtils.U u = this.__u;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
o_matxw2xc.setValue(initMatXw2Xc);
double breakLoopErrorThresh = this.getBreakLoopErrorThresh();
double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2();
double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh();
double maxLoop = this.getMaxLoop();
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
for (int i = 0; ; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return false;
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
err1 /= num;
if (err1 < breakLoopErrorThresh)
{
break;
}
if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh))
{
break;
}
if (i == maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if(!jus.push(this._ref_matXc2U,o_matxw2xc, worldCoord[j],du[j],1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
// *err = err1;
return true;
}
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l,
NyARDoublePoint3d[] worldCoord_l, int num_l,
NyARDoublePoint2d[] screenCoord_r,
NyARDoublePoint3d[] worldCoord_r, int num_r,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc)
{
System.Diagnostics.Debug.Assert(num_l + num_r >= 3);
double err0 = 0, err1;
// 6*2*num?
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num_l + num_r)
{
this.__jus = new NyARIcpUtils.JusStack(num_l + num_r);
this.__du = NyARDoublePoint2d.createArray(num_l + num_r);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
NyARIcpUtils.U u = this.__u;
NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44();
matXw2Xc.setValue(initMatXw2Xc);
matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L);
matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R);
for (int i = 0; ; i++)
{
matXw2Ul.mul(matXc2Ul, matXw2Xc);
matXw2Ur.mul(matXc2Ur, matXw2Xc);
err1 = 0.0;
for (int j = 0; j < num_l; j++)
{
if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j]))
{
return(false);
}
double dx = screenCoord_l[j].x - u.x;
double dy = screenCoord_l[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
for (int j = 0; j < num_r; j++)
{
if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j]))
{
return(false);
}
double dx = screenCoord_r[j].x - u.x;
double dy = screenCoord_r[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j + num_l].x = dx;
du[j + num_l].y = dy;
}
err1 /= (num_l + num_r);
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2 &&
err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this.maxLoop)
{
break;
}
err0 = err1;
for (int j = 0; j < num_l; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0))
{
return(false);
}
}
for (int j = 0; j < num_r; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0))
{
return(false);
}
}
if (!dS.setJusArray(jus))
{
return(false);
}
dS.makeMat(matXw2Xc);
}
return(false);
}
public bool icpStereoPoint(NyARDoublePoint2d[] screenCoord_l,
NyARDoublePoint3d[] worldCoord_l, int num_l,
NyARDoublePoint2d[] screenCoord_r,
NyARDoublePoint3d[] worldCoord_r, int num_r,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 matXw2Xc)
{
System.Diagnostics.Debug.Assert(num_l + num_r >= 3);
double err0 = 0, err1;
// 6*2*num?
NyARIcpUtils.DeltaS dS = this.__dS;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num_l + num_r)
{
this.__jus = new NyARIcpUtils.JusStack(num_l + num_r);
this.__du = NyARDoublePoint2d.createArray(num_l + num_r);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
NyARIcpUtils.U u = this.__u;
NyARDoubleMatrix44 matXc2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXc2Ur = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ul = new NyARDoubleMatrix44();
NyARDoubleMatrix44 matXw2Ur = new NyARDoubleMatrix44();
matXw2Xc.setValue(initMatXw2Xc);
matXc2Ul.mul(this._ref_matXcl2Ul, this._matC2L);
matXc2Ur.mul(this._ref_matXcr2Ur, this._matC2R);
for (int i = 0; ; i++)
{
matXw2Ul.mul(matXc2Ul, matXw2Xc);
matXw2Ur.mul(matXc2Ur, matXw2Xc);
err1 = 0.0;
for (int j = 0; j < num_l; j++)
{
if (!u.setXbyMatX2U(matXw2Ul, worldCoord_l[j]))
{
return false;
}
double dx = screenCoord_l[j].x - u.x;
double dy = screenCoord_l[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
for (int j = 0; j < num_r; j++)
{
if (!u.setXbyMatX2U(matXw2Ur, worldCoord_r[j]))
{
return false;
}
double dx = screenCoord_r[j].x - u.x;
double dy = screenCoord_r[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j + num_l].x = dx;
du[j + num_l].y = dy;
}
err1 /= (num_l + num_r);
if (err1 < this.breakLoopErrorThresh)
{
break;
}
if (i > 0 && err1 < ICP_BREAK_LOOP_ERROR_THRESH2
&& err1 / err0 > this.breakLoopErrorRatioThresh)
{
break;
}
if (i == this.maxLoop)
{
break;
}
err0 = err1;
for (int j = 0; j < num_l; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_l[j], du[j], 1.0))
{
return false;
}
}
for (int j = 0; j < num_r; j++)
{
if (!jus.push(this._ref_matXc2U, matXw2Xc, worldCoord_r[j], du[j], 1.0))
{
return false;
}
}
if (!dS.setJusArray(jus))
{
return false;
}
dS.makeMat(matXw2Xc);
}
return false;
}
public override bool icpPoint(NyARDoublePoint2d[] screenCoord,
NyARDoublePoint3d[] worldCoord, int num,
NyARDoubleMatrix44 initMatXw2Xc, NyARDoubleMatrix44 o_matxw2xc, NyARTransMatResultParam o_result_param)
{
double err0 = 0, err1;
System.Diagnostics.Debug.Assert(num >= 4);
NyARIcpUtils.DeltaS dS = this.__dS;
NyARIcpUtils.U u = this.__u;
//ワークオブジェクトのリセット
if (this.__jus.getArraySize() < num)
{
this.__jus = new NyARIcpUtils.JusStack(num);
this.__du = NyARDoublePoint2d.createArray(num);
}
NyARIcpUtils.JusStack jus = this.__jus;
NyARDoublePoint2d[] du = this.__du;
o_matxw2xc.setValue(initMatXw2Xc);
double breakLoopErrorThresh = this.getBreakLoopErrorThresh();
double breakLoopErrorThresh2 = this.getBreakLoopErrorThresh2();
double breakLoopErrorRatioThresh = this.getBreakLoopErrorRatioThresh();
double maxLoop = this.getMaxLoop();
NyARDoubleMatrix44 matXw2U = this.__matXw2U;
for (int i = 0; ; i++)
{
matXw2U.mul(this._ref_matXc2U, o_matxw2xc);
err1 = 0.0;
for (int j = 0; j < num; j++)
{
if (!u.setXbyMatX2U(matXw2U, worldCoord[j]))
{
return(false);
}
double dx = screenCoord[j].x - u.x;
double dy = screenCoord[j].y - u.y;
err1 += dx * dx + dy * dy;
du[j].x = dx;
du[j].y = dy;
}
err1 /= num;
if (err1 < breakLoopErrorThresh)
{
break;
}
if ((i > 0) && (err1 < breakLoopErrorThresh2) && (err1 / err0 > breakLoopErrorRatioThresh))
{
break;
}
if (i == maxLoop)
{
break;
}
err0 = err1;
jus.clear();
for (int j = 0; j < num; j++)
{
if (!jus.push(this._ref_matXc2U, o_matxw2xc, worldCoord[j], du[j], 1.0))
{
return(false);
}
}
if (!dS.setJusArray(jus))
{
return(false);
}
dS.makeMat(o_matxw2xc);
}
if (o_result_param != null)
{
o_result_param.last_error = err1;
}
// *err = err1;
return(true);
}
