Absolute constraints (ConstraintAbs)

I created absolute constraints for Position, Orientation and Velocity (38a5f8a7)

I called these constraints respectively ConstraintAbsP, ConstraintAbsO, ConstraintAbsV gtests related to these constraints are passing (3d9f904a)

@jsola I thought about creating a ConstraintAbsolute that could handle any of these cases but we need to know the size of the stateblocks in the constructor and... it is different if we consider P/V or Q.

These constraints can used with a FeatureFix. Next step would be to change the name of constraint_fix_3D into ConstraintAbsPose if we want to merge this in master. And if we want to :

• Are we OK with the name of these constraints
• Should I create a specific feature for each of them ? (I think we don't need to...)
• Do we want a constraint for P, Q and V ? If yes then how should we name it ?

Hi Dinesh. I have not thought much on this. What about a generic constructor like this:

ConstraintBlockAbsolute(StateBlockPtr& block, const VectorXs& state, const MatrixXs& cov)

Would it work for you? Inside, you have access to:

block->getSize()
block->getLocalSize()
state.size()
cov.rows()
cov.cols()

with which you can assert all sizes before doing the stuff. For the StateQuaternion case, it is a little more tricky, and maybe we want to create a ConstraintQuaternionAbsolute just for it.

OK let me some time to think about it... but I think it can work

what is the 'Clock' for in ConstraintClockAbsolute ?

What is ConstraintClockAbsolute? Never heard of it!

It must be ConstraintBlockAbsolute !! :-)

BTW I just implemented minus() in LocalParametrization, so that now you can compare quaternions or any other state block, just do:

if (block->getLocalParamPtr() == nullptr)
 err = abs_values - block.getState();
else
 err = block->getLocalParamPtr()->minus(block->getState(), abs_values);

With this you can compare any block against an absolute value, and get an error that you can evaluate.

Remember that errors for quaternions are 3-vectors in the tangent space, and their covariances are 3x3 matrices.

'ConstraintBlockAbsolute' implemented and working for Position and Velocity (1ef7bb5a)

next steps :

• code and test ConstraintQuaternionAbsolute
• choose one prototype and clean
• remove ConstraintAbsP, ConstraintAbsO and ConstraintAbsV from WOLF

Note : @jsola, you were proposing ConstraintBlockAbsolute(StateBlockPtr& block, const VectorXs& state, const MatrixXs& cov) but actually, state and cov are in the feature to which we add the constraint. So actually we could do

ConstraintBlockAbsolute(StateBlockPtr& block, const FeatureBasePtr& _ftr_ptr) or just ConstraintBlockAbsolute(StateBlockPtr& block)
since we usually access the state and covariance through the feature and the usual case is to add the constraint to the feature right ?

Or maybe you were thinking about other use cases for ConstraintBlockAbsolute ?

Yes, of course the references to Feature measurement and covariance could be avoided. Then only the state block would be needed.

The constraints are implemented and pushed in branch ConstraintAbs (db1debc2 for ConstraintBlockAbsolute and 94928397 for ConstraintQuaternionAbsolute)

There is only one type of constraint added in wolf.h for both of them (CTR_BLOCK_ABS)

Dinesh, about the Absolute for Quaternion, one comment: There are two ways to comupte the difference between 2 quaternions:

• diff1 = log(q1.conj * q2)
• diff2 = log(q2 * q1.conj)

In the first case, the associated diff1 error is expressed in the frame defined by q1, as you can see by isolating q2:

• q2 = q1 * exp(diff1) --> the exp on the right means local.

This would therefore be a local error. In the second case, the associated diff2 error is defined in global frame, as you see if you isolate q2:

• q2 = exp(diff2) * q1 --> the exp on the left means global.

Therefore, there are 2 possible implementations of the QuaternionAbsolute constraint. However, I can only imagine the one that sets a global error, because the absolute orientation provided to the Feature will probably be a quaternion in global reference, and the covariance of the error is likely to be expressed in global coordinates.

For example: imagine you want to constrain a quaternion to be vertical, and leave Yaw out of the constraint. You would define a covariances matrix in the global frame, like so:

• Q = [s2 0 0 ; 0 s2 0 ; 0 0 1e20]

where s2 is sigma_squared, valid for roll and pitch , and 1e10 is a huge variance for the yaw part.

If you were to use the first diff1 version, this covariance would be considered local, and therefore you would not be getting the expected result after optimization of the Absolute constraint.

So: you need to code the quaternion residual as per diff2 above, not diff1 as you did.

