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

    void paramsCallback(navigation::ForceFieldConfig &config, uint32_t level)
    {
        ROS_DEBUG_STREAM("Force field reconfigure Request ->" << " kp_x:" << config.kp_x
                         << " kp_y:" << config.kp_y
                         << " kp_z:" << config.kp_z
                         << " kd_x:" << config.kd_x
                         << " kd_y:" << config.kd_y
                         << " kd_z:" << config.kd_z
                         << " ro:"   << config.ro);

        kp << config.kp_x,
                config.kp_y,
                config.kp_z;

        kd << config.kd_x,
                config.kd_y,
                config.kd_z;

        kp_mat << kp.x(), 0, 0,
                0, kp.y(), 0,
                0, 0, kp.z();
        kd_mat << kd.x(), 0, 0,
                0, kd.y(), 0,
                0, 0, kd.z();

        ro = config.ro;

        laser_max_distance = config.laser_max_distance;
        //slave_to_master_scale=Eigen::Matrix<double,3,1> (fabs(config.master_workspace_size.x/config.slave_workspace_size.x), fabs(config.master_workspace_size.y/config.slave_workspace_size.y), fabs(config.master_workspace_size.z/config.slave_workspace_size.z));
    }

	/**
	 * @brief Send a safety zone (volume), which is defined by two corners of a cube,
	 * to the FCU.
	 *
	 * @note ENU frame.
	 */
	void send_safety_set_allowed_area(Eigen::Vector3d p1, Eigen::Vector3d p2)
	{
		ROS_INFO_STREAM_NAMED("safetyarea", "SA: Set safty area: P1 " << p1 << " P2 " << p2);

		p1 = ftf::transform_frame_enu_ned(p1);
		p2 = ftf::transform_frame_enu_ned(p2);

		mavlink::common::msg::SAFETY_SET_ALLOWED_AREA s;
		m_uas->msg_set_target(s);

		// TODO: use enum from lib
		s.frame = utils::enum_value(mavlink::common::MAV_FRAME::LOCAL_NED);

		// [[[cog:
		// for p in range(1, 3):
		//     for v in ('x', 'y', 'z'):
		//         cog.outl("s.p%d%s = p%d.%s();" % (p, v, p, v))
		// ]]]
		s.p1x = p1.x();
		s.p1y = p1.y();
		s.p1z = p1.z();
		s.p2x = p2.x();
		s.p2y = p2.y();
		s.p2z = p2.z();
		// [[[end]]] (checksum: c996a362f338fcc6b714c8be583c3be0)

		UAS_FCU(m_uas)->send_message_ignore_drop(s);
	}

BoundingBox Face::boundingBox() const
{
    if (isBoundary()) {
        return BoundingBox(he->vertex->position, he->next->vertex->position);
    }
    
    Eigen::Vector3d p1 = he->vertex->position;
    Eigen::Vector3d p2 = he->next->vertex->position;
    Eigen::Vector3d p3 = he->next->next->vertex->position;
    
    Eigen::Vector3d min = p1;
    Eigen::Vector3d max = p1;
    
    if (p2.x() < min.x()) min.x() = p2.x();
    if (p3.x() < min.x()) min.x() = p3.x();
    
    if (p2.y() < min.y()) min.y() = p2.y();
    if (p3.y() < min.y()) min.y() = p3.y();
    
    if (p2.z() < min.z()) min.z() = p2.z();
    if (p3.z() < min.z()) min.z() = p3.z();
    
    if (p2.x() > max.x()) max.x() = p2.x();
    if (p3.x() > max.x()) max.x() = p3.x();
    
    if (p2.y() > max.y()) max.y() = p2.y();
    if (p3.y() > max.y()) max.y() = p3.y();
    
    if (p2.z() > max.z()) max.z() = p2.z();
    if (p3.z() > max.z()) max.z() = p3.z();
    
    return BoundingBox(min, max);
}

