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;
}