I recall that you might make use of the LocalParametrizationQuaternion, which you can define local or global through the template parameter DQ_LOCAL or DQ_GLOBAL, and where you have access to the method minus() which performs diff1 for DQ_LOCAL and diff2 for DQ_GLOBAL. As of now, minus() works with Eigen::Map<const MatrixXs>& as inputs, so you can call minus() with e.g.

Quaternions q1, q2;
Vector3s v;
Map<const VectorXs> qm1(q1.coeffs().data(), 4);
Map<const VectorXs> qm2(q2.coeffs().data(), 4);
Map<const VectorXs> vm(v.data(), 3);
LocalParametrizationQuaternionGlobal QPglobal;
QPglobal.minus(qm1,qm2,vm); // since 'vm' is Map, the result is available in 'v' too.

Although things are even simpler inside the ConstraintBlockAbsolute::operator() (the code is possibly incorrect, but the idea is there)

ConstraintBlockAbsolute::operator(const T* const _x, T* _residuals) const
{
    StateBlockPtr sb = [... get the relevant state block ptr ...];
    Map<const VectorXs> x_est(_x, sb->getSize());
    Map<const VectorXs> x_mea(getMeasurement().data(), sb->getSize());
    VectorXs er(sb->getLocalSize());
    Map<VectorXs> res(_residuals, sb->getLocalSize());

    if (sb->hasLocalParametrization())
    {
        Map<VectorXs> erm(er.data(), 3);
        sb->getLocalParametrizationPtr()->minus(x_est, x_mea, erm);
    }
    else
        er = x_mea - x_est;
    res = getMeasSqrtInfoMat() * er;
}

Yes so the use of minus function from localParametrization is what needs to be done next.

However I had some doubts concerning the following :

• StateQuaternion is deriving from StateBlock but the default behaviour is to create a LocalParametrizationQuaternion<DQ_LOCAL> and there seems to be no way to create a StateQuaternion with LocalParametrizationQuaternion<DQ_GLOBAL> but I may be wrong.

• Functions in Local parametrization are using Scalar MatrixBase argument but Constraints are using a template because Ceres will call this function with Jets right ?

I don't know if we can change the stateQuaternion for a global parametrization... maybe we can try. I can´t see now the impact of this on the other parts of the project...

I guess you could change the local param a posteriori to make it Global, or modify the constructor so that this can be selected on demand.

And no, Ceres will call computeJacobian() to get the jacobian of the local param, so no need for Jets here. However, ceres still calls with raw pointers, and the LocalParametrizationWrapper converts them to Eigen::Map

You can also leave ConstraintAbsoluteQuaternion a separate function, and use your instantiation of localParametrizationQuaternionGlobal to make the math, or your own math -- as long as it is global.

Having an instantiation of LocalParametrizationQuaternion<DQ_GLOBAL> in ConstraintQuaternionAbsolute class was the first idea that I had. But at the end we may need to implement some way to define whether we want a quaternion using DQ_GLOBAL or DQ_LOCAL.

The minus function is implemented so that it can be used with Scalar types, however in ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) we use both templates and Scalar vectors.

So I thought we needed to have template functions in LocalParametrizationQuaternion but this class is already using full template specialization (defining functions for both DQ_GLOBAL and DQ_LOCAL).

Thus we may need to rethink the design of this class if we want template functions

You are right there is this templates issue. I don't know how to handle it.

Maybe the best is to make a custom ConstraintQuaternionAbsolute... In a way, see that the Absolute constraint requires Global quaternion parametrization, which makes sense (relative would be Local...)

You can implement the 'minus' equivalent directly from rotations.h:

minus(q1,q2) = "q2 (-) q1" = log_q(q2 * q1.conjugate()); // this is a global 'minus'

I think I am correct :-) but not sure after holidays...

Actually we have :

minus(q1,q2) = log_q(q1.conjugate() * q2); // this is a local 'minus'

we could use this function as :

minus(q2.conjugate(),q1.conjugate()) = log_q( (q2.conjugate()).conjugate() * q1.conjugate())
= log_q(q2 * q1.conjugate()); //this is global 'minus'

but... maybe not the best option

Though not super elegant, it is interesting. Where is this code?

The code is in branch ConstraintAbs (c9f527a0)

mentioned in merge request !149 (merged)

Should we remove ConstraintFix ?

In my opinion, ConstraintFix could/should be renamed.

• for 2D, ConstraintPose2D
• for 3D, ConstraintPose3D

I think ConstraintFix3D also exists? Or is ConstraintFix aware of the dimension?

You are right : ConstraintFix3D also exists and ConstraintFix seems to be adapted to 2D cases

I´ll open a branch for a rename

I merged a renaming patch into master.

With the patch, you'll start using gitlab's continuous integration, which will trigger at each push you do in your branch.

Cool I think this issue is solved then.

I'm closing it !

closed