public static void ReprojectionError(
Image <Arthmetic, double> estReal, List <PointF> img,
Image <Arthmetic, double> K, Image <Arthmetic, double> R, Image <Arthmetic, double> t,
out double mean, out double median, out List <double> errors)
{
var P = ComputeMatrix.Camera(K, R, t);
var estImg = P.Multiply(estReal);
errors = new List <double>();
for (int i = 0; i < img.Count; ++i)
{
var estPoint = new Image <Arthmetic, double>(new double[, , ]
{
{ { estImg[0, i] / estImg[2, i] } }, { { estImg[1, i] / estImg[2, i] } },
});
var realPoint = new Image <Arthmetic, double>(new double[, , ]
{
{ { img[i].X } }, { { img[i].Y } },
});
errors.Add(estPoint.Sub(realPoint).Norm);
}
mean = errors.Sum() / errors.Count;
median = errors[errors.Count / 2];
}
public static int TriangulateChieral(
List <PointF> left, List <PointF> right, Image <Arthmetic, double> K,
Image <Arthmetic, double> R, Image <Arthmetic, double> t,
out Image <Arthmetic, double> pts3d)
{
// init 3d point matrix
pts3d = new Image <Arthmetic, double>(left.Count, 4);
// init projection matrices
var P1 = ComputeMatrix.Camera(K);
var P2 = ComputeMatrix.Camera(K, R, t);
// Transform points lists into matrices
var img1 = new Image <Arthmetic, double>(left.Count, 2);
var img2 = new Image <Arthmetic, double>(left.Count, 2);
for (int i = 0; i < left.Count; ++i)
{
img1[0, i] = left[i].X;
img1[1, i] = left[i].Y;
img2[0, i] = right[i].X;
img2[1, i] = right[i].Y;
}
CvInvoke.TriangulatePoints(P1, P2, img1, img2, pts3d);
// Scale points, so that W = 1
for (int i = 0; i < left.Count; ++i)
{
pts3d[0, i] = pts3d[0, i] / pts3d[3, i];
pts3d[1, i] = pts3d[1, i] / pts3d[3, i];
pts3d[2, i] = pts3d[2, i] / pts3d[3, i];
pts3d[3, i] = pts3d[3, i] / pts3d[3, i];
}
// compute points in front of camera (TODO: how does it work?)
var AX1 = P1.Multiply(pts3d);
var BX1 = P2.Multiply(pts3d);
int num = 0;
for (int i = 0; i < left.Count; i++)
{
if (AX1[2, i] * pts3d[3, i] > 0 && BX1[2, i] * pts3d[3, i] > 0)
{
num++;
}
}
return(num);
}
private void ResetPoints()
{
P1 = ComputeMatrix.Camera(K);
P2 = ComputeMatrix.Camera(K, R12, C2);
P3 = ComputeMatrix.Camera(K, R13, C3);
ptsReal = new List <Image <Arthmetic, double> >();
pts1Ref = new List <PointF>();
pts2Ref = new List <PointF>();
pts3Ref = new List <PointF>();
Random rand = new Random(564073);
for (int i = 0; i < pointsCount; ++i)
{
var real = new Image <Arthmetic, double>(new double[, , ] {
{ { rand.Next(100, 200) } },
{ { rand.Next(100, 200) } },
{ { rand.Next(50, 100) } },
{ { 1 } },
});
ptsReal.Add(real);
var i1 = P1.Multiply(real).ToPointF();
pts1Ref.Add(i1);
var i2 = P2.Multiply(real).ToPointF();
pts2Ref.Add(i2);
var i3 = P3.Multiply(real).ToPointF();
pts3Ref.Add(i3);
}
double rangeLx = pts1Ref.Max((x) => x.X) - pts1Ref.Min((x) => x.X);
double rangeLy = pts1Ref.Max((x) => x.Y) - pts1Ref.Min((x) => x.Y);
double rangeRx = pts2Ref.Max((x) => x.X) - pts2Ref.Min((x) => x.X);
double rangeRy = pts2Ref.Max((x) => x.Y) - pts2Ref.Min((x) => x.Y);
pixelRange = 0.25 * (rangeLx + rangeLy + rangeRx + rangeRy);
}
