What is the right way to use QuaternionManifold for a simple ICP-like optimization?

[Xiaochen Fan]

Hi, I'm trying to use ceres for a 3d-3d ICP optimization problem w.r.t quaternion. I read BA example and wrote my own test code, but found the result is unstable and not correct. So what is the right way to use quaternion as parameters?

Here is my code

```

#include <iostream>
#include "ceres/ceres.h"
#include "ceres/rotation.h"

std::vector<double> normalize(const std::vector<double>& vec) {
    std::vector<double> normalized_vec(vec.size());

    // Calculate the magnitude of the vector
    double magnitude = 0.0;
    for (double val : vec) {
        magnitude += val * val;
    }
    magnitude = sqrt(magnitude);

    // Normalize each element
    for (size_t i = 0; i < vec.size(); ++i) {
        normalized_vec[i] = vec[i] / magnitude;
    }

    return normalized_vec;
}

struct Cost3dFunction
{
    const double* const observed3D; // 3D scene point
    const double* const model3D;    // 3D model point

    double weight3D;

    Cost3dFunction(const double* const observed3D, const double* const model3D, double weight3D)
        : observed3D(observed3D), model3D(model3D), weight3D(weight3D)
    {
    }

    template <typename T>
    bool operator()(const T* const pose, T* residual) const
    {
       
        T rotated_model[3];

        ceres::QuaternionRotatePoint(pose, (const T*)model3D, rotated_model);

         // Compute 3D loss
        residual[0] = rotated_model[0] - observed3D[0];
        residual[1] = rotated_model[1] - observed3D[1];
        residual[2] = rotated_model[2] - observed3D[2];

        return true;
    }
};

int main()
{
    // Initialize the problem
    ceres::Problem problem;
   
    std::vector<double> pose({ 1.0, 0.2, 0., 0. });
    pose = normalize(pose);


    const std::vector<double> modelPoint0{ 1, 2, 3 };
    const std::vector<double> modelPoint1{ 1, 2, 6 };

    const std::vector<double> modelPoint2{ 2, 2, 3 };
    const std::vector<double> modelPoint3{ 6, 2, 4 };
    const std::vector<double> modelPoint4{ 1, 7, 3 };

    std::vector<std::vector<double>> modelPoints({ modelPoint0 ,modelPoint1 ,modelPoint2 ,modelPoint3, modelPoint4 });
    std::vector<std::vector<double>> scenePoints(modelPoints);


    // Add 3d cost function to the problem
    for (int i = 0; i < 5; ++i) {
        const double* modelPoint = modelPoints[i].data();
        const double* scenePoint = scenePoints[i].data();

        ceres::CostFunction* cost_function = new
            ceres::AutoDiffCostFunction<Cost3dFunction, 3, 4>(
                new Cost3dFunction(scenePoint, modelPoint, 1));

        problem.AddResidualBlock(cost_function, nullptr, pose.data());
    }

    problem.SetManifold(pose.data(), new ceres::QuaternionManifold);

   
    // Configure solver options
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

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

    // Output the result
    std::cout << summary.FullReport() << std::endl;


    std::cout << pose[0] << std::endl;
    std::cout << pose[1] << std::endl;
    std::cout << pose[2] << std::endl;
    std::cout << pose[3] << std::endl;


    return 0;

}

```

I used very simple case, but the result is not as expected. Any advice for this? 

Thanks,

Xiaochen

[Sergiu Deitsch]

I haven't looked at your code in detail and possibly overlooked other issues. However, this particular line caught my attention:

 ceres::QuaternionRotatePoint(pose, (const T*)model3D, rotated_model);

The cast does not do what you think it does. In fact, the cast invokes undefined behavior since the involved types are unrelated.

You need to cast the points element wise. Do not use old-style casts in general to avoid this and similar issues in the future.

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/4f461513-850c-4509-91ff-18bdb99bc581n%40googlegroups.com.

[Xiaochen Fan]

Thanks a lot for your help! I realised the mistake. It works now.