void calculateInverseTransformationMatrix(const Eigen::Matrix4d& src, Eigen::Matrix4d& dst)
{
dst = Eigen::Matrix4d::Identity();
Eigen::Matrix3d R_trans = src.block(0, 0, 3, 3).transpose();
dst.block(0, 0, 3, 3) = R_trans;
dst.block(0, 3, 3, 1) = -R_trans * src.block(0, 3, 3, 1);
}
void setTransform(Eigen::Matrix3d &rot, Eigen::Vector3d &t, Eigen::Matrix4d &Tf)
{
Tf.block(0,0,3,3) << rot;
Tf.block(0,3,3,1) << t;
Tf.block(3,0,1,4) << 0.0,0.0,0.0,1.0;
}
Eigen::Matrix4d recenter_transform3d(const Eigen::Matrix4d &transfo, const Eigen::Vector3d& center)
{
//% remove former translation part
Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3);
//% create translations
Eigen::Matrix4d t1 = create_translation3d(-center);
Eigen::Matrix4d t2 = create_translation3d(center);
//% compute translated transform
res = t2*res*t1;
return res;
}
void Manipulator::computeCabs()
{
if(C_abs_.size() != T_abs_.size())
{
std::stringstream msg;
msg << "C_abs_.size() != T_abs_.size()" << std::endl
<< " C_abs_.size : " << C_abs_.size() << std::endl
<< " T_abs_.size : " << T_abs_.size();
throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
}
else if(C_abs_.size() != link_.size())
{
std::stringstream msg;
msg << "C_abs_.size() != link_.size()" << std::endl
<< " C_abs_.size : " << C_abs_.size() << std::endl
<< " link_.size : " << link_.size();
throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
}
else if(C_abs_.size() == 0)
{
std::stringstream msg;
msg << "C_abs_.size() == 0" << std::endl
<< " C_abs_.size : " << C_abs_.size();
throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
}
for(uint32_t i = 0; i < C_abs_.size(); ++i)
{
Eigen::Matrix4d Tlc = Eigen::Matrix4d::Identity();
Tlc.block(0, 3, 3, 1) = link_[i]->C;
C_abs_[i] = T_abs_[i] * Tlc;
}
}
int GraphOptimizer_G2O::addVertex(Eigen::Matrix4d& vertexPose, int id, bool isFixed)
{
g2o::Vector3d t(vertexPose(0,3),vertexPose(1,3),vertexPose(2,3));
Eigen::Matrix3d rot = vertexPose.block(0,0,3,3);
g2o::Quaterniond q(rot);
q.normalize();
// set up node
g2o::VertexSE3 *vc = new g2o::VertexSE3();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
vc->setEstimate(cam);
vc->setId(id); // vertex id
//set pose fixed
if (isFixed) {
vc->setFixed(true);
}
// add to optimizer
optimizer.addVertex(vc);
vertexIdVec.push_back(id);
return id;
}
geometry_msgs::Pose Sensors::robot2sensorTransformation(geometry_msgs::Pose pose)
{
Eigen::Matrix4d robotPoseMat, robot2sensorMat, sensorPoseMat;
//Robot matrix pose
Eigen::Matrix3d R; Eigen::Vector3d T1(pose.position.x,pose.position.y,pose.position.z);
tf::Quaternion qt(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
tf::Matrix3x3 R1(qt);
tf::matrixTFToEigen(R1,R);
robotPoseMat.setZero ();
robotPoseMat.block (0, 0, 3, 3) = R;
robotPoseMat.block (0, 3, 3, 1) = T1;
robotPoseMat (3, 3) = 1;
//transformation matrix
qt = tf::createQuaternionFromRPY(sensorRPY[0],sensorRPY[1],sensorRPY[2]);
tf::Matrix3x3 R2(qt);Eigen::Vector3d T2(sensorPose[0], sensorPose[1], sensorPose[2]);
tf::matrixTFToEigen(R2,R);
robot2sensorMat.setZero ();
robot2sensorMat.block (0, 0, 3, 3) = R;
robot2sensorMat.block (0, 3, 3, 1) = T2;
robot2sensorMat (3, 3) = 1;
//preform the transformation
sensorPoseMat = robotPoseMat * robot2sensorMat;
Eigen::Matrix4d sensor2sensorMat; //the frustum culling sensor needs this
//the transofrmation is rotation by +90 around x axis of the sensor
sensor2sensorMat << 1, 0, 0, 0,
0, 0,-1, 0,
0, 1, 0, 0,
0, 0, 0, 1;
Eigen::Matrix4d newSensorPoseMat = sensorPoseMat * sensor2sensorMat;
geometry_msgs::Pose p;
Eigen::Vector3d T3;Eigen::Matrix3d Rd; tf::Matrix3x3 R3;
Rd = newSensorPoseMat.block (0, 0, 3, 3);
tf::matrixEigenToTF(Rd,R3);
T3 = newSensorPoseMat.block (0, 3, 3, 1);
p.position.x=T3[0];p.position.y=T3[1];p.position.z=T3[2];
R3.getRotation(qt);
p.orientation.x = qt.getX(); p.orientation.y = qt.getY();p.orientation.z = qt.getZ();p.orientation.w = qt.getW();
return p;
}
void xyzrpyToTransformationMatrix(const Eigen::Vector3d& xyz, const Eigen::Vector3d& rpy, Eigen::Matrix4d& T)
{
T = Eigen::Matrix4d::Identity();
Eigen::Matrix3d R;
rpyToRotationMatrix(rpy, R);
T.block(0, 0, 3, 3) = R;
T.block(0, 3, 3, 1) = xyz;
}
Eigen::Matrix4d createHomogeneousTransformMatrix(tf::StampedTransform transform)
{
tf::Point p = transform.getOrigin();
tf::Quaternion q = transform.getRotation();
tf::Matrix3x3 R1(q);
Eigen::Matrix3d R2;
tf::matrixTFToEigen(R1, R2);
ROS_INFO_STREAM("R2:\n"<<R2);
Eigen::Matrix4d T;
T.block(0, 0, 3, 3) << R2;
T.block(0, 3, 3, 1) << p.x(), p.y(), p.z();
T.row(3) << 0, 0, 0, 1;
return T;
}
/**
* @function getPlaneTransform
* @brief Assumes plane coefficients are of the form ax+by+cz+d=0, normalized
*/
Eigen::Matrix4d getPlaneTransform ( pcl::ModelCoefficients &coeffs,
double up_direction,
bool flatten_plane ) {
Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
if( coeffs.values.size() <= 3 ) {
std::cout << "[ERROR] Coefficient size less than 3. I will output nonsense values"<<std::endl;
return tf;
}
double a = coeffs.values[0];
double b = coeffs.values[1];
double c = coeffs.values[2];
double d = coeffs.values[3];
//asume plane coefficients are normalized
Eigen::Vector3d position(-a*d, -b*d, -c*d);
Eigen::Vector3d z(a, b, c);
//if we are flattening the plane, make z just be (0,0,up_direction)
if(flatten_plane) {
z << 0, 0, up_direction;
}
else {
//make sure z points "up" (the plane normal and the table must have a > 90 angle, hence the cosine must be negative)
if ( z.dot( Eigen::Vector3d(0, 0, up_direction) ) > 0) {
z = -1.0 * z;
printf("Changing sign \n");
coeffs.values[0]*= -1; coeffs.values[1]*= -1; coeffs.values[2]*= -1; coeffs.values[3]*= -1;
}
}
//try to align the x axis with the x axis of the original frame
//or the y axis if z and x are too close too each other
Eigen::Vector3d x; x << 1, 0, 0;
if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d y = z.cross(x); y.normalized();
x = y.cross(z); x.normalized();
Eigen::Matrix3d rotation;
rotation.block(0,0,3,1) = x;
rotation.block(0,1,3,1) = y;
rotation.block(0,2,3,1) = z;
Eigen::Quaterniond orientation( rotation );
tf.block(0,0,3,3) = orientation.matrix();
tf.block(0,3,3,1) = position;
return tf;
}
void
calibrate_stereo(const observation_pts_v_t& left_points, const observation_pts_v_t& right_points,
const object_pts_v_t& object_points, const cv::Size& image_size, PinholeCameraModel& left,
PinholeCameraModel& right)
{
PinholeCameraModel camera;
StereoCameraModel scam;
std::vector<cv::Mat> rvecs, tvecs;
cv::Mat K_left, K_right, D_right, D_left, R, T, E, F;
int flags =cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_ASPECT_RATIO /*| cv::CALIB_ZERO_TANGENT_DIST|cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 |cv::CALIB_FIX_K3*/;
cv::Size left_size(image_size), right_size(image_size);
double left_rms = cv::calibrateCamera(object_points, left_points, left_size, K_left, D_left, rvecs, tvecs, flags);
double right_rms = cv::calibrateCamera(object_points, right_points, right_size, K_right, D_right, rvecs, tvecs,
flags);
double stereo_rms = cv::stereoCalibrate(object_points, left_points, right_points, K_left, D_left, K_right, D_right,
left_size, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 500, 1e-6),
cv::CALIB_FIX_INTRINSIC | flags);
std::cout << "image size:" << image_size.width << ":" << image_size.height << std::endl;
std::cout << "R:" << R << std::endl;
std::cout << "T:" << T << std::endl;
std::cout << "D:" << D_left << std::endl;
std::cout << "left rms:" << left_rms << " right rms:" << right_rms << " stereo rms:" << stereo_rms << std::endl;
left.setParams(left_size, K_left, D_left, cv::Mat_<double>::eye(3, 3), K_left);
right.setParams(right_size, K_right, D_right, cv::Mat_<double>::eye(3, 3), K_right);
left.writeCalibration("left.yml");
right.writeCalibration("right.yml");
Eigen::Matrix3d Re;
Eigen::Vector3d Te;
Eigen::Matrix4d Pe = Eigen::Matrix4d::Identity();
cv2eigen(R, Re);
cv2eigen(T.t(), Te);
std::cout << Te << std::endl;
Pe.block(0, 0, 3, 3) = Re;
Pe.block(0, 3, 3, 1) = Te;
Pose P;
P.transform.matrix() = Pe;
scam.setParams(left, right, P);
scam.writeCalibration("stereo.yml");
}
Eigen::Matrix4d create_rotation3d_line_angle(const Eigen::Vector3d& center, Eigen::Vector3d v, double theta)
{
//% normalize vector
v.normalize();
//% compute projection matrix P and anti-projection matrix
Eigen::Matrix3d P = v * v.transpose();
Eigen::Matrix3d Q;
Q << 0, -v.coeff(2), v.coeff(1),
v.coeff(2), 0, -v.coeff(0),
-v.coeff(1), v.coeff(0), 0;
Eigen::Matrix3d I = Eigen::Matrix3d::Identity();
//% compute vectorial part of the transform
Eigen::Matrix4d mat = Eigen::Matrix4d::Identity();
mat.block(0, 0, 3, 3) = P + (I - P)*cos(theta) + Q*sin(theta);
//% add translation coefficient
mat = recenter_transform3d(mat, center);
return mat;
}
void Manipulator::computeJacobian(int32_t idx, Eigen::MatrixXd& J)
{
J = Eigen::MatrixXd::Zero(6, dof_);
if(idx < dof_) // Not required to consider end-effector
{
for(uint32_t i = 0; i <= idx; ++i)
{
if(link_[i]->ep) // joint_type is prismatic
{
J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
}
else // joint_type is revolute
{
Eigen::Matrix4d Tib;
math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
Eigen::Matrix4d Cin = Tib * C_abs_[idx];
Eigen::Vector3d P = Cin.block(0, 3, 3, 1);
J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
}
}
}
else // Required to consider the offset of end-effector
{
--idx;
for(uint32_t i = 0; i <= idx; ++i)
{
if(link_[i]->ep) // joint_type is prismatic
{
J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
}
else // joint_type is revolute
{
Eigen::Matrix4d Tib;
math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
Eigen::Matrix4d Cin = Tib * C_abs_[idx];
Eigen::Vector3d P = Cin.block(0, 3, 3, 1);
J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
}
}
Eigen::MatrixXd J_Pne = Eigen::MatrixXd::Identity(6, 6);
Eigen::Vector3d Pne;
if(C_abs_.size() - 1 - 1 >= 0.0)
{
Pne = T_abs_[T_abs_.size() - 1].block(0, 3, 3, 1) - C_abs_[C_abs_.size() - 1 - 1].block(0, 3, 3, 1);
}
else
{
std::stringstream msg;
msg << "C_abs_.size() <= 1" << std::endl
<< "Manipulator doesn't have enough links." << std::endl;
throw ahl_utils::Exception("Manipulator::computeJacobian", msg.str());
}
Eigen::Matrix3d Pne_cross;
Pne_cross << 0.0, Pne.coeff(2), -Pne.coeff(1),
-Pne.coeff(2), 0.0, Pne.coeff(0),
Pne.coeff(1), -Pne.coeff(0), 0.0;
J_Pne.block(0, 3, 3, 3) = Pne_cross;
J = J_Pne * J;
}
}
void MolconvWindow::minimizeRMSD(molconv::moleculePtr refMol, molconv::moleculePtr otherMol)
{
if (refMol->size() != otherMol->size())
{
QMessageBox::critical(this, tr("Alignment impossible"), tr("Only molecules with equal number of atoms can be aligned."));
return;
}
int Natoms = refMol->size();
Eigen::Vector3d center = refMol->center();
Eigen::Vector3d shift = center - otherMol->center();
Eigen::Vector3d newOrigin = otherMol->internalOriginPosition() + shift;
updateActiveMolecule(otherMol);
d->m_MoleculeSettings->setMolecule(otherMol);
d->m_MoleculeSettings->moveMolecule(double(newOrigin(0)), double(newOrigin(1)), double(newOrigin(2)), 0.0, 0.0, 0.0);
Eigen::MatrixXd Xr = Eigen::MatrixXd::Zero(3,Natoms);
Eigen::MatrixXd Xo = Eigen::MatrixXd::Zero(3,Natoms);
for (int i = 0; i < Natoms; i++)
{
Xr.col(i) = refMol->atom(i)->position() - center;
Xo.col(i) = otherMol->atom(i)->position() - center;
}
Eigen::Matrix3d corr = Xo * Xr.transpose();
// construct the quaternion matrix
Eigen::Matrix4d F = Eigen::Matrix4d::Zero();
F(0,0) = corr(0,0) + corr(1,1) + corr(2,2);
F(1,1) = corr(0,0) - corr(1,1) - corr(2,2);
F(2,2) = -corr(0,0) + corr(1,1) - corr(2,2);
F(3,3) = -corr(0,0) - corr(1,1) + corr(2,2);
F(0,1) = corr(1,2) - corr(2,1);
F(0,2) = corr(2,0) - corr(0,2);
F(0,3) = corr(0,1) - corr(1,0);
F(1,2) = corr(0,1) + corr(1,0);
F(1,3) = corr(0,2) + corr(2,0);
F(2,3) = corr(1,2) + corr(2,1);
F(1,0) = F(0,1);
F(2,0) = F(0,2);
F(3,0) = F(0,3);
F(2,1) = F(1,2);
F(3,1) = F(1,3);
F(3,2) = F(2,3);
Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> Feig(F);
Eigen::Vector4d Feval = Feig.eigenvalues();
Eigen::Matrix4d Fevec = Feig.eigenvectors();
// the optimal rotation corresponds to either the first or the last eigenvector, depending on which eigenvalue is larger
Eigen::Vector4d lQuart = std::abs(double(Feval(0))) > std::abs(double(Feval(3))) ? Fevec.block(0, 0, 4, 1) : Fevec.block(0, 3, 4, 1);
Eigen::Matrix3d rotmat = Eigen::Matrix3d::Zero();
rotmat(0,0) = lQuart(0) * lQuart(0) + lQuart(1) * lQuart(1) - lQuart(2) * lQuart(2) - lQuart(3) * lQuart(3);
rotmat(1,1) = lQuart(0) * lQuart(0) - lQuart(1) * lQuart(1) + lQuart(2) * lQuart(2) - lQuart(3) * lQuart(3);
rotmat(2,2) = lQuart(0) * lQuart(0) - lQuart(1) * lQuart(1) - lQuart(2) * lQuart(2) + lQuart(3) * lQuart(3);
rotmat(0,1) = 2.0 * (lQuart(1) * lQuart(2) - lQuart(0) * lQuart(3));
rotmat(0,2) = 2.0 * (lQuart(1) * lQuart(3) + lQuart(0) * lQuart(2));
rotmat(1,2) = 2.0 * (lQuart(2) * lQuart(3) - lQuart(0) * lQuart(1));
rotmat(1,0) = 2.0 * (lQuart(1) * lQuart(2) + lQuart(0) * lQuart(3));
rotmat(2,0) = 2.0 * (lQuart(1) * lQuart(3) - lQuart(0) * lQuart(2));
rotmat(2,1) = 2.0 * (lQuart(2) * lQuart(3) + lQuart(0) * lQuart(1));
std::array<double,3> newEulers = molconv::Molecule::rot2euler(rotmat);
double newPhi = newEulers[2];
double newTheta = newEulers[1];
double newPsi = newEulers[0];
d->m_MoleculeSettings->moveMolecule(double(newOrigin(0)), double(newOrigin(1)), double(newOrigin(2)), newPhi, newTheta, newPsi);
updateAxes();
}
void RevoluteZ::transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T)
{
T = T_org;
T.block(0, 0, 3, 3) = T_org.block(0, 0, 3, 3) * this->T(q).block(0, 0, 3, 3);
}
void PrismaticZ::transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T)
{
T = T_org;
T.block(0, 3, 3, 1) += this->T(q).block(0, 3, 3, 1);
}