Numerical Differentiation fails

[Aravindhan K Krishnan]

Hi

I am constructing a simple example that uses the
NumericDiffCostFunction. I am implementing the Iterative Closest Point
Algorithm where the residual is defined as follows

r_ i = p_i - T * q_i

where T is the transformation to be estimated and p_i and q_i are the
corresponding points in the two point clouds based on the nearest
neighbor metric.

The solver returns the following error
>> Residual and Jacobian evaluation failed.

I looked into numeric_diff_cost_function.h and
EVALUATE_JACOBIAN_FOR_BLOCK(..) always returns true. Can somebody tell
me what is going wrong here? I am copying the code snippet below for
reference.

---
struct ICPFunctor
{
ICPFunctor (pcl::PointXYZ& in_pt)
{
pt = in_pt;
}

bool operator () (double const* const roll_,
double const* const pitch_,
double const* const yaw_,
double const* const tx_,
double const* const ty_,
double const* const tz_,
double* residuals) const
{
double roll = *roll_;
double pitch = *pitch_;
double yaw = *yaw_;
double tx = *tx_;
double ty = *ty_;
double tz = *tz_;

Eigen::Matrix3f R = Eigen::Matrix3f::Zero ();
R = Eigen::AngleAxisf (roll , Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxisf (pitch , Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxisf (yaw , Eigen::Vector3f::UnitZ ());

Eigen::Vector3f v = R * pt.getVector3fMap ();
v[0] += tx;
v[1] += ty;
v[2] += tz;

pcl::PointXYZ npt;
npt.x = v[0];
npt.y = v[1];
npt.z = v[2];

const int K = 1;
vector <int> ids;
vector <float> dists;

if (::kdtree.nearestKSearch (npt, K, ids, dists) > 0) { // this
succeeds all the time. So residuals are computed..
Eigen::Vector3f r = npt.getVector3fMap () -
target->points[ids[0]].getVector3fMap ();
residuals[0] = r[0];
residuals[1] = r[1];
residuals[2] = r[2];

cout << "Residuals : " << r[0] << " " << r[1] << " " << r[2] << "\n";
}

return true;
}

protected:

pcl::PointXYZ pt;
};


void ICPNonLinearOptimizationND (PointCloudPtr& source, PointCloudPtr& target,
Transformation& transform /* gives the
initial estimate */ )
{
double roll = transform.roll * M_PI / 180;
double pitch = transform.pitch * M_PI / 180;
double yaw = transform.yaw * M_PI / 180;

double tx = transform.tx;
double ty = transform.ty;
double tz = transform.tz;

::target = target;
::kdtree.setInputCloud (target);

ceres::Problem problem;
for (int i = 0; i < source->points.size (); i++) {
ceres::CostFunction* cost_function = new
ceres::NumericDiffCostFunction <ICPFunctor, ceres::FORWARD, 3, 1, 1,
1, 1, 1, 1>
(new ICPFunctor
(source->points[i]), ceres::DO_NOT_TAKE_OWNERSHIP);

problem.AddResidualBlock (cost_function, NULL, &roll, &pitch,
&yaw, &tx, &ty, &tz);
}

ceres::Solver::Options options;
options.max_num_iterations = 500;
options.linear_solver_type = ceres::DENSE_QR;
//options.minimizer_type = ceres::LINE_SEARCH;
options.minimizer_progress_to_stdout = true;

ceres::Solver::Summary summary;
Solve (options, &problem, &summary);
std::cout << summary.FullReport() << "\n\n";

cout << "Roll = " << roll* 180 / M_PI << "\n";
cout << "Pitch = " << pitch * 180 / M_PI << "\n";
cout << "Yaw = " << yaw * 180 / M_PI << "\n";
cout << "TX = " << tx << "\n";
cout << "TY = " << ty << "\n";
cout << "TZ = " << tz << "\n";

transform.roll = roll;
transform.pitch = pitch;
transform.yaw = yaw;
transform.tx = tx;
transform.ty = ty;
transform.tz = tz;
}


Thanks,
Aravindhan

[Sameer Agarwal]

if you run your solver with -logtostderr it will tell you which evaluation failed.

also do not use floats, you are losing a lot of precision in the numeric differentiation.

Sameer

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.
To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver+unsubscribe@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CACSWV0SO1d1UdtnbawMun_9OhzYsDL6ekzia%3DrOR%3DPNH5_cizA%40mail.gmail.com.
For more options, visit https://groups.google.com/d/optout.

[Rémi Cura]

Shouldn't you have only one residuals,
and if you use 3, shouldn't they be squared?

At the very least the residual should be positive right?

Also in this case the jacobian looks easy to compute (it is along the direction of your translation, with a value of  find_correct_sign * norm(residual) )
Cheers,
Rémi-C

To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.

[Keir Mierle]

On Tue, Nov 4, 2014 at 2:36 AM, Rémi Cura <remi...@gmail.com> wrote:

Shouldn't you have only one residuals,
and if you use 3, shouldn't they be squared?

No; the squaring is implied in Ceres. For example, if your cost function outputs a 3-dimensional residual

r[0] = ex

r[1] = ey

r[2] = ez

then the contribution to the overall cost function is 1/2(ex^2 + ey^2 + ez^2)

Ceres must do this internally to properly handle robust cost functions.

At the very least the residual should be positive right?

Again no; the idea with the residuals is that you send to Ceres the measurement "error vector". Ceres takes the magnitude. Take a look at the bundle adjustment example.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/e8ce4654-7927-4f70-ad46-fb469dc90b75%40googlegroups.com.

[Keir Mierle]

Hi Aravindhan,

Maybe I'm misunderstanding, but it appears you are doing a nearest neighbor lookup in the cost functor. This means your cost function is non-differentiable. I don't think this is the problem here, but something to think about.

Also please post the Full Report content.

[Aravindhan K Krishnan]

Hi Kier

Thanks for replying to Remi's email.

The nearest neighbour search is used to find the correspondences i.e
find the <p_i, q_i> pairs in the two point clouds. Once this is done,
the function becomes differentiable. (The optimization function is ||
p_i - T q_i||). However the correspondence changes every iteration (as
nearest neighbour search happens every iteration); so the
differentiable function also changes. I defined the Jacobian by
deriving it analytically and the algorithm converges. Additionally, I
am trying to get it work using Numerical differentiation.

I changed all my variables to 'double' as suggested by Sameer and ran
the solver using -logtostderr but I get the following error.
>> ERROR: unknown command line flag 'logtostderr'

I set GLOG_logtostderr=1 but the problem still persists. However, I
get a verbose error report when I comment 'ParseCommandLineFlags (.)'
in my program after setting GLOG_logstderr=1. I am pasting the report
below. I see some 'nan' values. Why is this happening? Please let me
know.


---
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: 6 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.0838025 -0.735204 0.00317638

Parameter Block 0, size: 1

1.309 | 0 -0.12017 -0.00998722

Parameter Block 1, size: 1

0 | 0.040749 0.00523688 -0.00140322

Parameter Block 2, size: 1

1.21355e-311 | -nan -nan -nan

Parameter Block 3, size: 1

6.92979e-310 | -nan -nan -nan

Parameter Block 4, size: 1

0.2 | 0 1 0

Parameter Block 5, size: 1

6.92979e-310 | -nan -nan -nan


W1105 11:13:57.599735 4851 trust_region_minimizer.cc:205]
Terminating: Residual and Jacobian evaluation failed.

Ceres Solver Report
-------------------
Original Reduced
Parameter blocks 6 6
Parameters 6 6
Residual blocks 397 397
Residual 1191 1191

Minimizer TRUST_REGION

Dense linear algebra library EIGEN
Trust region strategy LEVENBERG_MARQUARDT

Given Used
Linear solver DENSE_QR DENSE_QR
Threads 1 1
Linear solver threads 1 1

Cost:
Initial -1.000000e+00

Minimizer iterations 0
Successful steps 0
Unsuccessful steps 0

Time (in seconds):
Preprocessor 0.000

Residual evaluation 0.000
Jacobian evaluation 0.006
Linear solver 0.000
Minimizer 0.007

Postprocessor 0.000
Total 0.007

Termination: FAILURE (Residual and Jacobian
evaluation failed.)


--
Aravindhan

> https://groups.google.com/d/msgid/ceres-solver/CADpYijGcVPvLfff0DAEapcPf5MBe%2BQFAgLb8XQcfCT3d%3Dhvdzg%40mail.gmail.com.

[Sameer Agarwal]

Aravindan,

The nearest neighbour search is used to find the correspondences i.e
find the <p_i, q_i> pairs in the two point clouds. Once this is done,
the function becomes differentiable. (The optimization function is ||
p_i - T q_i||). However the correspondence changes every iteration (as
nearest neighbour search happens every iteration); so the
differentiable function also changes.

Changing the objective function every iteration is a very bad idea.

 

I changed all my variables to 'double' as suggested by Sameer and ran
the solver using -logtostderr but I get the following error.
>> ERROR: unknown command line flag 'logtostderr'

gflags is not working for you. I never use flags with just glog, so I am not sure what is going on. But I recommend having gflags enabled.

 

Parameter Block 4, size: 1

