So recently I found myself needing to use Google’s Ceres Solver to perform a non-linear batch optimization for my masters. ¬†This should have been easy as Ceres makes implementing this sort of thing rather trivial. I took a look at their “Simple bundle adjustment” code and soon realized that my Eigen Quaternions and the Ceres Quaternions are in a different order.

Eigen uses the correct initialization order [w, x, y, z] however in memory it stores Quaternions as [x, y, z, w]. Thus you cannot use the Ceres QuaternionLocalParameterization to ensure the optimization generates normalized quaternions. Furthermore, you cannot use any of the Ceres rotation functions to transform the world points into the camera frame. So after some investigation and some help from the awesome guys on the Ceres Solver Google group I finally got it all working. I implemented a LocalParameterization to handle Eigen Quaternions and with some help I managed to use the Eigen based rotations with Ceres Jet objects.

You can find the Parameterization code on my github: https://github.com/system123/ceres_extensions

So let’s look at a simple example of how to use this along with Eigen Quaternions. The example is a partial bundle adjustment, whereby we only optimize over the external camera parameters and not the structure.

Our cost function object looks as follows:

struct ReprojectionError {

    ReprojectionError(const Eigen::Vector3d observed, const Eigen::Vector3d worldPoint) {
        observed_x = observed.x()/observed.z();
        observed_y = observed.y()/observed.z();

        X = worldPoint.x();
        Y = worldPoint.y();
        Z = worldPoint.z();
    }

    // Factory to hide the construction of the CostFunction object from the client code.
    static ceres::CostFunction* Create(const Eigen::Vector3d observed, const Eigen::Vector3d worldPoint) {
        return (new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3>(new ReprojectionError(observed, worldPoint)));
    }

    template <typename T>
    bool operator()(const T* const camera_rotation, const T* const camera_translation, T* residuals) const {

        // Make sure the Eigen::Vector world point is using the ceres::Jet type as it's Scalar type
        Eigen::Matrix<T,3,1> point;
        point << T(X), T(Y), T(Z);

        // Map the T* array to an Eigen Quaternion object (with appropriate Scalar type)
        Eigen::Quaternion<T> q = Eigen::Map<const Eigen::Quaternion<T>>(camera_rotation);

        // Rotate the point using Eigen rotations
        Eigen::Matrix<T,3,1> p = q * point;

        // Map T* to Eigen Vector3 with correct Scalar type
        Eigen::Matrix<T,3,1> t = Eigen::Map<const Eigen::Matrix<T,3,1>>(camera_translation);
        p += t;

        // Cast CCS 3D point to plane at z = 1 
        T xp = p[0] / p[2];
        T yp = p[1] / p[2];

        // The error is the difference between the predicted and observed position.
        residuals[0] = xp - T(observed_x);
        residuals[1] = yp - T(observed_y);

        return true;
    }

    double observed_x;
    double observed_y;
    double X;
    double Y;
    double Z;

};

Next we need to create a ceres::Problem, add our residual blocks and apply our parameterization. The code below assumes we have a camera object which contains the following:

A rotation (q) expressed in an Eigen::Quaterniond

A translation (t) using Eigen::Vector3d

and a std::vector<FeaturePoints> which contain 2D -> 3D feature correspondences each represented using an Eigen::Vector3d

 

#include "ceres_extensions.h"

ceres::Problem problem;

// Use my LocalParameterization
ceres::LocalParameterization *quaternion_parameterization = new ceres_ext::EigenQuaternionParameterization;

for (auto cam = cameras.begin(); cam != cameras.end(); cam++) {

    for (auto fpt = cam->features.begin(); fpt != cam->features.end(); fpt++) {

        ceres::CostFunction* cost_function = ReprojectionError::Create( fpt->ImgPoint->X, fpt->worldPoint->X );
        problem.AddResidualBlock(cost_function, NULL, cam->q.coeffs().data(), cam->t.data());

     }     

    // Apply the parameterization
    problem.SetParameterization(cam->q.coeffs().data(), quaternion_parameterization);
}

// Set a few options
ceres::Solver::Options options;
options.use_nonmonotonic_steps = true;
options.preconditioner_type = ceres::SCHUR_JACOBI;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_num_iterations = 100;

ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

std::cout << "Final report:\n" << summary.FullReport();

It’s that simple. You can now use any Eigen functions inside your Ceres cost function by mapping them to the appropriate types, furthermore, you can use Eigen quaternions without the mess of having to convert them to Ceres form first.

Many when I have a few extra minutes I will put a pull request together to get the EigenQuaternionParameterization included in Ceres.

Happy coding.