int main( int argc, char** argv )
{
google::InitGoogleLogging(argv[0]);
if ((argc < 3) || (argc > 4)) {
std::cerr << "usage: gettruth <measurement_file> <camera file> [<output file>]\n";
return 1;
}
FILE* file;
if (argc == 3) {
file = stdout;
} else {
file = fopen(argv[3], "w");
}
// get camera model
calibu::Rig<double> rig;
calibu::LoadRig( std::string(argv[2]), &rig );
if (rig.cameras_.size() == 0) {
fprintf(stderr, "No cameras in this rig or no camera file provided\n");
exit(0);
}
calibu::CameraInterface<double>* cam = rig.cameras_[0];
LocalizationProblem ctx;
if (!ctx.LoadFile(argv[1])) {
std::cerr << "ERROR: unable to open file " << argv[1] << "\n";
return 1;
}
// Build the problem.
Problem problem;
for (int i = 0; i < ctx.num_observations(); ++i) {
const Vector2d& measurement = ctx.observation(i);
const Vector3d& landmark = ctx.landmark_for_observation(i);
ceres::CostFunction* cost_function = ProjectionCost( landmark, measurement, cam );
problem.AddResidualBlock( cost_function, NULL, ctx.pose_data_for_observation(i) );
}
fprintf(stdout, "\n\nThe problem has been set up with %d residuals and %d residual blocks\n", problem.NumResiduals(), problem.NumResidualBlocks());
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.minimizer_progress_to_stdout = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
for (int ii = 0; ii < ctx.num_observations(); ii++) {
fprintf(file, "%f, %f, %f, %f, %f, %f\n", ii + 1, ctx.pose_for_observation(ii)(0), ctx.pose_for_observation(ii)(1),
ctx.pose_for_observation(ii)(2), ctx.pose_for_observation(ii)(3), ctx.pose_for_observation(ii)(4), ctx.pose_for_observation(ii)(5));
}
fclose(file);
return 0;
}