void GlobalOptimization::inputOdom(double t, Eigen::Vector3d OdomP, Eigen::Quaterniond OdomQ)
{
	mPoseMap.lock();
    vector<double> localPose{OdomP.x(), OdomP.y(), OdomP.z(), 
    					     OdomQ.w(), OdomQ.x(), OdomQ.y(), OdomQ.z()};
    localPoseMap[t] = localPose;


    Eigen::Quaterniond globalQ;
    globalQ = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomQ;
    Eigen::Vector3d globalP = WGPS_T_WVIO.block<3, 3>(0, 0) * OdomP + WGPS_T_WVIO.block<3, 1>(0, 3);
    vector<double> globalPose{globalP.x(), globalP.y(), globalP.z(),
                              globalQ.w(), globalQ.x(), globalQ.y(), globalQ.z()};
    globalPoseMap[t] = globalPose;
    lastP = globalP;
    lastQ = globalQ;

    geometry_msgs::PoseStamped pose_stamped;
    pose_stamped.header.stamp = ros::Time(t);
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose.position.x = lastP.x();
    pose_stamped.pose.position.y = lastP.y();
    pose_stamped.pose.position.z = lastP.z();
    pose_stamped.pose.orientation.x = lastQ.x();
    pose_stamped.pose.orientation.y = lastQ.y();
    pose_stamped.pose.orientation.z = lastQ.z();
    pose_stamped.pose.orientation.w = lastQ.w();
    global_path.header = pose_stamped.header;
    global_path.poses.push_back(pose_stamped);

    mPoseMap.unlock();
}

void FeatureTracker::showUndistortion(const string &name)
{
    cv::Mat undistortedImg(ROW + 600, COL + 600, CV_8UC1, cv::Scalar(0));
    vector<Eigen::Vector2d> distortedp, undistortedp;
    for (int i = 0; i < COL; i++)
        for (int j = 0; j < ROW; j++)
        {
            Eigen::Vector2d a(i, j);
            Eigen::Vector3d b;
            m_camera->liftProjective(a, b);
            distortedp.push_back(a);
            undistortedp.push_back(Eigen::Vector2d(b.x() / b.z(), b.y() / b.z()));
            //printf("%f,%f->%f,%f,%f\n)\n", a.x(), a.y(), b.x(), b.y(), b.z());
        }
    for (int i = 0; i < int(undistortedp.size()); i++)
    {
        cv::Mat pp(3, 1, CV_32FC1);
        pp.at<float>(0, 0) = undistortedp[i].x() * FOCAL_LENGTH + COL / 2;
        pp.at<float>(1, 0) = undistortedp[i].y() * FOCAL_LENGTH + ROW / 2;
        pp.at<float>(2, 0) = 1.0;
        //cout << trackerData[0].K << endl;
        //printf("%lf %lf\n", p.at<float>(1, 0), p.at<float>(0, 0));
        //printf("%lf %lf\n", pp.at<float>(1, 0), pp.at<float>(0, 0));
        if (pp.at<float>(1, 0) + 300 >= 0 && pp.at<float>(1, 0) + 300 < ROW + 600 && pp.at<float>(0, 0) + 300 >= 0 && pp.at<float>(0, 0) + 300 < COL + 600)
        {
            undistortedImg.at<uchar>(pp.at<float>(1, 0) + 300, pp.at<float>(0, 0) + 300) = cur_img.at<uchar>(distortedp[i].y(), distortedp[i].x());
        }
        else
        {
            //ROS_ERROR("(%f %f) -> (%f %f)", distortedp[i].y, distortedp[i].x, pp.at<float>(1, 0), pp.at<float>(0, 0));
        }
    }
    cv::imshow(name, undistortedImg);
    cv::waitKey(0);
}

