C++ Matrix3f::eulerAngles方法代码示例

void _eePose::getRollPitchYaw(double * roll, double * pitch, double * yaw) {
  Eigen::Quaternionf quat(qw, qx, qy, qz);

  Eigen::Matrix3f matrix = quat.toRotationMatrix();
  Eigen::Vector3f angles = matrix.eulerAngles(0,1,2);
  *roll = ((double) angles(0));
  *pitch = ((double) angles(1));
  *yaw = ((double) angles(2));

  
}

bool FruitDetector::earlyRejectAlignment(const Eigen::Matrix3f& rotation) {
  // get euler angles. (Z-Y-X) convention --> (Yaw, Pitch, Roll)
  const Eigen::Vector3f euler_angles = rotation.eulerAngles(2, 1, 0);
  const float yaw = euler_angles[0];
  const float pitch = euler_angles[1];
  const float roll = euler_angles[2];
  std::cout << "roll: " << roll << "\npitch: " << pitch << "\nyaw: " << yaw << std::endl;
  // 20 degrees = 0.35 radians
  if(roll < 0.35 && pitch < 0.35)
    return false;
  return true;
}

bool FruitDetector::enhanceAlignment(
  const Eigen::Matrix3f& new_rotation, const boost::shared_ptr<Fruit>& f) {
  // get euler angles. (Z-Y-X) convention --> (Yaw, Pitch, Roll)
  const Eigen::Vector3f euler_angles = new_rotation.eulerAngles(2, 1, 0);
  const float new_yaw = euler_angles[0];
  const float new_pitch = euler_angles[1];
  const float new_roll = euler_angles[2];

  const float old_roll = f->get_RPY().roll;
  const float old_pitch = f->get_RPY().pitch;
  // check if the new aligned model is less tilted
  if (new_roll < old_roll && new_pitch < old_pitch)
    return true;
  return false;
}

void SQ_fitter<PointT>::getBoundingBox(const PointCloudPtr &_cloud,
				       double _dim[3],
				       double _trans[3],
				       double _rot[3] ) {

  // 1. Compute the bounding box center
  Eigen::Vector4d centroid;
  pcl::compute3DCentroid( *_cloud, centroid );
  _trans[0] = centroid(0);
  _trans[1] = centroid(1); 
  _trans[2] = centroid(2);

  // 2. Compute main axis orientations
  pcl::PCA<PointT> pca;
  pca.setInputCloud( _cloud );
  Eigen::Vector3f eigVal = pca.getEigenValues();
  Eigen::Matrix3f eigVec = pca.getEigenVectors();
  // Make sure 3 vectors are normal w.r.t. each other
  Eigen::Vector3f temp;
  eigVec.col(2) = eigVec.col(0); // Z
  Eigen::Vector3f v3 = (eigVec.col(1)).cross( eigVec.col(2) );
  eigVec.col(0) = v3;
  Eigen::Vector3f rpy = eigVec.eulerAngles(2,1,0);
 
  _rot[0] = (double)rpy(2);
  _rot[1] = (double)rpy(1);
  _rot[2] = (double)rpy(0);

  // Transform _cloud
  Eigen::Matrix4f transf = Eigen::Matrix4f::Identity();
  transf.block(0,3,3,1) << (float)centroid(0), (float)centroid(1), (float)centroid(2);
  transf.block(0,0,3,3) = eigVec;

  Eigen::Matrix4f tinv; tinv = transf.inverse();
  PointCloudPtr cloud_temp( new pcl::PointCloud<PointT>() );
  pcl::transformPointCloud( *_cloud, *cloud_temp, tinv );

  // Get maximum and minimum
  PointT minPt; PointT maxPt;
  pcl::getMinMax3D( *cloud_temp, minPt, maxPt );
  
  _dim[0] = ( maxPt.x - minPt.x ) / 2.0;
  _dim[1] = ( maxPt.y - minPt.y ) / 2.0;
  _dim[2] = ( maxPt.z - minPt.z ) / 2.0;

}

void addCoordinateFrame (geometry_msgs::Pose pose) {
	cloud_viewer.addCoordinateSystem (0.5);
	Eigen::Vector3f t;
	t(0) = pose.position.x;
	t(1) = pose.position.y;
	t(2) = -pose.position.z; // PCL seems to have a strange Z axis
	Eigen::Quaternionf q;
	q.x() = pose.orientation.x;
	q.y() = pose.orientation.y;
	q.z() = pose.orientation.z;
	q.w() = pose.orientation.w;
	Eigen::Matrix3f R = q.toRotationMatrix();
	Eigen::Vector3f r = R.eulerAngles(0, 1, 2);
	Eigen::Affine3f A;
	pcl::getTransformation (t(0), t(1), t(2), r(0), r(1), r(2), A);
	cloud_viewer.addCoordinateSystem (0.2, A);
}

Eigen::Matrix4f CUDACameraTrackingMultiResRGBD::computeBestRigidAlignment(CameraTrackingInput cameraTrackingInput, Eigen::Matrix3f& intrinsics, Eigen::Matrix4f& globalDeltaTransform, unsigned int level, CameraTrackingParameters cameraTrackingParameters, unsigned int maxInnerIter, float condThres, float angleThres, LinearSystemConfidence& conf) 
{
	Eigen::Matrix4f deltaTransform = globalDeltaTransform;

	conf.reset();

	Matrix6x7f system;

	Eigen::Matrix3f ROld = deltaTransform.block(0, 0, 3, 3);
	Eigen::Vector3f eulerAngles = ROld.eulerAngles(2, 1, 0);

	float3 anglesOld;	   anglesOld.x = eulerAngles.x(); anglesOld.y = eulerAngles.y(); anglesOld.z = eulerAngles.z();
	float3 translationOld; translationOld.x = deltaTransform(0, 3); translationOld.y = deltaTransform(1, 3); translationOld.z = deltaTransform(2, 3);

	m_CUDABuildLinearSystem->applyBL(cameraTrackingInput, intrinsics, cameraTrackingParameters, anglesOld, translationOld, m_imageWidth[level], m_imageHeight[level], level, system, conf);

	Matrix6x6f ATA = system.block(0, 0, 6, 6);
	Vector6f ATb = system.block(0, 6, 6, 1);

	if (ATA.isZero()) {
		return m_matrixTrackingLost;
	}

	Eigen::JacobiSVD<Matrix6x6f> SVD(ATA, Eigen::ComputeFullU | Eigen::ComputeFullV);
	Vector6f x = SVD.solve(ATb);

	//computing the matrix condition
	Vector6f evs = SVD.singularValues();
	conf.matrixCondition = evs[0]/evs[5];

	Vector6f xNew; xNew.block(0, 0, 3, 1) = eulerAngles; xNew.block(3, 0, 3, 1) = deltaTransform.block(0, 3, 3, 1);
	xNew += x;

	deltaTransform = delinearizeTransformation(xNew, Eigen::Vector3f(0.0f, 0.0f, 0.0f), 1.0f, level);
	if(deltaTransform(0, 0) == -std::numeric_limits<float>::infinity())
	{
		conf.trackingLostTresh = true;
		return m_matrixTrackingLost;
	}

	return deltaTransform;
}