C++ Vector3d::y方法代码示例

bool LinearAlgebra::isLinearIndependent(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
	bool result = false;
	int rank;

	/* TODO: test if the vectors a and b are linear independent.
	   Return true if they are independent, false if they are dependent.*/
	Eigen::MatrixXd A(2,3);
	A << 	a.x(), a.y(), a.z(),
			b.x(), b.y(), b.z();
	//std::cout<<"A:"<<A<<endl;
	Eigen::FullPivLU<Eigen::MatrixXd> luA(A);
	rank = luA.rank();

	if (rank == 2)
		result = true;
	else
		result = false;

	return result;
}

//! Compute gravitational acceleration due to J2.
Eigen::Vector3d computeGravitationalAccelerationDueToJ2(
        const Eigen::Vector3d& positionOfBodySubjectToAcceleration,
        const double gravitationalParameterOfBodyExertingAcceleration,
        const double equatorialRadiusOfBodyExertingAcceleration,
        const double j2CoefficientOfGravityField,
        const Eigen::Vector3d& positionOfBodyExertingAcceleration )
{
    // Set constant values reused for optimal computation of acceleration components.
    const double distanceBetweenBodies = ( positionOfBodySubjectToAcceleration
                                           - positionOfBodyExertingAcceleration ).norm( );

    const double preMultiplier = -gravitationalParameterOfBodyExertingAcceleration
            / std::pow( distanceBetweenBodies, 4.0 ) * 1.5 * j2CoefficientOfGravityField
            * equatorialRadiusOfBodyExertingAcceleration
            * equatorialRadiusOfBodyExertingAcceleration;

    const double scaledZCoordinate = ( positionOfBodySubjectToAcceleration.z( )
                                       - positionOfBodyExertingAcceleration.z( ) )
            / distanceBetweenBodies;

    const double scaledZCoordinateSquared = scaledZCoordinate * scaledZCoordinate;

    const double factorForXAndYDirections = ( 1.0 - 5.0 * scaledZCoordinateSquared )
            / distanceBetweenBodies;

    // Compute components of acceleration due to J2-effect.
    Eigen::Vector3d gravitationalAccelerationDueToJ2 = Eigen::Vector3d::Constant( preMultiplier );

    gravitationalAccelerationDueToJ2( basic_astrodynamics::xCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.x( )
                 - positionOfBodyExertingAcceleration.x( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::yCartesianPositionIndex )
            *= ( positionOfBodySubjectToAcceleration.y( )
                 - positionOfBodyExertingAcceleration.y( ) ) * factorForXAndYDirections;

    gravitationalAccelerationDueToJ2( basic_astrodynamics::zCartesianPositionIndex )
            *= ( 3.0 - 5.0 * scaledZCoordinateSquared ) * scaledZCoordinate;

    return gravitationalAccelerationDueToJ2;
}

void draw()
{
    glBegin(GL_LINES);
    for (EdgeCIter e = mesh.edges.begin(); e != mesh.edges.end(); e++) {
        
        int s = 3;
        Eigen::Vector3d v = e->he->vertex->position;
        Eigen::Vector3d u = e->he->flip->vertex->position - v;
        u.normalize();
        double dl = e->length() / (double)s;
        
        double c = 0.0;
        double c2 = 0.0;
        if (curvature == 0) {
            c = e->he->vertex->gaussCurvature;
            c2 = e->he->flip->vertex->gaussCurvature;
            
        } else if (curvature == 1) {
            c = e->he->vertex->meanCurvature;
            c2 = e->he->flip->vertex->meanCurvature;
        
        } else {
            c = normalCurvatures[e->he->vertex->index];
            c2 = normalCurvatures[e->he->flip->vertex->index];
        }
        double dc = (c2 - c) / (double)s;
        
        for (int i = 0; i < s; i++) {
            if (c < 0) glColor4f(0.0, 0.0, fabs(c), 0.6);
            else glColor4f(c, 0.0, 0.0, 0.6);
            glVertex3d(v.x(), v.y(), v.z());
            
            c += dc;
            if (c < 0) glColor4f(0.0, 0.0, fabs(c), 0.6);
            else glColor4f(c, 0.0, 0.0, 0.6);
            v += u * dl;
            glVertex3d(v.x(), v.y(), v.z());
        }
    }
    glEnd();
}

//! Calculate the Cartesian position from geodetic coordinates.
Eigen::Vector3d convertGeodeticToCartesianCoordinates( const Eigen::Vector3d geodeticCoordinates,
                                                       const double equatorialRadius,
                                                       const double flattening )
{
    // Pre-compute values for efficiency.
    const double sineLatitude = std::sin( geodeticCoordinates.y( ) );
    const double centerOffset = equatorialRadius /
            std::sqrt( 1.0 - flattening * ( 2.0 - flattening ) * sineLatitude * sineLatitude );

    // Calculate Cartesian coordinates, Montenbruck and Gill (2000), Eq. (5.82).
    Eigen::Vector3d cartesianCoordinates = Eigen::Vector3d::Zero( );
    cartesianCoordinates.x( ) =
            ( centerOffset + geodeticCoordinates.x( ) ) *
            std::cos( geodeticCoordinates.y( ) ) * std::cos( geodeticCoordinates.z( ) );
    cartesianCoordinates.y( ) = cartesianCoordinates.x( ) * std::tan( geodeticCoordinates.z( ) );
    cartesianCoordinates.z( ) = ( ( 1.0 - flattening ) * ( 1.0 - flattening ) * centerOffset +
                                  geodeticCoordinates.x( ) ) * sineLatitude;

    // Return Cartesian coordinates.
    return cartesianCoordinates;
}

Eigen::Vector4d Face::plane() const
{
    Eigen::Vector3d a = he->vertex->position;

    Eigen::Vector3d n = normal();
    n.normalize();

    Eigen::Vector4d p;
    p << n.x(), n.y(), n.z(), -n.dot(a);

    return p;
}

Hole HoleFinderPrivate::reduceHoles(const QVector<Hole *> &holeVec)
{
  // Average the positions, weighted by volume
  Eigen::Vector3d center (0.0, 0.0, 0.0);
  double denom = 0.0;
  foreach (Hole *hole, holeVec) {
    const double volume = hole->volume();
    center += hole->center * volume;
    denom += volume;
  }
  center /= denom;

  // Wrap center into the unit cell
  this->cartToFrac(&center);
  if ((center.x() = fmod(center.x(), 1.0)) < 0) ++center.x();
  if ((center.y() = fmod(center.y(), 1.0)) < 0) ++center.y();
  if ((center.z() = fmod(center.z(), 1.0)) < 0) ++center.z();
  this->fracToCart(&center);

  return Hole(center, 0.0); // radius is set during resizeHoles();
}

void OrientationCell::rotateSurf(Eigen::Matrix3d& rotation, Eigen::Vector3d& center, PlanCloud& planCloud)
{
	rotate(rotation, planCloud.T());
	rotate(rotation, planCloud.B());
	rotate(rotation, planCloud.N());
	*(planCloud.frame()) << planCloud.T(), planCloud.B(), planCloud.N();

	Eigen::Vector3d OgOi(planCloud.origin()->x() - center.x(),
						 planCloud.origin()->y() - center.y(),
						 planCloud.origin()->z() - center.z());
	OgOi = rotation*OgOi;
	*(planCloud.origin()) << OgOi.x(), OgOi.y(), OgOi.z();
}

bool addConeToCollisionModel(const std::string &armName, double length, double radius)
{
    ros::NodeHandle nh("~") ;

    ros::service::waitForService("/environment_server/set_planning_scene_diff");
    ros::ServiceClient get_planning_scene_client =
      nh.serviceClient<arm_navigation_msgs::GetPlanningScene>("/environment_server/set_planning_scene_diff");

    arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
    arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;

    arm_navigation_msgs::AttachedCollisionObject att_object;
    att_object.link_name = armName + "_gripper";

    att_object.object.id = "attached_cone";
    att_object.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;

    att_object.object.header.frame_id = "base_link";
    att_object.object.header.stamp = ros::Time::now();

    Eigen::Vector3d p = robot_helpers::getPose(armName).inverse().translation() ;


    arm_navigation_msgs::Shape object;

    shapes::Mesh mesh ;
    makeSolidCone(mesh, radius, length, 10, 20) ;

    if(!planning_environment::constructObjectMsg(&mesh, object)) {
      ROS_WARN_STREAM("Object construction fails");
    }

    geometry_msgs::Pose pose;
    pose.position.x = p.x();
    pose.position.y = p.y();
    pose.position.z = p.z() ;
    pose.orientation.x = 0;
    pose.orientation.y = 0;
    pose.orientation.z = 0;
    pose.orientation.w = 1;

    att_object.object.shapes.push_back(object);
    att_object.object.poses.push_back(pose);

    planning_scene_req.planning_scene_diff.attached_collision_objects.push_back(att_object);

    if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) return false;


    return true ;
}