Eigen::Vector3d PIDController::compute_linvel_effort(Eigen::Vector3d goal, Eigen::Vector3d current, ros::Time last_time){
	double lin_vel_x = pid_linvel_x.computeCommand(goal.x() - current.x(), ros::Time::now() - last_time);
	double lin_vel_y = pid_linvel_y.computeCommand(goal.y() - current.y(), ros::Time::now() - last_time);
	double lin_vel_z = pid_linvel_z.computeCommand(goal.z() - current.z(), ros::Time::now() - last_time);

	return Eigen::Vector3d(lin_vel_x, lin_vel_y, lin_vel_z);
}

  bool RobotLaser::write(std::ostream& os) const
  {
    os << _laserParams.type << " " << _laserParams.firstBeamAngle << " " << _laserParams.fov << " "
      << _laserParams.angularStep << " " << _laserParams.maxRange << " " << _laserParams.accuracy << " "
      << _laserParams.remissionMode << " ";
    os << ranges().size();
    for (size_t i = 0; i < ranges().size(); ++i)
      os << " " << ranges()[i];
    os << " " << _remissions.size();
    for (size_t i = 0; i < _remissions.size(); ++i)
      os << " " << _remissions[i];

    // odometry pose
    Eigen::Vector3d p = (_odomPose * _laserParams.laserPose).toVector();
    os << " " << p.x() << " " << p.y() << " " << p.z();
    p = _odomPose.toVector();
    os << " " << p.x() << " " << p.y() << " " << p.z();

    // crap values
    os << FIXED(" " <<  _laserTv << " " <<  _laserRv << " " << _forwardSafetyDist << " "
        << _sideSaftyDist << " " << _turnAxis);
    os << FIXED(" " << timestamp() << " " << hostname() << " " << loggerTimestamp());

    return os.good();
  }

//! Calculate geodetic coordinates ( altitude, geodetic latitude, longitude ) of a position vector.
Eigen::Vector3d convertCartesianToGeodeticCoordinates( const Eigen::Vector3d cartesianCoordinates,
                                                       const double equatorialRadius,
                                                       const double flattening,
                                                       const double tolerance )
{
    using coordinate_conversions::convertCartesianToSpherical;

    // Determine spherical coordinates of position vector.
    const Eigen::Vector3d sphericalCoordinates
            = convertCartesianToSpherical( cartesianCoordinates );
    Eigen::Vector3d geodeticCoordinates = Eigen::Vector3d::Zero( );

    // Calculate auxiliary variables of geodetic coordinates.
    std::pair< double, double > auxiliaryVariables =
            calculateGeodeticCoordinatesAuxiliaryQuantities(
                cartesianCoordinates, equatorialRadius,
                calculateEllipticity( flattening ), tolerance );

    // Calculate altitude.
    geodeticCoordinates.x( ) = calculateAltitudeOverOblateSpheroid(
                cartesianCoordinates, auxiliaryVariables.second, auxiliaryVariables.first );

    // Calculate geodetic latitude.
    geodeticCoordinates.y( ) = calculateGeodeticLatitude(
                cartesianCoordinates, auxiliaryVariables.second );

    // Set longitude.
    geodeticCoordinates.z( ) = sphericalCoordinates.z( );
    return geodeticCoordinates;
}

  inline void BasisSet::pointD(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 xx = 0.0, yy = 0.0, zz = 0.0, xy = 0.0, xz = 0.0, yz = 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);
      xx += set->m_gtoCN[cIndex++] * tmpGTO; // Dxx
      yy += set->m_gtoCN[cIndex++] * tmpGTO; // Dyy
      zz += set->m_gtoCN[cIndex++] * tmpGTO; // Dzz
      xy += set->m_gtoCN[cIndex++] * tmpGTO; // Dxy
      xz += set->m_gtoCN[cIndex++] * tmpGTO; // Dxz
      yz += set->m_gtoCN[cIndex++] * tmpGTO; // Dyz
    }

    // Save values to the matrix
    int baseIndex = set->m_moIndices[basis];
    out.coeffRef(baseIndex  , 0) = delta.x() * delta.x() * xx;
    out.coeffRef(baseIndex+1, 0) = delta.y() * delta.y() * yy;
    out.coeffRef(baseIndex+2, 0) = delta.z() * delta.z() * zz;
    out.coeffRef(baseIndex+3, 0) = delta.x() * delta.y() * xy;
    out.coeffRef(baseIndex+4, 0) = delta.x() * delta.z() * xz;
    out.coeffRef(baseIndex+5, 0) = delta.y() * delta.z() * yz;
  }

