Verified Commit 4241b221 authored by Justin Carpentier's avatar Justin Carpentier Committed by Justin Carpentier
Browse files

[Joint] Fix inversion

parent 2269be6b
......@@ -436,7 +436,7 @@ namespace se3
// compute inverse
data.Dinv.setIdentity();
I.template block<3,3>(Inertia::ANGULAR,Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
data.UDinv.template middleRows<3>(Inertia::LINEAR) = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
......
......@@ -432,7 +432,7 @@ namespace se3
// compute inverse
data.Dinv.setIdentity();
I.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).llt().solveInPlace(data.Dinv);
data.U.template middleRows<3>(Inertia::LINEAR).llt().solveInPlace(data.Dinv);
data.UDinv.template middleRows<3>(Inertia::LINEAR).setIdentity(); // can be put in data constructor
data.UDinv.template middleRows<3>(Inertia::ANGULAR).noalias() = data.U.template middleRows<3>(Inertia::ANGULAR) * data.Dinv;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment