/**
* @brief CameraDirectLinearTransformation::rq3
* Perform 3 RQ decomposition of matrix A and save them in matrix R and matrix Q
* http://www.csse.uwa.edu.au/~pk/research/matlabfns/Projective/rq3.m
* @param A
* @param R
* @param Q
*/
void CameraDirectLinearTransformation::rq3(const Matrix3d &A, Matrix3d &R, Matrix3d& Q)
{
// Find rotation Qx to set A(2,1) to 0
double c = -A(2,2)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
double s = A(2,1)/sqrt(A(2,2)*A(2,2)+A(2,1)*A(2,1));
Matrix3d Qx,Qy,Qz;
Qx << 1 ,0,0, 0,c,-s, 0,s,c;
R = A*Qx;
// Find rotation Qy to set A(2,0) to 0
c = R(2,2)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
s = R(2,0)/sqrt(R(2,2)*R(2,2)+R(2,0)*R(2,0) );
Qy << c, 0, s, 0, 1, 0,-s, 0, c;
R*=Qy;
// Find rotation Qz to set A(1,0) to 0
c = -R(1,1)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
s = R(1,0)/sqrt(R(1,1)*R(1,1)+R(1,0)*R(1,0));
Qz << c ,-s, 0, s ,c ,0, 0, 0 ,1;
R*=Qz;
Q = Qz.transpose()*Qy.transpose()*Qx.transpose();
// Adjust R and Q so that the diagonal elements of R are +ve
for (int n=0; n<3; n++)
{
if (R(n,n)<0)
{
R.col(n) = - R.col(n);
Q.row(n) = - Q.row(n);
}
}
}
Needle::Needle(const Vector3d& pos, const Matrix3d& rot, double degrees, double r, float c0, float c1, float c2, World* w, ThreadConstrained* t, int constrained_vertex_num)
: EnvObject(c0, c1, c2, NEEDLE)
, angle(degrees)
, radius(r)
, thread(t)
, constraint(constrained_vertex_num)
, world(w)
, position_offset(0.0, 0.0, 0.0)
, rotation_offset(Matrix3d::Identity())
{
assert(((thread == NULL) && (constrained_vertex_num == -1)) || ((thread != NULL) && (constrained_vertex_num != -1)));
updateConstraintIndex();
assert(degrees > MIN_ANGLE);
assert(r > MIN_RADIUS);
double arc_length = radius * angle * M_PI/180.0;
int pieces = ceil(arc_length/2.0);
for (int piece=0; piece<pieces; piece++) {
Intersection_Object* obj = new Intersection_Object();
obj->_radius = radius/8.0;
i_objs.push_back(obj);
}
if (thread != NULL) {
Matrix3d new_rot = AngleAxisd(-M_PI/2.0, rot.col(0)) * (AngleAxisd((angle) * M_PI/180.0, rot.col(1)) * rot);
setTransform(pos - radius * rot.col(1), new_rot);
} else {
setTransform(pos, rot);
}
}
geometry_msgs::Pose MoveitPlanningInterface::convToPose(Vector3f plane_normal, Vector3f major_axis, Vector3f centroid) {
geometry_msgs::Pose pose;
Affine3d Affine_des_gripper;
Vector3d xvec_des,yvec_des,zvec_des,origin_des;
float temp;
temp = major_axis[0];
major_axis[0] = major_axis[1];
major_axis[1] = temp;
Vector3f rotation = major_axis;
major_axis[0] = (sqrt(3)/2)*rotation[0] - rotation[1]/2;
major_axis[1] = (sqrt(3)/2)*rotation[1] + rotation[0]/2;
Matrix3d Rmat;
for (int i=0;i<3;i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
// origin_des[2]+=0.02; //raise up 2cm
yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear()=Rmat;
Affine_des_gripper.translation()=origin_des;
//convert des pose from Affine to geometry_msgs::PoseStamped
pose = transformEigenAffine3dToPose(Affine_des_gripper);
return pose;
}
Vector4d gc_av_to_asd(Vector6d av) {
Vector4d asd;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
// Vector2d w(y.norm(), x.norm());
// w /= w.norm();
// asd(3) = asin(w(1));
double depth = x.norm() / y.norm();
// double sig_d = log( (2.0*depth + 1.0) / (1.0 - 2.0*depth));
// double quotient = depth / 0.5;
// int integer_quotient = (int)quotient;
// double floating_quotient = quotient - (double)integer_quotient;
// depth = depth * floating_quotient;
// double sig_d = log(2.0*depth + 1.0) - log(1.0 - 2.0*depth);
// double sig_d = atan(1.0 / (1.0*depth) );
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth);
// double sig_d = atan(depth);
// double sig_d = atan2(1.0, depth) / 4.0;
double sig_d = 1.0 / exp(-depth);
asd(3) = sig_d;
// cout << "sig_d = " << sig_d << endl;
// asd(3) = depth;
// double sig_d = tan(1.0/depth);
// double sig_d = tan(2.0/depth);
// double sig_d = tan(2.0*depth);
// double sig_d = (1.0 - exp(-1.0/depth)) / (2.0*(1.0 + exp(-1.0/depth)));
// double sig_d = (1.0 - exp(-depth)) / (2.0*(1.0 + exp(-depth)));
// double sig_d = 1.0 / (1.0 + exp(-1.0/depth));
// double sig_d = 1.0 / (1.0 + exp(-depth));
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
asd(0) = aa(0);
asd(1) = aa(1);
asd(2) = aa(2);
return asd;
}
void Needle::updateTransformFromAttachment()
{
if (isThreadAttached()) {
const Matrix3d thread_rot = thread->rotationAtConstraint(constraint_ind);
const Vector3d thread_pos = thread->positionAtConstraint(constraint_ind);
const Matrix3d needle_rot = AngleAxisd(angle * M_PI/180.0, thread_rot.col(1)) * thread_rot;
const Vector3d needle_pos = thread_pos - radius * (AngleAxisd(-angle * M_PI/180.0, needle_rot.col(1)) * needle_rot).col(2);
setTransform(needle_pos, needle_rot);
}
}
Matrix3d toMatrix3d(const btMatrix3x3& basis)
{
Matrix3d rotation;
btVector3 col0 = basis.getColumn(0);
btVector3 col1 = basis.getColumn(1);
btVector3 col2 = basis.getColumn(2);
rotation.col(0) = toVector3d(col0);
rotation.col(1) = toVector3d(col1);
rotation.col(2) = toVector3d(col2);
return rotation;
}
void Needle::getEndEffectorTransform(Vector3d& ee_pos, Matrix3d& ee_rot)
{
ee_rot = rotation * rotation_offset.transpose();
ee_rot.col(1) = ee_rot.col(2).cross(ee_rot.col(0));
ee_rot.col(2) = ee_rot.col(0).cross(ee_rot.col(1));
ee_rot.col(0).normalize();
ee_rot.col(1).normalize();
ee_rot.col(2).normalize();
ee_pos = position - ee_rot*rotation_offset*position_offset;
}
inline Matrix3d d_Tinvpsi_d_psi(const SE3Quat & T, const Vector3d & psi) {
Matrix3d R = T.rotation().toRotationMatrix();
Vector3d x = invert_depth(psi);
Vector3d r1 = R.col(0);
Vector3d r2 = R.col(1);
Matrix3d J;
J.col(0) = r1;
J.col(1) = r2;
J.col(2) = -R*x;
J*=1./psi.z();
return J;
}
// Original code from the ROS vslam package pe3d.cpp
// uses the SVD procedure for aligning point clouds
// SEE: Arun, Huang, Blostein: Least-Squares Fitting of Two 3D Point Sets
Eigen::Matrix4d threePointSvd(Eigen::MatrixXd const & p0, Eigen::MatrixXd const & p1)
{
using namespace Eigen;
SM_ASSERT_EQ_DBG(std::runtime_error, p0.rows(), 3, "p0 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p1.rows(), 3, "p1 must be a 3xK matrix");
SM_ASSERT_EQ_DBG(std::runtime_error, p0.cols(), p1.cols(), "p0 and p1 must have the same number of columns");
Vector3d c0 = p0.rowwise().mean();
Vector3d c1 = p1.rowwise().mean();
Matrix3d H(Matrix3d::Zero());
// subtract out
// p0a -= c0;
// p0b -= c0;
// p0c -= c0;
// p1a -= c1;
// p1b -= c1;
// p1c -= c1;
// Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
// p1c*p0c.transpose();
for(int i = 0; i < p0.cols(); ++i)
{
H += (p0.col(i) - c0) * (p1.col(i) - c1).transpose();
}
// do the SVD thang
JacobiSVD<Matrix3d> svd(H,ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2) * -1.0;
R = V * svd.matrixU().transpose();
}
Vector3d tr = c0-R.transpose()*c1; // translation
// transformation matrix, 3x4
Matrix4d tfm(Matrix4d::Identity());
// tfm.block<3,3>(0,0) = R.transpose();
// tfm.col(3) = -R.transpose()*tr;
tfm.topLeftCorner<3,3>() = R.transpose();
tfm.topRightCorner<3,1>() = tr;
return tfm;
}
Vector6d gc_asd_to_av(Vector4d asd) {
Vector6d av;
Vector3d aa = asd.head(3);
// double d_inv = asd(3);
// double sig_d_inv = (1.0 - exp(-asd(3))) / (2.0 * (1.0 + exp(-asd(3))));
// double sig_d_inv = -log(1.0/asd(3) - 1.0);
// double sig_d_inv = log( (2.0 * asd(3) + 1.0) / (1.0 - 2.0*asd(3)) );
// double sig_d_inv = atan(asd(3)) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0) / 2.0;
// double sig_d_inv = atan2(asd(3), 1.0);
// double sig_d_inv = atan2(asd(3), 1.0) * 1.0;
// double sig_d_inv = tan(4.0 * asd(3));
double sig_d_inv = log(asd(3));
// cout << "sig_d_inv = " << sig_d_inv << endl;
// double sig_d_inv = cos(asd(3)) / sin(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
// double sig_d_inv = sin(asd(3)) / cos(asd(3));
Matrix3d R = gc_Rodriguez(aa);
// av.head(3) = R.col(2) / sig_d_inv;
av.head(3) = R.col(2) * sig_d_inv;
av.tail(3) = R.col(0);
return av;
}
Vector6d gc_aid_to_av(Vector4d aid) {
Vector6d av;
Vector3d aa = aid.head(3);
double d = 1.0 / aid(3);
Matrix3d R = gc_Rodriguez(aa);
av.head(3) = R.col(2) * d;
av.tail(3) = R.col(0);
// Vector6d av;
// double a = aid[0];
// double b = aid[1];
// double g = aid[2];
// double t = aid[3];
//
// double s1 = sin(a);
// double c1 = cos(a);
// double s2 = sin(b);
// double c2 = cos(b);
// double s3 = sin(g);
// double c3 = cos(g);
//
// Matrix3d R;
// R <<
// c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
// c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
// -s2, s1 * c2, c1 * c2;
//
// double d = 1.0 / t;
// av.head(3) = -R.col(2) * d;
// av.tail(3) = R.col(1);
return av;
}
Matrix4d TrfmRotateExpMap::getSecondDeriv(const Dof *q1, const Dof *q2){
Vector3d q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
// derivative wrt which mDofs
int j=-1, k=-1;
for(unsigned int i=0; i<mDofs.size(); i++) {
if(q1==mDofs[i]) j=i;
if(q2==mDofs[i]) k=i;
}
assert(j!=-1);
assert(k!=-1);
assert(j>=0 && j<=2);
assert(k>=0 && k<=2);
Matrix3d R = utils::rotation::expMapRot(q);
Matrix3d J = utils::rotation::expMapJac(q);
Matrix3d Jjss = utils::makeSkewSymmetric(J.col(j));
Matrix3d Jkss = utils::makeSkewSymmetric(J.col(k));
Matrix3d dJjdkss = utils::makeSkewSymmetric(utils::rotation::expMapJacDeriv(q, k).col(j));
Matrix3d d2Rdidj = (Jjss*Jkss + dJjdkss)*R;
Matrix4d d2Rdidj4 = Matrix4d::Zero();
d2Rdidj4.topLeftCorner(3,3) = d2Rdidj;
return d2Rdidj4;
}
int main(int, char**)
{
cout.precision(3);
Matrix3d m = Matrix3d::Identity();
m.col(1) = Vector3d(4,5,6);
cout << m << endl;
return 0;
}
Vector6d gc_orth_to_av(Vector4d auth) {
Vector6d av;
double a = auth[0];
double b = auth[1];
double g = auth[2];
double t = auth[3];
double s1 = sin(a);
double c1 = cos(a);
double s2 = sin(b);
double c2 = cos(b);
double s3 = sin(g);
double c3 = cos(g);
Matrix3d R;
R <<
c2 * c3, s1 * s2 * c3 - c1 * s3, c1 * s2 * c3 + s1 * s3,
c2 * s3, s1 * s2 * s3 + c1 * c3, c1 * s2 * s3 - s1 * c3,
-s2, s1 * c2, c1 * c2;
double d = cos(t) / sin(t);
av.head(3) = -R.col(2) * d;
av.tail(3) = R.col(1);
// Vector3d u1 = R.col(0);
// Vector3d u2 = R.col(1);
//
// double sigma1 = cos(t);
// double sigma2 = sin(t);
//
// double d = cos(t) / sin(t);
//
// // Vector3d n = sigma1 * u1;
// // Vector3d v = sigma2 * u2;
// Vector3d n = u1;
// Vector3d v = u2;
// av.head(3) = -v.cross(n) * d;
// av.tail(3) = v;
return av;
}
Vector4d gc_av_to_aid(Vector6d av) {
Vector4d aid;
Vector3d a = av.head(3);
Vector3d x = av.tail(3); // v
Vector3d y = a.cross(x); // n
aid(3) = x.norm() / y.norm();
x /= x.norm();
y /= y.norm();
Vector3d z = x.cross(y);
Matrix3d R;
R.col(0) = x;
R.col(1) = y;
R.col(2) = z;
Vector3d aa = gc_Rodriguez(R);
aid(0) = aa(0);
aid(1) = aa(1);
aid(2) = aa(2);
// Vector4d aid;
// Vector3d a = av.head(3);
// Vector3d v = av.tail(3); // v
// Vector3d n = a.cross(v); // n
//
// aid(3) = v.norm() / n.norm();
//
// Vector3d x = n / n.norm();
// Vector3d y = v / v.norm();
// Vector3d z = x.cross(y);
//
// aid[0] = atan2( y(2), z(2) );
// aid[1] = asin( - x(2) );
// aid[2] = atan2( x(1), x(0) );
return aid;
}
Matrix3d
getOrientationAndCentriods(Vector3d & p0a,
Vector3d & p0b,
Vector3d & p0c,
Vector3d & p1a,
Vector3d & p1b,
Vector3d & p1c,
Vector3d & c0,
Vector3d & c1)
{
c0 = (p0a+p0b+p0c)*(1.0/3.0);
c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3d H = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R;
R = V * svd.matrixU().transpose();
double det = R.determinant();
if (det < 0.0)
{
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
return R;
}
//computes the angle between tan_theta and rot.col(match_axis) to a precision such that rot_theta = AngleAxisd(angle, rot.col(rotate_axis)) * rot and rot(match_axis) = rot_theta
double angle_match_rotation(Matrix3d& rot_theta, const Vector3d& tan_theta, const Matrix3d& rot, int rotate_axis, int match_axis)
{
double angle = angle_between(rot.col(match_axis), tan_theta);
rot_theta = Eigen::AngleAxisd(angle, rot.col(rotate_axis)) * rot;
if (angle_between(rot_theta.col(match_axis), tan_theta) < 1e-5)
return angle;
rot_theta = Eigen::AngleAxisd(-angle, rot.col(rotate_axis)) * rot;
if (angle_between(rot_theta.col(match_axis), tan_theta) < 1e-5)
return -angle;
cout << "WARNING: angle_match_rotation failed to find the angle naively. Now trying to find it iteratively." << endl;
int i = 0;
double test_angle;
while ((test_angle = angle_between(rot_theta.col(match_axis), tan_theta)) >= 1e-5) {
angle -= test_angle;
rot_theta = Eigen::AngleAxisd(-angle, rot.col(rotate_axis)) * rot;
printf("recomp angle %d: \n%.20f \n%.20f \n%.20f\n", i, angle_between(rot.col(2), rot_theta.col(2)), angle, angle_between(rot_theta.col(2), tan_theta));
cout << "theta: " << angle << " vector " << rot_theta.col(2).transpose() << " should be approx equal to " << tan_theta.normalized().transpose() << endl;
i++;
}
return -angle;
}
//---------------------------------------------------------------------
// Gram-Schmidt orthonormalization.
//
void orthonormalize(Matrix3d &mat) {
for(int i = 0; i < 3; ++i) {
Vector3d accum = Vector3d::Zero();
for(int j = 0; j < i; ++j) {
// accum += projection(vectors[i], vectors[j])
Vector3d vi = mat.col(i);
Vector3d vj = mat.col(j);
double scale = vi.dot(vj) / vj.squaredNorm();
vj *= scale;
accum += vj;
}
mat.col(i) -= accum;
mat.col(i).normalize();
}
// Convert really tiny numbers to zero
for(int i = 0; i < 3; ++i)
for(int j = 0; j < 3; ++j)
if(-1e-10 < mat(i, j) && mat(i, j) < 1e-10)
mat(i, j) = 0.0;
}
geometry_msgs::Pose BaxterArmCommander::normalToPose(Vector3f plane_normal, Vector3f major_axis, Vector3f centroid) {
geometry_msgs::Pose pose;
Affine3d Affine_des_gripper;
Vector3d xvec_des,yvec_des,zvec_des,origin_des;
Matrix3d Rmat;
for (int i=0;i<3;i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; //want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
// origin_des[2]+=0.02; //raise up 2cm
yvec_des = zvec_des.cross(xvec_des); //construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear()=Rmat;
Affine_des_gripper.translation()=origin_des;
//convert des pose from Affine to geometry_msgs::PoseStamped
pose = transformEigenAffine3dToPose(Affine_des_gripper);
return pose;
}
//==============================================================================
double State::getCoronalRightLegAngle() const
{
Matrix3d comR = getCOMFrame().linear();
Vector3d comY = comR.col(1);
Vector3d thighAxisZ = mRightThigh->getTransform().linear().col(2);
Vector3d projThighAZ = (comR.transpose() * thighAxisZ);
projThighAZ[0] = 0.0;
projThighAZ.normalize();
double angle = _getAngleBetweenTwoVectors(projThighAZ, comY);
Vector3d cross = comY.cross(projThighAZ);
if (cross[0] > 0.0)
return angle;
else
return -angle;
}
//==============================================================================
double State::getCoronalPelvisAngle() const
{
Matrix3d comR = getCOMFrame().linear();
Vector3d comY = comR.col(1);
Vector3d pelvisZ = mPelvis->getTransform().linear().col(2);
Vector3d projPelvisZ = (comR.transpose() * pelvisZ);
projPelvisZ[0] = 0.0;
projPelvisZ.normalize();
double angle = _getAngleBetweenTwoVectors(projPelvisZ, comY);
Vector3d cross = comY.cross(projPelvisZ);
if (cross[0] > 0.0)
return angle;
else
return -angle;
}
Matrix4d TrfmRotateExpMap::getDeriv(const Dof *d){
Vector3d q(mDofs[0]->getValue(), mDofs[1]->getValue(), mDofs[2]->getValue());
// derivative wrt which dof
int j=-1;
for(unsigned int i=0; i<mDofs.size(); i++) if(d==mDofs[i]) j=i;
assert(j!=-1);
assert(j>=0 && j<=2);
Matrix3d R = utils::rotation::expMapRot(q);
Matrix3d J = utils::rotation::expMapJac(q);
Matrix3d dRdj = utils::makeSkewSymmetric(J.col(j))*R;
Matrix4d dRdj4d = Matrix4d::Zero();
dRdj4d.topLeftCorner(3,3) = dRdj;
return dRdj4d;
}
// assumes tan is normalized
void rotation_from_tangent(const Vector3d& tan, Matrix3d& rot) {
rot.col(0) = tan;
int max_coor = 0, min_coor = 0;
for (int i = 1; i < 3; i++) {
if (tan(i) > tan(max_coor))
max_coor = i;
if (tan(i) <= tan(min_coor))
min_coor = i;
}
Vector3d arb = tan + Vector3d(1.0, 1.0, 1.0);
arb(max_coor) *= 0.1;
arb(min_coor) *= 10.0;
rot.col(1) = arb - arb.dot(tan) * tan;
rot.col(1).normalize();
rot.col(2) = rot.col(0).cross(rot.col(1));
rot.col(2).normalize();
}
//This is hack to easily determine the displacement of each control
void World::applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
vector<Vector3d> pre_positions;
if ((cursors.size() == 2) || (displacements.size() == 2)) {
pre_positions.resize(2);
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
pre_positions[i] = cursors[i]->end_eff->getPosition();
}
} else {
for (int i = 0; i < displacements.size(); i++) {
displacements[i] = -1;
}
}
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
if ((cursors.size() == 2) || (displacements.size() == 3)) {
Matrix<double,6,1> u_tran;
for (int i = 0; i < cursors.size(); i++) {
assert(cursors[i]->isAttached());
if (cursors[i]->isAttached())
//cout << "norm of end effector " << i << ": " << (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm() << endl;
displacements[i] = (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm();
u_tran.segment(i*3,3) = (cursors[i]->end_eff->getPosition() - pre_positions[i]);
}
displacements[2] = u_tran.norm();
}
}
void World::applyRelativeControl(const vector<Control*>& controls, double thresh, bool limit_displacement)
{
assert(cursors.size() == controls.size());
for (int i = 0; i < cursors.size(); i++) {
Cursor* cursor = cursors[i];
Matrix3d rotate(controls[i]->getRotate());
AngleAxisd rotate_aa(rotate);
AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
double trans_norm = controls[i]->getTranslate().norm();
const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm),
normRand(0, thresh*trans_norm));
const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
if (controls[i]->getButton(UP))
cursor->openClose(limit_displacement);
if (controls[i]->getButton(DOWN))
cursor->attachDettach(limit_displacement);
}
for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
threads[thread_ind]->minimize_energy();
}
vector<EndEffector*> end_effs;
getObjects<EndEffector>(end_effs);
for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
end_effs[ee_ind]->updateTransformFromAttachment();
}
}
void DecomposeEssentialUsingHorn90(double _E[9], double _R1[9], double _R2[9], double _t1[3], double _t2[3]) {
//from : http://people.csail.mit.edu/bkph/articles/Essential.pdf
#ifdef USE_EIGEN
using namespace Eigen;
Matrix3d E = Map<Matrix<double,3,3,RowMajor> >(_E);
Matrix3d EEt = E * E.transpose();
Vector3d e0e1 = E.col(0).cross(E.col(1)),e1e2 = E.col(1).cross(E.col(2)),e2e0 = E.col(2).cross(E.col(0));
Vector3d b1,b2;
#if 1
//Method 1
Matrix3d bbt = 0.5 * EEt.trace() * Matrix3d::Identity() - EEt; //Horn90 (12)
Vector3d bbt_diag = bbt.diagonal();
if (bbt_diag(0) > bbt_diag(1) && bbt_diag(0) > bbt_diag(2)) {
b1 = bbt.row(0) / sqrt(bbt_diag(0));
b2 = -b1;
} else if (bbt_diag(1) > bbt_diag(0) && bbt_diag(1) > bbt_diag(2)) {
b1 = bbt.row(1) / sqrt(bbt_diag(1));
b2 = -b1;
} else {
b1 = bbt.row(2) / sqrt(bbt_diag(2));
b2 = -b1;
}
#else
//Method 2
if (e0e1.norm() > e1e2.norm() && e0e1.norm() > e2e0.norm()) {
b1 = e0e1.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else if (e1e2.norm() > e0e1.norm() && e1e2.norm() > e2e0.norm()) {
b1 = e1e2.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
} else {
b1 = e2e0.normalized() * sqrt(0.5 * EEt.trace()); //Horn90 (18)
b2 = -b1;
}
#endif
//Horn90 (19)
Matrix3d cofactors; cofactors.col(0) = e1e2; cofactors.col(1) = e2e0; cofactors.col(2) = e0e1;
cofactors.transposeInPlace();
//B = [b]_x , see Horn90 (6) and http://en.wikipedia.org/wiki/Cross_product#Conversion_to_matrix_multiplication
Matrix3d B1; B1 << 0,-b1(2),b1(1),
b1(2),0,-b1(0),
-b1(1),b1(0),0;
Matrix3d B2; B2 << 0,-b2(2),b2(1),
b2(2),0,-b2(0),
-b2(1),b2(0),0;
Map<Matrix<double,3,3,RowMajor> > R1(_R1),R2(_R2);
//Horn90 (24)
R1 = (cofactors.transpose() - B1*E) / b1.dot(b1);
R2 = (cofactors.transpose() - B2*E) / b2.dot(b2);
Map<Vector3d> t1(_t1),t2(_t2);
t1 = b1; t2 = b2;
cout << "Horn90 provided " << endl << R1 << endl << "and" << endl << R2 << endl;
#endif
}
// assumes cv::Mats are of type <float>
int PoseEstimator::estimate(const matches_t &matches,
const cv::Mat &train_kpts, const cv::Mat &query_kpts,
const cv::Mat &train_pts, const cv::Mat &query_pts,
const cv::Mat &K, const double baseline)
{
// make sure we're using floats
if (train_kpts.depth() != CV_32F ||
query_kpts.depth() != CV_32F ||
train_pts.depth() != CV_32F ||
query_pts.depth() != CV_32F)
throw std::runtime_error("Expected input to be of floating point (CV_32F)");
int nmatch = matches.size();
cout << endl << "[pe3d] Matches: " << nmatch << endl;
// set up data structures for fast processing
// indices to good matches
std::vector<Eigen::Vector3f> t3d, q3d;
std::vector<Eigen::Vector2f> t2d, q2d;
std::vector<int> m; // keep track of matches
for (int i=0; i<nmatch; i++)
{
const float* ti = train_pts.ptr<float>(matches[i].trainIdx);
const float* qi = query_pts.ptr<float>(matches[i].queryIdx);
const float* ti2 = train_kpts.ptr<float>(matches[i].trainIdx);
const float* qi2 = query_kpts.ptr<float>(matches[i].queryIdx);
// make sure we have points within range; eliminates NaNs as well
if (matches[i].distance < 60 && // TODO: get this out of the estimator
ti[2] < maxMatchRange &&
qi[2] < maxMatchRange)
{
m.push_back(i);
t2d.push_back(Eigen::Vector2f(ti2[0],ti2[1]));
q2d.push_back(Eigen::Vector2f(qi2[0],qi2[1]));
t3d.push_back(Eigen::Vector3f(ti[0],ti[1],ti[2]));
q3d.push_back(Eigen::Vector3f(qi[0],qi[1],qi[2]));
}
}
nmatch = m.size();
cout << "[pe3d] Filtered matches: " << nmatch << endl;
if (nmatch < 3) return 0; // can't do it...
int bestinl = 0;
Matrix3f Kf;
cv::cv2eigen(K,Kf); // camera matrix
float fb = Kf(0,0)*baseline; // focal length times baseline
float maxInlierXDist2 = maxInlierXDist*maxInlierXDist;
float maxInlierDDist2 = maxInlierDDist*maxInlierDDist;
// set up minimum hyp pixel distance
float minpdist = minPDist * Kf(0,2) * 2.0;
// RANSAC loop
int numchoices = 1 + numRansac / 10;
for (int ii=0; ii<numRansac; ii++)
{
// find a candidate
int k;
int a=rand()%nmatch;
int b;
for (k=0; k<numchoices; k++)
{
b=rand()%nmatch;
if (a!=b && (t2d[a]-t2d[b]).norm() > minpdist
&& (q2d[a]-q2d[b]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices) continue;
int c;
for (k=0; k<numchoices+numchoices; k++)
{
c=rand()%nmatch;
if (c!=b && c!=a && (t2d[a]-t2d[c]).norm() > minpdist
&& (t2d[b]-t2d[c]).norm() > minpdist
&& (q2d[a]-q2d[c]).norm() > minpdist
&& (q2d[b]-q2d[c]).norm() > minpdist)
// TODO: add distance check
break;
}
if (k >= numchoices+numchoices) continue;
// get centroids
Vector3f p0a = t3d[a];
Vector3f p0b = t3d[b];
Vector3f p0c = t3d[c];
Vector3f p1a = q3d[a];
Vector3f p1b = q3d[b];
Vector3f p1c = q3d[c];
Vector3f c0 = (p0a+p0b+p0c)*(1.0/3.0);
Vector3f c1 = (p1a+p1b+p1c)*(1.0/3.0);
// subtract out
p0a -= c0;
p0b -= c0;
p0c -= c0;
p1a -= c1;
p1b -= c1;
p1c -= c1;
Matrix3f Hf = p1a*p0a.transpose() + p1b*p0b.transpose() +
p1c*p0c.transpose();
Matrix3d H = Hf.cast<double>();
#if 0
cout << p0a.transpose() << endl;
cout << p0b.transpose() << endl;
cout << p0c.transpose() << endl;
#endif
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
// cout << "[PE test] R: " << endl << R << endl;
// cout << "[PE test] t: " << tr.transpose() << endl;
Vector3f trf = tr.cast<float>(); // convert to floats
Matrix3f Rf = R.cast<float>();
Rf = Kf*Rf;
trf = Kf*trf;
// find inliers, based on image reprojection
int inl = 0;
for (int i=0; i<nmatch; i++)
{
const Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2
&& dd*dd < maxInlierDDist2)
// inl+=(int)fsqrt(ipt.z()); // clever way to weight closer points
inl++;
}
if (inl > bestinl)
{
bestinl = inl;
trans = tr.cast<float>(); // convert to floats
rot = R.cast<float>();
}
}
cout << "[pe3d] Best inliers: " << bestinl << endl;
// printf("Total ransac: %d Neg det: %d\n", ntot, nneg);
// reduce matches to inliers
matches_t inls; // temporary for current inliers
inliers.clear();
Matrix3f Rf = Kf*rot;
Vector3f trf = Kf*trans;
// cout << "[pe3e] R: " << endl << rot << endl;
cout << "[pe3d] t: " << trans.transpose() << endl;
AngleAxisf aa;
aa.fromRotationMatrix(rot);
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
for (int i=0; i<nmatch; i++)
{
Vector3f &pt1 = q3d[i];
Vector3f ipt = Rf*pt1+trf; // transform query point
float iz = 1.0/ipt.z();
Vector2f &kp = t2d[i];
// cout << kp.transpose() << " " << pt1.transpose() << " " << ipt.transpose() << endl;
float dx = kp.x() - ipt.x()*iz;
float dy = kp.y() - ipt.y()*iz;
float dd = fb/t3d[i].z() - fb/ipt.z(); // diff of disparities, could pre-compute t3d
if (dx*dx < maxInlierXDist2 && dy*dy < maxInlierXDist2 &&
dd*dd < maxInlierDDist2)
inls.push_back(matches[m[i]]);
}
cout << "[pe3d] Final inliers: " << inls.size() << endl;
// polish with SVD
if (polish)
{
Matrix3d Rd = rot.cast<double>();
Vector3d trd = trans.cast<double>();
StereoPolish pol(5,false);
pol.polish(inls,train_kpts,query_kpts,train_pts,query_pts,K,baseline,
Rd,trd);
AngleAxisd aa;
aa.fromRotationMatrix(Rd);
cout << "[pe3d] Polished t: " << trd.transpose() << endl;
cout << "[pe3d] Polished AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
int num = inls.size();
// get centroids
Vector3f c0(0,0,0);
Vector3f c1(0,0,0);
for (int i=0; i<num; i++)
{
c0 += t3d[i];
c1 += q3d[i];
}
c0 = c0 / (float)num;
c1 = c1 / (float)num;
// subtract out and create H matrix
Matrix3f Hf;
Hf.setZero();
for (int i=0; i<num; i++)
{
Vector3f p0 = t3d[i]-c0;
Vector3f p1 = q3d[i]-c1;
Hf += p1*p0.transpose();
}
Matrix3d H = Hf.cast<double>();
// do the SVD thang
JacobiSVD<Matrix3d> svd(H, ComputeFullU | ComputeFullV);
Matrix3d V = svd.matrixV();
Matrix3d R = V * svd.matrixU().transpose();
double det = R.determinant();
//ntot++;
if (det < 0.0)
{
//nneg++;
V.col(2) = V.col(2)*-1.0;
R = V * svd.matrixU().transpose();
}
Vector3d cd0 = c0.cast<double>();
Vector3d cd1 = c1.cast<double>();
Vector3d tr = cd0-R*cd1; // translation
aa.fromRotationMatrix(R);
cout << "[pe3d] t: " << tr.transpose() << endl;
cout << "[pe3d] AA: " << aa.angle()*180.0/M_PI << " " << aa.axis().transpose() << endl;
#if 0
// system
SysSBA sba;
sba.verbose = 0;
// set up nodes
// should have a frame => node function
Vector4d v0 = Vector4d(0,0,0,1);
Quaterniond q0 = Quaternion<double>(Vector4d(0,0,0,1));
sba.addNode(v0, q0, f0.cam, true);
Quaterniond qr1(rot); // from rotation matrix
Vector4d temptrans = Vector4d(trans(0), trans(1), trans(2), 1.0);
// sba.addNode(temptrans, qr1.normalized(), f1.cam, false);
qr1.normalize();
sba.addNode(temptrans, qr1, f1.cam, false);
int in = 3;
if (in > (int)inls.size())
in = inls.size();
// set up projections
for (int i=0; i<(int)inls.size(); i++)
{
// add point
int i0 = inls[i].queryIdx;
int i1 = inls[i].trainIdx;
Vector4d pt = query_pts[i0];
sba.addPoint(pt);
// projected point, ul,vl,ur
Vector3d ipt;
ipt(0) = f0.kpts[i0].pt.x;
ipt(1) = f0.kpts[i0].pt.y;
ipt(2) = ipt(0)-f0.disps[i0];
sba.addStereoProj(0, i, ipt);
// projected point, ul,vl,ur
ipt(0) = f1.kpts[i1].pt.x;
ipt(1) = f1.kpts[i1].pt.y;
ipt(2) = ipt(0)-f1.disps[i1];
sba.addStereoProj(1, i, ipt);
}
sba.huber = 2.0;
sba.doSBA(5,10e-4,SBA_DENSE_CHOLESKY);
int nbad = sba.removeBad(2.0);
// cout << endl << "Removed " << nbad << " projections > 2 pixels error" << endl;
sba.doSBA(5,10e-5,SBA_DENSE_CHOLESKY);
// cout << endl << sba.nodes[1].trans.transpose().head(3) << endl;
// get the updated transform
trans = sba.nodes[1].trans.head(3);
Quaterniond q1;
q1 = sba.nodes[1].qrot;
rot = q1.toRotationMatrix();
// set up inliers
inliers.clear();
for (int i=0; i<(int)inls.size(); i++)
{
ProjMap &prjs = sba.tracks[i].projections;
if (prjs[0].isValid && prjs[1].isValid) // valid track
inliers.push_back(inls[i]);
}
#if 0
printf("Inliers: %d After polish: %d\n", (int)inls.size(), (int)inliers.size());
#endif
#endif
}
inliers = inls;
return (int)inls.size();
}
int
PatchFit::fit()
{
// first check whether cloud subsampling is required
RandomSample<PointXYZ> *rs;
rs = new RandomSample<PointXYZ>();
if (this->ssmax_)
{
rs->setInputCloud(cloud_);
rs->setSample(ssmax_);
rs->setSeed(rand());
rs->filter(*cloud_);
}
delete (rs);
//remove NAN points from the cloud
std::vector<int> indices;
removeNaNFromPointCloud(*cloud_, *cloud_, indices); //is_dense should be false
// Step 1: fit plane by lls
Vector4d centroid;
compute3DCentroid(*cloud_, centroid);
p_->setC(centroid.head(3));
cloud_vec.resize(cloud_->points.size(),3);
fit_cloud_vec.resize(cloud_->points.size(),3);
int vec_counter = 0;
for (size_t i = 0; i<cloud_->points.size (); i++)
{
cloud_vec.row(vec_counter) = cloud_->points[i].getVector3fMap ().cast<double>();
// for fitting sub the centroid
fit_cloud_vec.row(vec_counter) = cloud_vec.row(vec_counter) - centroid.head(3).transpose();
vec_counter++;
}
cloud_vec.conservativeResize(vec_counter-1, 3); //normal resize does not keep old values
VectorXd b(fit_cloud_vec.rows());
b.fill(0.0);
Vector3d zl = lls(fit_cloud_vec,b);
p_->setR(zh.cross(zl));
double ss = p_->getR().norm();
double cc = zh.adjoint()*zl;
double t = atan2(ss,cc);
double th = sqrt(sqrt(EPS))*10.0;
double aa;
if (t>th)
aa = t/ss;
else
aa = 6.0/(6.0-t*t);
p_->setR(aa*p_->getR());
// Save the zl and the centroid p.c, for patch center constraint's use
plane_n = zl;
plane_c = p_->getC();
// Step 2: continue surface fitting if not plane
VectorXd a;
if (st_a) // general paraboloid
{
if (ccon_)
{
a.resize(6);
a << 0.0, 0.0, p_->getR(), 0.0;
}
else
{
a.resize(8);
a << 0.0, 0.0, p_->getR(), p_->getC();
}
a = wlm(cloud_vec, a);
// update the patch
p_->setR(a.segment(2,3));
if (ccon_)
p_->setC(plane_c + a(5)*plane_n);
else
p_->setC(a.segment(5,3));
Vector2d k;
k << a(0), a(1);
// refine into a specific paraboloid type
if (k.cwiseAbs().maxCoeff() < this->ktol_)
{
// fitting planar paraboloid
p_->setName("planar paraboloid");
p_->setS(Patch::s_p);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
p_->setR(p_->rcanon2(p_->getR(),2,3)); //TBD not updating correctly
}
else if (k.cwiseAbs().minCoeff() < this->ktol_)
{
// fitting cylindric paraboloid
p_->setName("cylindric paraboloid");
p_->setS(Patch::s_y);
p_->setB(Patch::b_r);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
kx = k(1);
ky = k(0);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
Eigen::Matrix<double,1,1> k_c_p;
k_c_p << ky;
p_->setK(k_c_p);
}
else if (fabs(k(0)-k(1)) < this->ktol_)
{
// fitting circular paraboloid
p_->setName("circular paraboloid");
p_->setS(Patch::s_o);
p_->setB(Patch::b_c);
Matrix<double,1,1> k_c_p;
k_c_p << k.mean();
p_->setK(k_c_p);
p_->setR(p_->rcanon2(p_->getR(),2,3));
}
else
{
if (sign(k(0),k(1)))
{
// fitting elliptic paraboloid
p_->setName("elliptic paraboloid");
p_->setS(Patch::s_e);
}
else
{
// fitting hyperbolic paraboloid
p_->setName("hyperbolic paraboloid");
p_->setS(Patch::s_h);
}
p_->setB(Patch::b_e);
p_->setK(k);
kx = k(0);
ky = k(1);
if (fabs(kx) > fabs(ky))
{
// swap curvatures
Vector2d k_swap;
k_swap << ky, kx;
p_->setK(k_swap);
Matrix3d ww;
ww << yh, -xh, zh;
Matrix3d rr;
rr = p_->rexp(p_->getR());
rr = rr*ww;
p_->setR(p_->rlog(rr));
}
}
}
else
{
// plane fitting
p_->setS(Patch::s_p);
p_->setB(this->bt_);
Matrix<double,1,1> k_p;
k_p << 0.0;
p_->setK(k_p);
}
// Step 5: fit boundary; project to local frame XY plane
Xform3 proj;
Matrix3d rrinv;
rrinv = p_->rexp(-p_->getR());
MatrixXd ux(cloud_vec.rows(),1), uy(cloud_vec.rows(),1), uz(cloud_vec.rows(),1);
ux = cloud_vec.col(0);
uy = cloud_vec.col(1);
uz = cloud_vec.col(2);
proj.transform(ux,uy,uz,p_->rexp(-p_->getR()), (-rrinv*p_->getC()),0,0);
// (normalized) moments
double mx = ux.mean();
double my = uy.mean();
double vx = (ux.array().square()).mean();
double vy = (uy.array().square()).mean();
double vxy = (ux.array() * uy.array()).mean();
// Step 5
lambda = sqrt(2.0) * boost::math::erf_inv(this->bcp_);
//STEP 6: cylindric parab: aa rect bound. This step also sets p.c as the 1D
// data centroid along the local frame x axis, which is the symmetry axis of
// the cylinder.
if (p_->getS() == Patch::s_y)
{
// fitting boundary: cyl parab, aa rect
VectorXd d(2);
d << lambda * sqrt(vx-mx*mx) , lambda * (sqrt(vy));
p_->setD(d);
p_->setC(p_->rexp(p_->getR())*mx*xh + p_->getC());
}
// STEP 7: circ parab: circ bound
if (p_->getS() == Patch::s_o)
{
// fitting boundary: cir parab, circ
VectorXd d(1);
d << lambda * max(sqrt(vx),sqrt(vy));
p_->setD(d);
}
// STEP 8: ell or hyp parab: ell bound
if (p_->getS() == Patch::s_e || p_->getS() == Patch::s_h)
{
// fitting boundary: ell/hyb parab, ell
VectorXd d(2);
d << lambda * sqrt(vx), lambda * sqrt(vy);
p_->setD(d);
}
// STEP 9: plane bounds
if (p_->getS() == Patch::s_p)
{
// fitting boundary: plane
Matrix3d rr = p_->rexp(p_->getR());
p_->setC(rr*(mx*xh+my*yh) + p_->getC());
double a = vx-mx*mx;
double b = 2.0*(vxy-mx*my);
double c = vy-my*my;
lambda = -log(1.0-this->bcp_);
double d = sqrt(b*b+(a-c)*(a-c));
double wp = a+c+d;
double wn = a+c-d;
Vector2d l;
l << sqrt(lambda*wp), sqrt(lambda*wn);
VectorXd d_p(1);
switch (p_->getB())
{
case Patch::b_c:
d_p << l.maxCoeff();
p_->setD(d_p);
break;
case Patch::b_e:
p_->setD(l);
break;
case Patch::b_r:
p_->setD(l);
break;
default:
cerr << "Invalid pach boundary for plane" << endl;
}
if (p_->getB() != Patch::b_c)
{
double t = 0.5 * atan2(b,a-c);
Matrix3d rr = p_->rexp(p_->getR());
Vector3d xl = rr.col(0);
Vector3d yl = rr.col(1);
Vector3d zl = rr.col(2);
xl = (cos(t)*xl) + (sin(t)*yl);
yl = zl.cross(xl);
Matrix3d xyzl;
xyzl << xl, yl, zl;
p_->setR(p_->rlog(xyzl));
}
} //plane boundary
return (1);
}
void GLThread::DrawAxesAtPoint(const Vector3d& pt, const Matrix3d& rot_in, ColorCode color_axis, bool skipX)
{
glDisable(GL_TEXTURE_2D);
glPushMatrix();
//Matrix3d rot = Eigen::AngleAxisd(M_PI/2.0, Vector3d::UnitX())*rot_in;
Matrix3d rot = Eigen::AngleAxisd(M_PI/2.0, rot_in.col(0))*rot_in;
const double radius = 0.15;
const double radius_cone_max = 0.4;
Vector3d diff_pos = pt-display_start_pos;
double rotation_scale_factor = 6.0;
double cone_scale_factor = 0.07;
Matrix3d rotations_project = rot*rotation_scale_factor;
double pts[4][3];
double pts_cone[4][3];
double radius_cone[4];
radius_cone[0] = radius_cone_max*2;
radius_cone[1] = radius_cone_max;
radius_cone[2] = 1e-3;
radius_cone[3] = 0;
for (int i=0; i < 3; i++)
{
pts[0][i] = diff_pos(i)-rotations_project(i,0);
pts[1][i] = diff_pos(i);
}
for (int coord=0; coord < 3; coord++)
{
if (coord == 0 && skipX)
continue;
for (int i=0; i < 3; i++)
{
pts[2][i] = diff_pos(i) + rotations_project(i,coord);
pts[3][i] = diff_pos(i) + 2*rotations_project(i,coord);
pts_cone[0][i] = diff_pos(i) + rotations_project(i,coord)*(1.0-cone_scale_factor);
pts_cone[1][i] = diff_pos(i) + rotations_project(i,coord);
pts_cone[2][i] = diff_pos(i) + rotations_project(i,coord)*(1.0+cone_scale_factor);
pts_cone[3][i] = diff_pos(i) + rotations_project(i,coord)*(1.0+cone_scale_factor*2);
}
float thread_color_array[4][3];
float stripe_color_array[4][3];
for (int i=0; i < 4; i++)
{
for (int j=0; j < 3; j++)
{
thread_color_array[i][j] = thread_color_float[j];
stripe_color_array[i][j] = stripe_color_float[j];
}
}
const float alpha = 0.85;
// gleTextureMode (GLE_TEXTURE_VERTEX_MODEL_CYL);
if (color_axis == material)
{
if (coord == 0)
glColor4f(0.7, 0.2, 0, alpha);
else if (coord == 1)
glColor4f((thread_color_float[0])*0.8, (thread_color_float[1])*0.8, (thread_color_float[2])*0.8, alpha);
else
glColor4f((stripe_color_float[0])*0.8, (stripe_color_float[1])*0.8, (stripe_color_float[2])*0.8, alpha);
} else if (color_axis == bishop) {
if (coord == 0)
glColor4f(0.7, 0.2, 0, alpha);
else if (coord == 1)
glColor3d(0.5, 0.5, 0.5);
else
glColor3d(0.17, 0.17, 0.17);
} else {
if (coord == 0)
glColor3d(1.0, 0.0, 0.0);
else if (coord == 1)
glColor3d(0.0, 1.0, 0.0);
else
glColor3d(0.0, 0.0, 1.0);
}
glePolyCylinder(4, pts, 0x0,radius);
glePolyCone(4, pts_cone, 0x0, radius_cone);
}
glPopMatrix();
glEnable(GL_TEXTURE_2D);
}
/** Return the orientation of the frame at the specified time.
*
* This method returns identity when there frame is not defined or
* degenerate for one of the following reasons:
* - One of the directions is null
* - The primary and secondary axes are not orthogonal
* - The primary or secondary vectors is zero (or very close to zero) at the specified time
* - The primary or secondary vectors are either aligned or exactly opposite (or very close
* to such a configuration.)
*/
Quaterniond TwoVectorFrame::orientation(double tdbSec) const
{
if (!m_valid)
{
return Quaterniond::Identity();
}
else
{
Vector3d v0 = m_primary->direction(tdbSec);
Vector3d v1 = m_secondary->direction(tdbSec);
if (v0.isZero() || v1.isZero())
{
// The primary or secondary vectors are zero at the current time
return Quaterniond::Identity();
}
v0.normalize();
v1.normalize();
if (isNegativeAxis(m_primaryAxis))
{
v0 = -v0;
}
if (isNegativeAxis(m_secondaryAxis))
{
v1 = -v1;
}
Vector3d v2 = v0.cross(v1);
if (v2.isZero())
{
// Primary and secondary directions are (nearly) collinear and thus
// don't determine an orientation.
return Quaterniond::Identity();
}
v2.normalize();
int dir0 = (int) m_primaryAxis;
int dir1 = (int) m_secondaryAxis;
int axis0 = dir0 % 3;
int axis1 = dir1 % 3;
bool rightHanded = ((axis0 + 1) % 3) == (axis1 % 3);
// axis2 is whatever axis is not axis0 or axis1
int axis2 = 3 - (axis0 + axis1);
Matrix3d m;
m.col(axis0) = v0;
m.col(axis1) = v2.cross(v0);
if (rightHanded)
{
m.col(axis2) = v2;
}
else
{
m.col(axis2) = -v2;
}
return Quaterniond(m);
}
}
