int main(int argc, char** argv) { int numPoints; int maxIterations; bool verbose; std::vector<int> gaugeList; g2o::CommandArgs arg; arg.param("numPoints", numPoints, 100, "number of points sampled from the circle"); arg.param("i", maxIterations, 10, "perform n iterations"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.parseArgs(argc, argv); // generate random data Eigen::Vector2d center(4, 2); double radius = 2.; Eigen::Vector2d* points = new Eigen::Vector2d[numPoints]; for (int i = 0; i < numPoints; ++i) { double r = g2o::Sampler::uniformRand(radius-0.1, radius+0.1); double angle = g2o::Sampler::uniformRand(0., 2. * M_PI); points[i].x() = center.x() + r * cos(angle); points[i].y() = center.y() + r * sin(angle); } // some handy typedefs typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver; typedef g2o::LinearSolverCSparse<MyBlockSolver::PoseMatrixType> MyLinearSolver; // setup the solver g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); MyLinearSolver* linearSolver = new MyLinearSolver(); MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); optimizer.setAlgorithm(solver); // build the optimization problem given the points // 1. add the circle vertex VertexCircle* circle = new VertexCircle(); circle->setId(0); circle->setEstimate(Eigen::Vector3d(3,3,3)); // some initial value for the circle optimizer.addVertex(circle); // 2. add the points we measured for (int i = 0; i < numPoints; ++i) { EdgePointOnCircle* e = new EdgePointOnCircle; e->setInformation(Eigen::Matrix<double, 1, 1>::Identity()); e->setVertex(0, circle); e->setMeasurement(points[i]); optimizer.addEdge(e); } // perform the optimization optimizer.initializeOptimization(); optimizer.setVerbose(verbose); optimizer.optimize(maxIterations); if (verbose) cout << endl; // print out the result cout << "Iterative least squares solution" << endl; cout << "center of the circle " << circle->estimate().head<2>().transpose() << endl; cout << "radius of the cirlce " << circle->estimate()(2) << endl; cout << "error " << errorOfSolution(numPoints, points, circle->estimate()) << endl; cout << endl; // solve by linear least squares // Let (a, b) be the center of the circle and r the radius of the circle. // For a point (x, y) on the circle we have: // (x - a)^2 + (y - b)^2 = r^2 // This leads to // (-2x -2y 1)^T * (a b c) = -x^2 - y^2 (1) // where c = a^2 + b^2 - r^2. // Since we have a bunch of points, we accumulate Eqn (1) in a matrix and // compute the normal equation to obtain a solution for (a b c). // Afterwards the radius r is recovered. Eigen::MatrixXd A(numPoints, 3); Eigen::VectorXd b(numPoints); for (int i = 0; i < numPoints; ++i) { A(i, 0) = -2*points[i].x(); A(i, 1) = -2*points[i].y(); A(i, 2) = 1; b(i) = -pow(points[i].x(), 2) - pow(points[i].y(), 2); } Eigen::Vector3d solution = (A.transpose()*A).ldlt().solve(A.transpose() * b); // calculate the radius of the circle given the solution so far solution(2) = sqrt(pow(solution(0), 2) + pow(solution(1), 2) - solution(2)); cout << "Linear least squares solution" << endl; cout << "center of the circle " << solution.head<2>().transpose() << endl; cout << "radius of the cirlce " << solution(2) << endl; cout << "error " << errorOfSolution(numPoints, points, solution) << endl; // clean up delete[] points; return 0; }
int main(int argc, char** argv) { int numPoints; int maxIterations; bool verbose; std::vector<int> gaugeList; g2o::CommandArgs arg; arg.param("numPoints", numPoints, 100, "number of points sampled from the circle"); arg.param("i", maxIterations, 10, "perform n iterations"); arg.param("v", verbose, false, "verbose output of the optimization process"); arg.parseArgs(argc, argv); // generate random data Eigen::Vector2d center(4, 2); double radius = 2.; Eigen::Vector2d* points = new Eigen::Vector2d[numPoints]; for (int i = 0; i < numPoints; ++i) { double r = g2o::sampleUniform(radius-0.1, radius+0.1); double angle = g2o::sampleUniform(0., 2. * M_PI); points[i].x() = center.x() + r * cos(angle); points[i].y() = center.y() + r * sin(angle); } // some handy typedefs typedef g2o::BlockSolver< g2o::BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > MyBlockSolver; typedef g2o::LinearSolverCSparse<MyBlockSolver::PoseMatrixType> MyLinearSolver; // setup the solver g2o::SparseOptimizer optimizer; optimizer.setVerbose(false); MyLinearSolver* linearSolver = new MyLinearSolver(); MyBlockSolver* solver_ptr = new MyBlockSolver(linearSolver); g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr); //g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr); optimizer.setAlgorithm(solver); // build the optimization problem given the points // 1. add the circle vertex VertexCircle* circle = new VertexCircle(); circle->setId(0); circle->setEstimate(Eigen::Vector3d(3,3,3)); // some initial value for the circle optimizer.addVertex(circle); // 2. add the points we measured for (int i = 0; i < numPoints; ++i) { EdgePointOnCircle* e = new EdgePointOnCircle; e->setInformation(Eigen::Matrix<double, 1, 1>::Identity()); e->setVertex(0, circle); e->setMeasurement(points[i]); optimizer.addEdge(e); } // perform the optimization optimizer.initializeOptimization(); optimizer.setVerbose(verbose); optimizer.optimize(maxIterations); // print out the result cout << "Estimated center of the circle " << circle->estimate().head<2>().transpose() << endl; cout << "Estimated radius of the cirlce " << circle->estimate()(2) << endl; // clean up delete[] points; return 0; }