I am learning Ceres and want to use Rodrigues formula as the representation for the rotation matrices.
My current approach is to use the AutoDiffManifold and provide a struct RodriguesFunctor which implements the plus and minus operator.
struct RodriguesFunctor {
template <typename T>
bool Plus(const T* x, const T* delta, T* x_plus_delta) const {
const Eigen::Vector3<T> eigenX = Eigen::Map<const Eigen::Vector3<T>>(x);
const Eigen::Vector3<T> eigenDelta =
Eigen::Map<const Eigen::Vector3<T>>(delta);
Eigen::Vector3<T> xPlusDelta =
rodriguesAddCeres<T>(eigenX, eigenDelta, eigenDelta);
memcpy(x_plus_delta, xPlusDelta.data(), 3 * sizeof(T));
return true;
}
template <typename T>
bool Minus(const T* x, const T* y, T* y_minus_x) const {
const Eigen::Vector3<T> eigenX = Eigen::Map<const Eigen::Vector3<T>>(x);
const Eigen::Vector3<T> eigenY = Eigen::Map<const Eigen::Vector3<T>>(y);
Eigen::Vector3<T> yMinusX = rodriguesMinus<T>(eigenY, eigenX);
memcpy(y_minus_x, yMinusX.data(), 3 * sizeof(T));
return true;
}
const int nbr_of_params = 3;
};
template <typename T>
Eigen::Matrix3<T> computeRotation(const Eigen::Vector3<T>& omega) {
T theta = omega.norm();
T delta = T(0.0000000001);
Eigen::Matrix3<T> result;
result << T(1), T(0), T(0), T(0), T(1), T(0), T(0), T(0), T(1);
if (theta <= delta && theta >= -delta) {
return result;
}
Eigen::Vector3<T> w{omega[0] / theta, omega[1] / theta, omega[2] / theta};
const Eigen::Matrix3<T> K = createTwistMatrix(w);
const Eigen::Matrix3<T> K2 = K * K;
result += sin(theta) * K + (T(1) - cos(theta)) * K2;
return result;
}
template <typename T>
Eigen::Vector3<T> logRodriguesCeres(const Eigen::Matrix3<T>& R,
const Eigen::Vector3<T>& delta) {
T norm_w = acos((R.trace() - T(1)) / T(2));
if (abs(norm_w) > T(0.0000001)) {
Eigen::Vector3<T> result{
norm_w / (T(2) * sin(norm_w)) * (R(2, 1) - R(1, 2)),
norm_w / (T(2) * sin(norm_w)) * (R(0, 2) - R(2, 0)),
norm_w / (T(2) * sin(norm_w)) * (R(1, 0) - R(0, 1))};
return result;
}
return delta;
}
template <typename T>
Eigen::Vector3<T> rodriguesAddCeres(const Eigen::Vector3<T>& a,
const Eigen::Vector3<T>& b,
const Eigen::Vector3<T>& delta) {
return logRodriguesCeres<T>(computeRotation<T>(a) * computeRotation<T>(b),
delta);
}
The cost is defined as follows:
class PointToLineError {
public:
PointToLineError(const Eigen::Vector2d& p1, const Eigen::Vector2d& p2,
const Eigen::Matrix3d& K, const Eigen::Vector3d& model_pt)
: p1_(p1), p2_(p2), K_(K), model_pt_(model_pt) {}
template <typename T>
bool operator()(const T* const rotation, const T* const translation,
T* residuals) const {
const Eigen::Vector3<T> omega{rotation[0], rotation[1], rotation[2]};
const Eigen::Vector3<T> t{translation[0], translation[1],
translation[2]};
const Eigen::Vector2<T> p =
projectPoint<T>(omega, t, K_.cast<T>(), model_pt_.cast<T>());
const Eigen::Vector2<T> associated_point =
computeAssociatedPoint<T>(p1_.cast<T>(), p2_.cast<T>(), p);
residuals[0] = p.x() - associated_point.x();
residuals[1] = p.y() - associated_point.y();
return true;
}
private:
const Eigen::Vector2d p1_;
const Eigen::Vector2d p2_;
const Eigen::Matrix3d K_;
const Eigen::Vector3d model_pt_;
};
template <typename T>
Eigen::Vector2<T> projectPoint(const Eigen::Vector3<T>& omega,
const Eigen::Vector3<T>& t,
const Eigen::Matrix3<T>& K,
const Eigen::Vector3<T>& point3d) {
Eigen::Vector3<T> p3 = K * (computeRotation<T>(omega) * point3d + t);
Eigen::Vector2<T> p(p3.x() / p3.z(), p3.y() / p3.z());
return p;
}
template <typename T>
Eigen::Vector2<T> computeAssociatedPoint(const Eigen::Vector2<T>& p1,
const Eigen::Vector2<T>& p2,
const Eigen::Vector2<T>& p) {
Eigen::Vector2<T> v = p2 - p1;
Eigen::Vector2<T> u = p - p1;
Eigen::Vector2<T> projection = (u.dot(v) / v.dot(v)) * v;
return p1 + projection;
}
void optimizeContours(
const Eigen::Matrix3d& intrinsics,
const std::vector<Eigen::Vector3d>& model_points_3d,
const std::vector<Eigen::Vector2d>& observed_image_points_2d,
const cv::Mat& debug_image, double io_camera[6]) {
const auto nbr_rotation_params = 3;
const auto nbr_translation_params = 3;
const auto rotation_tangent_dim = 3;
double omega[3] = {io_camera[0], io_camera[1], io_camera[2]};
double t[3] = {io_camera[3], io_camera[4], io_camera[5]};
CeresProblem<RodriguesFunctor, nbr_rotation_params, nbr_translation_params,
rotation_tangent_dim>
problem(omega, t);
const auto rec = rerun::RecordingStream("Optimize Contours - Projections");
rec.spawn().exit_on_failure();
const auto model_line_points =
sample3Dpoints(model_points_3d, observed_image_points_2d);
for (int i = 0; i < model_line_points.size(); ++i) {
PointToLineError* constraint = new PointToLineError(
model_line_points[i].p1, model_line_points[i].p2, intrinsics,
model_line_points[i].p);
problem.addCost<PointToLineError>(constraint);
}
const auto model_data = ModelDataRodrigues{
problem.rotation, problem.translation, model_points_3d,
observed_image_points_2d, intrinsics};
IterationProjectionCallback callback(model_data, model_line_points, &rec,
debug_image);
problem.solve(callback);
memcpy(io_camera, problem.rotation, 3 * sizeof(double));
memcpy(io_camera + 3, problem.translation, 3 * sizeof(double));
}
template <typename T, int rotation_params, int translation_params,
int rotation_tangent_dim>
struct CeresProblem {
CeresProblem(double* rotation_, double* translation_)
: rotation(rotation_), translation(translation_) {
manifold = new ceres::AutoDiffManifold<T, rotation_params,
rotation_tangent_dim>;
problem.AddParameterBlock(rotation, rotation_params, manifold);
problem.AddParameterBlock(translation, translation_params);
}
template <typename Error>
void addCost(Error* constraint) {
auto cost_function =
new ceres::AutoDiffCostFunction<Error, 2, rotation_params,
translation_params>(constraint);
problem.AddResidualBlock(cost_function, nullptr,
{rotation, translation});
}
void solve(ceres::IterationCallback& callback) {
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.minimizer_progress_to_stdout = false;
options.max_num_iterations = 100;
options.function_tolerance = 1e-8;
options.parameter_tolerance = 1e-8;
options.callbacks.push_back(&callback);
options.update_state_every_iteration = true;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << std::endl;
}
double* rotation;
double* translation;
ceres::Problem problem;
ceres::Manifold* manifold;
};
The reason to why I add logRodriguesCeres is to be able to return delta instead of the constant vector (0,0,0) which would destroy the derivatives.
However, the rotation parameters are not updated during the optimisation. So something is wrong with my implementation. It is more a guess that return delta correct. The translation parameters are updated.
If anyone has a good idea of how to debug this I would be very grateful!
Error Found
The solution was to update the logRodrigues and computeRotation to not return constants. (And yes, the code can be improved :) )
template <typename T>
Eigen::Matrix3<T> computeRotation(const Eigen::Vector3<T>& omega) {
T theta = omega.norm();
T delta = T(0.0000000001);
if (theta <= delta) {
Eigen::Matrix3<T> result = Eigen::Matrix3<T>::Identity();
Eigen::Vector3<T> w{omega[0], omega[1], omega[2]};
const Eigen::Matrix3<T> K = createTwistMatrix(w);
result += K; // Note that we return I + K (sin(t)/t -> 1, t->0)
return result;
}
Eigen::Matrix3<T> result = Eigen::Matrix3<T>::Identity();
Eigen::Vector3<T> w{omega[0] / theta, omega[1] / theta, omega[2] / theta};
const Eigen::Matrix3<T> K = createTwistMatrix(w);
const Eigen::Matrix3<T> K2 = K * K;
result += sin(theta) * K + (T(1) - cos(theta)) * K2;
return result;
}
template <typename T>
Eigen::Vector3<T> logRodrigues(const Eigen::Matrix3<T>& R) {
T norm_w = acos((R.trace() - T(1)) / T(2));
if (abs(norm_w) > T(0.0000001)) {
Eigen::Vector3<T> result{
norm_w / (T(2) * sin(norm_w)) * (R(2, 1) - R(1, 2)),
norm_w / (T(2) * sin(norm_w)) * (R(0, 2) - R(2, 0)),
norm_w / (T(2) * sin(norm_w)) * (R(1, 0) - R(0, 1))};
return result;
}
Eigen::Vector3<T> result(R(2, 1) - R(1, 2), R(0, 2) - R(2, 0),
R(1, 0) - R(0, 1));
return result;
}