void draw()
{
    glColor4f(0.0, 0.0, 1.0, 0.5);
    glLineWidth(1.0);
    glBegin(GL_LINES);
    for (EdgeCIter e = mesh.edges.begin(); e != mesh.edges.end(); e ++) {
        Eigen::Vector3d a = e->he->vertex->position;
        Eigen::Vector3d b = e->he->flip->vertex->position;
            
        glVertex3d(a.x(), a.y(), a.z());
        glVertex3d(b.x(), b.y(), b.z());
    }
    
    glEnd();
    
    for (VertexIter v = mesh.vertices.begin(); v != mesh.vertices.end(); v++) {
        if (v->handle) {
            glColor4f(0.0, 1.0, 0.0, 0.5);
            glPointSize(4.0);
            glBegin(GL_POINTS);
            glVertex3d(v->position.x(), v->position.y(), v->position.z());
            glEnd();
        }
        
        if (v->anchor) {
            glColor4f(1.0, 0.0, 0.0, 0.5);
            glPointSize(2.0);
            glBegin(GL_POINTS);
            glVertex3d(v->position.x(), v->position.y(), v->position.z());
            glEnd();
        }
    }
}

void mesh_core::Plane::leastSquaresGeneral(
      const EigenSTL::vector_Vector3d& points,
      Eigen::Vector3d* average)
{
  if (points.empty())
  {
    normal_ = Eigen::Vector3d(0,0,1);
    d_ = 0;
    if (average)
      *average = Eigen::Vector3d::Zero();
    return;
  }

  // find c, the average of the points
  Eigen::Vector3d c;
  c.setZero();

  EigenSTL::vector_Vector3d::const_iterator p = points.begin();
  EigenSTL::vector_Vector3d::const_iterator end = points.end();
  for ( ; p != end ; ++p)
    c += *p;

  c *= 1.0/double(points.size());

  // Find the matrix
  Eigen::Matrix3d m;
  m.setZero();

  p = points.begin();
  for ( ; p != end ; ++p)
  {
    Eigen::Vector3d cp = *p - c;
    m(0,0) += cp.x() * cp.x();
    m(1,0) += cp.x() * cp.y();
    m(2,0) += cp.x() * cp.z();
    m(1,1) += cp.y() * cp.y();
    m(2,1) += cp.y() * cp.z();
    m(2,2) += cp.z() * cp.z();
  }
  m(0,1) = m(1,0);
  m(0,2) = m(2,0);
  m(1,2) = m(2,1);

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(m);
  if (eigensolver.info() == Eigen::Success)
  {
    normal_ = eigensolver.eigenvectors().col(0);
    normal_.normalize();
  }
  else
  {
    normal_ = Eigen::Vector3d(0,0,1);
  }

  d_ = -c.dot(normal_);

  if (average)
    *average = c;
}

/**
* \ingroup Communication
* Move the Object at absolute position given a Optotrak coordinate system (calibration) synchronously (blocking call)
*
* \param point Final absolute point to reach
* \param calibration Current calibration of Optotrak coordinate system
* \param objectspeed Object speed
**/
void moveObjectAbsolute(const Eigen::Vector3d &point, const Eigen::Vector3d &calibration, int objectspeed)
{ 

	int nstepsY = -(point.y()-calibration.y())/BROWN_MOTORSTEPSIZE;
	int nstepsZ =  (point.z()-calibration.z())/BROWN_MOTORSTEPSIZE;

	cerr << "Object is brought to " << point.y() << " Y and " << point.z() << " Z" << endl;
}

double errorOfLineInPlane(Segment_3 &l, Eigen::Vector3d& point, Eigen::Vector3d& normal)
{
	Plane_3 plane( Point_3(point.x(), point.y(), point.z()), 
				  Vector_3(normal.x(), normal.y(), normal.z()) );		
	double dist1 = squared_distance(plane, l.point(0));
	double dist2 = squared_distance(plane, l.point(1));
	return 0.5*(sqrt(dist1)+sqrt(dist2));
}

 inline Eigen::Matrix3d _skew(const Eigen::Vector3d& t){
   Eigen::Matrix3d S;
   S <<
     0,  -t.z(),   t.y(),
     t.z(),     0,     -t.x(),
     -t.y(),     t.x(),   0;
   return S;
 }

Eigen::Vector3d MetricRectification::normalizeLine(Eigen::Vector3d p0, Eigen::Vector3d p1)
{
	Eigen::Vector3d l = p0.cross(p1);
	l.x() = l.x() / l.z();
	l.y() = l.y() / l.z();
	l.z() = 1.0;
	//return l;
	return p0.cross(p1).normalized();
}