Pose getDirTransform(Pose transformation) {
Matrix4d dirTransformation = transformation.asMatrix();
dirTransformation.invert();
dirTransformation.transpose();
Pose dirTransform(dirTransformation);
return dirTransform;
}
int main(int, char**)
{
cout.precision(3);
Matrix4d X = Matrix4d::Random(4,4);
Matrix4d A = X + X.transpose();
cout << "Here is a random symmetric 4x4 matrix:" << endl << A << endl;
Tridiagonalization<Matrix4d> triOfA(A);
Vector3d hc = triOfA.householderCoefficients();
cout << "The vector of Householder coefficients is:" << endl << hc << endl;
return 0;
}
/**
* input needs at least 2 correspondences of non-parallel lines
* the resulting R and t works as below: x'=Rx+t for point pair(x,x');
* @param vLineA
* @param vLineB
* @param R
* @param t
*/
void
LineExtract::ComputeRelativeMotion_svd(vector<Line3d> vLineA, vector<Line3d> vLineB, Matrix3d &R, Vector3d &t) {
if (vLineA.size() < 2) {
cerr << "Error in computeRelativeMotion_svd: input needs at least 2 pairs!\n";
return;
}
// convert to the representation of Zhang's paper
for (int i = 0; i < vLineA.size(); ++i) {
Vector3d l, m;
if (vLineA[i].u.norm() < 0.9) {
l = vLineA[i].EndB - vLineA[i].EndA;
m = (vLineA[i].EndA + vLineA[i].EndB) * 0.5;
vLineA[i].u = l / l.norm();
vLineA[i].d = vLineA[i].u.cross(m);
// cout<<"in computeRelativeMotion_svd compute \n";
}
if (vLineB[i].u.norm() < 0.9) {
l = vLineB[i].EndB - vLineB[i].EndA;
m = (vLineB[i].EndA + vLineB[i].EndB) * 0.5;
vLineB[i].u = l * (1 / l.norm());
vLineB[i].d = vLineB[i].u.cross(m);
}
}
Matrix4d A = Matrix4d::Zero();
for (int i = 0; i < vLineA.size(); ++i) {
Matrix4d Ai = Matrix4d::Zero();
Ai.block<1, 3>(0, 1) = vLineA[i].u - vLineB[i].u;
Ai.block<3, 1>(1, 0) = vLineB[i].u - vLineA[i].u;
Ai.bottomRightCorner<3, 3>(1, 1) = SO3d::hat((vLineA[i].u + vLineB[i].u)).matrix();
A = A + Ai.transpose() * Ai;
}
Eigen::JacobiSVD<Matrix4d> svd(A, Eigen::ComputeFullV | Eigen::ComputeFullV);
Vector4d q = svd.matrixU().col(3);
R = Eigen::Quaterniond(q).matrix();
Matrix3d uu = Matrix3d::Zero();
Vector3d udr = Vector3d::Zero();
for (int i = 0; i < vLineA.size(); ++i) {
uu = uu + SO3d::hat(vLineB[i].u) * SO3d::hat(vLineB[i].u).matrix().transpose();
udr = udr + SO3d::hat(vLineB[i].u).transpose() * (vLineB[i].d - R * vLineA[i].d);
}
t = uu.inverse() * udr;
}
LieAlgebra MinEnFilter::computeGradient(const LieGroup& S, const MatrixXd& Gk, const MatrixXd& Disps_inhom) {
/* DO NOT CHANGE THIS FUNCTION */
LieAlgebra L;
MatrixXd iEg = ((S.E).lu().solve(Gk.transpose())).transpose();
VectorXd kappa = iEg.col(2);
MatrixXd h = iEg.leftCols(2).cwiseQuotient(kappa*MatrixXd::Ones(1,2));
MatrixXd z = Disps_inhom.leftCols(2) - h;
MatrixXd Qvec(1,2);
Qvec << q1/nPoints,q2/nPoints;
MatrixXd qvec = MatrixXd::Ones(nPoints,1) * Qvec;
MatrixXd b12 = (kappa.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(qvec.cwiseProduct(z));
MatrixXd kappa2 = kappa.cwiseProduct(kappa);
MatrixXd aux1 = (iEg.leftCols(2)).cwiseProduct(z.cwiseProduct(qvec));
MatrixXd b3 = -((kappa2.cwiseInverse()*MatrixXd::Ones(1,2)).cwiseProduct(aux1)).rowwise().sum();
MatrixXd B(nPoints,4);
B << b12, b3, MatrixXd::Zero(nPoints,1);
MatrixXd Ones = MatrixXd::Ones(1,4);
MatrixXd A1 = iEg.col(0) * Ones;
MatrixXd A2 = iEg.col(1) * Ones;
MatrixXd A3 = iEg.col(2) * Ones;
MatrixXd A4 = iEg.col(3) * Ones;
MatrixXd G1 = (B.cwiseProduct(A1)).colwise().sum();
MatrixXd G2 = (B.cwiseProduct(A2)).colwise().sum();
MatrixXd G3 = (B.cwiseProduct(A3)).colwise().sum();
MatrixXd G4 = (B.cwiseProduct(A4)).colwise().sum();
Matrix4d G;
G << G1,
G2,
G3,
G4;
G = MEFcommon::prSE(G.transpose());
if (order == 1) {
L = LieAlgebra(G);
} else if (order >=2) {
L = LieAlgebra(G, VectorXd::Zero((order-1)*6),order);
} else {
cerr << "Parameter order must be greater than zero." << endl;
exit(1);
}
return L;
}
pair<Matrix4d, Matrix4d> C3Trajectory::transformation_pair(const Vector6d &q) {
Matrix4d R;
R.block<3,3>(0, 0) = AttitudeHelpers::EulerToRotation(q.tail(3));
R.block<1,3>(3, 0).fill(0);
R.block<3,1>(0, 3).fill(0);
R(3, 3) = 1;
Matrix4d T = Matrix4d::Identity();
T.block<3,1>(0, 3) = -q.head(3);
pair<Matrix4d, Matrix4d> result;
result.first = R.transpose()*T; // NED -> BODY
T.block<3,1>(0, 3) = q.head(3);
result.second = T*R; // BODY -> NED
return result;
}
