Esempio n. 1
0
Eigen::Vector3d GetCameraCenterWorld() {
	GLdouble projmat[16];
	GLdouble modelmat[16];

	glGetDoublev( GL_PROJECTION_MATRIX, projmat);
	glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
	Eigen::Matrix4d InvModelMat;
	Eigen::Matrix4d InvProjMat;
	InvModelMat.setZero();
	InvProjMat.setZero();
	for(int i=0; i<4; i++){
		for(int j=0; j<4; j++){
			InvModelMat(i,j)=modelmat[4*i+j];
			InvProjMat(i,j)=projmat[4*i+j];
			//printf("%lf ", InvModelMat[i][j]);
		}
		//printf("\n");
	}
	InvModelMat=(InvModelMat*InvProjMat).transpose();
	InvModelMat = (InvModelMat.inverse());
	Eigen::Vector4d cc(0, 0, 0, 1);
	cc=InvModelMat*cc;
	if(cc.norm()!=0) cc=cc/cc[3];
	return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
double
RotationAverage::chordal(Sophus::SO3d *avg)
{
    double max_angle=0;
    Eigen::Matrix4d Q;
    Q.setZero();
    auto rest = rotations.begin();
    rest++;
    for(auto && t: zip_range(weights, rotations))
    {
        double w=t.get<0>();
        Sophus::SO3d& q = t.get<1>();
        Q += w * q.unit_quaternion().coeffs() * q.unit_quaternion().coeffs().transpose();
        max_angle = std::accumulate(rest, rotations.end(), max_angle,
                [&q](double max, const Sophus::SO3d& other)
                {
                return std::max(max,
                    q.unit_quaternion().angularDistance(other.unit_quaternion()));
                });
    }
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> solver;
    solver.compute(Q);
    Eigen::Vector4d::Map(avg->data()) = solver.eigenvectors().col(3);
    return max_angle;
 }
Esempio n. 3
0
bool ViewControl::ConvertToPinholeCameraParameters(
        PinholeCameraIntrinsic &intrinsic, Eigen::Matrix4d &extrinsic)
{
    if (window_height_ <= 0 || window_width_ <= 0) {
        PrintWarning("[ViewControl] ConvertToPinholeCameraParameters() failed because window height and width are not set.\n");
        return false;
    }
    if (GetProjectionType() == ProjectionType::Orthogonal) {
        PrintWarning("[ViewControl] ConvertToPinholeCameraParameters() failed because orthogonal view cannot be translated to a pinhole camera.\n");
        return false;
    }
    SetProjectionParameters();
    intrinsic.width_ = window_width_;
    intrinsic.height_ = window_height_;
    intrinsic.intrinsic_matrix_.setIdentity();
    double fov_rad = field_of_view_ / 180.0 * M_PI;
    double tan_half_fov = std::tan(fov_rad / 2.0);
    intrinsic.intrinsic_matrix_(0, 0) = intrinsic.intrinsic_matrix_(1, 1) =
            (double)window_height_ / tan_half_fov / 2.0;
    intrinsic.intrinsic_matrix_(0, 2) = (double)window_width_ / 2.0 - 0.5;
    intrinsic.intrinsic_matrix_(1, 2) = (double)window_height_ / 2.0 - 0.5;
    extrinsic.setZero();
    Eigen::Vector3d front_dir = front_.normalized();
    Eigen::Vector3d up_dir = up_.normalized();
    Eigen::Vector3d right_dir = right_.normalized();
    extrinsic.block<1, 3>(0, 0) = right_dir.transpose();
    extrinsic.block<1, 3>(1, 0) = -up_dir.transpose();
    extrinsic.block<1, 3>(2, 0) = -front_dir.transpose();
    extrinsic(0, 3) = -right_dir.dot(eye_);
    extrinsic(1, 3) = up_dir.dot(eye_);
    extrinsic(2, 3) = front_dir.dot(eye_);
    extrinsic(3, 3) = 1.0;
    return true;
}
Esempio n. 4
0
Eigen::Vector3d GetPointEyeToWorld(const Eigen::Ref<const Eigen::Vector3d>& _pt) {
	GLdouble modelmat[16];

	glGetDoublev( GL_MODELVIEW_MATRIX, modelmat);
	Eigen::Matrix4d InvModelMat;
	InvModelMat.setZero();
	for(int i=0; i<4; i++){
		for(int j=0; j<4; j++){
			InvModelMat(i,j)=modelmat[4*i+j];
		}
	}
	InvModelMat=(InvModelMat.transpose());
	InvModelMat=(InvModelMat.inverse());
	Eigen::Vector4d cc( _pt(0),_pt(1), _pt(2),1);
	cc=InvModelMat*cc;
	if(cc.norm()!=0) cc=cc/cc[3];
	return Eigen::Vector3d(cc[0], cc[1], cc[2]);
}
void LeePositionController::InitializeParameters() {
  calculateAllocationMatrix(vehicle_parameters_.rotor_configuration_, &(controller_parameters_.allocation_matrix_));
  // To make the tuning independent of the inertia matrix we divide here.
  normalized_attitude_gain_ = controller_parameters_.attitude_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();
  // To make the tuning independent of the inertia matrix we divide here.
  normalized_angular_rate_gain_ = controller_parameters_.angular_rate_gain_.transpose()
      * vehicle_parameters_.inertia_.inverse();

  Eigen::Matrix4d I;
  I.setZero();
  I.block<3, 3>(0, 0) = vehicle_parameters_.inertia_;
  I(3, 3) = 1;
  angular_acc_to_rotor_velocities_.resize(vehicle_parameters_.rotor_configuration_.rotors.size(), 4);
  // Calculate the pseude-inverse A^{ \dagger} and then multiply by the inertia matrix I.
  // A^{ \dagger} = A^T*(A*A^T)^{-1}
  angular_acc_to_rotor_velocities_ = controller_parameters_.allocation_matrix_.transpose()
      * (controller_parameters_.allocation_matrix_
      * controller_parameters_.allocation_matrix_.transpose()).inverse() * I;
  initialized_params_ = true;
}
Esempio n. 6
0
void
pcl::visualization::Camera::computeProjectionMatrix (Eigen::Matrix4d& proj) const
{
  float top    = static_cast<float> (clip[0]) * tanf (0.5f * static_cast<float> (fovy));
  float left   = -top * static_cast<float> (window_size[0] / window_size[1]);
  float right  = -left;
  float bottom = -top;

  float temp1, temp2, temp3, temp4;
	temp1 = 2.0f * static_cast<float> (clip[0]);
	temp2 = 1.0f / (right - left);
	temp3 = 1.0f / (top - bottom);
	temp4 = 1.0f / static_cast<float> (clip[1] - clip[0]);

  proj.setZero ();

	proj(0,0) = temp1 * temp2;
	proj(1,1) = temp1 * temp3;
	proj(0,2) = (right + left) * temp2;
	proj(1,2) = (top + bottom) * temp3;
	proj(2,2) = (-clip[1] - clip[0]) * temp4;
	proj(3,2) = -1.0;
	proj(2,3) = (-temp1 * clip[1]) * temp4;
}
geometry_msgs::Pose C_HunoLimbKinematics::ForwardKinematics(const double *thetas_C)
{ 
  geometry_msgs::Pose outLimbPose;
  Eigen::Vector3d rot_axis = Eigen::Vector3d::Zero();
  double rot_angle;

  Eigen::Matrix4d limbTF = Eigen::Matrix4d::Identity();
  Eigen::Matrix4d expXihatTheta = Eigen::Matrix4d::Identity();
  Eigen::Matrix<double, 6, 5> tempJacobian;
  tempJacobian.setZero(); // initialize temporary variable to zeros
  int limbCtr = 0;

  /* Lock jacobian matrix while being formed */
  m_isJacobianLocked = true;

  /* Calculate joint transformation matrices sequentially and populate jacobian */
  for (int jointIdx = 0; jointIdx < m_numJoints; jointIdx++)
  {
    /* use previous expXihatTheta to generate adjoint matrix
       and fill next column of jacobian */
    tempJacobian.col(limbCtr) = AdjointMatrix(expXihatTheta) * m_zetas_M.col(jointIdx);

    /* calculate for next joint */
    expXihatTheta = ExpXihatTheta(jointIdx, (*(thetas_C+jointIdx))*DEG2RAD);
    limbTF *= expXihatTheta;

    /* increment counter */
    limbCtr++; 
  }
  
  if (limbCtr == m_numJoints)
  {
    limbTF *= m_g0Mat_M; // apply configuration matrix 
    m_jacobian_M = tempJacobian.block(0,0, m_numJoints, m_numJoints); // save jacobian
    
    m_isJacobianLocked = false; // unlock jacobian matrix
  }
  else
  { // reset matrices since something didn't add up.
    limbTF.setZero();
    m_jacobian_M.setZero();
  }

  /* save limb transformation matrix into pose message */
  outLimbPose.position.x = limbTF(0,3);
  outLimbPose.position.y = limbTF(1,3);
  outLimbPose.position.z = limbTF(2,3);

  //back out rotation unit vector and angle from rotation matrix
  rot_angle = acos( ( ((limbTF.block<3,3>(0,0)).trace())-1 ) /2 );
  if (sin(rot_angle) != 0)
  {
    rot_axis(0) = (limbTF(2,1)-limbTF(1,2)) / (2*sin(rot_angle));
    rot_axis(1) = (limbTF(0,2)-limbTF(2,0)) / (2*sin(rot_angle));
    rot_axis(2) = (limbTF(1,0)-limbTF(0,1)) / (2*sin(rot_angle));
  }
  // else rot_axis is zeroes

  outLimbPose.orientation.x = rot_axis(0)*sin(rot_angle/2);
  outLimbPose.orientation.y = rot_axis(1)*sin(rot_angle/2);
  outLimbPose.orientation.z = rot_axis(2)*sin(rot_angle/2);
  outLimbPose.orientation.w = cos(rot_angle/2);

  return outLimbPose; 
} // end ForwardKinematics()
Esempio n. 8
0
 void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) {
     m.setZero();
 }