         0.2 |            0            1            0

Parameter Block 5, size: 1

6.92979e-310 |         -nan         -nan         -nan

These are suspicious parameter block values. do you really have stuff which is 1e-310 ? I think you are feeding some bogus data into the solver.

Sameer

 

>> email to ceres-solver+unsubscribe@googlegroups.com.

>> To view this discussion on the web visit
>> https://groups.google.com/d/msgid/ceres-solver/CACSWV0SO1d1UdtnbawMun_9OhzYsDL6ekzia%3DrOR%3DPNH5_cizA%40mail.gmail.com.
>> For more options, visit https://groups.google.com/d/optout.
>
>
> --
> You received this message because you are subscribed to the Google Groups
> "Ceres Solver" group.
> To unsubscribe from this group and stop receiving emails from it, send an

> email to ceres-solver+unsubscribe@googlegroups.com.

> To view this discussion on the web visit
> https://groups.google.com/d/msgid/ceres-solver/CADpYijGcVPvLfff0DAEapcPf5MBe%2BQFAgLb8XQcfCT3d%3Dhvdzg%40mail.gmail.com.
>
> For more options, visit https://groups.google.com/d/optout.

--
You received this message because you are subscribed to the Google Groups "Ceres Solver" group.

To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver+unsubscribe@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CACSWV0TVawti7qT1yd3sx%2BLwZsPkt3ocOSSy6-ZQg%3DnHZ9%3DqWQ%40mail.gmail.com.

[Aravindhan K Krishnan]

Hi Sameer

Thanks for your inputs. The solver now converges after initializing all parameters to ‘0’. The garbage values could have caused issues.

Regarding the objective function, I am implementing an expectation maximization algorithm where the hidden variables (correspondences identified using nearest neighbour search) are estimated first, followed by the estimation of other parameters given the hidden variables. So, the objective function is bound to change in every iteration. 

--

Aravindhan

To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CAK0oyEpz7fRsEzdRc-WooQeiHgS4NA1AacoVdpiD_HN83dOs%3DA%40mail.gmail.com.

[Sameer Agarwal]

when you say "every iteration", do you mean every iteration of expectation minimization? or every iteration of ceres? the former is fine, the latter is bad.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/02BB0B2E-EC3F-4F61-8C59-58F93C46CE36%40gmail.com.

[Aravindhan K Krishnan]

I meant every iteration of expectation maximization..

Thanks,

Aravindhan

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CABqdRUAGQ-jWE%3D4znsqkAJtE5wJG4%2BWVLZhLyNm808K8JJwn_g%40mail.gmail.com.

[Rémi Cura]

Yep thanks for the precisions.
In my problem I get a correct solution only using squared residual and non squared jacobian,
I guess it is related to my forces.
Cheers,
Rémi-C

[Greg Coombe]

We (at Matterport) used a similar ICP-based algorithm for aligning point clouds. Our first attempt was to re-select correspondences every Ceres iteration, but the results were poor. Instead, we pick a smaller number of points, run to convergence, then pick another set of points and repeat. The convergence is significantly faster (changing the problem from underneath the solver is a bad idea). If you're concerned that each Ceres run lasts too long, you can set the max number of iterations to a small number.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/a8130e35-071d-465e-84e2-2fe2a42dc656%40googlegroups.com.

[Andrew Fitzgibbon]

 

Hi all,

 

A subject close to my heart.   Let me first make a plug for Champleboux et al [1] and my paper [2].

 

Then note that the objective implemented in ICPFunctor below is essentially of the form (pseudocode, if you can read C++, Matlab, and Python, you'll be fine :)) 

vector<Vector2> q; // "source" points
vector<Vector2> p; // "target" points (assume just 2 of them for a sec)
double f(Vector2 x)
   double E = 0;
   for i in range(q):
      E += min(sumsq(q[i] + x - p[0]), sumsq(q[i] + x - p[1]))
   return E

This is a huge simplification (only two target points p[0], p[1]; searching only for translation vector x) but encapsulates the key feature: the objective is a sum over min().

 

A few complications: First, to get this into the residuals form Ceres needs, you need to pull out closest-point indices:

void f(Vector2 x, vector<Vector2>& residuals)
   for i in range(q):
      j = argmin_k sumsq(q[i] + x - p[k])
      residuals[i] = q[i] + x - p[j]

and observe that summing the squared residuals gives exactly the function above.  

To simplify even further, that function is a sum of terms that look quite like (for scalar x)

f(x) = min((x-1)^2, (x-2)^2)

So, as Keir says, it's nondifferentiable, but only at x=1.5, and it also has multiple minima, so for any x, reporting the derivative of whichever quadratic part you're in is probably fine.

You're not really changing the objective under Ceres's feet, but it's a nasty objective, and you're occasionally reporting a dodgy derivative.

The multiple minima problem is less bad when you have many points, as they tend to smear out, but it's still a relatively tricky objective.  If you explicitly smooth the objective (e.g. with softmax) you can report truthful derivatives,  but the concavity may still mess up the Gauss-Newton approximation (I have never checked this, just a hunch).

In all, I still prefer it to the block coordinate descent implied by ICP, but in practice you need to switch on robust kernels to see any real advantage, and then you have a whole nother bunch of issues....

A.

[1] G. Champleboux, S. Lavallée, R. Szeliski, L. Brunie, From accurate range imaging sensor calibration to accurate model-based 3D object localization, CVPR 1992.

[2] A. Fitzgibbon, Robust Registration of 2D and 3D Point Sets, BMVC 2001.

 

On Monday, November 3, 2014 11:04:43 PM UTC, Aravindhan K Krishnan wrote:

      const int K = 1;
      vector <int> ids;
      vector <float> dists;
 
      if (::kdtree.nearestKSearch (npt, K, ids, dists) > 0) { // this succeeds all the time. So residuals are computed..

        Eigen::Vector3f r = ...

Perhaps consider this instead?

      if (::kdtree.nearestKSearch (npt, K, ids, dists) <= 0)
         return false;

      Eigen::Vector3f r = ...