SfM & BA - adding a simple constraint on the translational part of the camera poses

[Clemens Arth]

Hi,

thanks for opening the ceres-solver to the public - it is really a great tool and makes development of many things much easier... however, here's my problem:

I'm integrating the solver into an existing SfM pipeline, and what I actually want to do is adding a constraint on the translation part of the cameras. Since we have a prior from a highly accurate differential GPS source, I simply wanted to add a constraint such that a translation away from the position estimate during BA is penalized. In other words, assume we have a reconstruction of a small urban area, and for each camera position we have a location estimate, I want to do BA that actually minimizes the reprojection error, but also minimizes the distance between the location estimate and the translational part of the camera pose.

So here's what I've done so far:
1) take the simple_bundle_adjust example from the sources, and
2) modify it like this (note that I have a block of 6 parameters for the camera, the intrinsics is handled separately):

// adding error function
struct TranslationOffsetError {
  TranslationOffsetError(double t_x, double t_y, double t_z)
      : t_x(t_x), t_y(t_y), t_z(t_z) {}    
     
  template <typename T>
  bool operator()(const T* const cameraRT,
                  T* offset) const {
    offset[0] = t_x - cameraRT[3]; // cameraRT[0..2] is the rotational part
    offset[1] = t_y - cameraRT[4];
    offset[2] = t_z - cameraRT[5];
  }
  double t_x;
  double t_y;
  double t_z;
};

...

//somewhere in main add residual block:
...

  for (int c = 0; c < bal_problem->num_cameras(); c++)
  {
    ceres::CostFunction* cost_function =
        new ceres::AutoDiffCostFunction<TranslationOffsetError, 3, 6>(
        new TranslationOffsetError(bal_problem->cameralocation(c)[0],bal_problem->cameralocation(c)[1],bal_problem->cameralocation(c)[2]));    
    problem.AddResidualBlock(cost_function,
      NULL, // squared loss,
      bal_problem->mutable_cameraRT_for_observation(c));
  }

...

Actually I thought this would be the most natural way to register some reconstruction if any prior is available, but unfortunately it does not work that way since I face the following issue:

Error in evaluating the ResidualBlock.

There are two possible reasons. Either the CostFunction did not evaluate and fill all    
residual and jacobians that were requested or there was a non-finite value (nan/infinite)
generated during the or jacobian computation.

Residual Block size: 1 parameter blocks x 3 residuals

For each parameter block, the value of the parameters are printed in the first column  
and the value of the jacobian under the corresponding residual. If a ParameterBlock was
held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry
of the Jacobian/residual array was requested but was not written to by user code, it is
indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating
to Inf or NaN is also an error.  

Residuals:                0            0            0

Parameter Block 0, size: 6

           0 | Not Computed  Not Computed  Not Computed  
           0 | Not Computed  Not Computed  Not Computed  
           0 | Not Computed  Not Computed  Not Computed  
           0 | Not Computed  Not Computed  Not Computed  
           0 | Not Computed  Not Computed  Not Computed  
           0 | Not Computed  Not Computed  Not Computed  

Any help is highly appreciated
Regards
Clemens

[Keir Mierle]

The residual is missing a return true.

--
--
----------------------------------------
Ceres Solver Google Group
http://groups.google.com/group/ceres-solver?hl=en?hl=en
 
 
 

[Clemens Arth]

Hi Sameer and Keir,

shame on me that I forgot the casting and the return statement.

Thanks a lot!
Clemens

[Sameer Agarwal]

I think we need to update that warning, if a false value is detected from the evaluate function.

Sameer

[Keir Mierle]

Unfortunately there is nothing we can do here without changing the autodiff function signature. The issue is that random undefined memory is getting read as the return status of the residual. Once you go into undefined behaviour territory, you're lost.

The real answer is that the compiler should never have let this compile. A coworker and myself tripped over this exact bug a few years ago and submitted it to the GCC maintainers; so it's possible Clemens is using an older GCC.

Keir

[Clemens Arth]

Hi,

the GCC I'm using is

gcc version 4.6.3 (Ubuntu/Linaro 4.6.3-1ubuntu5)

from the recent Ubuntu distribution.
Clemens