Skip to content

Instantly share code, notes, and snippets.

@system123
Created March 3, 2014 16:26
Show Gist options
  • Save system123/9328646 to your computer and use it in GitHub Desktop.
Save system123/9328646 to your computer and use it in GitHub Desktop.
Ceres Bundle Adjustment Issues
struct QuaternionBasedAngularError {
QuaternionBasedAngularError(float x_hat, float y_hat) : observed_x(x_hat), observed_y(y_hat) {
ray_rotation[0] = y_hat*sin_half_theta;
ray_rotation[1] = -x_hat*sin_half_theta;
ray_rotation[2] = 0.0;
ray_rotation[3] = cos_half_theta;
}
static ceres::CostFunction* Create(const double observed_x, const double observed_y) {
return (new ceres::AutoDiffCostFunction<QuaternionBasedAngularError, 2, 4, 3, 3>(new QuaternionBasedAngularError(observed_x, observed_y)));
}
//
template <typename T>
bool operator()(const T* const camera_rotation, const T* const camera_translation, const T* const point, T* residuals) const {
T p[3];
ceres::QuaternionRotatePoint(camera_rotation, point, p);
p[0] += camera_translation[0];
p[1] += camera_translation[1];
p[2] += camera_translation[2];
T p_hat[3];
// THIS DOESN"T WORK. SAYS I CAN"T CONVERT DOUBLE ARRAY TO T
ceres::QuaternionRotatePoint(T(ray_rotation), p, p_hat);
T xp = - p[0] / p[2];
T yp = - p[1] / p[2];
residuals[0] = (xp*xp);
residuals[1] = (yp*yp);
return true;
}
double observed_x;
double observed_y;
double ray_rotation[4];
};
void run_bundle_adjustment() {
ceres::Problem problem;
int num_features = 6;
std::vector<ImgPoint> features;
double points3d[18];
double q[8] = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0};
double t[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
bool lock_first_camera = false;
//for each camera we have 2 cameras and 6 3D points
for (int j = 0; j < 2; ++j) {
for (int i = 0; i < num_features; ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function = QuaternionBasedAngularError::Create(features[i + num_features*j].x(), features[i + num_features*j].y());
problem.AddResidualBlock(cost_function, NULL, (q + 4*j), (t + 3*j), (points3d + 3*i + num_features*j));
if (!lock_first_camera) {
problem.SetParameterBlockConstant(q);
problem.SetParameterBlockConstant(t);
lock_first_camera = true;
}
}
}
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.preconditioner_type = ceres::SCHUR_JACOBI;
options.linear_solver_type = ceres::ITERATIVE_SCHUR;
options.use_inner_iterations = true;
options.max_num_iterations = 100;
options.minimizer_progress_to_stdout = true;
// Solve!
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << "Final report:\n" << summary.FullReport();
std::cout << "Q: " << q[1] << q[2] << q[3] << q[0] << std::endl;
std::cout << "T: " << t[0] << t[1] << t[2] << std::endl;
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment