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

51 views

### Xiaochen Fan

Mar 7, 2024, 10:41:41 PMMar 7
to Ceres Solver
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.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

Mar 8, 2024, 3:51:56 PMMar 8
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.