//==============================================================================
void OdeMesh::fillArrays(const aiScene* scene, const Eigen::Vector3d& scale)
{
  mVertices.clear();
  mNormals.clear();
  mIndices.clear();

  // Cound the total numbers of vertices and indices.
  auto mNumVertices = 0u;
  auto mNumIndices = 0u;
  for (auto i = 0u; i < scene->mNumMeshes; ++i)
  {
    const auto mesh = scene->mMeshes[i];

    mNumVertices += mesh->mNumVertices;
    mNumIndices += mesh->mNumFaces;
  }
  mNumVertices *= 3u;
  mNumIndices *= 3u;
  // The number of indices of each face is always 3 because we use the assimp
  // option `aiProcess_Triangulate` when loading meshes.

  mVertices.resize(mNumVertices);
  mNormals.resize(mNumVertices);
  mIndices.resize(mNumIndices);

  auto vertexIndex = 0u;
  auto indexIndex = 0u;
  for (auto i = 0u; i < scene->mNumMeshes; ++i)
  {
    const auto mesh = scene->mMeshes[i];

    for (auto j = 0u; j < mesh->mNumVertices; ++j)
    {
      mVertices[vertexIndex] = mesh->mVertices[j].x * scale.x();
      mNormals[vertexIndex++] = mesh->mNormals[j].x;

      mVertices[vertexIndex] = mesh->mVertices[j].y * scale.y();
      mNormals[vertexIndex++] = mesh->mNormals[j].y;

      mVertices[vertexIndex] = mesh->mVertices[j].z * scale.z();
      mNormals[vertexIndex++] = mesh->mNormals[j].z;
    }

    for (auto j = 0u; j < mesh->mNumFaces; ++j)
    {
      mIndices[indexIndex++] = mesh->mFaces[j].mIndices[0];
      mIndices[indexIndex++] = mesh->mFaces[j].mIndices[1];
      mIndices[indexIndex++] = mesh->mFaces[j].mIndices[2];
    }
  }
}

