Ejemplo n.º 1
0
 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;

}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
0
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;
}
Ejemplo n.º 6
0
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;

}
Ejemplo n.º 7
0
 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;
 }
Ejemplo n.º 8
0
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;
}
Ejemplo n.º 9
0
/**
 * @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;
}
Ejemplo n.º 10
0
  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");
  }
Ejemplo n.º 11
0
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;
}
Ejemplo n.º 12
0
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;
  }
}
Ejemplo n.º 13
0
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();
}
Ejemplo n.º 14
0
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);
}
Ejemplo n.º 15
0
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);
}