Pose-graph Optimization

[Vali Vasiliu]

 

Hello Ceres community,

I am quite new to Ceres and its intricacies, as well as SLAM problems in general, but I'll try to describe my problem in an understandable manner.

For my optimization problem I receive the nodes poses as Eigen::Transform objects {{R,t},{0,0,0,1}}  (R for rotation matrix and t for translation). And the constraint between them is passed as an Eigen::Transform as well. I want to optimize the nodes poses to properly fit in the constraints given.

My error function is: e_ij = t2v (Z_ij^(-1) * ( X_i^(-1) * X_j )), where X_i is the source's node pose as a Transform matrix, X_j target's source's node pose and

Z_ij the constraint between the two of them. t2v is a function to convert from an Eigen::Transform into a 6d vector (t1,t2,t3,roll,pitch,yaw).

Lastly I have the Mahalanobis distance between the 2 nodes as: F_ij = e_ij' * covariance_matrix * e_ij. Which I was planning to have as a residual.

The covariance_matrix is passed along with my constraints from the frontend.

I tried using numerical differentiation:

struct Functor{

     

     // constraint either 16 block or 6 block if I convert it, covariance 36 sized block, source and target 6 sized block (6d vector representation)

     // residual 1 sized block residual 

     bool operator()(const double* const constraint, const double* const covariance, const double* const source , const double* const target,

               double* residuals) const

    {

        // here I would use Eigen library to arrive at my end residual

    }

};

// constraint and covariance are set constant via setParameterBlockConstant

From what I read on this group doing inverses might not go well with the optimization, apart from that I have other problems such as getting invalid error from my constraint, not exactly sure why (this I was getting even with nothing in the operator() code block). I thought of using the autodiff, yet I didn't come up with a way to compute my error in a templated fashion considering the inverse. 

An advice regarding the implementation would be much appreciated, or even a different error function which would easier to implement, but still mathematically viable.

the error function was obtained from a Slam robotics course, link below:

/http://www.dis.uniroma1.it/~grisetti/teaching/lectures-ls-slam-master/web/

[Vali Vasiliu]

  An update,

  i managed to compute the residuals for my cost function like this:

  

  struct Functor{

     

      // each block is of size 6 of type (t1,t2,t3,roll,pitch,yaw) as explained above also there are 6 residuals

      bool operator() (const double* const constraint, const double* const x, const double* const y, double *residuals) const

      {

         // here I computed my error function e_ij described above, as a 6d vector and I set the residuals to take the e_ij's values

         // residuals[k] = e_ij[k]

         // the problem appears when I try to add my covariance matrix (6x6) if I add it as a parameter block (also set as a constant block) then I get an              // error, such as invalid value (-nan) and I do not know why, for testing purposes the covariance was set to identity.

      }

  };

[Sameer Agarwal]

Vali,

My replies are inline.

On Thu, Jul 2, 2015 at 1:22 AM Vali Vasiliu <vali15...@gmail.com> wrote:

 

Hello Ceres community,

I am quite new to Ceres and its intricacies, as well as SLAM problems in general, but I'll try to describe my problem in an understandable manner.

For my optimization problem I receive the nodes poses as Eigen::Transform objects {{R,t},{0,0,0,1}}  (R for rotation matrix and t for translation). And the constraint between them is passed as an Eigen::Transform as well. I want to optimize the nodes poses to properly fit in the constraints given.

My error function is: e_ij = t2v (Z_ij^(-1) * ( X_i^(-1) * X_j )), where X_i is the source's node pose as a Transform matrix, X_j target's source's node pose and

Z_ij the constraint between the two of them. t2v is a function to convert from an Eigen::Transform into a 6d vector (t1,t2,t3,roll,pitch,yaw).

A couple of things to note here. 

1. Eigen transform objects are very general transformations. You are working with purely rigid transformation. There are two ways to do go about this.

1. Use a 16 dimensional parameter block and use a local parameterization for SE(3) to make sure that you always have a rigid transformation. If you do not do that, you will get general homogeneous transformation.

2. Convert the transform into a six dimensional vector, 3 for rotation and 3 for translation.

 

Lastly I have the Mahalanobis distance between the 2 nodes as: F_ij = e_ij' * covariance_matrix * e_ij. Which I was planning to have as a residual.

The covariance_matrix is passed along with my constraints from the frontend.

I tried using numerical differentiation:

struct Functor{

     

     // constraint either 16 block or 6 block if I convert it, covariance 36 sized block, source and target 6 sized block (6d vector representation)

     // residual 1 sized block residual 

     bool operator()(const double* const constraint, const double* const covariance, const double* const source , const double* const target,

               double* residuals) const

    {

        // here I would use Eigen library to arrive at my end residual

    }

};

// constraint and covariance are set constant via setParameterBlockConstant

why are you passing the constraint and covariance as parameter block. you can, but there is no reason to.

 

From what I read on this group doing inverses might not go well with the optimization,

