se3 parameterization, unable to hold translation part constant

[Adrian Haarbach]

Hi,

I implemented se3 poses using parameter blocks of size 7 (4 quaternion coeffs, then 3 translation coeffs), and for testing purposes want to be able to hold either orientation or position constant like so:

ceres::LocalParameterization* se3_param;

if(FLAGS_oriFixed){
    se3_param = new ceres::SubsetParameterization(7,std::vector<int>{0,1,2,3});
}else if(FLAGS_posFixed){
    se3_param = new ceres::ProductParameterization(new eigen_quaternion::EigenQuaternionParameterization(),new ceres::SubsetParameterization(3,std::vector<int>{0,1,2}));
}else{
    se3_param = new ceres::ProductParameterization(new eigen_quaternion::EigenQuaternionParameterization(),new ceres::IdentityParameterization(3));
}

Holding the orientation constant (FLAGS_oriFixed) is no problem by using SubsetParameterization and passing the indices of the first 4 quaternion coeffs. Holding nothing constant also works fine.
However, I am having trouble when holding the translation coeffs (FLAGS_posFixed) constant, because of the requirement to use a LocalParameterization for the Quaternion to make optimization more efficient.

In particular, the part

new ceres::SubsetParameterization(3,std::vector<int>{0,1,2}

fails with:

local_parameterization.cc:103 Check failed: constant_parameters.size() < size Number of parameters held constant should be less than the size of the parameter block. If you wish to hold the entire parameter block constant, then a efficient way is to directly mark it as constant instead of using a LocalParameterization to do so.

I looked in the ceres code and found the reason:

CHECK_LT(constant_parameters.size(), size)
    << "Number of parameters held constant should be less "
    << "than the size of the parameter block. If you wish "
    << "to hold the entire parameter block constant, then a "
    << "efficient way is to directly mark it as constant "
    << "instead of using a LocalParameterization to do so.";

so is there another way to set part of a ProductParameterization constant? Or could you soften the requirement above, e.g replace CHECK_LT with CHECK_LE?

thanks!

Adrian

[Sameer Agarwal]

Adrian,

Interesting problem. This is a bug IMO, but it relates to a bigger issue, which is whether local parameterizations can have a local size of zero. 

I need to think the implications of relaxing that constraint through. But doing that and actually making sure everything works will take me some time. In the interim you can write your own local parameterization which does the needful. 

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/c43ff099-d7e5-4b06-ad60-e645e717d2b7%40googlegroups.com.
For more options, visit https://groups.google.com/d/optout.

[Luis-Jorge Romeo]

I usually represent my orientations with quaternions in ceres, and I've always used separated parameter blocks for translation and orientation. 

Never had problems using SetParameterBlockConstant() in translations nor orientations.

Regards,

Jorge Romeo

To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver+unsubscribe@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/c43ff099-d7e5-4b06-ad60-e645e717d2b7%40googlegroups.com.
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+unsubscribe@googlegroups.com.

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

[Sameer Agarwal]

Jorge,

Representing them with separate parameter blocks results in a certain amount of performance loss. Which may not matter in some cases though.

Sameer

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/c43ff099-d7e5-4b06-ad60-e645e717d2b7%40googlegroups.com.
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/CAK0oyEpGSGfPjQKAw9wRTxW_k9O%2BRSXyG6yy8UWJ8Nqq5MPPDQ%40mail.gmail.com.

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/CAB_M7uX8joGR5POX_Evz6Wk4obkSVfOF5af-PJZ2Tp5YAPQgEQ%40mail.gmail.com.

[Luis-Jorge Romeo]

Hi Sameer,

you refer to performance loss in terms of computing time or precision?

Regards,

To unsubscribe from this group and stop receiving emails from it, send an email to ceres-solver+unsubscribe@googlegroups.com.

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/c43ff099-d7e5-4b06-ad60-e645e717d2b7%40googlegroups.com.
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+unsubscribe@googlegroups.com.

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

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

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CAB_M7uX8joGR5POX_Evz6Wk4obkSVfOF5af-PJZ2Tp5YAPQgEQ%40mail.gmail.com.
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+unsubscribe@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/CABqdRUAVSLhBHTa7KTHgwLdYLRbA8EUTUvyAaOK0PLHJ%3DrJFJA%40mail.gmail.com.

[Sameer Agarwal]

computing time.

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/c43ff099-d7e5-4b06-ad60-e645e717d2b7%40googlegroups.com.
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/CAK0oyEpGSGfPjQKAw9wRTxW_k9O%2BRSXyG6yy8UWJ8Nqq5MPPDQ%40mail.gmail.com.

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/CAB_M7uX8joGR5POX_Evz6Wk4obkSVfOF5af-PJZ2Tp5YAPQgEQ%40mail.gmail.com.
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/CABqdRUAVSLhBHTa7KTHgwLdYLRbA8EUTUvyAaOK0PLHJ%3DrJFJA%40mail.gmail.com.

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/CAB_M7uWGd8Mf%3DO-8t9pU2grsjayWOKqCwq8zQWbekctnUMTyOQ%40mail.gmail.com.

[Sameer Agarwal]

Adrian,

https://ceres-solver-review.googlesource.com/#/c/8820/ is out for review, which should address this issue.

Sameer

[Adrian Haarbach]

Sameer,

thanks for integrating the change into 1.12.0-rc1.

Jorge,

I do have cost functions depending on up to 8 SE(3) poses (surface measurement errors between two different time instants on a cubic spline trajectory), and If I would split orientation and position into separate parameter blocks, that would make 16, which becomes quite messy, since position and orientation do belong together, but have to be passed to the cost function separately without introducing typos.

But even more seriously, the limit on the size of the parameter blocks in AutoDiffCostFunction is 10 (http://ceres-solver.org/nnls_modeling.html#_CPPv2N5ceres20AutoDiffCostFunctionE). I don't want to use DynamicAutoDiffCostFunction just because I have to split up my parameters.

All,

In general, I think it would be a nice feature to add a new composite type SE(3)/rigid body motion, consisting of a translation vector and a quaternion, to the next Ceres Version. Rigid body motions are used extensively and in each example I look, the translation and the rotation part has to be treated separately, instead of just using an overloaded * operator, like in Eigen::Isometry3d().

Adrian

[Sameer Agarwal]

Hi Adrian,

All,

In general, I think it would be a nice feature to add a new composite type SE(3)/rigid body motion, consisting of a translation vector and a quaternion, to the next Ceres Version. Rigid body motions are used extensively and in each example I look, the translation and the rotation part has to be treated separately, instead of just using an overloaded * operator, like in Eigen::Isometry3d().

So this should be quite straightforward to do today with ProductParameterization. Are you saying that we should add the corresponding ProductParameterization object as part of the ceres API?

Or are you asking for something more with say more extensive support for SE(3) transformations? 

Sameer

 

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/0e3bb1e7-3b6f-4afe-979a-a8f296d0ac3e%40googlegroups.com.

[Adrian Haarbach]

Hi Sameer,

the latter.

But the question remains of where one should define such a RigidBodyMotion3 interface. Directly in Ceres or in the Eigen geometry module?

Unfortunately the Eigen Transform interface (https://eigen.tuxfamily.org/dox-devel/classEigen_1_1Transform.html), of which Affine3 and Isometry3 are subclasses, uses a matrix to store the transformation. I do not know if there is a possibility to use a different internal storage format.

Maybe I can talk to the Eigen guys and convince them that Eigen needs a RigidBodyMotion3 interface, which can be implemented using a vector and a unit quaternion rather than a matrix.

Then, a call to RigidBodyMotion3d()::data() should return the 7d vector of position and quaternion coefficients on which ceres can apply its RigidBodyMotion3ProductParameterization.

Adrian

[Adrian Haarbach]

I just saw that the support for a SE(3) transformation class was asked in the Eigen KDE forum quite some time ago:

Extend eigen: add new class:
I would like to extend Eigen in such a way to support "Frames". A frame is a 7D vector containing a quaternion and a vector3: hence it is a compact representation of an homogeneous matrix (only much less redundant).

https://forum.kde.org/viewtopic.php?f=74&t=118581#p297313

The answer that ggael (Eigen's main developer) gave was to just define such a Frame class outside of eigen:

template<typename Scalar>
class Frame {
Quaternion<Scalar> m_orientation;
Matrix<Scalar,3,1> m_position;
};

which is similar to what I am using just that I use Eigen's Map<> interface for Quaternion and Vector3 and save the actual coefficiens in a private Scalar[7] coeffs array, which can be accessed via .data() and passed to the cost functions.

Adrian

[Sameer Agarwal]

Adrian,

Isn't part of what you are asking for addressed by Sophus?

https://github.com/strasdat/Sophus

Sameer

To view this discussion on the web visit https://groups.google.com/d/msgid/ceres-solver/840e52b0-873d-41d7-ad76-649b53674b3c%40googlegroups.com.

[Adrian Haarbach]

Yes, partly, Sophus' SE3Group does store a Quaternion and a translation vector sequentially. 

Here are the relevant code extracts, stripped of all but member variable definitions:

/**
 * \brief SO3 default type - Constructors and default storage for SO3 Type
 */
template <typename _Scalar>
class SO3Group {
    Eigen::Quaternion<Scalar> unit_quaternion_;
};

/**
 * \brief SE3 default type - Constructors and default storage for SE3 Type
 */
template <typename _Scalar>
class SE3Group{
    Sophus::SO3Group<Scalar> so3_;
    Eigen::Matrix<Scalar, 3, 1> translation_;
};

source: sophus/so3.hpp and sophus/se3.hpp, 

which is indeed the same as the Frame class from above.

I am just a little worried that se3.data() == so3.data() == unit_quaternion_.coeffs().data();

really always returns the correct array of 7 doubles laid out sequentially and without any padding, even when EIGEN_MAKE_ALIGNED_OPERATOR_NEW is present.

Also, I find the API of sophus/se3.hpp too verbose for the use case that I am mostly interested in.