C++ Vector3f::cross方法代码示例

void fuzzyAffines()
{
    std::vector<Eigen::Matrix4f> trans;
    trans.reserve(count/10);
    for( size_t i=0; i<count/10; i++ )
    {
        Eigen::Vector3f x = Eigen::Vector3f::Random();
        Eigen::Vector3f y = Eigen::Vector3f::Random();

        x.normalize();
        y.normalize();

        Eigen::Vector3f z = x.cross(y);
        z.normalize();

        y = z.cross(x);
        y.normalize();

        Eigen::Affine3f t = Eigen::Affine3f::Identity();
        Eigen::Matrix3f r = Eigen::Matrix3f::Identity();

        r.col(0) = x;
        r.col(1) = y;
        r.col(2) = z;

        t.rotate(r);
        t.translate( 0.5f * Eigen::Vector3f::Random() + Eigen::Vector3f(0.5,0.5,0.5) );

        trans.push_back( t.matrix() );
    }

    s_plot.setColor( Eigen::Vector4f(1,0,0,1) );
    s_plot.setLineWidth( 3.0 );
    s_plot( trans, nox::plot<float>::Pos | nox::plot<float>::CS );
}

void SensorFusion::reset(const Eigen::Vector3f& accel, const Eigen::Vector3f& magnetom)
{
	G_Dt = 0;

	Eigen::Vector3f temp1;
	Eigen::Vector3f temp2;
	Eigen::Vector3f xAxis;
	xAxis << 1.0f, 0.0f, 0.0f;

	timestamp = highResClock.now();

	// GET PITCH
	// Using y-z-plane-component/x-component of gravity vector
	pitch = -atan2f(accel(0), sqrtf(accel(1) * accel(1) + accel(2) * accel(2)));

	// GET ROLL
	// Compensate pitch of gravity vector
	temp1 = accel.cross(xAxis);
	temp2 = xAxis.cross(temp1);

	// Normally using x-z-plane-component/y-component of compensated gravity vector
	// roll = atan2(temp2[1], sqrt(temp2[0] * temp2[0] + temp2[2] * temp2[2]));
	// Since we compensated for pitch, x-z-plane-component equals z-component:
	roll = atan2f(temp2(1), temp2(2));

	// GET YAW
	compassHeading(magnetom);
	yaw = magHeading;

	// Init rotation matrix
	init_rotation_matrix(dcmMatrix, yaw, pitch, roll);
}

// http://www.scratchapixel.com/lessons/3d-basic-rendering/ray-tracing-rendering-a-triangle/ray-triangle-intersection-geometric-solution
float Triangle::checkHit(Eigen::Vector3f eye, Eigen::Vector3f dir) {
   double u, v, t;

   // first check for circumsphere hit
   Eigen::Vector3f dist = eye - center;

   double A = dot(dir, dir);
   double B = dot((2*dir), dist);
   double C = dot(dist, dist) - radius*radius;

   Eigen::Vector3f quad = QuadraticFormula(A, B, C);
   float result;

   if (quad(0) == 0) {
      //SHOULD BE AN ERROR
      result = 0;
   }

   if (quad(0) == 1) {
      result = quad(1);
   }

   if (fabs(quad(1)) <= fabs(quad(2))) {
      result = quad(1);
   } else {
      result = quad(2);
   }

   // failure to even hit the circumsphere
   if (result < 0) {
      return 0;
   }

   Eigen::Vector3f ab = b - a;
   Eigen::Vector3f ac = c - a;
   Eigen::Vector3f pvec = dir.cross(ac);
   double det = dot(ab, pvec);

   #ifdef CULLING
   // if the determinant is negative the triangle is backfacing
   // if the determinant is close to 0, the ray misses the triangle
   if (det < kEpsilon) return 0;
   #else
   // ray and triangle are parallel if det is close to 0
   if (fabs(det) < kEpsilon) return 0;
   #endif
   double invDet = 1 / det;

   Eigen::Vector3f tvec = eye - a;
   u = dot(tvec, pvec) * invDet;
   if (u < 0 || u > 1) return 0;

   Eigen::Vector3f qvec = tvec.cross(ab);
   v = dot(dir, qvec) * invDet;
   if (v < 0 || u + v > 1) return 0;

   t = dot(ac, qvec) * invDet;

   return t;
}

