Exemple #1
0
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;
}
Exemple #3
0
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;
}
Exemple #4
0
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;
}
Exemple #5
0
// --------- 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;
} 
Exemple #6
0
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;
}
Exemple #7
0
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;
}