DH Chain class update

[marip8]

This PR contains updates to the DH chain class for better integration into Ceres cost functions. The major proposed changes are:

1. Template the createTransform function of the DHTransform class
2. Template the getFK function of the DHChain class
3. Add overload to the createTransform function that allows the user to pass in a pointer of DH parameter offsets for each DH transform
4. Add overload to the getFK function that allows the user to pass in an array of DH parameter offsets for the entire chain
5. Removed the GaussianNoiseDHTransform class because inheritance is not possible with template virtual functions. Instead a "noisy" transform can be created by passing in some DH parameter offsets to the DHTransform class

[schornakj]

I was trying to figure out a good way to make the DHChain object play nicely with the Ceres AutoDiffCostFunction pattern and unfortunately it's quite challenging, so partly as an exercise for myself I'm going to write out what I've been thinking.

When I write Ceres optimizations they have a few key parts. I'll use a circle fit optimization as an example since I have the code handy:

• Declaration of an object that holds the optimization parameters, which are initialized with some guesses:

  std::vector<double> circle_params(3, 0.0);
  circle_params[0] = x;
  circle_params[1] = y;
  circle_params[2] = r_sqrt;

• A cost class with a constructor that takes an observation as a parameter, and assigns the observation to private members:

  class CircleDistCost
  {
  public:
  CircleDistCost(const double& x, const double& y)
  : x_(x), y_(y)
  {}
  ...

• The operator() function within the cost class, which is templated to take either doubles or jets, and where the input parameter is the current "guess" of the optimization parameters (a simplification, but that's how I think of it). This function uses the observation we passed in earlier to calculate a residual.

  template<typename T>
  bool operator()(const T* const sample,
                T* residual) const
  {
  T r = sample[2] * sample[2];
  T x_pos = x_ - sample[0];
  T y_pos = y_ - sample[1];
  residual[0] = r * r - x_pos * x_pos - y_pos * y_pos;
  return true;
  }

• In problem setup, a cost block is created for each observation. It's templated to the type of the cost function (CircleDistCost), the size of the residual (1), and the size of the optimization parameter block (3).

  auto* cost_block = new ceres::AutoDiffCostFunction<CircleDistCost, 1, 3>(cost_fn);

• For each observation we add a residual block to the problem, using the cost function we just created, optionally a loss function (usually just nullptr), and a pointer to the optimization parameters we created in the first step:

  problem.AddResidualBlock(cost_block, loss_fn, circle_params.data());

Assuming we follow a similar pattern, I think this has several implications for the design of the DHTransform and DHChain classes:

• For kinematic calibration of a DH chain, there are two kinds of observations: the measured positions of each joint of the chain, and some info about the pose at the end of the chain (either 2D/3D camera image points, or a directly-observed pose from a tracking system). This means that the constructor for the cost class should take a vector<double> for the joint angles and either an Isometry3d (for pose observations) or a vector<double> (for image point observations).
• The residual for this problem is the difference between the calculated and observed transform between the reference frame and the end of the chain. This can be represented in several ways, depending on the nature of the observations.
• The DHChain object should be used in the cost function to calculate the residual for a given set of DH offsets. Each DHTransform also requires some info about the type of joint (linear vs revolute) which isn't a part of the optimization parameters. I think this means that the DHChain object needs to be created during problem setup and then copied to each instance of the cost class so that this joint info is consistently defined. We could do this by adding a DHChain parameter to the constructor of the cost class. DHChain also need a way for its offsets to be set in the residual function for a given set of optimization parameters without rebuilding the whole chain from scratch.
• The DH offsets need to be initialized in the same scope as initial problem setup so that a pointer to them can be passed to each residual block. The number of DH offsets (and therefore the number of DHTransforms in the chain) needs to be known when the optimization function is compiled, since this number is used to template AutoDiffCostFunction. We could do this by templating the optimize function with the number of DHTransforms, but no matter how we approach it this imposes some limitations on how the problem and optimization can be used.

[marip8]

@schornakj here is the branch with my WIP attempt at a DH-chain-based kinematic calibration cost function for reference

[marip8]

Notes on the class design updates:

• Use Eigen vectors for joint state and DH offsets
  • Easy to template for ceres::Jet and double types
  • Easy to convert to and from raw data pointers (T* Matrix<T>::data() and Eigen::Map<Eigen::Matrix<T>>(raw_ptr)
  • Convenient to cast matrices and vectors of doubles (i.e. observations, joint states, etc.) to matrices and vectors of jets for interaction with optimization variables
• Planning on using ceres::DynamicAutoDiffCostFunction cost function type

Comments

For kinematic calibration of a DH chain, there are two kinds of observations: the measured positions of each joint of the chain, and some info about the pose at the end of the chain (either 2D/3D camera image points, or a directly-observed pose from a tracking system). This means that the constructor for the cost class should take a vector for the joint angles and either an Isometry3d (for pose observations) or a vector (for image point observations).

I'm thinking about using Eigen vectors instead because they provide more convenient casting functions to allow interaction with non-template Eigen types (i.e. doing transform math, etc.)

The DHChain object should be used in the cost function to calculate the residual for a given set of DH offsets. Each DHTransform also requires some info about the type of joint (linear vs revolute) which isn't a part of the optimization parameters. I think this means that the DHChain object needs to be created during problem setup and then copied to each instance of the cost class so that this joint info is consistently defined. We could do this by adding a DHChain parameter to the constructor of the cost class. DHChain also need a way for its offsets to be set in the residual function for a given set of optimization parameters without rebuilding the whole chain from scratch.

Correct, each cost function instance will need to own a few things:

• Observed feature coordinates
• Target feature coordinates
• Camera DH chain
• Camera DH chain joint values for the specific observation
• Target DH chain
• Target DH chain joint values for the specific observation

These changes also add an overload to the DHTransform::createTransform() function that allows us to provide DH parameter offsets as an Eigen vector (which is easily convertible from a raw pointer)

The DH offsets need to be initialized in the same scope as initial problem setup so that a pointer to them can be passed to each residual block.

Correct. I'm planning on making these Eigen::MatrixX4d matrices, from which we can easily get a raw data pointer and can easily reconstruct into the Eigen type

The number of DH offsets (and therefore the number of DHTransforms in the chain) needs to be known when the optimization function is compiled, since this number is used to template AutoDiffCostFunction. We could do this by templating the optimize function with the number of DHTransforms, but no matter how we approach it this imposes some limitations on how the problem and optimization can be used.

Not necessarily. We can use the DynamicAutoDiffCostFunction which allows you to specify dynamically-sized parameter blocks

[schornakj]

Based on our offline conversations I think we're in agreement about how the optimization should work.

Not necessarily. We can use the DynamicAutoDiffCostFunction which allows you to specify dynamically-sized parameter blocks

Forgot about the existence of this, sorry! That's a good solution to this particular problem.

[marip8]

Closing, as this is replaced by #60