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

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.

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.

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

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