You are working with rigid transformation, there is no reason to invert any matrices, the inverse of the matrix is trivial to compute using the transpose of the rotation matrix.

 

apart from that I have other problems such as getting invalid error from my constraint, not exactly sure why (this I was getting even with nothing in the operator() code block). I thought of using the autodiff, yet I didn't come up with a way to compute my error in a templated fashion considering the inverse. 

do not pass the covariance as a parameter block. for the error between two nodes, if the error is 

e_ij' * covariance_matrix^{-1} * e_ij. 

the residual function you need to define is   covariance_matrix^{-1/2} e_ij

and the inverse square root of the covariance matrix can be precomputed without any autodiff and passed to the cost functor for application.

Sameer

An advice regarding the implementation would be much appreciated, or even a different error function which would easier to implement, but still mathematically viable.

the error function was obtained from a Slam robotics course, link below:

/http://www.dis.uniroma1.it/~grisetti/teaching/lectures-ls-slam-master/web/

--
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...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/d0c1c350-cec2-4f16-bae0-c2e8dac67842%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Vali Vasiliu]

 Thank you for the quick reply, Sameer.

 I applied some changes to my design, I passed my transform as a 6d vector (3 for translation + 3 for rotation).

 And I also made my own function to perform the inverse of a transform ( {{R^T, -R^T*t};{0,0,0,1}} ).

 Currently the residual is: residuals[k] = e_ij[k] without the covariance, which works, but when I try to involve the covariance in a computation it throws

a -nan value. (be it that I pass the covariance as parameter block or as an argument to the functor's constructor, the covariance is only the identity matrix)

 Regarding your question:

"why are you passing the covariance as parameter block?"

 The covariance is not a constant over the whole set of constraints, from the specification of my problem, the covariance could vary between two nodes, though the same as with the constraint should not be changed during optimization. That's why my first idea was to pass it as a parameter block and set it constant.

[Sameer Agarwal]

Vali,
Hard to debug this without seeing your code.
Sameer

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/ee3cbefa-cb1f-403c-b9b5-53c124c819b9%40googlegroups.com.

[Vali Vasiliu]

 Hello Sameer,

 I've been working on something different over the last 2 weeks, but now I'm back on my ceres code. Here is my cost functor:

struct NumFunctorOptimizationError{

NumFunctorOptimizationError(const double* cov)

:covariance(cov) {}

// 6,6,6, -- block sizes ; 6 -- residuals size

bool operator() (const double* const transf, const double* const x, const double* const y, 

double* residuals) const

{

// step 1:

// get A = constraint^(-1)*(x^(-1)*y)

std::vector<double> x_v(x, x + 6);

Eigen::MatrixXd x_inv_mat = getInverseTransformMatrix(vectorToTransform(x_v));

std::vector<double> y_v(y, y + 6);

Eigen::MatrixXd y_mat = vectorToTransform(y_v).matrix();

std::vector<double> z_v(transf, transf + 6);

Eigen::MatrixXd z_inv_mat = getInverseTransformMatrix(vectorToTransform(z_v));

Eigen::MatrixXd A = z_inv_mat * (x_inv_mat * y_mat);

// step 2:

// get (6D vector) e_ij = concat(A.translation, A.rotation.toEulerAngles)

Eigen::Matrix3d rotation_A;

rotation_A << A(0,0), A(0,1), A(0,2),

 A(1,0), A(1,1), A(1,2),

 A(2,0), A(2,1), A(2,2);

Eigen::Vector3d eulerAngles_A = rotation_A.eulerAngles(0,1,2);

Eigen::VectorXd error_vector(6);

error_vector << A(0,3), A(1,3), A(2,3),

eulerAngles_A[0], eulerAngles_A[1], eulerAngles_A[2]; // roll, pitch, yaw

// step 3:

// get residuals[0] = e_ij

for(unsigned int i = 0; i < 6; i++)

residuals[i] = error_vector[i];

return true;

}

const double* covariance;

};

 And here are my settings for the solver:

solver_options.use_nonmonotonic_steps = true;

solver_options.num_threads = NUM_THREADS; // NUM_THREADS currently 1

// Solver::Options::linear_solver_type by default SPARSE_NORMAL_CHOLESKY

solver_options.use_inner_iterations = true;

The test input for the solver is:

VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1

VERTEX_SE3:QUAT 2 0 0 0 0 0 0 1

VERTEX_SE3:QUAT 3 0 0 0 0 0 0 1

FIX 1

EDGE_SE3:QUAT 1 2 1 0 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

EDGE_SE3:QUAT 2 3 0 1 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

EDGE_SE3:QUAT 3 1 -0.8 -0.7 0.1 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

This is a g2o type format that I use with g2o_viewer for visualizing the results. Basically vertex has an index, translation(3 parameters) and quaternion,

fix 1 means that the vertex 1 should not be changed( which I set with setParameterBlockConstant), the edge has source id, target id, translation, quaternion and the information matrix(20 -- upper triangular). The edges are the constraints in my problem. The edges and the vertexes are transformation matrices in my problem, but as I said I use this output for visualization.

The output to the left is g2o and the one to the right is Ceres. The Ceres one should have been resembling the g2o, but I do not know what the issue might be.

Ceres results:
VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1

VERTEX_SE3:QUAT 2 0.266596 0.233271 -0.0333244 0 0 0 1

VERTEX_SE3:QUAT 3 0.533244 0.466589 -0.0666556 0 0 0 1

g2o results:

VERTEX_SE3:QUAT 1 0 0 0 0 0 0 1 

VERTEX_SE3:QUAT 2 0.9592 -0.0665657 -0.0131483 -0.00922077 -0.00931239 0.0788347 0.996802 

VERTEX_SE3:QUAT 3 0.761413 0.854265 -0.0461453 0.00939163 -0.0173813 0.0557359 0.99825

When adding a new vertex and a new edge (left g2o, right ceres):

New vertex, new edge

VERTEX_SE3:QUAT 4 0 0 0 0 0 0 1
EDGE_SE3:QUAT 3 4 0 1 0 0 0 0 1 1 0 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 1 0 0 0 1 0 0 1 0 1

The error function should be the same in g2o and the one I used in the cost functor, while the trust region strategy is levenberg for both. I can't pinpoint where the change in results comes from, though I tried different settings for ceres, the results resemble the pictures above either way.

[Sameer Agarwal]

Vali,

Can you also share the code for how you are constructing the problem? and your implementation of transformToMatrix and and getInverseTransformMatrix.

Sameer

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/a6734327-9ebb-4a2a-a5da-77f6d8b37883%40googlegroups.com.

[Vali Vasiliu]

 These are the helper functions for the cost functor:

 

Transform vectorToTransform(const ParameterBlock& vec)

{

if(vec.size() == 6)

{

Transform tf(Eigen::Translation<double, 3> (vec[0],vec[1],vec[2]));

double roll = vec[3];

double pitch = vec[4];

double yaw = vec[5];

Eigen::Affine3d rx,ry,rz,r;

rx = Eigen::Affine3d(Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX()));

ry = Eigen::Affine3d(Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY()));

rz = Eigen::Affine3d(Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ()));

