Creating an expression version of Rot3::AxisAngle

[Audren Cloitre]

Hello,

I have been trying to implement an Expression-version of the static function Rot3::AxisAngle but my code does not compile, and I do not understand why. The compiler complains that too many arguments are passed to the function, but I only pass two arguments: the axis and angle of rotation. I appended a minimal example of my code at the end of this message (about 20 lines).

To give some background, my goal with this code is to calibrate the location of a single-axis gimbal, and as such, I would like to optimize the orientation of the axis of rotation based on multiple measurements. The idea is to separate the axis of rotation and angle of rotation into two variables, with the axis of rotation being shared between all measurements, and only the angle of rotation being allowed to change across measurements.

I used the example InverseKinematicsExampleExpressions.cpp as a starting point, and it shows how to convert a static function to work with Expressions. This is done with the Expmap function from Pose2, and the compiler does not have any issue with it.

Below is a minimal example to reproduce the issue. I will appreciate any insight anyone can share as to why one function compiles without issue and the other does not. Thank you for taking the time to read my message.

Best regards,

Audren Cloitre

#include <gtsam/geometry/Rot3.h>
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/expressions.h>

using namespace gtsam;

// Expression version of Pose2::Expmap, taken from InverseKinematicsExampleExpressions.cpp
inline Pose2_ Expmap(const Vector3_& xi)
{
  return Pose2_(&Pose2::Expmap, xi);
}

// Expression version of Rot3::AxisAngle
inline Rot3_ AxisAngle(const Unit3_& axis, const Double_& angle)
{
  Rot3 (*f)(const Unit3&, double) = &Rot3::AxisAngle;
  return Rot3_(f, axis, angle);
}

// Main function
int main(int argc, char** argv)
{
  return 0;
}

[Dellaert, Frank]

Reading this on a plane :-) Might be reference issue: expressions need const T&. “double” might not match. 

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Audren Cloitre <a.cl...@gmail.com>
Sent: Thursday, December 2, 2021 7:36:47 PM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Creating an expression version of Rot3::AxisAngle

 

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/5ba8160a-46c9-4bc7-a7e7-c38abbebc6fdn%40googlegroups.com.

[Audren Cloitre]

Thank you Prof. Dellaert.

I changed the function definition of Rot3::AxisAngle from Rot3::AxisAngle(const Unit3& axis, double angle) to Rot3::AxisAngle(const Unit3& axis, const double& angle).

I recompiled my local copy of gtsam but the problem remains, and I get the same error on compilation of the minimal example.

The error message mentions OptionalJacobian; shoud the original AxisAngle function in Rot3 have optional Jacobian matrices as input to be used in Expressions?

I copied the full error message below:

In file included from /usr/include/boost/function/detail/maybe_include.hpp:33:0,
                 from /usr/include/boost/function/detail/function_iterate.hpp:14,
                 from /usr/include/boost/preprocessor/iteration/detail/iter/forward1.hpp:67,
                 from /usr/include/boost/function.hpp:64,
                 from /usr/local/include/gtsam/base/Matrix.h:32,
                 from /usr/local/include/gtsam/base/Manifold.h:22,
                 from /usr/local/include/gtsam/base/Lie.h:25,
                 from /usr/local/include/gtsam/base/VectorSpace.h:11,
                 from /usr/local/include/gtsam/geometry/Point2.h:20,
                 from /usr/local/include/gtsam/geometry/Unit3.h:23,
                 from /usr/local/include/gtsam/geometry/Rot3.h:26,
                 from minimal_example.cpp:1:
