How to estimate parameter uncertainty using Ceres Solver? - c++

I am using Ceres Solver to perform non-linear curve fits on small data sets. Following the examples I am able to generate perfectly reasonable fit parameters for models that match my data well. I am also trying to compute the parameter variances and this is where things are falling apart. The code executes but results seem incorrect, often many orders of magnitude larger than the the parameter itself. The number of points (x, y) in the data sets I am fitting is similar to the number of parameter in the fit models, e.g. 4 data points, 3 parameters.
I came across a similar SO question here: Ceres: Compute uncertainty on parameter, which was helpful in linking the Ceres wiki on using the covariance class, but the issue was not marked as resolved. I, like the previous poster, looked at the parameter variances produced using the Python lmfit (https://lmfit.github.io/lmfit-py/index.html) package and found that it provides much more reasonable results.
The Ceres description of the covariance class (http://ceres-solver.org/nnls_covariance.html#example-usage) described a potential issue where if the residuals of the cost functor are not scaled correctly, i.e. by the positive semi-definite covariance matrix of the observed data, then the parameter covariance matrix computation can't be trusted. Not being a mathematician, I am not certain how to satisfy this requirement.
Below is some sample code showing the cost function that I've implemented as well as the usage of the covariance class. Any advice would be greatly appreciated.
Cost function:
struct BiExponential1 {
BiExponential1( double x, double y, double s ) : x_( x ), y_( y ), s_( s ) {}
template <typename T>
bool operator()( const T* const a, const T* const b, const T* const c, T* residual ) const {
residual[0] = y_ - a[0] * ( exp( -b[0] * x_ ) - exp( -c[0] * x_ ) ); // observed - estimated = y - ( a' [exp( -b' * x ) - exp(-c' * x)] )
return true;
}
private:
const double x_;
const double y_;
};
Solver/Covariance usage:
double a = init_value_a;
double b = init_value_b;
double c = init_value_c;
double data_x[nData] = {<dummy data>}
double data_y[nData] = {<dummy data>}
for ( int i = 0; i < nData; ++i ) {
problem.AddParameterBlock( &a, 1 );
problem.AddParameterBlock( &b, 1 );
problem.AddParameterBlock( &c, 1 );
problem.AddResidualBlock(
new ceres::AutoDiffCostFunction<BiExponential1, 1, 1, 1, 1>(
new BiExponential1( data_x[i], data_y[i] ) ),
nullptr,
&a,
&b,
&c );
}
// Run the solver and record the results.
ceres::Solve( solverOptions, &problem, &summary );
// Variance estimates
// Code adapted from: http://ceres-solver.org/nnls_covariance.html#example-usage
Covariance::Options covOptions;
// TESTED non-default algorithm type - no effect.
//covOptions.algorithm_type = ceres::CovarianceAlgorithmType::DENSE_SVD;
Covariance covariance( covOptions );
std::vector<std::pair<const double*, const double*> > covariance_blocks;
covariance_blocks.push_back( std::make_pair( &a, &a ) );
covariance_blocks.push_back( std::make_pair( &b, &b ) );
covariance_blocks.push_back( std::make_pair( &c, &c ) );
covariance_blocks.push_back( std::make_pair( &a, &b ) );
covariance_blocks.push_back( std::make_pair( &a, &c ) );
covariance_blocks.push_back( std::make_pair( &b, &c ) );
CHECK( covariance.Compute( covariance_blocks, &problem ) );
// Get the diagonal variance terms
double covariance_aa[1 * 1];
double covariance_bb[1 * 1];
double covariance_cc[1 * 1];
covariance.GetCovarianceBlock( &a, &a, covariance_aa );
covariance.GetCovarianceBlock( &b, &b, covariance_bb );
covariance.GetCovarianceBlock( &c, &c, covariance_cc );

Related

boost error: expression cannot be used as a function

I am trying to use the boost RK4 integration together with the rigid body dynamics library.
I am getting a strange error I dont recognize, that I think has to do with the operator() override but I am not sure based on this post.
My code is similar to this example. Which makes me think that either I am missing something obvious. I think that it may be a compiler problem.
This is my class I use to convert from RBDL to boost.
class rbdlToBoost {
public:
rbdlToBoost(Model* model) : model(model)
{
q = VectorNd::Zero(model->dof_count);
qd = VectorNd::Zero(model->dof_count);
qdd = VectorNd::Zero(model->dof_count);
tau = VectorNd::Zero(model->dof_count);
}
//3c. Boost uses this 'operator()' function to evaluate the state
// derivative of the pendulum.
void operator() (const state_type &x, state_type &dxdt, const double t)
{
//do stuff
}
private:
Model* model;
VectorNd q, qd, qdd, tau;
};
This is my main I am using to test the integration. This is a minimal example I put together.
#include "rbdl/Model.h"
#include "rbdl/Dynamics.h"
#include "rbdl_model_tests/DynamicTesting.h"
int main(int argc, char const *argv[])
{
int nPts = 100;
double t0 = 0;
double t1 = 3;
double t = 0; //time
double ts = 0; //scaled time
double dtsdt = M_PI/(t1-t0); //dertivative scaled time
double tp = 0;
double dt = (t1-t0)/((double)nPts);
//Integration settings
double absTolVal = 1e-10;
double relTolVal = 1e-6;
double a_x = 1.0 , a_dxdt = 1.0;
Model* model = NULL;
model = new Model();
rbdlToBoost rbdlBoostModel(model);
state_type xState(2);
int steps = 0;
xState[0] = -M_PI/4.0;
xState[1] = 0;
controlled_stepper_type controlled_stepper(
default_error_checker< double , range_algebra , default_operations >
( absTolVal , relTolVal , a_x , a_dxdt )
);
integrate_adaptive(
controlled_stepper ,
model , xState , tp , t , (t-tp)/10 );//This seems to be the problem
tp = t;
return 0;
}
I am getting this error:
~/catkin_ws/src/ambf_control_system/rbdl_model_tests/src/tempDyn.cpp:37:47: required from here
/usr/include/boost/numeric/odeint/stepper/controlled_runge_kutta.hpp:481:12: error: expression cannot be used as a function
sys( x , m_dxdt.m_v ,t );
It turns out it was a simple fix
I just had to change.
I was passing in the wrong variable into the function. I had to pass in my custom class variable (rbdlBoostModel) not my RBDL model (model).
integrate_adaptive(
controlled_stepper ,
model , xState , tp , t , (t-tp)/10 );//This seems to be the problem
to
integrate_adaptive(
controlled_stepper ,
rbdlBoostModel , xState , tp , t , (t-tp)/10 );

Solving differential Equation using BOOST Libraries

I have to find next state of my robot which can be found though solving differential equations. In MATLAB i used ode45 function and on c++ i found on internet that i have to use some method like stepper runga kutta dopri5. I tried to understand its implementation and somehow got an idea. Now my states are X,Y and theta and to find next states my differential equations are
Xdot=v*cos(theta)
Ydot=v*sin(theta)
thetadot=w
Now there is a public function stepper.do_step_impl(System,state,...etc) where what i understood is that system represents a function whose parameters could be &state,dstate and t. Now here is my problem. My differential equations have variables v and w which i need in my System function but according to my understanding System function could have only fixed parameters i.e State, dstate and t. How do i put v and w in it? I hope someone understands my question. Kindly help. Below is my code
using namespace boost::numeric::odeint;
typedef std::vector< double > state_type;
void pdot_w( const state_type &state, state_type &dstate, const double t )
{
double p_robot_xdot = v*cos(state[2]);
double p_robot_ydot = v*sin(state[2]);
double thetadot = w;
dstate[0] = p_robot_xdot;
dstate[1] = p_robot_ydot;
dstate[2] = thetadot;
}
runge_kutta_dopri5<state_type> stepper;
stepper.do_step(pdot_w, state , 0, 0.01 );
The answer depends on how you want to pass the parameters v and w to the integrator.
One approach in C++11 is to use a lambda. Your function and a call to the stepper would look like this (depending on context, the capture [] may need to be more explicit):
void pdot_w(const state_type &state,
state_type & dstate,
const double t,
const double v,
const double w) {
dstate[0] = v * cos(state[2]);
dstate[1] = v * sin(state[2]);
dstate[2] = w;
}
runge_kutta_dopri5<state_type> stepper;
/* v = 4 and w = 5 example */
stepper.do_step([](const state_type & state, state_type & d_state, const double t) {
return pdot_w(state, d_state, t, 4, 5);
}, state,
0, 0.01);

pass parameters of double but get Jet<double,6>when using ceres solver

I'm a new learner to Ceres Solver, when adding the residualblock using
problem.AddResidualBlock( new ceres::AutoDiffCostFunction<Opt, 1, 6> (new Opt(Pts[i][j].x, Pts[i][j].y, Pts[i][j].z, Ns[i].at<double>(0, 0), Ns[i].at<double>(1, 0), Ns[i].at<double>(2, 0), Ds[i], weights[i]) ),
NULL,
param );
where param is double[6];
struct Opt
{
const double ptX, ptY, ptZ, nsX, nsY, nsZ, ds, w;
Opt( double ptx, double pty, double ptz, double nsx, double nsy, double nsz, double ds1, double w1):
ptX(ptx), ptY(pty), ptZ(ptz), nsX(nsx), nsY(nsy), nsZ(nsz), ds(ds1), w(w1) {}
template<typename T>
bool operator()(const T* const x, T* residual) const
{
Mat R(3, 3, CV_64F), r(1, 3, CV_64F);
Mat inverse(3,3, CV_64F);
T newP[3];
T xyz[3];
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
Rodrigues(r, R);
inverse = R.inv();
newP[0]=T(ptX)-x[3];
newP[1]=T(ptY)-x[4];
newP[2]=T(ptZ)-x[5];
xyz[0]= inverse.at<T>(0, 0)*newP[0] + inverse.at<T>(0, 1)*newP[1] + inverse.at<T>(0, 2)*newP[2];
xyz[1] = inverse.at<T>(1, 0)*newP[0] + inverse.at<T>(1, 1)*newP[1] + inverse.at<T>(1, 2)*newP[2];
xyz[2] = inverse.at<T>(2, 0)*newP[0] + inverse.at<T>(2, 1)*newP[1] + inverse.at<T>(2, 2)*newP[2];
T ds1 = T(nsX) * xyz[0] + T(nsY) * xyz[1] + T(nsZ) * xyz[2];
residual[0] = (ds1 - T(ds)) * T(w);
}
};
but when I output the x[0], I got this:
[-1.40926 ; 1, 0, 0, 0, 0, 0]
after I change the type of the x to double
I got this error :
note: no known conversion for argument 1 from ‘const ceres::Jet<double, 6>* const’ to ‘const double*’
in
bool operator()(const double* const x, double* residual) const
what's wrong with my codes?
Thanks a lot!
I am guessing you are using cv::Mat.
The reason the functor is templated is because Ceres evaluates it using doubles when it needs just the residuals, and evaluates with ceres:Jet objects when it needs to compute the Jacobian. So your attempt to fill r as
for (int i = 0; i < 3; i++){
r.at<T>(i) = T(x[i]);
cout<<x[i]<<endl;
}
are trying to convert a Jet into a double. Which is what the compiler is correctly complaining about.
you can re-write your code as (I have not compiled it, so there maybe a minor typo or two).
template<typename T>
bool operator()(const T* const x, T* residual) const {
const T inverse_rotation[3] = {-x[0], -x[1], -x[3]};
const T newP[3] = {ptX - x[3], ptY - x[4]. ptZ - x[5]};
T xyz[3];
ceres::AngleAxisRotatePoint(inverse_rotation, newP, xyz);
const T ds1 = nsX * xyz[0] + nsY * xyz[1] + nsZ * xyz[2];
residual[0] = (ds1 - ds) * w;
return true;
}
The automatic derivatives (AutoDiff) needs a templated cost function to keep track of the operations.
Please take a look at the ceres documentation (http://ceres-solver.org/nnls_modeling.html#autodiffcostfunction). There are a lot of nice examples too. I used them as starting point for my first ceres experiments.
I'm not sure if you can use ceres cost functions with OpenCV functions. In most cases Eigen is used to make the cost function.
Ceres comes with a lot of "ready-to-use" components for cost functions like yours.

ODEint: adaptive integration with arbitrary precision

Is it possible for ODEint to use adaptive integration routines with arbitrary precision arithmetic? For example, I'd like to use the Boost multiprecision libraries with the integrate_adaptive() function with a controlled stepper. The ODEint documentation gives examples for using arbitrary precision arithmetic for integrate_const(), but I can't modify them to use the adaptive integrator.
I've also tried using iterators (e.g. make_adaptive_time_iterator...) but run into similar problems. For concreteness this is a simple code I am looking to get working:
#include <iostream>
//[ mp_lorenz_defs
#include <boost/numeric/odeint.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
using namespace std;
using namespace boost::numeric::odeint;
typedef boost::multiprecision::cpp_dec_float_50 value_type;
//typedef double value_type;
typedef boost::array< value_type , 3 > state_type;
//]
//[ mp_lorenz_rhs
struct lorenz
{
void operator()( const state_type &x , state_type &dxdt , value_type t ) const
{
const value_type sigma( 10 );
const value_type R( 28 );
const value_type b( value_type( 8 ) / value_type( 3 ) );
dxdt[0] = sigma * ( x[1] - x[0] );
dxdt[1] = R * x[0] - x[1] - x[0] * x[2];
dxdt[2] = -b * x[2] + x[0] * x[1];
}
};
//]
int main( int argc , char **argv )
{
//[ mp_lorenz_int
state_type x = {{ value_type( 10.0 ) , value_type( 10.0 ) , value_type( 10.0 ) }};
auto stepper = make_controlled( 1.0e-16 , 1.0e-16 , runge_kutta_cash_karp54< state_type >() );
cout.precision(50);
integrate_adaptive( stepper ,
lorenz() , x , value_type( 0.0 ) , value_type( 0.1 ) , value_type( value_type( 1.0 ) / value_type( 2000.0 ) ) );
//]
cout << x[0] << endl;
return 0;
}
Compiling this returns the error:
Lorenz_mp2.cpp:52:19: error: no matching function for call to 'make_controlled'
auto stepper = make_controlled( value_type(1.0e-16) , value_type(1.0e-16) , runge_kutta_cash_karp54< state_type >() );
If I change the typedef for value_type to double it compiles and runs fine.
Using adaptive integrators with arbitrary precision is possible with odeint. Your code is almost correct, you only forgot to also configure the value_type (used for the internal constants of the stepper, and for the "time" variable t) to be the arbitrary precision type. If you check back in the docs (http://headmyshoulder.github.io/odeint-v2/doc/boost_numeric_odeint/tutorial/using_arbitrary_precision_floating_point_types.html) you'll see that this is done by a second template argument to the stepper. So the correct stepper definition within make_controlled should be:
runge_kutta_cash_karp54< state_type , value_type >()
With this, it should compile and run with arbitrary precision.
Independent of this issue, I would like to add that using conversion from double to a multi_prec type is potentially problematic. For example, 0.1 can not be accurately represented as a double (http://www.exploringbinary.com/why-0-point-1-does-not-exist-in-floating-point/). Hence you will end up with a multi_prec value that is not exactly 0.1. I'd advice to always convert from Integers and for example express 0.1 as value_type(1)/value_type(10).

Unittest++: test for multiple possible values

i am currently implementing a simple ray tracer in c++. I have a class named OrthonormalBasis, which generates three orthogonal unit vectors from one or two specified vectors, for example:
void
OrthonormalBasis::init_from_u ( const Vector& u )
{
Vector n(1,0,0);
Vector m(0,1,0);
u_ = unify(u);
v_ = cross(u_,n);
if ( v_.length() < ONB_EPSILON )
v_ = cross(u_,m);
w_ = cross(u_,v_);
}
I am testing all my methods with the Unittest++ framework. The Problem is, that there is more than one possible solution for a valid orthonormal basis. For example this test:
TEST ( orthonormalbasis__should_init_from_u )
{
Vector u(1,0,0);
OrthonormalBasis onb;
onb.init_from_u(u);
CHECK_EQUAL( Vector( 1, 0, 0 ), onb.u() );
CHECK_EQUAL( Vector( 0, 0, 1 ), onb.v() );
CHECK_EQUAL( Vector( 0, 1, 0 ), onb.w() );
}
sometimes it succeeds, sometimes it fails, because the vectors v and w could also have a negative 1, and still represent a valid orthonormal basis. Is there a way to specify multiple expected values? Or do you know another way to do that?
It is important, that i get the actual and expected values printed to the stdout, in order to debug the methods so this solution won't do the job:
TEST ( orthonormalbasis__should_init_from_u )
{
Vector u(1,0,0);
OrthonormalBasis onb;
onb.init_from_u(u);
CHECK_EQUAL( Vector( 1, 0, 0 ), onb.u() );
CHECK(
Vector( 0, 0, 1 ) == onb.v() ||
Vector( 0, 0,-1 ) == onb.v() );
CHECK(
Vector( 0, 1, 0 ) == onb.w() ||
Vector( 0,-1, 0 ) == onb.w() );
}
Surely if all you are testing is whether your basis is orthonormal, then that's what you need to test?
// check orthogonality
CHECK_EQUAL( 0, dot(onb.u(), onb.v));
CHECK_EQUAL( 0, dot(onb.u(), onb.w));
CHECK_EQUAL( 0, dot(onb.v(), onb.w));
// check normality
CHECK_EQUAL( 1, dot(onb.u(), onb.u));
CHECK_EQUAL( 1, dot(onb.v(), onb.v));
CHECK_EQUAL( 1, dot(onb.w(), onb.w));
One possibility is to create your own CHECK_MULTI function:
void CHECK_MULTI(TYPE actual, vector<TYPE> expected, const char* message)
{
for (element in expected) {
if (element == actual) {
// there's a test here so the test count is correct
CHECK(actual, element);
return;
}
}
CHECK(actual, expected);
}
I'd use a utility function or class so you can do something like this:
CHECK_EQUAL(VectorList(0,0,1)(0,0,-1), onb.v());
Given, that interpretation of equality is somewhat weird, but it should print you all values you want to see without the need to introduce a custom macro.
If you are worried about EQUAL in that context, a custom macro like CHECK_CONTAINS() shouldn't be too hard to do.
VectorList would be constructed as a temporary and operator() be used to insert values into the contained list of Vectors, similar to Boost.Assign.
Basic approach:
class VectorList {
std::vector<Vector> data_;
public:
VectorList(double a, double b, double c) {
data_.push_back(Vector(a,b,c));
}
VectorList& operator()(double a, double b, double c) {
data_.push_back(Vector(a,b,c));
return *this;
}
bool operator==(const Vector& rhs) const {
return std::find(data_.begin(), data_.end(), rhs) != data_.end();
}
};