int main(int argc, char** argv) {
	Eigen::Matrix3d m;
	m<<3, 1, 2, 
           1, 3, 7,
	   2, 7, 4;

	Eigen::EigenSolver<Eigen::Matrix3d> solver( m );
	for( int i = 0 ; i < 3 ; ++i ) {
		const double ev = solver.eigenvalues()( i,0 ).real(); //固有値
		const Eigen::Vector3d v =  solver.eigenvectors().col(i).real(); //固有ベクトル
		std::cerr<<i<<" : "<<ev<<" ( "<<v.x()<<", "<<v.y()<<", "<<v.z()<<")"<<std::endl;
	}
	return 0;
}

static void subject_publish_callback(const ViconDriver::Subject &subject)
{
  if(!running)
    return;

  static std::map<std::string, ros::Publisher> vicon_publishers;
  std::map<std::string, ros::Publisher>::iterator it;

  it = vicon_publishers.find(subject.name);
  if(it == vicon_publishers.end())
  {
    ros::Publisher pub = nh->advertise<vicon::Subject>(subject.name, 10);
    it = vicon_publishers.insert(std::make_pair(subject.name, pub)).first;
  }

  if(loadCalib(subject.name))
  {
    Eigen::Affine3d current_pose = Eigen::Affine3d::Identity();
    current_pose.translate(Eigen::Vector3d(subject.translation));
    current_pose.rotate(Eigen::Quaterniond(subject.rotation));
    current_pose = current_pose * calib_pose[subject.name];
    const Eigen::Vector3d position(current_pose.translation());
    const Eigen::Quaterniond rotation(current_pose.rotation());

    vicon::Subject subject_ros;
    subject_ros.header.seq = subject.frame_number;
    subject_ros.header.stamp = ros::Time(subject.time_usec / 1e6);
    subject_ros.header.frame_id = "/vicon";
    subject_ros.name = subject.name;
    subject_ros.occluded = subject.occluded;
    subject_ros.position.x = position.x();
    subject_ros.position.y = position.y();
    subject_ros.position.z = position.z();
    subject_ros.orientation.x = rotation.x();
    subject_ros.orientation.y = rotation.y();
    subject_ros.orientation.z = rotation.z();
    subject_ros.orientation.w = rotation.w();
    subject_ros.markers.resize(subject.markers.size());
    for(size_t i = 0; i < subject_ros.markers.size(); i++)
    {
      subject_ros.markers[i].name = subject.markers[i].name;
      subject_ros.markers[i].subject_name = subject.markers[i].subject_name;
      subject_ros.markers[i].position.x = subject.markers[i].translation[0];
      subject_ros.markers[i].position.y = subject.markers[i].translation[1];
      subject_ros.markers[i].position.z = subject.markers[i].translation[2];
      subject_ros.markers[i].occluded = subject.markers[i].occluded;
    }
    it->second.publish(subject_ros);
  }
}

