Rotation Averaging With CERES

[AVISHEK CHATTERJEE]

I am trying to implement the relative rotation averaging problem using L_{1/2} loss function in ceres. Following is my code.

I have a doubt. In the following code whether L_{1/2} loss function is applied to individual components of the residuals (residuals[0], residuals[1] and residuals[2] separately) or is in applied to the length / norm of the vector [residuals[0], residuals[1] residuals[2]].

In other words which of the following is minimized in the code bellow

(1) L_{1/2}(residuals[0]) + L_{1/2}(residuals[1]) + L_{1/2}(residuals[2])

or (2) L_{1/2}( sqrt(residuals[0]*residuals[0]+residuals[1]*residuals[1]+residuals[2]*residuals[2]))

With thanks in advance.

/*Relative Rotation Averaging

Problem Description:

Input: Pairwise relative rotations R_{ij} between the nodes i and j

Output: Absolute rotations R_k for node k

Such that R_{ij}=R_j*R_i^T

We minimize 

\sum_{ij}  \rho( norm( RotationMatrixToAngleAxis(inv(R_j)*R_{ij}*R_i ) ) )

-------------------------------------------------------------------------------------------------*/

#include "ceres/ceres.h"

#include "glog/logging.h"

#include "stdio.h"

#include "stdlib.h"

#include "ceres/rotation.h"

#include "ceres/loss_function.h"

using ceres::AutoDiffCostFunction;

using ceres::CostFunction;

using ceres::Problem;

using ceres::Solver;

using ceres::Solve;

///////////////////////////////////////////////////////////////////////////////////////////////////

struct ResidualError

{

ResidualError(double q0,double q1,double q2,double q3):q0(q0),q1(q1),q2(q2),q3(q3){}

template <typename T>

bool operator()(const T* const Wi, const T* const Wj, T* residuals) const 

{

T Qi[4];  ceres::AngleAxisToQuaternion(Wi,Qi); 

T Qj[4];  ceres::AngleAxisToQuaternion(Wj,Qj); 

T Qij[4]; Qij[0]=T(q0); Qij[1]=T(q1); Qij[2]=T(q2); Qij[3]=T(q3);

T Qtmp[4],Qres[4]; Qj[0]=-Qj[0]; ceres::QuaternionProduct(Qj,Qij,Qtmp); ceres::QuaternionProduct(Qtmp,Qi,Qres);

ceres::QuaternionToAngleAxis(Qres, residuals);

return true;

}

static ceres::CostFunction* Create(const double q0,const double q1,const double q2,const double q3)

{

return (new ceres::AutoDiffCostFunction<ResidualError,3,3,3>(new ResidualError(q0,q1,q2,q3)));

}

private:

double q0,q1,q2,q3;

};

///////////////////////////////////////////////////////////////////////////////////////////////////

int main(int argc, char** argv) 

{

  google::InitGoogleLogging(argv[0]);

  // Reading data from a file. The file format is as  follows

  // NumEdges(M) NumNodes(N)

  // i j (Quaternion rotation between i and j)

  // ...

  // (Initial angle-axis rotation of node i)

  // ...

  FILE* DataFile=fopen(argv[1],"r");

  int M,N;  fscanf(DataFile,"%d %d\n",&M,&N);

  int*IJ=(int*)malloc(2*M*sizeof(int));  double*Qij=(double*)malloc(4*M*sizeof(double));  double*Wk=(double*)malloc(3*N*sizeof(double));

  for(int i=0;i<M;++i){fscanf(DataFile,"%d %d %lf %lf %lf %lf\n",&IJ[2*i],&IJ[2*i+1],&Qij[4*i],&Qij[4*i+1],&Qij[4*i+2],&Qij[4*i+3]);}

  for(int i=0;i<N;++i){fscanf(DataFile,"%lf %lf %lf\n",&Wk[3*i],&Wk[3*i+1],&Wk[3*i+2]);}

  fclose(DataFile);

  //-----------------------------------------------------------------------------------------------

  // Qij is a M X 4 matrix with M number of relative rotations stored as quaternions

  // Wk is a N X 3 matrix with N number of relative rotations stored in angle-axis forms

  ceres::Problem problem;

  for (int i=0;i<M;++i){problem.AddResidualBlock(ResidualError::Create(Qij[4*i],Qij[4*i+1],Qij[4*i+2],Qij[4*i+3]),new ceres::EllHalfLoss(),Wk+3*IJ[2*i],Wk+3*IJ[2*i+1]);}

  

  Solver::Options options; options.minimizer_progress_to_stdout = true;

  Solver::Summary summary;

  Solve(options, &problem, &summary);

  

  std::cout << summary.BriefReport() << "\n";

  

  // Writing the result to a file with the format as follows

  // (Angle-axis rotation of node i)

  // ...

  FILE* ResultFile=fopen(argv[2],"w"); for(int i=0;i<N;++i){fprintf(ResultFile,"%lf %lf %lf\n",Wk[3*i],Wk[3*i+1],Wk[3*i+2]);}  fclose(ResultFile);

  return 0;

}