void stateCallback(const nav_msgs::Odometry::ConstPtr& state)
{
  
	
		vel[0] = state->twist.twist.linear.x;
		vel[1] = state->twist.twist.linear.y;
		vel[2] = state->twist.twist.linear.z;
		pos[2] = state->pose.pose.position.z;
    
		//q.x() = state->pose.pose.orientation.x;
		//q.y() = state->pose.pose.orientation.y;
		//q.z() = state->pose.pose.orientation.z;
		//q.w() = state->pose.pose.orientation.w; 
    float q_x = state->pose.pose.orientation.x;
		float q_y = state->pose.pose.orientation.y;
		float q_z = state->pose.pose.orientation.z;
		float q_w = state->pose.pose.orientation.w;

float  yaw = atan2(2*(q_w*q_z+q_x*q_y),1-2*(q_y*q_y+q_z*q_z));
//Eigen::Matrix3d R(q);
	force(0) = offset_x+k_vxy*(vel_des(0)-vel(0))+m*acc_des(0);
	if(pd_control==0)
  force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+m*acc_des(1);
  else if(pd_control==1)
  {
    pos[1]=state->pose.pose.position.y;
    force(1) = offset_y+k_vxy*(vel_des(1)-vel(1))+k_xy*(0-pos[1])+m*acc_des(1);
  }
	force(2) = (k_z*(pos_des(2)-pos(2))+k_vz*(vel_des(2)-vel(2))+m*g(2)+m*acc_des(2));


  b3 = force.normalized();
  b2 = b3.cross(b1w);
  b1 = b2.cross(b3);
  R_des<<b1[0],b2[0],b3[0],
    b1[1],b2[1],b3[1],
    b1[2],b2[2],b3[2];
  Eigen::Quaternionf q_des(R_des);
  	
  quadrotor_msgs::SO3Command command;
	command.force.x = force[0];
  command.force.y = force[1];
  command.force.z = force[2];
  command.orientation.x = q_des.x();
  command.orientation.y = q_des.y();
  command.orientation.z = q_des.z();
  command.orientation.w = q_des.w();
  command.kR[0] = k_R;
  command.kR[1] = k_R;
  command.kR[2] = k_R;
  command.kOm[0] = k_omg;
  command.kOm[1] = k_omg;
  command.kOm[2] = k_omg;
	command.aux.current_yaw = yaw;
	command.aux.enable_motors = true;
	command.aux.use_external_yaw = true;
  control_pub.publish(command);
}

Eigen::Vector3f lineNormalized(Eigen::Vector3f p0, Eigen::Vector3f p1)
{
	Eigen::Vector3f l = p0.cross(p1);
	l.x() = l.x() / l.z();
	l.y() = l.y() / l.z();
	l.z() = 1.0f;
	//return l;
	return p0.cross(p1).normalized();
}

  void MsgToPoint3D(const TPPLPoint &pt, const cob_3d_mapping_msgs::Shape::ConstPtr& new_message, Eigen::Vector3f &pos, Eigen::Vector3f &normal) {
    if(new_message->params.size()==4) {
      Eigen::Vector3f u,v,origin;
      Eigen::Affine3f transformation;
      normal(0)=new_message->params[0];
      normal(1)=new_message->params[1];
      normal(2)=new_message->params[2];
      origin(0)=new_message->centroid.x;
      origin(1)=new_message->centroid.y;
      origin(2)=new_message->centroid.z;
      //std::cout << "normal: " << normal << std::endl;
      //std::cout << "centroid: " << origin << std::endl;
      v = normal.unitOrthogonal ();
      u = normal.cross (v);
      pcl::getTransformationFromTwoUnitVectorsAndOrigin(v, normal,  origin, transformation);

      transformation=transformation.inverse();

      Eigen::Vector3f p3;
      p3(0)=pt.x;
      p3(1)=pt.y;
      p3(2)=0;
      pos = transformation*p3;
    }
    else if(new_message->params.size()==5) {
      Eigen::Vector2f v,v2,n2;
      v(0)=pt.x;
      v(1)=pt.y;
      v2=v;
      v2(0)*=v2(0);
      v2(1)*=v2(1);
      n2(0)=new_message->params[3];
      n2(1)=new_message->params[4];

      //dummy normal
      normal(0)=new_message->params[0];
      normal(1)=new_message->params[1];
      normal(2)=new_message->params[2];

      Eigen::Vector3f x,y, origin;
      x(0)=1.f;
      y(1)=1.f;
      x(1)=x(2)=y(0)=y(2)=0.f;

      Eigen::Matrix<float,3,2> proj2plane_;
      proj2plane_.col(0)=normal.cross(y);
      proj2plane_.col(1)=normal.cross(x);

      origin(0)=new_message->centroid.x;
      origin(1)=new_message->centroid.y;
      origin(2)=new_message->centroid.z;

      pos = origin+proj2plane_*v + normal*(v2.dot(n2));
      normal += normal*(v).dot(n2);
    }
  }

    /**
     * @brief Computes the trackball's rotation, using stored initial and final position vectors.
     */
    void computeRotationAngle (void)
    {
        //Given two position vectors, corresponding to the initial and final mouse coordinates, calculate the rotation of the sphere that will keep the mouse always in the initial position.

        if(initialPosition.norm() > 0) {
            initialPosition.normalize();
        }
        if(finalPosition.norm() > 0) {
            finalPosition.normalize();
        }

        //cout << "Initial Position: " << initialPosition.transpose() << " Final Position: " << finalPosition.transpose() << endl << endl;

        Eigen::Vector3f rotationAxis = initialPosition.cross(finalPosition);

        if(rotationAxis.norm() != 0) {
            rotationAxis.normalize();
        }

        float dot = initialPosition.dot(finalPosition);

        float rotationAngle = (dot <= 1) ? acos(dot) : 0;//If, by losing floating point precision, the dot product between the initial and final positions is bigger than one, ignore the rotation.

        Eigen::Quaternion<float> q (Eigen::AngleAxis<float>(rotationAngle,rotationAxis));

        quaternion = q * quaternion;
        quaternion.normalize();
    }