/usr/include/boost/function/function_template.hpp: In instantiation of ‘static R boost::detail::function::function_invoker4<FunctionPtr, R, T0, T1, T2, T3>::invoke(boost::detail::function::function_buffer&, T0, T1, T2, T3) [with FunctionPtr = gtsam::Rot3 (*)(const gtsam::Unit3&, const double&); R = gtsam::Rot3; T0 = const gtsam::Unit3&; T1 = const double&; T2 = gtsam::OptionalJacobian<3, 2>; T3 = gtsam::OptionalJacobian<3, 1>]’:
/usr/include/boost/function/function_template.hpp:925:38:   required from ‘void boost::function4<R, T1, T2, T3, T4>::assign_to(Functor) [with Functor = gtsam::Rot3 (*)(const gtsam::Unit3&, const double&); R = gtsam::Rot3; T0 = const gtsam::Unit3&; T1 = const double&; T2 = gtsam::OptionalJacobian<3, 2>; T3 = gtsam::OptionalJacobian<3, 1>]’
/usr/include/boost/function/function_template.hpp:716:7:   required from ‘boost::function4<R, T1, T2, T3, T4>::function4(Functor, typename boost::enable_if_c<(! boost::is_integral<Functor>::value), int>::type) [with Functor = gtsam::Rot3 (*)(const gtsam::Unit3&, const double&); R = gtsam::Rot3; T0 = const gtsam::Unit3&; T1 = const double&; T2 = gtsam::OptionalJacobian<3, 2>; T3 = gtsam::OptionalJacobian<3, 1>; typename boost::enable_if_c<(! boost::is_integral<Functor>::value), int>::type = int]’
/usr/include/boost/function/function_template.hpp:1061:16:   required from ‘boost::function<R(T0, T1, T2, T3)>::function(Functor, typename boost::enable_if_c<(! boost::is_integral<Functor>::value), int>::type) [with Functor = gtsam::Rot3 (*)(const gtsam::Unit3&, const double&); R = gtsam::Rot3; T0 = const gtsam::Unit3&; T1 = const double&; T2 = gtsam::OptionalJacobian<3, 2>; T3 = gtsam::OptionalJacobian<3, 1>; typename boost::enable_if_c<(! boost::is_integral<Functor>::value), int>::type = int]’
minimal_example.cpp:17:30:   required from here
/usr/include/boost/function/function_template.hpp:101:19: error: too many arguments to function
           return f(BOOST_FUNCTION_ARGS);

[Audren Cloitre]

I think I found a way to make my system work.

Instead of using the AxisAngle representation, I will use the ExpMap representation from Rot3, which combines the axis and angle into one 3d vector, and provides Jacobian output.

I will split the axis and angle in the same fashion as is done in the InverseKinematicsExampleExpressions.cpp, by multiplying the angle with a Unit3 vector, and using the same scalar product defined in the example.

With this approach, I get the following new minimal example to compile without problems.

In hope this helps anyone having similar issues with setting up Expressions.

#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/expressions.h>

using namespace gtsam;

// Scalar multiplication of a vector, with derivatives.
inline Vector3 scalarMultiply(const double& s, const Vector3& v,
                              OptionalJacobian<3, 1> Hs,
                              OptionalJacobian<3, 3> Hv) {
  if (Hs) *Hs = v;
  if (Hv) *Hv = s * I_3x3;
  return s * v;
}

// Expression version of scalar product, using above function.
inline Vector3_ operator*(const Double_& s, const Vector3_& v) {
  return Vector3_(&scalarMultiply, s, v);
}

// Expression version of Unit3::unitVector
inline Vector3_ unitVector(const Unit3_& v) { return Vector3_(v, &Unit3::unitVector); }

// Expression version of Rot3::Expmap
inline Rot3_ Expmap(const Vector3_& xi) { return Rot3_(&Rot3::Expmap, xi); }

// Main function
int main(int argc, char** argv)
{

  Double_ theta('x', 0);
  Unit3_ axis('x', 1);
  Rot3_ rotation = Expmap(theta * unitVector(axis));
  return 0;
}

[Dellaert, Frank]

I’m glad you found a workaround, but indeed, every function used with expressions has to have optionalJacobian arguments, which is demanded by the constructors in expression.h

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Audren Cloitre <a.cl...@gmail.com>

Sent: Friday, December 3, 2021 11:56:19 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Creating an expression version of Rot3::AxisAngle

 

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/a3c87e91-42e5-4539-9bb0-14203f812c68n%40googlegroups.com.

[Audren Cloitre]

Thank you Professor.

I had the idea of using the Expmap  representation by looking into the implementation of the AxisAngle function. It calls internally the expmap function with the axis and angle combined into one vector.

Another way to solve the issue would have been to change the function definition of the AxisAngle function to include OptionalJacobians, and fill them using the output Jacobians of the expmap function.

Thank you again for help!