static public void Test() { // generate x_i, y_i observations on test function var random = new Random(); int n = 200; var X = new Matrix(n, 1); var Y = new Matrix(n, 1); { double a = 100; double b = 102; for (int i = 0; i < n; i++) { double x = random.NextDouble() / (Math.PI / 4.0) - Math.PI / 8.0; double y = a * Math.Cos(b * x) + b * Math.Sin(a * x) + random.NextDouble() * 0.1; X[i] = x; Y[i] = y; } } Function f = delegate(Matrix parameters) { // return y_i - f(x_i, parameters) as column vector var error = new Matrix(n, 1); double a = parameters[0]; double b = parameters[1]; for (int i = 0; i < n; i++) { double y = a * Math.Cos(b * X[i]) + b * Math.Sin(a * X[i]); error[i] = Y[i] - y; } return(error); }; var levenbergMarquardt = new LevenbergMarquardt(f); var parameters0 = new Matrix(2, 1); parameters0[0] = 90; parameters0[1] = 96; var rmsError = levenbergMarquardt.Minimize(parameters0); }
static public void Test() { // generate x_i, y_i observations on test function var random = new Random(); int n = 200; var X = new Matrix(n, 1); var Y = new Matrix(n, 1); { double a = 100; double b = 102; for (int i = 0; i < n; i++) { double x = random.NextDouble() / (Math.PI / 4.0) - Math.PI / 8.0; double y = a * Math.Cos(b * x) + b * Math.Sin(a * x) + random.NextDouble()*0.1; X[i] = x; Y[i] = y; } } Function f = delegate(Matrix parameters) { // return y_i - f(x_i, parameters) as column vector var error = new Matrix(n, 1); double a = parameters[0]; double b = parameters[1]; for (int i = 0; i < n; i++) { double y = a * Math.Cos(b * X[i]) + b * Math.Sin(a * X[i]); error[i] = Y[i] - y; } return error; }; var levenbergMarquardt = new LevenbergMarquardt(f); var parameters0 = new Matrix(2, 1); parameters0[0] = 90; parameters0[1] = 96; var rmsError = levenbergMarquardt.Minimize(parameters0); }
public static double CalibrateCameraExtrinsicsOnly(List<List<Matrix>> worldPointSets, List<List<System.Drawing.PointF>> imagePointSets, Matrix cameraMatrix, ref List<Matrix> rotations, ref List<Matrix> translations) { int nSets = worldPointSets.Count; int nPoints = 0; for (int i = 0; i < nSets; i++) nPoints += worldPointSets[i].Count; // for later var distCoeffs = Matrix.Zero(2, 1); //// if necessary run DLT on each point set to get initial rotation and translations //if (rotations == null) //{ // rotations = new List<Matrix>(); // translations = new List<Matrix>(); // for (int i = 0; i < nSets; i++) // { // Matrix R, t; // CameraMath.DLT(cameraMatrix, distCoeffs, worldPointSets[i], imagePointSets[i], out R, out t); // var r = CameraMath.RotationVectorFromRotationMatrix(R); // rotations.Add(r); // translations.Add(t); // } //} // Levenberg-Marquardt for camera matrix (ignore lens distortion for now) // pack parameters into vector // parameters: camera has f, cx, cy; each point set has rotation + translation (6) //int nParameters = 3 + 6 * nSets; int nParameters = 6 * nSets; var parameters = new Matrix(nParameters, 1); { int pi = 0; //parameters[pi++] = cameraMatrix[0, 0]; // f //parameters[pi++] = cameraMatrix[0, 2]; // cx //parameters[pi++] = cameraMatrix[1, 2]; // cy for (int i = 0; i < nSets; i++) { parameters[pi++] = rotations[i][0]; parameters[pi++] = rotations[i][1]; parameters[pi++] = rotations[i][2]; parameters[pi++] = translations[i][0]; parameters[pi++] = translations[i][1]; parameters[pi++] = translations[i][2]; } } // size of our error vector int nValues = nPoints * 2; // each component (x,y) is a separate entry LevenbergMarquardt.Function function = delegate (Matrix p) { var fvec = new Matrix(nValues, 1); // unpack parameters int pi = 0; //double f = p[pi++]; //double cx = p[pi++]; //double cy = p[pi++]; var K = Matrix.Identity(3, 3); //K[0, 0] = f; //K[1, 1] = f; //K[0, 2] = cx; //K[1, 2] = cy; K[0, 0] = cameraMatrix[0, 0]; K[1, 1] = cameraMatrix[1, 1]; K[0, 2] = cameraMatrix[0, 2]; K[1, 2] = cameraMatrix[1, 2]; var d = Matrix.Zero(2, 1); int fveci = 0; for (int i = 0; i < nSets; i++) { var rotation = new Matrix(3, 1); rotation[0] = p[pi++]; rotation[1] = p[pi++]; rotation[2] = p[pi++]; var R = RotationMatrixFromRotationVector(rotation); var t = new Matrix(3, 1); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; var worldPoints = worldPointSets[i]; var imagePoints = imagePointSets[i]; var x = new Matrix(3, 1); for (int j = 0; j < worldPoints.Count; j++) { // transform world point to local camera coordinates x.Mult(R, worldPoints[j]); x.Add(t); // fvec_i = y_i - f(x_i) double u, v; CameraMath.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[j]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } } return fvec; }; // optimize var calibrate = new LevenbergMarquardt(function); calibrate.minimumReduction = 1.0e-4; calibrate.Minimize(parameters); //while (calibrate.State == LevenbergMarquardt.States.Running) //{ // var rmsError = calibrate.MinimizeOneStep(parameters); // Console.WriteLine("rms error = " + rmsError); //} //for (int i = 0; i < nParameters; i++) // Console.WriteLine(parameters[i] + "\t"); //Console.WriteLine(); // unpack parameters { int pi = 0; //double f = parameters[pi++]; //double cx = parameters[pi++]; //double cy = parameters[pi++]; //cameraMatrix[0, 0] = f; //cameraMatrix[1, 1] = f; //cameraMatrix[0, 2] = cx; //cameraMatrix[1, 2] = cy; for (int i = 0; i < nSets; i++) { rotations[i][0] = parameters[pi++]; rotations[i][1] = parameters[pi++]; rotations[i][2] = parameters[pi++]; translations[i][0] = parameters[pi++]; translations[i][1] = parameters[pi++]; translations[i][2] = parameters[pi++]; } } return calibrate.RMSError; }
public static double CalibrateCameraExtrinsicsOnly(List <List <Matrix> > worldPointSets, List <List <System.Drawing.PointF> > imagePointSets, Matrix cameraMatrix, ref List <Matrix> rotations, ref List <Matrix> translations) { int nSets = worldPointSets.Count; int nPoints = 0; for (int i = 0; i < nSets; i++) { nPoints += worldPointSets[i].Count; // for later } var distCoeffs = Matrix.Zero(2, 1); //// if necessary run DLT on each point set to get initial rotation and translations //if (rotations == null) //{ // rotations = new List<Matrix>(); // translations = new List<Matrix>(); // for (int i = 0; i < nSets; i++) // { // Matrix R, t; // CameraMath.DLT(cameraMatrix, distCoeffs, worldPointSets[i], imagePointSets[i], out R, out t); // var r = CameraMath.RotationVectorFromRotationMatrix(R); // rotations.Add(r); // translations.Add(t); // } //} // Levenberg-Marquardt for camera matrix (ignore lens distortion for now) // pack parameters into vector // parameters: camera has f, cx, cy; each point set has rotation + translation (6) //int nParameters = 3 + 6 * nSets; int nParameters = 6 * nSets; var parameters = new Matrix(nParameters, 1); { int pi = 0; //parameters[pi++] = cameraMatrix[0, 0]; // f //parameters[pi++] = cameraMatrix[0, 2]; // cx //parameters[pi++] = cameraMatrix[1, 2]; // cy for (int i = 0; i < nSets; i++) { parameters[pi++] = rotations[i][0]; parameters[pi++] = rotations[i][1]; parameters[pi++] = rotations[i][2]; parameters[pi++] = translations[i][0]; parameters[pi++] = translations[i][1]; parameters[pi++] = translations[i][2]; } } // size of our error vector int nValues = nPoints * 2; // each component (x,y) is a separate entry LevenbergMarquardt.Function function = delegate(Matrix p) { var fvec = new Matrix(nValues, 1); // unpack parameters int pi = 0; //double f = p[pi++]; //double cx = p[pi++]; //double cy = p[pi++]; var K = Matrix.Identity(3, 3); //K[0, 0] = f; //K[1, 1] = f; //K[0, 2] = cx; //K[1, 2] = cy; K[0, 0] = cameraMatrix[0, 0]; K[1, 1] = cameraMatrix[1, 1]; K[0, 2] = cameraMatrix[0, 2]; K[1, 2] = cameraMatrix[1, 2]; var d = Matrix.Zero(2, 1); int fveci = 0; for (int i = 0; i < nSets; i++) { var rotation = new Matrix(3, 1); rotation[0] = p[pi++]; rotation[1] = p[pi++]; rotation[2] = p[pi++]; var R = RotationMatrixFromRotationVector(rotation); var t = new Matrix(3, 1); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; var worldPoints = worldPointSets[i]; var imagePoints = imagePointSets[i]; var x = new Matrix(3, 1); for (int j = 0; j < worldPoints.Count; j++) { // transform world point to local camera coordinates x.Mult(R, worldPoints[j]); x.Add(t); // fvec_i = y_i - f(x_i) double u, v; CameraMath.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[j]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); calibrate.minimumReduction = 1.0e-4; calibrate.Minimize(parameters); //while (calibrate.State == LevenbergMarquardt.States.Running) //{ // var rmsError = calibrate.MinimizeOneStep(parameters); // Console.WriteLine("rms error = " + rmsError); //} //for (int i = 0; i < nParameters; i++) // Console.WriteLine(parameters[i] + "\t"); //Console.WriteLine(); // unpack parameters { int pi = 0; //double f = parameters[pi++]; //double cx = parameters[pi++]; //double cy = parameters[pi++]; //cameraMatrix[0, 0] = f; //cameraMatrix[1, 1] = f; //cameraMatrix[0, 2] = cx; //cameraMatrix[1, 2] = cy; for (int i = 0; i < nSets; i++) { rotations[i][0] = parameters[pi++]; rotations[i][1] = parameters[pi++]; rotations[i][2] = parameters[pi++]; translations[i][0] = parameters[pi++]; translations[i][1] = parameters[pi++]; translations[i][2] = parameters[pi++]; } } return(calibrate.RMSError); }