Ceres Bundle Adjustment Issues

cost_function.cpp

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];
};

run_bundle_adjust.cpp

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;
}