float OptSO3::linesearch(Matrix3f& R, Matrix3f& M_t_min, const Matrix3f& H,
float N, float t_max, float dt)
{
Matrix3f A = R.transpose() * H;
EigenSolver<MatrixXf> eig(A);
MatrixXcf U = eig.eigenvectors();
MatrixXcf invU = U.inverse();
VectorXcf d = eig.eigenvalues();
#ifndef NDEBUG
cout<<"A"<<endl<<A<<endl;
cout<<"U"<<endl<<U<<endl;
cout<<"d"<<endl<<d<<endl;
#endif
Matrix3f R_t_min=R;
float f_t_min = 999999.0f;
float t_min = 0.0f;
//for(int i_t =0; i_t<10; i_t++)
for(float t =0.0f; t<t_max; t+=dt)
{
//float t= ts[i_t];
VectorXcf expD = ((d*t).array().exp());
MatrixXf MN = (U*expD.asDiagonal()*invU).real();
Matrix3f R_t = R*MN.topLeftCorner(3,3);
float detR = R_t.determinant();
float maxDeviationFromI = ((R_t*R_t.transpose()
- Matrix3f::Identity()).cwiseAbs()).maxCoeff();
if ((R_t(0,0)==R_t(0,0))
&& (abs(detR-1.0f)< 1e-2)
&& (maxDeviationFromI <1e-1))
{
float f_t = evalCostFunction(R_t)/float(N);
#ifndef NDEBUG
cout<< " f_t = "<<f_t<<endl;
#endif
if (f_t_min > f_t && f_t != 0.0f)
{
R_t_min = R_t;
M_t_min = MN.topLeftCorner(3,3);
f_t_min = f_t;
t_min = t;
}
}else{
cout<<"R_t is corruputed detR="<<detR
<<"; max deviation from I="<<maxDeviationFromI
<<"; nans? "<<R_t(0,0)<<" f_t_min="<<f_t_min<<endl;
}
}
if(f_t_min == 999999.0f) return f_t_min;
// case where the MN is nans
R = R_t_min;
#ifndef NDEBUG
#endif
cout<<"R: det(R) = "<<R.determinant()<<endl<<R<<endl;
cout<< "t_min="<<t_min<<" f_t_min="<<f_t_min<<endl;
return f_t_min;
}
int main()
{
Matrix3f A;
A << 1, 2, 1,
2, 1, 0,
-1, 1, 2;
cout << "Here is the matrix A:\n" << A << endl;
cout << "The determinant of A is " << A.determinant() << endl;
cout << "The inverse of A is:\n" << A.inverse() << endl;
}
Eigen::Matrix4f ConsistencyTest::initPose2D( std::map<unsigned, unsigned> &matched_planes )
{
//Calculate rotation
Matrix3f normalCovariances = Matrix3f::Zero();
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
normalCovariances += PBMTarget.vPlanes[it->second].v3normal * PBMSource.vPlanes[it->first].v3normal.transpose();
normalCovariances(1,1) += 100; // Rotation "restricted" to the y axis
JacobiSVD<MatrixXf> svd(normalCovariances, ComputeThinU | ComputeThinV);
Matrix3f Rotation = svd.matrixU() * svd.matrixV().transpose();
if(Rotation.determinant() < 0)
// Rotation.row(2) *= -1;
Rotation = -Rotation;
// Calculate translation
Vector3f translation;
Vector3f center_data = Vector3f::Zero(), center_model = Vector3f::Zero();
Vector3f centerFull_data = Vector3f::Zero(), centerFull_model = Vector3f::Zero();
unsigned numFull = 0, numNonStruct = 0;
for(map<unsigned, unsigned>::iterator it = matched_planes.begin(); it != matched_planes.end(); it++)
{
if(PBMSource.vPlanes[it->first].bFromStructure) // The certainty in center of structural planes is too low
continue;
++numNonStruct;
center_data += PBMSource.vPlanes[it->first].v3center;
center_model += PBMTarget.vPlanes[it->second].v3center;
if(PBMSource.vPlanes[it->first].bFullExtent)
{
centerFull_data += PBMSource.vPlanes[it->first].v3center;
centerFull_model += PBMTarget.vPlanes[it->second].v3center;
++numFull;
}
}
if(numFull > 0)
{
translation = (-centerFull_model + Rotation * centerFull_data) / numFull;
}
else
{
translation = (-center_model + Rotation * center_data) / numNonStruct;
}
translation[1] = 0; // Restrict no translation in the y axis
// Form SE3 transformation matrix. This matrix maps the model into the current data reference frame
Eigen::Matrix4f rigidTransf;
rigidTransf.block(0,0,3,3) = Rotation;
rigidTransf.block(0,3,3,1) = translation;
rigidTransf.row(3) << 0,0,0,1;
return rigidTransf;
}
bool Triangle::any_intersect(const Ray& ray, Hit& hit, float tmin)
{
Vector3f direction = ray.getDirection();
Vector3f origin = ray.getOrigin();
float list[9] = { (a.x()) - (b.x()),(a.y()) - (b.y()),(a.z()) - (b.z()), (a.x()) - (c.x()),(a.y()) - (c.y()),(a.z()) - (c.z()), (a[0]) - (origin[0]),(a[1]) - (origin[1]),(a[2]) - (origin[2]), };
Matrix3f A = Matrix3f(list[0], list[3], direction[0], list[1], list[4], direction[1], list[2], list[5], direction[2]);
float determinant = A.determinant();
if (determinant == 0) { return false; }
float t = Matrix3f::determinant3x3(list[0], list[3], list[6], list[1], list[4], list[7], list[2], list[5], list[8]) / determinant;
if (t <= tmin || t >= hit.getT()) { return false; }
float beta = Matrix3f::determinant3x3(list[6], list[3], direction[0], list[7], list[4], direction[1], list[8], list[5], direction[2]) / determinant;
float gamma = Matrix3f::determinant3x3(list[0], list[6], direction[0], list[1], list[7], direction[1], list[2], list[8], direction[2]) / determinant;
if ((beta >= 0 && gamma >= 0 && beta + gamma <= 1))
{
return true;
}
return false;
}
// --------- deprecated --------------
void OptSO3::rectifyRotation(Matrix3f& R)
{
float detR = R.determinant();
if (abs(detR-1.0) <1e-6) return;
// use projection of R onto SO3 to rectify the rotation matrix
//cout<<"det(R)="<<R.determinant()<<endl<<R<<endl;
Matrix3f M = R.transpose()*R;
EigenSolver<Matrix3f> eig(M);
Matrix3cf U = eig.eigenvectors();
Vector3cf d = eig.eigenvalues();
if (d(2).real() > 1e-6)
{
// http://lcvmwww.epfl.ch/new/publications/data/articles/63/simaxpaper.pdf
// Eq. (3.7)
// Moakher M (2002). "Means and averaging in the group of rotations."
d = ((d.array().sqrt()).array().inverse());
Matrix3cf D = d.asDiagonal();
D(2,2) *= detR>0.0?1.0f:-1.0f;
R = R*(U*D*U.transpose()).real();
}else{
//http://www.ti.inf.ethz.ch/ew/courses/GCMB07/material/lecture03/HornOrthonormal.pdf
//Horn; Closed-FormSolutionofAbsoluteOrientation UsingOrthonormalMatrices
d = ((d.array().sqrt()).array().inverse());
d(2) = 0.0f;
Matrix3cf Sp = d.asDiagonal();
JacobiSVD<Matrix3f> svd(R*Sp.real());
R = R*Sp.real() + (detR>0.0?1.0f:-1.0f)*svd.matrixU().col(2)*svd.matrixV().col(2).transpose();
}
// Matrix3d M = (R.transpose()*R).cast<double>();
// EigenSolver<Matrix3d> eig(M);
// MatrixXd U = eig.eigenvectors();
// Matrix3d D = eig.eigenvalues();
//cout<<"det(R)="<<R.determinant()<<endl<<R<<endl;
// R.col(0).normalize();
// R.col(2) = R.col(0).cross(R.col(1));
// R.col(2).normalize();
// R.col(1) = R.col(2).cross(R.col(0));
//cout<<"det(R)="<<R.determinant()<<endl<<R<<endl;
}
bool Rectangle::intersect(const Ray& r, Hit& h, float tmin)
{
Vector3f a = vertex0;
Vector3f b = vertex0 + side_1;
Vector3f c = vertex0 + side_2;
Vector3f direction = r.getDirection();
Vector3f origin = r.getOrigin();
float list[9] = { (a.x()) - (b.x()),(a.y()) - (b.y()),(a.z()) - (b.z()), (a.x()) - (c.x()),(a.y()) - (c.y()),(a.z()) - (c.z()), (a[0]) - (origin[0]),(a[1]) - (origin[1]),(a[2]) - (origin[2]), };
Matrix3f A = Matrix3f(list[0], list[3], direction[0], list[1], list[4], direction[1], list[2], list[5], direction[2]);
float determinant = A.determinant();
if (determinant == 0) { return false; }
float t = Matrix3f::determinant3x3(list[0], list[3], list[6], list[1], list[4], list[7], list[2], list[5], list[8]) / determinant;
if (t <= tmin || t >= h.getT()) { return false; }
float beta = Matrix3f::determinant3x3(list[6], list[3], direction[0], list[7], list[4], direction[1], list[8], list[5], direction[2]) / determinant;
float gamma = Matrix3f::determinant3x3(list[0], list[6], direction[0], list[1], list[7], direction[1], list[2], list[8], direction[2]) / determinant;
if ((beta >= 0 && gamma >= 0 && beta <= 1 && gamma <= 1))
{
h.set(t, this->material, normal);
return true;
}
return false;
}
bool Triangle::intersect(const Ray& ray, Hit& hit, float tmin)
{
Vector3f direction = ray.getDirection();
Vector3f origin = ray.getOrigin();
float list[9] = { (a.x()) - (b.x()),(a.y()) - (b.y()),(a.z()) - (b.z()), (a.x()) - (c.x()),(a.y()) - (c.y()),(a.z()) - (c.z()), (a[0]) - (origin[0]),(a[1]) - (origin[1]),(a[2]) - (origin[2]), };
Matrix3f A = Matrix3f(list[0], list[3], direction[0], list[1], list[4], direction[1], list[2], list[5], direction[2]);
float determinant = A.determinant();
if (determinant == 0) { return false; }
float t = Matrix3f::determinant3x3(list[0], list[3], list[6], list[1], list[4], list[7], list[2], list[5], list[8]) / determinant;
if (t <= tmin || t >= hit.getT()) { return false; }
float beta = Matrix3f::determinant3x3(list[6], list[3], direction[0], list[7], list[4], direction[1], list[8], list[5], direction[2]) / determinant;
float gamma = Matrix3f::determinant3x3(list[0], list[6], direction[0], list[1], list[7], direction[1], list[2], list[8], direction[2]) / determinant;
if ((beta >= 0 && gamma >= 0 && beta + gamma <= 1) )
{
if(hasTex)
{
hit.hasTex = true; hit.setTexCoord((1 - beta - gamma)*texCoords[0] + beta*texCoords[1] + gamma*texCoords[2]);
}
hit.set(t, this->material, ((1-beta-gamma)*normals[0]+beta*normals[1]+gamma*normals[2]).normalized());
return true;
}
return false;
}
/**************************************************************************************************
* Procecure *
* *
* Description: moveHandRelativeToObject *
* Class : ApproachMovementSpace *
**************************************************************************************************/
Matrix4f ApproachMovementSpace::moveHandRelativeToObject(float theta, float phi, float rho, bool update)
{
RobotPtr hand = eef -> getRobot();
// Don't let the hand collide with the object
if (rho < 10) rho = 10;
// Calculate new XYZ coordinates for the hand
Vector3f new_pos = object -> getGlobalPose().topLeftCorner (3,3) * calculateCarthesianCoordinates(theta, phi, rho);
new_pos += object -> getGlobalPose().topRightCorner(3,1);
Matrix4f new_pose = hand -> getGlobalPose();
Matrix3f orientation;
// Calculate First Vector of the orientation Matrix
// It is colinear with the distance vector between object and hand
orientation.block(0,0,3,1) = object -> getGlobalPose().topRightCorner(3,1) - new_pos;
// Calculate the Remainder two orientation vectors
Matrix3f inertia = object -> getGlobalPose().topLeftCorner(3,3);
inertia *= object -> getInertiaMatrix();
// Select two vectors from the objects inertia matrix
uint vectors_left = 2;
for (uint column = 0; column < 3; column += 1)
{
Vector3f v1 = orientation.block(0,0,3,1);
if( v1.dot( inertia.col(column) ) != 1)
{
orientation.block(0,vectors_left,3,1) = inertia.col(column);
vectors_left -= 1;
if (vectors_left == 0) break;
}
}
// Check if two new vectors were included
if ( vectors_left != 0)
{
cout << endl << "[Error] Function: moveHandRelativeToObject | The two vectors from the object's inertia matrix were not selected.";
exit(-1);
}
// Make the orientation matrix orthonormalized
orientation = gramSchmidtNormalization3f(orientation);
orientation *= offset.topLeftCorner(3,3);
// Check if the new orientation is mirrored or not
// If so, correct it
if ( ( orientation.determinant() - (-1) < 0.01) &&
( orientation.determinant() - (-1) > -0.01) )
{
orientation.col(1) *= -1;
}
// Update new hand pose
new_pose.topLeftCorner (3,3) = orientation;
new_pose.topRightCorner(3,1) = new_pos - orientation * offset.topRightCorner(3,1);
// Set updated pose
if (update) hand -> setGlobalPose(new_pose);
return new_pose;
}