int main(int argc, char** argv){
std::string help =
"Locates the 3D position of a sound source\n"
"Arguments: <timestamp1> <timestamp2> <timestamp3> <timestamp4>\n"
"Note: \n\ttimestamps must be in the correct order to obtain meaningful result\n";
if(std::strcmp(argv[1], "-h") == 0){
std::cout << help << std::endl;
} else if(argc < 5){
std::cout << "Usage:\n" << argv[0]
<< " <timestamp1> <timestamp2> <timestamp3> <timestamp4>"
<< std::endl;
}
double ts1 = std::atof(argv[1]);
double ts2 = std::atof(argv[2]);
double ts3 = std::atof(argv[3]);
double ts4 = std::atof(argv[4]);
const double initial_x = 10;
const double initial_y = 0;
const double initial_z = 0;
const double initial_t = ts1;
double x = initial_x;
double y = initial_y;
double z = initial_z;
double t = initial_t;
Problem problem;
CostFunction* h1cost = new AutoDiffCostFunction<Hydrophone1Cost ,1 ,1, 1, 1, 1>(new Hydrophone1Cost(ts1));
CostFunction* h2cost = new AutoDiffCostFunction<Hydrophone2Cost ,1 ,1, 1, 1, 1>(new Hydrophone2Cost(ts2));
CostFunction* h3cost = new AutoDiffCostFunction<Hydrophone3Cost ,1 ,1, 1, 1, 1>(new Hydrophone3Cost(ts3));
CostFunction* h4cost = new AutoDiffCostFunction<Hydrophone4Cost ,1 ,1, 1, 1, 1>(new Hydrophone4Cost(ts4));
problem.AddResidualBlock(h1cost, NULL, &x, &y, &z, &t);
problem.AddResidualBlock(h2cost, NULL, &x, &y, &z, &t);
problem.AddResidualBlock(h3cost, NULL, &x, &y, &z, &t);
problem.AddResidualBlock(h4cost, NULL, &x, &y, &z, &t);
Solver::Options options;
options.max_num_iterations = 100;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
std::cout << "Initial x = " << x
<< ", y = " << y
<< ", z = " << z
<< ", t = " << t
<< "\n";
// Run the solver!
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
std::cout << "Final x = " << x
<< ", y = " << y
<< ", z = " << z
<< ", t = " << t
<< "\n";
return 0;
}
int main(int argc, char *argv[]) {
FLAGS_log_dir = "logs/";
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
dataset::dataSet<double> data;
data.residual_type = "quadratic";
data.numPoints = 100;
data.range["begin"] = -5.0;
data.range["end"] = 5.0;
double A = 1.0;
double B = 0.0;
double C = 0.0;
double D = 1.0;
double E = 0.0;
dataset::makeSet(&data, {&A, &B, &C});
plot::plotData(&data);
double Ap = 3.45;
Problem problem;
for (int i = 0; i < 100; i++){
double x = data.xdata[i];
double y = data.ydata[i];
double r = 0.0;
CostFunction* cost = residual<double>::Create(x, y, "quadratic");
problem.AddResidualBlock(cost, NULL, &Ap, &B, &C);
}
Solver::Options options;
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
return 0;
}
double CeresSolverBase::launchProfiledSolveAndSummary(const std::unique_ptr<ceres::Solver::Options>& options, ceres::Problem* problem, bool profileSolve, std::vector<SolverIteration>& iters) {
Solver::Summary summary;
double elapsedTime;
{
ml::Timer timer;
Solve(*options, problem, &summary);
elapsedTime = timer.getElapsedTimeMS();
}
cout << "Solver used: " << summary.linear_solver_type_used << endl;
cout << "Minimizer iters: " << summary.iterations.size() << endl;
cout << "Total time: " << elapsedTime << "ms" << endl;
double iterationTotalTime = 0.0;
int totalLinearItereations = 0;
for (auto &i : summary.iterations)
{
iterationTotalTime += i.iteration_time_in_seconds;
totalLinearItereations += i.linear_solver_iterations;
cout << "Iteration: " << i.linear_solver_iterations << " " << i.iteration_time_in_seconds * 1000.0 << "ms," << " cost: " << i.cost << endl;
}
if (profileSolve) {
for (auto &i : summary.iterations) {
iters.push_back(SolverIteration(i.cost, i.iteration_time_in_seconds * 1000.0));
}
}
cout << "Total iteration time: " << iterationTotalTime << endl;
cout << "Cost per linear solver iteration: " << iterationTotalTime * 1000.0 / totalLinearItereations << "ms" << endl;
double cost = -1.0;
problem->Evaluate(Problem::EvaluateOptions(), &cost, nullptr, nullptr, nullptr);
cout << "Cost end: " << cost << endl;
cout << summary.FullReport() << endl;
return cost;
}
bool solve_translations_problem_l2_chordal
(
const int* edges,
const double* poses,
const double* weights,
int num_edges,
double loss_width,
double* X,
double function_tolerance,
double parameter_tolerance,
int max_iterations
)
{
// seed the random number generator
std::srand( std::time( NULL ) );
// re index the edges to be a sequential set
std::vector<int> reindexed_edges(edges, edges+2*num_edges);
std::vector<int> reindexed_lookup;
reindex_problem(&reindexed_edges[0], num_edges, reindexed_lookup);
const int num_nodes = reindexed_lookup.size();
// Init with a random guess solution
std::vector<double> x(3*num_nodes);
for (int i=0; i<3*num_nodes; ++i)
x[i] = (double)rand() / RAND_MAX;
// add the parameter blocks (a 3-vector for each node)
Problem problem;
for (int i=0; i<num_nodes; ++i)
problem.AddParameterBlock(&x[3*i], 3);
// set the residual function (chordal distance for each edge)
for (int i=0; i<num_edges; ++i) {
CostFunction* cost_function =
new AutoDiffCostFunction<ChordFunctor, 3, 3, 3>(
new ChordFunctor(poses+3*i, weights[i]));
if (loss_width == 0.0) {
// No robust loss function
problem.AddResidualBlock(cost_function, NULL, &x[3*reindexed_edges[2*i+0]], &x[3*reindexed_edges[2*i+1]]);
} else {
problem.AddResidualBlock(cost_function, new ceres::HuberLoss(loss_width), &x[3*reindexed_edges[2*i+0]], &x[3*reindexed_edges[2*i+1]]);
}
}
// Fix first camera in {0,0,0}: fix the translation ambiguity
x[0] = x[1] = x[2] = 0.0;
problem.SetParameterBlockConstant(&x[0]);
// solve
Solver::Options options;
#ifdef OPENMVG_USE_OPENMP
options.num_threads = omp_get_max_threads();
options.num_linear_solver_threads = omp_get_max_threads();
#endif // OPENMVG_USE_OPENMP
options.minimizer_progress_to_stdout = false;
options.logging_type = ceres::SILENT;
options.max_num_iterations = max_iterations;
options.function_tolerance = function_tolerance;
options.parameter_tolerance = parameter_tolerance;
// Since the problem is sparse, use a sparse solver iff available
if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE))
{
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
}
else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE))
{
options.sparse_linear_algebra_library_type = ceres::CX_SPARSE;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
}
else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::EIGEN_SPARSE))
{
options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
}
else
{
options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
}
Solver::Summary summary;
Solve(options, &problem, &summary);
std::cout << summary.FullReport() << "\n";
if (summary.IsSolutionUsable())
{
// undo the re indexing
for (int i=0; i<num_nodes; ++i) {
const int j = reindexed_lookup[i];
X[3*j+0] = x[3*i+0];
X[3*j+1] = x[3*i+1];
X[3*j+2] = x[3*i+2];
}
}
return summary.IsSolutionUsable();
}