///////////////////////////////////////////////////////////////////////////////////////////////////

added in loss_function.h

class CERES_EXPORT EllHalfLoss : public ceres::LossFunction 

{

public:

  explicit EllHalfLoss(){ }

  virtual void Evaluate(double, double*) const;

};

///////////////////////////////////////////////////////////////////////////////////////////////////

added in loss_function.cc

void EllHalfLoss::Evaluate(double s, double rho[3]) const 

{

  rho[0] = pow(s,0.25);

  rho[1] = 0.25*rho[0]/s;

  rho[2] = -3.0/16.0*rho[0]/s/s;

}

[Sameer Agarwal]

It's the latter.

--
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/e980077c-a20a-4d83-81dc-8eb7186e7b65%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[AVISHEK CHATTERJEE]

Thank you

Another query:

I am often getteing an error "levenberg_marquardt_strategy.cc:113] Linear solver failure. Failed to compute a finite step."

However it is observed that the given result in such cases is very accurate. For example if I initialize the problem to a point very close to the ground truth then the program is terminating with such message but providing a very good solution. On the other hand when there is no error message and the program reports CONVERGENCE, often the result is far from the ground truth. Does anyone has an idea why this might be so. Am I doing any mistake. Or is it that Levenberg Marquardt is not the right choice here?

[Sameer Agarwal]

Avishek,

There are a number of different things happening here. 

On Wed, Apr 20, 2016 at 6:37 AM AVISHEK CHATTERJEE <eeavishek...@gmail.com> wrote:

Thank you

Another query:

I am often getteing an error "levenberg_marquardt_strategy.cc:113] Linear solver failure. Failed to compute a finite step."

This just means that the jacobian being factored was too ill conditioned or rank deficient. It is a warning, and LM in Ceres is designed to handle this case. Unless you are seeing a large number of these I would not worry about it. If it is the case that you are seeing too many of these, I would recommend reducing the value of Solver::Options::max_trust_region_radius.

 

However it is observed that the given result in such cases is very accurate. For example if I initialize the problem to a point very close to the ground truth then the program is terminating with such message but providing a very good solution.

 

On the other hand when there is no error message and the program reports CONVERGENCE, often the result is far from the ground truth. Does anyone has an idea why this might be so. Am I doing any mistake. Or is it that Levenberg Marquardt is not the right choice here?

If you initialize far away from the optimal solution, then local landscape decides how the algorithm progresses. You are welcome to try the dogleg solver, but in my experience LM is very robust.  What kind of convergence does the solver report? Can you share the execution log and the output of Summary::FullReport?

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...@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/5570553c-947d-45cb-818e-3ac4b5b68d6d%40googlegroups.com.

[Mike Vitus]

I am not sure if you pasted all of your code so you might already be doing this.  Since you are using a quaternion representation for your orientation you will want to use the quaternion local parameterization provided with Ceres to update your parameter values. You can find an example of how to use this in the bundle adjuster sample in Ceres.