boost::optional<Intersection> Triangle::intersect(Ray ray) {
	Eigen::Vector3f s1 = ray.dir.cross(d2);
	const float div = s1.dot(d1);
	if(div <= 0) {  // parallel or back
		return boost::optional<Intersection>();
	}

	const float div_inv = 1 / div;
		
	Eigen::Vector3f s  = ray.org - p0;
	const float a = s.dot(s1) * div_inv;
	if(a < 0 || a > 1) {
		return boost::optional<Intersection>();
	}
				
	Eigen::Vector3f s2 = s.cross(d1);
	const float b = ray.dir.dot(s2) * div_inv;
		
	if(b < 0 || a + b > 1) {
		return boost::optional<Intersection>();
	}

	const float t = d2.dot(s2) * div_inv;
	if(t < 0) {
		return boost::optional<Intersection>();
	}

	return boost::optional<Intersection>(Intersection(
		t,
		ray.at(t),
		normal,
		(1 - a - b) * uv0 + a * uv1 + b * uv2,
		(1 - a - b) * ir0 + a * ir1 + b * ir2,
		attribute));
}

  bool SnapIt::checkPointInsidePlane(EigenVector3fVector &plane_points,
                                     Eigen::Vector3f normal,
                                     Eigen::Vector3f point)
  {
    if (isnan(point[0]) || isnan(point[1]) || isnan(point[2])) {
      return false;
    }
    for (size_t i = 0; i < plane_points.size(); i++) {
      Eigen::Vector3f B;
      Eigen::Vector3f O = plane_points[i];

      if (i == (plane_points.size() - 1)) {
        B = plane_points[0];
      }
      else {
        B = plane_points[i + 1];
      }
      Eigen::Vector3f OB = B - O;
      Eigen::Vector3f OP = point - O;
      if ((OB.cross(OP)).dot(normal) < 0) {
        return false;
      }
    }

    return true;
  }

