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; }
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; }
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; }
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()
void TrfmTranslateZ::applySecondDeriv( const Dof* q1, const Dof* q2, Eigen::Matrix4d& m ) { m.setZero(); }