Appy transformation to batch of points

[neanea04]

If I have have a list of 3D points how can I use SE3.transform_to on the entire list?

[nosmokingsurfer]

@neanea04 Hello,

When I was looking at the same question I found some examples in theseus unit test:
https://github.com/facebookresearch/theseus/blob/9a117fd02867c5007c6686e342630f110e488c65/tests/theseus_tests/geometry/test_se3.py#L240C45-L240C57

You need to pack your points into tensor of shape (B,3) where B is the batch size and pass tensor to the transform_to() function.

Cheers,
Alex

[luisenp]

Cost functions require your variables to be a subclass of Manifold, such as Vector, SE3, etc.

[neanea04]

Is there a way I can use transform_to for a batched Point3 so that it would compute for all the points at a time? I want to compute the error between two transformed pointclouds.

[luisenp]

Hi @neanea04, perhaps this example is relevant. Do note that labs.lie is now under torchlie.

Let me know if this is what you are looking for.

[neanea04]

Thanks a lot for the ideas. I am trying the following error function for my AutodiffCostfunction. v1,v2 are theseus SE3 variables. p1, p2 are point clouds as an theseus Variable with shape (N,3)

def batched_fm_3d_error(self,optim_vars,aux_vars):
        v1,v2 = optim_vars
        p1,p2 = aux_vars
        a = lieF.SE3.untransform(v1.tensor, p1.tensor)
        b = lieF.SE3.untransform(v2.tensor, p2.tensor)
        diff = torch.norm((a- b), p=2)
        diff = diff.unsqueeze(0).unsqueeze(0)
        return diff

But I am getting the following error.
Nonlinear optimizer. Iteration: 0. Error: 1.9431973524886494
Traceback (most recent call last):
File "main.py", line 80, in
main()
File "main.py", line 33, in main
info = pose_graph.optimize()
File "pose_graph.py", line 598, in optimize
updated_inputs, info = layer.forward(
File "theseus/theseus_layer.py", line 93, in forward
vars, info = _forward(
File "theseus/theseus_layer.py", line 171, in _forward
info = optimizer.optimize(**optimizer_kwargs)
File "theseus/optimizer/optimizer.py", line 51, in optimize
return self._optimize_impl(**kwargs)
File "theseus/optimizer/nonlinear/nonlinear_least_squares.py", line 251, in _optimize_impl
self._optimize_loop(
File "theseus/optimizer/nonlinear/nonlinear_least_squares.py", line 173, in _optimize_loop
err, all_rejected = self._step(
File "theseus/optimizer/nonlinear/nonlinear_least_squares.py", line 347, in _step
tensor_dict, err = self._compute_retracted_tensors_and_error(
File "theseus/optimizer/nonlinear/nonlinear_least_squares.py", line 317, in _compute_retracted_tensors_and_error
self.objective.retract_vars_sequence(
File "theseus/core/objective.py", line 903, in retract_vars_sequence
self._retract_method(
File "theseus/core/vectorizer.py", line 457, in _vectorized_retract_optim_vars
assert vectorized_delta.shape == (n_vars * batch_size, dof)
AssertionError

[luisenp]

Do you have a code example I can run? It's hard to offer suggestions just by looking at the error function.