Esempio n. 1
0
/**
 * @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);
        }
    }
}
Esempio n. 2
0
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;
}
Esempio n. 4
0
File: gc.cpp Progetto: rgkoo/slslam
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;
}
Esempio n. 5
0
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);
	}
}
Esempio n. 6
0
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;
}
Esempio n. 7
0
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;
}
Esempio n. 9
0
        // 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;
      
        }
Esempio n. 10
0
File: gc.cpp Progetto: rgkoo/slslam
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;
}
Esempio n. 11
0
File: gc.cpp Progetto: rgkoo/slslam
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;
}
Esempio n. 12
0
    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;
}
Esempio n. 14
0
File: gc.cpp Progetto: rgkoo/slslam
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;
}
Esempio n. 15
0
File: gc.cpp Progetto: rgkoo/slslam
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;
}
Esempio n. 16
0
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;
}
Esempio n. 18
0
//---------------------------------------------------------------------
// 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;
}
Esempio n. 19
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;
}
Esempio n. 20
0
//==============================================================================
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;
}
Esempio n. 21
0
//==============================================================================
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;
}
Esempio n. 22
0
    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();
}
Esempio n. 24
0
//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();
	}
}
Esempio n. 25
0
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
}
Esempio n. 27
0
  // 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);
}
Esempio n. 29
0
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);
}
Esempio n. 30
0
/** 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);
    }
}