cheers,
Mike

[AVISHEK CHATTERJEE]

Thanks a lot. As you have rightly pointed out, the rotation averaging problem, as implemented in the above code, is illposed because I did not fix the gauge freedom there. I guessed that and therefore added another cost function as followed

struct GaugeFreedom

{

template <typename T>

bool operator()(const T* const W0, T* residuals) const 

{

residuals[0]=W0[0]; residuals[1]=W0[1]; residuals[2]=W0[2];

return true;

}

};

//////////////////

problem.AddResidualBlock(new AutoDiffCostFunction<GaugeFreedom, 3, 3>(new GaugeFreedom), NULL, Wk);

Before this, I was also getting an error related to the Cholesky factorization. With the introduction of this new cost, the error related to the Cholesky factorization is gone, but the one with LM remained.

I shall share detailed log/report soon.

[AVISHEK CHATTERJEE]

Thanks Mike. I am using angle-axis parameterization for the unknown rotations (values to be solved). This is because with angle-axis parameterization the problem becomes unconstrained. I think it is difficult to implement the constraints like norm of a quaternion should be one or the rotation matrices should be orthonormal with positive determinant. Please correct me if I am wrong because I am new to ceres.

However, to multiply rotations, I need to use either quaternions or matrix form. Since the quaternion multiplication is cheaper, I am using this. I kept the given relative rotations in quaternion form to save some computation in conversion. 

I am pasting my current code here. Please see if you can suggest any improvement.

struct GaugeFreedom

{

template <typename T>

bool operator()(const T* const W0, T* residuals) const 

{

residuals[0]=W0[0]; residuals[1]=W0[1]; residuals[2]=W0[2];

return true;

}

};

///////////////////////////////////////////////////////////////////////////////////////////////////

int main(int argc, char** argv) 

{

  google::InitGoogleLogging(argv[0]);

  // Reading data from a file. The file format is as  follows

  // NumEdges(M) NumNodes(N)

  // i j (Quaternion rotation between i and j)

  // ...

  // (Initial angle-axis rotation of node i)

  // ...

  FILE* DataFile=fopen(argv[1],"r");

  int M,N;  fscanf(DataFile,"%d %d\n",&M,&N);

  int*IJ=(int*)malloc(2*M*sizeof(int));  double*Qij=(double*)malloc(4*M*sizeof(double));  double*Wk=(double*)malloc(3*N*sizeof(double));

  for(int i=0;i<M;++i){fscanf(DataFile,"%d %d %lf %lf %lf %lf\n",&IJ[2*i],&IJ[2*i+1],&Qij[4*i],&Qij[4*i+1],&Qij[4*i+2],&Qij[4*i+3]);}

  for(int i=0;i<N;++i){fscanf(DataFile,"%lf %lf %lf\n",&Wk[3*i],&Wk[3*i+1],&Wk[3*i+2]);}

  fclose(DataFile);

  //-----------------------------------------------------------------------------------------------

  // Qij is a M X 4 matrix with M number of relative rotations stored as quaternions

  // Wk is a N X 3 matrix with N number of relative rotations stored in angle-axis forms

  ceres::Problem problem;

  for (int i=0;i<M;++i){problem.AddResidualBlock(ResidualError::Create(Qij[4*i],Qij[4*i+1],Qij[4*i+2],Qij[4*i+3]),new ceres::EllHalfLoss(),Wk+3*IJ[2*i],Wk+3*IJ[2*i+1]);}

  problem.AddResidualBlock(new AutoDiffCostFunction<GaugeFreedom, 3, 3>(new GaugeFreedom), NULL, Wk);

  Solver::Options options; options.minimizer_progress_to_stdout = true;

  Solver::Summary summary;

  Solve(options, &problem, &summary);

  

  std::cout << summary.BriefReport() << "\n";

  

  // Writing the result to a file with the format as follows

  // (Angle-axis rotation of node i)

  // ...

  FILE* ResultFile=fopen(argv[2],"w"); for(int i=0;i<N;++i){fprintf(ResultFile,"%lf %lf %lf\n",Wk[3*i],Wk[3*i+1],Wk[3*i+2]);}  fclose(ResultFile);

  return 0;

}