r = rz*ry*rx;

tf.linear() = r.rotation();

return tf;

} else

{
                // here should be some error handling

}

}

 the Transform type is just an Eigen::Transform type.

Eigen::MatrixXd getInverseTransformMatrix(const Transform& tf)

{

Eigen::Matrix<double,4,4> inverse_mat;

Eigen::MatrixXd rot = tf.rotation().matrix();

Eigen::MatrixXd trans = tf.translation().matrix();

rot.transposeInPlace();

Eigen::Vector3d second = -rot*trans;

for(unsigned int i = 0; i < 4; i++)

  for(unsigned int j = 0; j < 4; j++)

  {

if(i == 3 && j == 3) inverse_mat(i,j) = 1;

else if(i == 3) inverse_mat(i,j) = 0;

else if(j == 3) inverse_mat(i,j) = second[i];

else inverse_mat(i,j) = rot(i,j);

  } 

return inverse_mat;

}

ParameterBlock transform2vector(const Transform& tf)

{

ParameterBlock pb(6);

Eigen::Matrix3d rotation = tf.rotation().matrix();

Eigen::Vector3d eulerAngles = rotation.eulerAngles(0,1,2);

Eigen::Vector3d translation = tf.translation().matrix();

pb[0] = translation[0];

pb[1] = translation[1];

pb[2] = translation[2];

pb[3] = eulerAngles[0]; // roll

pb[4] = eulerAngles[1]; // pitch

pb[5] = eulerAngles[2]; // yaw

return pb;

} // ParameterBlock is a typedef for std::vector<double> 

I tested the individual functions and they seemed to work properly when I was switching between different representations back and forth.

for constructing the problem:

... // vec6d_map is a map for vertices <id, 6d vector representation>

double *x = &vec6d_map[source][0];

double *y = &vec6d_map[target][0];
...

constraint_list.push_back(transform2vector(tf));

double *transf = &constraint_list[constraint_list.size()-1][0];
...

problem.AddResidualBlock(

new ceres::NumericDiffCostFunction<NumFunctorOptimizationError,ceres::CENTRAL,6,6,6,6>

(new NumFunctorOptimizationError(covf)), NULL, transf, x, y);

...

[Sameer Agarwal]

My apologies in the delay in replying.

The only thing I can think of is that there is something strange going on with your rotation <-> euler angle conversions. There are known singularities in that parameterization.

One suggestion would be to just take a single cost function for a pair of poses at the point which it is giving you NaN (this should be indicated in the logging) and evaluate it in a stepwise manner to observe where the NaN is being introduced. 

Sameer

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/8d2dc878-7dcf-4a44-885d-8b59348d4d1f%40googlegroups.com.