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 ?
Designs
Child items
...
Show closed items
Linked items
0
Link issues together to show that they're related.
Learn more.
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.
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 ?
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...)