public void TestOptimization() { // generate x_i, y_i observations on test function var random = new Random(); int n = 200; var matX = Vector <double> .Build.Dense(n); var matY = Vector <double> .Build.Dense(n); 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); matX[i] = x; matY[i] = y; } LevenbergMarquardt.Function f = (Vector <double> parameters) => { // return y_i - f(x_i, parameters) as column vector var error = Vector <double> .Build.Dense(n); double a2 = parameters[0]; double b2 = parameters[1]; for (int i = 0; i < n; i++) { double y = (a2 * Math.Cos(b2 * matX[i])) + (b2 * Math.Sin(a2 * matX[i])); error[i] = matY[i] - y; } return(error); }; var levenbergMarquardt = new LevenbergMarquardt(f); var parameters0 = Vector <double> .Build.Dense(2); parameters0[0] = 90; parameters0[1] = 96; var rmsError = levenbergMarquardt.Minimize(parameters0); }
static double CalibrateColorCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs, Matrix rotation, Matrix translation) { int nPoints = worldPoints.Count; { Matrix R, t; CameraMath.DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t); //var r = Orientation.RotationVector(R); var r = RoomAliveToolkit.ProjectorCameraEnsemble.RotationVectorFromRotationMatrix(R); rotation.Copy(r); translation.Copy(t); } // pack parameters into vector // parameters: fx, fy, cx, cy, k1, k2, + 3 for rotation, 3 translation = 12 int nParameters = 12; var parameters = new Matrix(nParameters, 1); { int pi = 0; parameters[pi++] = cameraMatrix[0, 0]; // fx parameters[pi++] = cameraMatrix[1, 1]; // fy parameters[pi++] = cameraMatrix[0, 2]; // cx parameters[pi++] = cameraMatrix[1, 2]; // cy parameters[pi++] = distCoeffs[0]; // k1 parameters[pi++] = distCoeffs[1]; // k2 parameters[pi++] = rotation[0]; parameters[pi++] = rotation[1]; parameters[pi++] = rotation[2]; parameters[pi++] = translation[0]; parameters[pi++] = translation[1]; parameters[pi++] = translation[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 fx = p[pi++]; double fy = p[pi++]; double cx = p[pi++]; double cy = p[pi++]; double k1 = p[pi++]; double k2 = p[pi++]; var K = Matrix.Identity(3, 3); K[0, 0] = fx; K[1, 1] = fy; K[0, 2] = cx; K[1, 2] = cy; var d = Matrix.Zero(5, 1); d[0] = k1; d[1] = k2; var r = new Matrix(3, 1); r[0] = p[pi++]; r[1] = p[pi++]; r[2] = p[pi++]; var t = new Matrix(3, 1); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; //var R = Orientation.Rodrigues(r); var R = RoomAliveToolkit.ProjectorCameraEnsemble.RotationMatrixFromRotationVector(r); var x = new Matrix(3, 1); int fveci = 0; for (int i = 0; i < worldPoints.Count; i++) { // transform world point to local camera coordinates x.Mult(R, worldPoints[i]); 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[i]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); 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 fx = parameters[pi++]; double fy = parameters[pi++]; double cx = parameters[pi++]; double cy = parameters[pi++]; double k1 = parameters[pi++]; double k2 = parameters[pi++]; cameraMatrix[0, 0] = fx; cameraMatrix[1, 1] = fy; cameraMatrix[0, 2] = cx; cameraMatrix[1, 2] = cy; distCoeffs[0] = k1; distCoeffs[1] = k2; rotation[0] = parameters[pi++]; rotation[1] = parameters[pi++]; rotation[2] = parameters[pi++]; translation[0] = parameters[pi++]; translation[1] = parameters[pi++]; translation[2] = parameters[pi++]; } return(calibrate.RMSError); }
static double CalibrateDepthCamera(List <Matrix> worldPoints, List <System.Drawing.PointF> imagePoints, Matrix cameraMatrix, Matrix distCoeffs) { int nPoints = worldPoints.Count; // pack parameters into vector // parameters: fx, fy, cx, cy, k1, k2 = 6 parameters int nParameters = 6; var parameters = new Matrix(nParameters, 1); { int pi = 0; parameters[pi++] = cameraMatrix[0, 0]; // fx parameters[pi++] = cameraMatrix[1, 1]; // fy parameters[pi++] = cameraMatrix[0, 2]; // cx parameters[pi++] = cameraMatrix[1, 2]; // cy parameters[pi++] = distCoeffs[0]; // k1 parameters[pi++] = distCoeffs[1]; // k2 } // 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 fx = p[pi++]; double fy = p[pi++]; double cx = p[pi++]; double cy = p[pi++]; double k1 = p[pi++]; double k2 = p[pi++]; var K = Matrix.Identity(3, 3); K[0, 0] = fx; K[1, 1] = fy; K[0, 2] = cx; K[1, 2] = cy; var d = Matrix.Zero(5, 1); d[0] = k1; d[1] = k2; int fveci = 0; for (int i = 0; i < worldPoints.Count; i++) { // fvec_i = y_i - f(x_i) double u, v; var x = worldPoints[i]; CameraMath.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[i]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); 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 fx = parameters[pi++]; double fy = parameters[pi++]; double cx = parameters[pi++]; double cy = parameters[pi++]; double k1 = parameters[pi++]; double k2 = parameters[pi++]; cameraMatrix[0, 0] = fx; cameraMatrix[1, 1] = fy; cameraMatrix[0, 2] = cx; cameraMatrix[1, 2] = cy; distCoeffs[0] = k1; distCoeffs[1] = k2; } return(calibrate.RMSError); }
private static double CalibrateColorCamera(List <Vector <double> > worldPoints, List <System.Drawing.PointF> imagePoints, Matrix <double> cameraMatrix, Vector <double> distCoeffs, Vector <double> rotation, Vector <double> translation, bool silent = true) { int nPoints = worldPoints.Count; { Matrix <double> R; Vector <double> t; DLT(cameraMatrix, distCoeffs, worldPoints, imagePoints, out R, out t); var r = RotationExtensions.MatrixToAxisAngle(R); r.CopyTo(rotation); t.CopyTo(translation); } // pack parameters into vector // parameters: fx, fy, cx, cy, k1, k2, + 3 for rotation, 3 translation = 12 int nParameters = 12; var parameters = Vector <double> .Build.Dense(nParameters); { int pi = 0; parameters[pi++] = cameraMatrix[0, 0]; // fx parameters[pi++] = cameraMatrix[1, 1]; // fy parameters[pi++] = cameraMatrix[0, 2]; // cx parameters[pi++] = cameraMatrix[1, 2]; // cy parameters[pi++] = distCoeffs[0]; // k1 parameters[pi++] = distCoeffs[1]; // k2 parameters[pi++] = rotation[0]; parameters[pi++] = rotation[1]; parameters[pi++] = rotation[2]; parameters[pi++] = translation[0]; parameters[pi++] = translation[1]; parameters[pi++] = translation[2]; } // size of our error vector int nValues = nPoints * 2; // each component (x,y) is a separate entry LevenbergMarquardt.Function function = delegate(Vector <double> p) { var fvec = Vector <double> .Build.Dense(nValues); // unpack parameters int pi = 0; double fx = p[pi++]; double fy = p[pi++]; double cx = p[pi++]; double cy = p[pi++]; double k1 = p[pi++]; double k2 = p[pi++]; var K = Matrix <double> .Build.DenseIdentity(3, 3); K[0, 0] = fx; K[1, 1] = fy; K[0, 2] = cx; K[1, 2] = cy; var d = Vector <double> .Build.Dense(5, 0); d[0] = k1; d[1] = k2; var r = Vector <double> .Build.Dense(3); r[0] = p[pi++]; r[1] = p[pi++]; r[2] = p[pi++]; var t = Vector <double> .Build.Dense(3); t[0] = p[pi++]; t[1] = p[pi++]; t[2] = p[pi++]; var R = RotationExtensions.AxisAngleToMatrix(r); int fveci = 0; for (int i = 0; i < worldPoints.Count; i++) { // transform world point to local camera coordinates var x = R * worldPoints[i]; x += t; // fvec_i = y_i - f(x_i) double u, v; KinectInternalCalibration.Project(K, d, x[0], x[1], x[2], out u, out v); var imagePoint = imagePoints[i]; fvec[fveci++] = imagePoint.X - u; fvec[fveci++] = imagePoint.Y - v; } return(fvec); }; // optimize var calibrate = new LevenbergMarquardt(function); while (calibrate.State == LevenbergMarquardt.States.Running) { var rmsError = calibrate.MinimizeOneStep(parameters); if (!silent) { Console.WriteLine("rms error = " + rmsError); } } if (!silent) { for (int i = 0; i < nParameters; i++) { Console.WriteLine(parameters[i] + "\t"); } Console.WriteLine(); } // unpack parameters { int pi = 0; double fx = parameters[pi++]; double fy = parameters[pi++]; double cx = parameters[pi++]; double cy = parameters[pi++]; double k1 = parameters[pi++]; double k2 = parameters[pi++]; cameraMatrix[0, 0] = fx; cameraMatrix[1, 1] = fy; cameraMatrix[0, 2] = cx; cameraMatrix[1, 2] = cy; distCoeffs[0] = k1; distCoeffs[1] = k2; rotation[0] = parameters[pi++]; rotation[1] = parameters[pi++]; rotation[2] = parameters[pi++]; translation[0] = parameters[pi++]; translation[1] = parameters[pi++]; translation[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); }