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