///////////////////////////////////////////////////////////////////////////////////////////////////

[Sameer Agarwal]

you can just hold one of the rotations constant.

won't that fix your gauge?

--

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/0b4cbc67-8536-4f80-9b44-de778fa0dec6%40googlegroups.com.

[AVISHEK CHATTERJEE]

Holding a rotation constant will of course fix the gauge freedom. In fact, in a different implementation,  (with our own optimization method) we keep the rotation of first node equal to zero. 

However, in the framework of ceres, I found an alternative approach slightly easier to implement. If I fix the rotation of the first node to zero. Then in "ResidualError"

I will have to check if any one the two nodes being considered is the first node or not. Depending on that I will have three different types of residual computation. That is also possible. However, instead of that I simply added one more equation/cost function, where I want the rotation of the first node to be equal to zero. These two approaches are equivalent. I just chose the later as it saved a few lines of coding for me. However, I shall try the alternative approach too, to see if anything changes.

[moll....@arcor.de]

Hi

Wouldn't simply setting the parameter block that corresponds to the first camera constant suffice (see http://ceres-solver.org/modeling.html#Problem::SetParameterBlockConstant__doubleP)?

As far as I can see, that way you would not have to encode any special case in your cost function.

Markus

----- Original Nachricht ----
Von: AVISHEK CHATTERJEE <eeavishek...@gmail.com>
An: Ceres Solver <ceres-...@googlegroups.com>
Datum: 21.04.2016 09:24
Betreff: Re: [ceres-solver] Re: Rotation Averaging With CERES

> <http://ceres-solver.org/nnls_modeling.html#_CPPv2N5ceres26QuaternionParamet
> erizationE>

> >>> provided with Ceres to update your parameter values. You can find an
> >>> example of how to use this in the bundle adjuster sample
> >>>

> <https://github.com/ceres-solver/ceres-solver/blob/master/examples/bundle_ad
> juster.cc#L306>

> >> email to ceres-solver...@googlegroups.com <javascript:>.

> >> To view this discussion on the web visit
> >>
> https://groups.google.com/d/msgid/ceres-solver/0b4cbc67-8536-4f80-9b44-de778
> fa0dec6%40googlegroups.com
> >>

> <https://groups.google.com/d/msgid/ceres-solver/0b4cbc67-8536-4f80-9b44-de77
> 8fa0dec6%40googlegroups.com?utm_medium=email&utm_source=footer>
> >> .

> >> 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...@googlegroups.com.
> To view this discussion on the web visit

> https://groups.google.com/d/msgid/ceres-solver/3c8c746f-6990-4e85-8063-a143f
> 19f0067%40googlegroups.com.

[AVISHEK CHATTERJEE]

Thanks, this seems to be helpful. But I have a doubt. I want to set first rotation to zero, i.e. from Wk to Wk+2. If I pass Wk the starting address of the parameters to SetParameterBlockConstant, How do I specify the number of parameters to be kept constant (3 in this case.) Is it determined by the length specified in AutoDiffCostFunction<>. If so, and if I have multiple parameter lengths passed to AutoDiffCostFunction<>, then which one is used?

[Pierre Moulon]

Hi,

All your "parameter block" will be set as constant else you have to use a ceres::SubsetParameterization in order to tell which group of ids must remain constant.

Regards/Cordialement,

Pierre M

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/16baa27f-01fc-43db-88a2-4ccec056c2e1%40googlegroups.com.

[AVISHEK CHATTERJEE]

After having gone through the documentations, I guess ceres keeps a fixed length for each starting address of the parameters. This lengths are decided during the calls to AddResidualBlock. And therefore it is enough to specify the starting address of the block which is to be kept constant. Length is automatically decided. Am I correct?  

[Sameer Agarwal]

On Thu, Apr 21, 2016 at 12:24 AM AVISHEK CHATTERJEE <eeavishek...@gmail.com> wrote:

Holding a rotation constant will of course fix the gauge freedom. In fact, in a different implementation,  (with our own optimization method) we keep the rotation of first node equal to zero. 

However, in the framework of ceres, I found an alternative approach slightly easier to implement. If I fix the rotation of the first node to zero. Then in "ResidualError"

I will have to check if any one the two nodes being considered is the first node or not. Depending on that I will have three different types of residual computation. That is also possible. However, instead of that I simply added one more equation/cost function, where I want the rotation of the first node to be equal to zero. These two approaches are equivalent. I just chose the later as it saved a few lines of coding for me. However, I shall try the alternative approach too, to see if anything changes.

You do not need to do any checks inside your cost function. You evaluate the cost function for a pair of rotations like you do normally and just set one of the rotations constant by calling SetParameterBlockConstant. Ceres takes care of making sure that the jacobian for that parameter block/rotation is zero and that it does not participate in the optimization.

Sameer

 

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/3c8c746f-6990-4e85-8063-a143f19f0067%40googlegroups.com.

[Sameer Agarwal]

It seems that you are using one large parameter block for all your rotations. You should be using one parameter block per rotation. And use SparseNormalCholesky as your linear solver.

Sameer

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/16baa27f-01fc-43db-88a2-4ccec056c2e1%40googlegroups.com.

[Chris Sweeney]

Hi Avishek,

An example of setting up a global rotations solver with Ceres can be found at my Theia library's nonlinear rotations solver. This is minimizing the relative rotation distance with the angle axis parameterization, so your problem would basically be the same but using your own cost function instead of the nonlinear one here.

Chris

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CABqdRUBs0vBNthXJ%2BE7_9iSHFcP0qAgME6LL2JZMdGhgDuZrcA%40mail.gmail.com.

[Pierre Moulon]

Hi Chris,

I saw that in your refinement you can apply a weight to the residual.

But as you are using a single loss_function... do you not prune too much values...

In the OpenMVG implementation of the L2 global rotation refinement I use the edge weight to re scale the loss function.

https://github.com/openMVG/openMVG/blob/master/src/openMVG/multiview/rotation_averaging_l2.cpp#L251

What is your feeling about this point?

Regards/Cordialement,

Pierre M

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CAB_h4O7DLPwVmKDF-dAjazg2ohNqC%3DENr3%2B1_MLCuc0WJnoVNA%40mail.gmail.com.

[Chris Sweeney]

The nonlinear solver in Theia uses a SoftL1 loss to try and remain robust to outliers (there is more outlier filtering that can be done before calling this function). In some limited tests, weighting the relative rotations based on e.g. inlier count did not affect performance much --  it will only converge to an acceptable solution if the initialization is very good even with a robust cost function.

If you want to chat more about this feel free to ping me. I don't want to steer the conversation too far away from Avishek's question on this thread.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CADjmpz9%2BKR8SbO9pqcpP-Nc3Tc5%2BqXCJdj%2BXqn0U6PTmqK9g8w%40mail.gmail.com.

[Mike Vitus]

That'll teach me for quickly skimming the code :-). 

[Sameer Agarwal]

you do not need to implement the constraint that the quaternion need to be unit norm etc.

just start with initial guesses of the quaternions to be unit norm and associated a quaternion parameterization, it will ensure that what you get out is also unit norm.

--
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/97ccb6d1-fcd3-41d6-a718-7546fa174454%40googlegroups.com.

[AVISHEK CHATTERJEE]

Hi all, I observe that when I feed the analytically evaluated Jacobian the solver gives slightly different results than if I use automatic differentiation with everything else being same in both the approaches. What could be the possible reasons. Is it that the difference in numerical precision of the automatic Jacobian and the Analytic Jacobian accumulated over iterations causing this difference? Is there any other possible reason? Is it possible to ensure that both give the same result? 

[Sameer Agarwal]

Threading is the other possible reason, are you using multi-threading?

--

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/79192efe-b38a-4aa6-b6d8-9ca81475d853%40googlegroups.com.