bool targetViewpoint(const Eigen::Vector3f& rayo,const Eigen::Vector3f& target,const Eigen::Vector3f& down,
                     Eigen::Affine3f& transf)
{
  // uz: versor pointing toward the destination
  Eigen::Vector3f uz = target - rayo;
  if (std::abs(uz.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": target point on ray origin!" << std::endl;
    return false;
  }
  uz.normalize();
  //std::cout << "uz " << uz.transpose() << ", norm " << uz .norm() << std::endl;
  // ux: versor pointing toward the ground
  Eigen::Vector3f ux = down - down.dot(uz) * uz;  
  if (std::abs(ux.norm()) < 1e-3) {
    std::cout << __FILE__ << "," << __LINE__ << ": ray to target toward ground direction!" << std::endl;
    return false;
  }
  ux.normalize();
  //std::cout << "ux " << ux.transpose() << ", norm " << ux.norm() << std::endl;
  Eigen::Vector3f uy = uz.cross(ux);
  //std::cout << "uy " << uy.transpose() << ", norm " << uy.norm() << std::endl;
  Eigen::Matrix3f rot;
  rot << ux.x(), uy.x(), uz.x(),
         ux.y(), uy.y(), uz.y(),
         ux.z(), uy.z(), uz.z();
  transf.setIdentity();
  transf.translate(rayo);
  transf.rotate(rot);
  //std::cout << __FILE__ << "\nrotation\n" << rot << "\ntranslation\n" << rayo << "\naffine\n" << transf.matrix() << std::endl;
  return true;
}

bool
VirtualTrackball::mouseMoved ( const MouseEvent* event )
{
        if ( !event->isLeftButtonPressed() ) return false;
        const Eigen::Vector3f p0 = this->_oldp;
        const Eigen::Vector3f p1 = this->project_on_sphere ( event->x(), event->y() );
        this->_oldp = p1;
        if ( ( p0 - p1 ).norm() < this->_eps ) return false; // do nothing
		//何か間違ってそうなので訂正してみる
        float radian = std::acos( p0.dot ( p1 ) )*0.5;
        const float cost = std::cos(radian);
        const float sint = std::sin(radian);
        //const float cost = p0.dot ( p1 );
        //const float sint = std::sqrt ( 1 - cost * cost );
        const Eigen::Vector3f axis = p0.cross ( p1 ).normalized();
        const Eigen::Quaternionf q ( -cost, sint * axis.x(), sint * axis.y(), sint * axis.z() );
        if( ( q.x()!=q.x() )|| ( q.y()!=q.y() )|| ( q.z()!=q.z() )|| ( q.w()!=q.w() ) ) return false;

        /*
        Eigen::Vector3f bmin , bmax;
        Mesh mesh;
        mesh = this->_model.getMesh();
        mesh.getBoundingBox(bmin,bmax);
        Eigen::Vector3f mc = (bmin + bmax)*0.5;
        Eigen::Vector3f c = Eigen::Matrix3f(q) * ( this->_model.getCamera().getCenter() - mc ) + mc ;
        this->_model.setCameraPosition(c.x(),c.y(),c.z());*/

        //this->_model.addRotation ( q );
        return true;
}

template <typename PointSource, typename PointTarget, typename NormalT, typename Scalar> int
pcl::registration::FPCSInitialAlignment <PointSource, PointTarget, NormalT, Scalar>::selectBaseTriangle (std::vector <int> &base_indices)
{
  int nr_points = static_cast <int> (target_indices_->size ());
  float best_t = 0.f;

  // choose random first point
  base_indices[0] = (*target_indices_)[rand () % nr_points];
  int *index1 = &base_indices[0];

  // random search for 2 other points (as far away as overlap allows)
  for (int i = 0; i < ransac_iterations_; i++)
  {
    int *index2 = &(*target_indices_)[rand () % nr_points];
    int *index3 = &(*target_indices_)[rand () % nr_points];

    Eigen::Vector3f u = target_->points[*index2].getVector3fMap () - target_->points[*index1].getVector3fMap ();
    Eigen::Vector3f v = target_->points[*index3].getVector3fMap () - target_->points[*index1].getVector3fMap ();
    float t = u.cross (v).squaredNorm (); // triangle area (0.5 * sqrt(t)) should be maximal

    // check for most suitable point triple
    if (t > best_t && u.squaredNorm () < max_base_diameter_sqr_ && v.squaredNorm () < max_base_diameter_sqr_)
    {
      best_t = t;
      base_indices[1] = *index2;
      base_indices[2] = *index3;
    }
  }

  // return if a triplet could be selected
  return (best_t == 0.f ? -1 : 0);
}

Transform<float, 3, Affine, AutoAlign> Plane::get2DArbitraryRefSystem() const
{
    Eigen::Vector3f n = normal_.getUnitNormal();

    /// seek for a good unit axis to project on plane
    /// and then use it as X direction of the 2d local system
    size_t min_id;
    n.array().abs().minCoeff(&min_id);

    DLOG(INFO) << "min id is " << min_id;
    Vector3f proj_axis = Vector3f::Zero();
    proj_axis(min_id) = 1; // unity on that axis

    // project the selected axis on the plane
    Vector3f second_ax = projectVectorOnPlane(proj_axis);
    second_ax.normalize();

    Vector3f first_ax = n.cross(second_ax);

    first_ax.normalize();

    Transform<float, 3, Affine, AutoAlign> T;
//    T.matrix().fill(0);
    T.matrix().col(0).head(3) = first_ax;
    T.matrix().col(1).head(3) = second_ax;
    T.matrix().col(2).head(3) = n;
//    T.matrix()(3, 3) = 1;


    DLOG(INFO) << "Transform computed \n " << T.inverse().matrix() << "normal was " << n;

    DLOG(INFO) << "In fact T*n " <<T.inverse() *n;
    return T.inverse();
}

Eigen::Matrix4f ForwardKinematicsLiego::getEpsilon(Eigen::Vector3f omega,
		Eigen::Vector3f q) {
	Eigen::Matrix4f eps = getDash(omega);
	omega = -omega.cross(q);
	eps(0, 3) = omega[0];
	eps(1, 3) = omega[1];
	eps(2, 3) = omega[2];
	return eps;
}

void compute_plane(Eigen::Vector4f& plane, const pcl::PointCloud<pcl::PointXYZ>& points, int* inds)
{
    Eigen::Vector3f first = points[inds[1]].getVector3fMap() - points[inds[0]].getVector3fMap();
    Eigen::Vector3f second = points[inds[2]].getVector3fMap() - points[inds[0]].getVector3fMap();
    Eigen::Vector3f normal = first.cross(second);
    normal.normalize();
    plane.segment<3>(0) = normal;
    plane(3) = -normal.dot(points[inds[0]].getVector3fMap());
}