Averaging ofMatrix4x4 for smoother motion?

i am looking for ways to smooth transformation data that i am getting from a point cloud matching library.

i am using ICP matching which returns a matrix4x4 (more specifically Eigen::Matrix4f).
i can cast that in to a ofMatrix4x4 easily.

but now i am trying to smooth the resulting motion.

ofxCv has an euler kalman example but it is only for rotation and ignores the translation aspect of the matrix.
also i am getting some strange gimbal flipping at points of the kalman rotation.

so i was wondering how to average or smooth a matrix over time. can i just accumulate them and divide them by x?

got something to work thanks to this post:

void setTransformationMatrix(ofMatrix4x4 _mat){

        icpTransformation = icpTransformation * _mat;
        ofQuaternion quat;
        ofMatrix4x4 mat_rot, mat_trans;
        ofVec3f trans;
        ofVec3f tra1, tra2, noV;
        ofQuaternion q1,q2,noQ;
        last_icpTransformation.decompose(tra1, q1, noV, noQ);
        icpTransformation.decompose(tra2, q2, noV, noQ);
        if(tra1.distance(tra2) > 2000){
            ofLog()<<"tra1.distance(tra2) "<<tra1.distance(tra2);
            last_icpTransformation = icpTransformation;
            quat.slerp(slerp_amt, q1, q2);
            trans.x = ofLerp(tra1.x,tra2.x,slerp_amt);
            trans.y = ofLerp(tra1.y,tra2.y,slerp_amt);
            trans.z = ofLerp(tra1.z,tra2.z,slerp_amt);
            //icpTransformation = mat_rot*mat_trans;
            //last_icpTransformation = icpTransformation;
            last_icpTransformation = mat_rot*mat_trans;
1 Like

Ohhh that code is so old! Hope it works. :slightly_smiling:

yes it did work and smoothed the motion.

but i am open for more update to date options too.

somehow i feel i should be pouring all the ofmatrix4x4 stuff in to ofnode but have not really seen the need for it. but ofnode seems to be the more versatile class.

@stephanschulz go for ofNode. It will save you a lot of head aches!

but if i want to smooth between the old and the new transformation matrix (rotation and translation) i would still need to extract the quaternion and translation and use slerp for the quaternion and lerp for the position. no?

here is my latest code for smoothing rotation and translation of a ofMatrix4x4.

i derive my transformation matrix by incrementing it every frame; i.e. all i have to work with is _mat which i get from the point cloud library and it describes the difference between last and new transformation of 2 point clouds.

it’s kind of working.
if slerp and lerp amount are not the same for rotation and translation then i get weird spiralling motions.

if they are the same then the motion makes sense.
i was hoping i can have different smoothing values for rotation and translation.
here a short video showing what happens when i change slerp and lerp.

is there a better way for smoothing?

    void advanceTransformationMatrix(ofMatrix4x4 _mat, ofVec3f _blobCenter){
        modelTransformation = modelTransformation * _mat;
        ofMatrix4x4 mat_rot, mat_trans;
        ofVec3f tra1, tra2, noV;
        ofQuaternion q1,q2,noQ;
        last_modelTransformation.decompose(tra1, q1, noV, noQ);
        modelTransformation.decompose(tra2, q2, noV, noQ);
        //slerp rotation
        ofQuaternion quat;
        quat.slerp(slerp_amt, q1, q2);
        //lerp translation
        ofVec3f trans;
        trans.x = ofLerp(tra1.x,tra2.x,lerp_amt);
        trans.y = ofLerp(tra1.y,tra2.y,lerp_amt);
        trans.z = ofLerp(tra1.z,tra2.z,lerp_amt);

        last_modelTransformation =  mat_rot * mat_trans;