vector<cv::Point2f> FeatureTracker::undistortedPoints()
{
    vector<cv::Point2f> un_pts;
    //cv::undistortPoints(cur_pts, un_pts, K, cv::Mat());
    for (unsigned int i = 0; i < cur_pts.size(); i++)
    {
        Eigen::Vector2d a(cur_pts[i].x, cur_pts[i].y);
        Eigen::Vector3d b;
        m_camera->liftProjective(a, b);
        un_pts.push_back(cv::Point2f(b.x() / b.z(), b.y() / b.z()));
    }

    return un_pts;
}

  inline void BasisSet::pointD5(BasisSet *set, const Eigen::Vector3d &delta,
                                const double &dr2, int basis,
                                Eigen::MatrixXd &out)
  {
    // D type orbitals have six components and each component has a different
    // independent MO weighting. Many things can be cached to save time though
    double d0 = 0.0, d1p = 0.0, d1n = 0.0, d2p = 0.0, d2n = 0.0;

    // Now iterate through the D type GTOs and sum their contributions
    unsigned int cIndex = set->m_cIndices[basis];
    for (unsigned int i = set->m_gtoIndices[basis];
         i < set->m_gtoIndices[basis+1]; ++i) {
      // Calculate the common factor
      double tmpGTO = exp(-set->m_gtoA[i] * dr2);
      d0  += set->m_gtoCN[cIndex++] * tmpGTO;
      d1p += set->m_gtoCN[cIndex++] * tmpGTO;
      d1n += set->m_gtoCN[cIndex++] * tmpGTO;
      d2p += set->m_gtoCN[cIndex++] * tmpGTO;
      d2n += set->m_gtoCN[cIndex++] * tmpGTO;
    }

    // Calculate the prefactors
    double xx = delta.x() * delta.x();
    double yy = delta.y() * delta.y();
    double zz = delta.z() * delta.z();
    double xy = delta.x() * delta.y();
    double xz = delta.x() * delta.z();
    double yz = delta.y() * delta.z();

    // Save values to the matrix
    int baseIndex = set->m_moIndices[basis];
    out.coeffRef(baseIndex  , 0) = (zz - dr2) * d0;
    out.coeffRef(baseIndex+1, 0) = xz * d1p;
    out.coeffRef(baseIndex+2, 0) = yz * d1n;
    out.coeffRef(baseIndex+3, 0) = (xx - yy) * d2p;
    out.coeffRef(baseIndex+4, 0) = xy * d2n;
  }

bool VectorVisualization::visualize(const grid_map::GridMap& map)
{
  if (!isActive()) return true;

  for (const auto& type : types_) {
    if (!map.exists(type)) {
      ROS_WARN_STREAM(
          "VectorVisualization::visualize: No grid map layer with name '" << type << "' found.");
      return false;
    }
  }

  // Set marker info.
  marker_.header.frame_id = map.getFrameId();
  marker_.header.stamp.fromNSec(map.getTimestamp());

  // Clear points.
  marker_.points.clear();
  marker_.colors.clear();

  for (grid_map::GridMapIterator iterator(map); !iterator.isPastEnd(); ++iterator)
  {
    if (!map.isValid(*iterator, positionLayer_) || !map.isValid(*iterator, types_)) continue;
    geometry_msgs::Vector3 vector;
    vector.x = map.at(types_[0], *iterator); // FIXME(cfo): segfaults when types is not set
    vector.y = map.at(types_[1], *iterator);
    vector.z = map.at(types_[2], *iterator);

    Eigen::Vector3d position;
    map.getPosition3(positionLayer_, *iterator, position);
    geometry_msgs::Point startPoint;
    startPoint.x = position.x();
    startPoint.y = position.y();
    startPoint.z = position.z();
    marker_.points.push_back(startPoint);

    geometry_msgs::Point endPoint;
    endPoint.x = startPoint.x + scale_ * vector.x;
    endPoint.y = startPoint.y + scale_ * vector.y;
    endPoint.z = startPoint.z + scale_ * vector.z;
    marker_.points.push_back(endPoint);

    marker_.colors.push_back(color_); // Each vertex needs a color.
    marker_.colors.push_back(color_);
  }

  publisher_.publish(marker_);
  return true;
}

bool Reader::updatePoint( const Eigen::Vector3d& offset )
{
    if( !m_point ) { return false; } // is it safe to do it without locking the mutex?
    boost::mutex::scoped_lock lock( m_mutex );
    if( !updated_ ) { return false; }
    m_translation = QVector3D( m_point->x(), m_point->y(), m_point->z() );
    m_offset = QVector3D( offset.x(), offset.y(), offset.z() );
    if( m_orientation )
    {
        const Eigen::Quaterniond& q = snark::rotation_matrix( *m_orientation ).quaternion();
        m_quaternion = QQuaternion( q.w(), q.x(), q.y(), q.z() );
    }
    updated_ = false;
    return true;
}