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

void addPath(const Eigen::Vector3f &pt, const Eigen::Vector3f &normal, pcl::PointCloud<pcl::PointNormal> &path) {
    pcl::PointNormal start_path;
    start_path.x = pt.x(); start_path.y = pt.y(); start_path.z = pt.z();
    start_path.normal[0] = normal.x(); start_path.normal[1] = normal.y(); start_path.normal[2] = normal.z();
    path.push_back(start_path);
}

NORI_NAMESPACE_BEGIN

NoriObject *loadFromXML(const std::string &filename) {
    /* Load the XML file using 'pugi' (a tiny self-contained XML parser implemented in C++) */
    pugi::xml_document doc;
    pugi::xml_parse_result result = doc.load_file(filename.c_str());

    /* Helper function: map a position offset in bytes to a more readable row/column value */
    auto offset = [&](ptrdiff_t pos) -> std::string {
        std::fstream is(filename);
        char buffer[1024];
        int line = 0, linestart = 0, offset = 0;
        while (is.good()) {
            is.read(buffer, sizeof(buffer));
            for (int i = 0; i < is.gcount(); ++i) {
                if (buffer[i] == '\n') {
                    if (offset + i >= pos)
                        return tfm::format("row %i, col %i", line + 1, pos - linestart);
                    ++line;
                    linestart = offset + i;
                }
            }
            offset += (int) is.gcount();
        }
        return "byte offset " + std::to_string(pos);
    };

    if (!result) /* There was a parser / file IO error */
        throw NoriException("Error while parsing \"%s\": %s (at %s)", filename, result.description(), offset(result.offset));

    /* Set of supported XML tags */
    enum ETag {
        /* Object classes */
        EScene                = NoriObject::EScene,
        EMesh                 = NoriObject::EMesh,
        EBSDF                 = NoriObject::EBSDF,
        EPhaseFunction        = NoriObject::EPhaseFunction,
        EEmitter            = NoriObject::EEmitter,
        EMedium               = NoriObject::EMedium,
        ECamera               = NoriObject::ECamera,
        EIntegrator           = NoriObject::EIntegrator,
        ESampler              = NoriObject::ESampler,
        ETest                 = NoriObject::ETest,
        EReconstructionFilter = NoriObject::EReconstructionFilter,

        /* Properties */
        EBoolean = NoriObject::EClassTypeCount,
        EInteger,
        EFloat,
        EString,
        EPoint,
        EVector,
        EColor,
        ETransform,
        ETranslate,
        EMatrix,
        ERotate,
        EScale,
        ELookAt,

        EInvalid
    };

    /* Create a mapping from tag names to tag IDs */
    std::map<std::string, ETag> tags;
    tags["scene"]      = EScene;
    tags["mesh"]       = EMesh;
    tags["bsdf"]       = EBSDF;
    tags["emitter"]  = EEmitter;
    tags["camera"]     = ECamera;
    tags["medium"]     = EMedium;
    tags["phase"]      = EPhaseFunction;
    tags["integrator"] = EIntegrator;
    tags["sampler"]    = ESampler;
    tags["rfilter"]    = EReconstructionFilter;
    tags["test"]       = ETest;
    tags["boolean"]    = EBoolean;
    tags["integer"]    = EInteger;
    tags["float"]      = EFloat;
    tags["string"]     = EString;
    tags["point"]      = EPoint;
    tags["vector"]     = EVector;
    tags["color"]      = EColor;
    tags["transform"]  = ETransform;
    tags["translate"]  = ETranslate;
    tags["matrix"]     = EMatrix;
    tags["rotate"]     = ERotate;
    tags["scale"]      = EScale;
    tags["lookat"]     = ELookAt;

    /* Helper function to check if attributes are fully specified */
    auto check_attributes = [&](const pugi::xml_node &node, std::set<std::string> attrs) {
        for (auto attr : node.attributes()) {
            auto it = attrs.find(attr.name());
            if (it == attrs.end())
                throw NoriException("Error while parsing \"%s\": unexpected attribute \"%s\" in \"%s\" at %s",
                                    filename, attr.name(), node.name(), offset(node.offset_debug()));
            attrs.erase(it);
        }
        if (!attrs.empty())
//.........这里部分代码省略.........

void glsl_program::set_uniform(unsigned loc, const Eigen::Vector3f& value) const
{
    if (used_program != this)
	throw runtime_error("glsl_program::set_uniform, program not bound!");
    glUniform3f(loc, value.x(), value.y(), value.z());
}

_eePose _eePose::applyRPYTo(double roll_z, double pitch_y, double yaw_x) const {

  Eigen::Vector3f localUnitX;
  {
    Eigen::Quaternionf qin(0, 1, 0, 0);
    Eigen::Quaternionf qout(0, 1, 0, 0);
    Eigen::Quaternionf eeqform(qw, qx, qy, qz);
    qout = eeqform * qin * eeqform.conjugate();
    localUnitX.x() = qout.x();
    localUnitX.y() = qout.y();
    localUnitX.z() = qout.z();
  }

  Eigen::Vector3f localUnitY;
  {
    Eigen::Quaternionf qin(0, 0, 1, 0);
    Eigen::Quaternionf qout(0, 1, 0, 0);
    Eigen::Quaternionf eeqform(qw, qx, qy, qz);
    qout = eeqform * qin * eeqform.conjugate();
    localUnitY.x() = qout.x();
    localUnitY.y() = qout.y();
    localUnitY.z() = qout.z();
  }

  Eigen::Vector3f localUnitZ;
  {
    Eigen::Quaternionf qin(0, 0, 0, 1);
    Eigen::Quaternionf qout(0, 1, 0, 0);
    Eigen::Quaternionf eeqform(qw, qx, qy, qz);
    qout = eeqform * qin * eeqform.conjugate();
    localUnitZ.x() = qout.x();
    localUnitZ.y() = qout.y();
    localUnitZ.z() = qout.z();
  }

  double sinBuff = 0.0;
  double angleRate = 1.0;
  Eigen::Quaternionf eeBaseQuat(qw, qx, qy, qz);
  sinBuff = sin(angleRate*yaw_x/2.0);
  Eigen::Quaternionf eeRotatorX(cos(angleRate*yaw_x/2.0), localUnitX.x()*sinBuff, localUnitX.y()*sinBuff, localUnitX.z()*sinBuff);
  sinBuff = sin(angleRate*pitch_y/2.0);
  Eigen::Quaternionf eeRotatorY(cos(angleRate*pitch_y/2.0), localUnitY.x()*sinBuff, localUnitY.y()*sinBuff, localUnitY.z()*sinBuff);
  sinBuff = sin(angleRate*roll_z/2.0);
  Eigen::Quaternionf eeRotatorZ(cos(angleRate*roll_z/2.0), localUnitZ.x()*sinBuff, localUnitZ.y()*sinBuff, localUnitZ.z()*sinBuff);

  eeRotatorX.normalize();
  eeRotatorY.normalize();
  eeRotatorZ.normalize();

  eeBaseQuat = eeRotatorX * eeRotatorY * eeRotatorZ * eeBaseQuat;
  eeBaseQuat.normalize();

  _eePose toreturn;
  toreturn.px = px;
  toreturn.py = py;
  toreturn.pz = pz;
  toreturn.qw = eeBaseQuat.w();
  toreturn.qx = eeBaseQuat.x();
  toreturn.qy = eeBaseQuat.y();
  toreturn.qz = eeBaseQuat.z();

  return toreturn;
}

		/** Computes local depth gradient (depth[m]/distance[m]) */
		Eigen::Vector2f computeGradient() const {
			return Eigen::Vector2f(normal.x() / normal.z(), normal.y() / normal.z());
		}

ZMPDistributor::ForceTorque ZMPDistributor::distributeZMP(const Eigen::Vector3f& localAnkleLeft,
                                                              const Eigen::Vector3f& localAnkleRight,
                                                              const Eigen::Matrix4f& leftFootPoseGroundFrame,
                                                              const Eigen::Matrix4f& rightFootPoseGroundFrame,
                                                              const Eigen::Vector3f& zmpRefGroundFrame,
                                                              Bipedal::SupportPhase phase)
{
    Eigen::Matrix4f groundPoseLeft  = Bipedal::projectPoseToGround(leftFootPoseGroundFrame);
    Eigen::Matrix4f groundPoseRight = Bipedal::projectPoseToGround(rightFootPoseGroundFrame);
    Eigen::Vector3f localZMPLeft    = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseLeft.inverse());
    Eigen::Vector3f localZMPRight   = VirtualRobot::MathTools::transformPosition(zmpRefGroundFrame, groundPoseRight.inverse());

    double alpha = computeAlpha(groundPoseLeft, groundPoseRight, zmpRefGroundFrame, localZMPLeft.head(2), localZMPRight.head(2), phase);

    //std::cout << "########## " << alpha << " ###########" << std::endl;

    ForceTorque ft;
    // kg*m/s^2 = N
    ft.leftForce  = -(1-alpha) * mass * gravity;
    ft.rightForce = -alpha     * mass * gravity;

    // Note we need force as kg*mm/s^2
    ft.leftTorque  = (localAnkleLeft  - localZMPLeft).cross(ft.leftForce * 1000);
    ft.rightTorque = (localAnkleRight - localZMPRight).cross(ft.rightForce * 1000);

    // ZMP not contained in convex polygone
    if (std::fabs(alpha) > std::numeric_limits<float>::epsilon()
     && std::fabs(alpha-1) > std::numeric_limits<float>::epsilon())
    {
        Eigen::Vector3f leftTorqueGroundFrame  = groundPoseLeft.block(0, 0, 3, 3)  * ft.leftTorque;
        Eigen::Vector3f rightTorqueGroundFrame = groundPoseRight.block(0, 0, 3, 3) * ft.rightTorque;
        Eigen::Vector3f tau0 = -1 * (leftTorqueGroundFrame + rightTorqueGroundFrame);

        //std::cout << "Tau0World: " << tau0.transpose() << std::endl;
        //std::cout << "leftTorqueWorld: "  << leftTorqueWorld.transpose() << std::endl;
        //std::cout << "rightTorqueWorld: " << rightTorqueWorld.transpose() << std::endl;

        // Note: Our coordinate system is different than in the paper!
        // Also this is not the same as the ground frame.
        Eigen::Vector3f xAxis = leftFootPoseGroundFrame.block(0,3,3,1) + localAnkleLeft
                              - localAnkleRight - rightFootPoseGroundFrame.block(0,3,3,1);
        xAxis /= xAxis.norm();
        Eigen::Vector3f zAxis(0, 0, 1);
        Eigen::Vector3f yAxis = zAxis.cross(xAxis);
        yAxis /= yAxis.norm();
        Eigen::Matrix3f centerFrame;
        centerFrame.block(0, 0, 3, 1) = xAxis;
        centerFrame.block(0, 1, 3, 1) = yAxis;
        centerFrame.block(0, 2, 3, 1) = zAxis;

        //std::cout << "Center frame:\n" << centerFrame << std::endl;

        Eigen::Vector3f centerTau0 = centerFrame.transpose() * tau0;
        Eigen::Vector3f leftTorqueCenter;
        leftTorqueCenter.x() = (1-alpha)*centerTau0.x();
        leftTorqueCenter.y() = centerTau0.y() < 0 ? centerTau0.y() : 0;
        leftTorqueCenter.z() = 0;
        Eigen::Vector3f rightTorqueCenter;
        rightTorqueCenter.x() = alpha*centerTau0.x();
        rightTorqueCenter.y() = centerTau0.y() < 0 ? 0 : centerTau0.y();
        rightTorqueCenter.z() = 0;

        //std::cout << "Tau0Center: " << centerTau0.transpose() << std::endl;
        //std::cout << "leftTorqueCenter: "  << leftTorqueCenter.transpose() << std::endl;
        //std::cout << "rightTorqueCenter: " << rightTorqueCenter.transpose() << std::endl;

        // tcp <--- ground frame <--- center frame
        ft.leftTorque  = groundPoseLeft.block(0, 0, 3, 3).transpose()  * centerFrame * leftTorqueCenter;
        ft.rightTorque = groundPoseRight.block(0, 0, 3, 3).transpose() * centerFrame * rightTorqueCenter;
    }

    // Torque depends on timestep, we need a way to do this correctly.
    const double torqueFactor = 1;
    // convert to Nm
    ft.leftTorque  *= torqueFactor / 1000.0 / 1000.0;
    ft.rightTorque *= torqueFactor / 1000.0 / 1000.0;

    //std::cout << "leftTorque: "  << ft.leftTorque.transpose() << std::endl;
    //std::cout << "rightTorque: " << ft.rightTorque.transpose() << std::endl;

    return ft;
}

void WorldDownloadManager::initRaycaster(bool has_intrinsics,const kinfu_msgs::KinfuCameraIntrinsics & intr,
  bool has_bounding_box_view,const kinfu_msgs::KinfuCloudPoint & bbox_min,const kinfu_msgs::KinfuCloudPoint & bbox_max)
{
  const uint rows = has_intrinsics ? intr.size_y : m_kinfu->rows();
  const uint cols = has_intrinsics ? intr.size_x : m_kinfu->cols();

  float cx,cy,fx,fy;
  float min_range = 0.0;

  if (has_intrinsics)
  {
    ROS_INFO("kinfu: custom intrinsics will be used.");
    cx = intr.center_x;
    cy = intr.center_y;
    fx = intr.focal_x;
    fy = intr.focal_y;
    min_range = intr.min_range;
  }
  else
    m_kinfu->getDepthIntrinsics(fx,fy,cx,cy);

  if (!m_raycaster || (uint(m_raycaster->rows) != rows) || (uint(m_raycaster->cols) != cols))
  {
    ROS_INFO("kinfu: initializing raycaster...");
    m_raycaster = RayCaster::Ptr(new RayCaster(rows,cols));
  }

  m_raycaster->setRaycastStep(m_kinfu->volume().getTsdfTruncDist() * 0.6);
  m_raycaster->setMinRange(min_range);
  m_raycaster->setIntrinsics(fx,fy,cx,cy);

  if (has_bounding_box_view)
    {
    const Eigen::Vector3f im(bbox_min.x,bbox_min.y,bbox_min.z);
    const Eigen::Vector3f iM(bbox_max.x,bbox_max.y,bbox_max.z);
    ROS_INFO("kinfu: raycaster will be limited to bounding box: %f %f %f - %f %f %f",im.x(),im.y(),im.z(),iM.x(),iM.y(),iM.z());
    const Eigen::Vector3f m = m_reverse_initial_transformation.inverse() * im;
    const Eigen::Vector3f M = m_reverse_initial_transformation.inverse() * iM;
    const Eigen::Vector3f mmin = m.array().min(M.array());
    const Eigen::Vector3f mmax = m.array().max(M.array());

    m_raycaster->setBoundingBox(mmin,mmax);
    }
  else
    m_raycaster->clearBoundingBox();
}

int
main (int argc, char** argv)
{

  ofstream os("out.g2o");
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

  int n;
  
  
  if (argc != 2)
  {
    std::cerr << "please specify the number of bloody clouds o be registered " << std::endl;
    exit (0);
  }
  n=atoi(argv[1]);
  
  std::cerr << "attempting to read " <<  n << " bloody clouds" << std::endl;
//  /////////////////////////////////////////////////////////////////////////////////////////////////////

//  /////////////////////////////////////////////////////////////////////////////////////////////////////
  
  pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
  //  sor.setLeafSize (0.01, 0.01, 0.01);
  sor.setLeafSize (0.05, 0.05, 0.05);
  //  sor.setLeafSize (0.05, 0.05, 0.05);
//  sor.setLeafSize (0.1, 0.1, 0.1);
//  sor.setLeafSize (0.4, 0.4, 0.4);
//  sor.setLeafSize (0.5, 0.5, 0.5);

  // sor.setInputCloud (inputCloud.makeShared());
  // std::cout<<"\n inputCloud.size()="<<inputCloud.size()<<std::endl;
  // sor.filter (inputCloudFiltered);
  // std::cout<<"\n inputCloudFiltered.size()="<<inputCloudFiltered.size()<<std::endl;

  // sor.setInputCloud (targetCloud.makeShared());
  // std::cout<<"\n targetCloud.size()="<<targetCloud.size()<<std::endl;
  // sor.filter (targetCloudFiltered);
  // std::cout<<"\n targetCloudFiltered.size()="<<targetCloudFiltered.size()<<std::endl;


//   pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

//   icp.setMaximumIterations(10000);

// //  icp.setMaxCorrespondenceDistance (0.6);
//   icp.setMaxCorrespondenceDistance (0.8);
// //  icp.setMaxCorrespondenceDistance (1.5);

// //  icp.setRANSACOutlierRejectionThreshold (0.1);
//   icp.setRANSACOutlierRejectionThreshold (0.6);
// //  icp.setRANSACOutlierRejectionThreshold (1.5);
// //  icp.setRANSACOutlierRejectionThreshold (5.0);


  std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > clouds(n);
  std::vector<pcl::PointCloud<pcl::PointXYZRGBA> > filteredClouds(n);

  Eigen::Matrix4f tTotal=Eigen::Matrix4f::Identity();


  const float min_scale = 0.0005; 
  const int nr_octaves = 4; 
  const int nr_scales_per_octave = 5; 
  const float min_contrast = 1; 
  
  //pcl::SIFTKeypoint<pcl::PointXYZRGBA, pcl::PointWithScale> sift;

  for (int i=0; i<n; i++){
    char filename[20];
    char ffilename[20];
    sprintf(filename,"base-%03d.pcd", i);
    sprintf(ffilename,"filt-%d.pcd", i);
    cerr << "filename= " << filename <<endl;
    clouds[i]=pcl::PointCloud<pcl::PointXYZRGBA>();
    cerr << "ok up to here" << endl;
    if (pcl::io::loadPCDFile<pcl::PointXYZRGBA> (filename, clouds[i]) == -1) //* load the file
      {
	std::cerr << "Couldn't read the "<< i <<"th pcd file \n" << std::endl;
	return (-1);
      }

    
    // {
    //   pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBA> ());;//new API
    //   pcl::PointCloud<pcl::PointWithScale>::Ptr sifts (new pcl::PointCloud<pcl::PointWithScale>);
    //   sift.setInputCloud(clouds[i]);
    //   sift.setSearchMethod (tree);
    //   sift.setScales(min_scale, nr_octaves, nr_scales_per_octave);
    //   sift.setMinimumContrast(min_contrast);
    //   sift.compute (*sifts);
    
    //   cerr << "Computed " << sifts->points.size () << " SIFT Keypoints " << endl;
    // }

    cerr << "sor input" << endl;
    sor.setInputCloud (clouds[i].makeShared());
    std::cout<<"\n inputCloud.size()="<<clouds[i].size()<<std::endl;

    cerr << "sor target" << endl;
//.........这里部分代码省略.........

//#include <osgGA/TrackballManipulator>
osg::Vec3f toOSGVector(Eigen::Vector3f v) { return osg::Vec3f(v.x(), v.y(), v.z());}

DWORD BuildCubesVert::runThread() {
	int i = cubeIndex;
	Eigen::Vector3f center = Eigen::Vector3f(
		(*mCubes)[i].X + (*mCubes)[i].Width/2.f, 
		(*mCubes)[i].Y + (*mCubes)[i].Height/2.f, 
		(*mCubes)[i].Z + (*mCubes)[i].Depth/2.f);

	int inside = 0;
	bool addFace[6];
	memset(addFace, true, 6);

	Eigen::Vector3f points[6];

	points[0] = center + Eigen::Vector3f((*mCubes)[i].Width, 0.f, 0.f);
	points[1] = center - Eigen::Vector3f((*mCubes)[i].Width, 0.f, 0.f);
	points[2] = center + Eigen::Vector3f(0.f, (*mCubes)[i].Height, 0.f);
	points[3] = center - Eigen::Vector3f(0.f, (*mCubes)[i].Height, 0.f);
	points[4] = center + Eigen::Vector3f(0.f, 0.f, (*mCubes)[i].Depth);
	points[5] = center - Eigen::Vector3f(0.f, 0.f, (*mCubes)[i].Depth);
	
	for (unsigned int p = 0; p < 6; p++) {
		kdtreeNode target;
		target.xyz[0] = points[p].x();
		target.xyz[1] = points[p].y();
		target.xyz[2] = points[p].z();

		std::pair<treeType::const_iterator,double> found = kdCubes->find_nearest(target);
		int k = found.first->index;

		//add the min/max vertex to the list
		Eigen::Vector3f t_min = Eigen::Vector3f(
			(*mCubes)[k].X, 
			(*mCubes)[k].Y,
			(*mCubes)[k].Z);
		Eigen::Vector3f t_max = Eigen::Vector3f(
			(*mCubes)[k].X + (*mCubes)[k].Width, 
			(*mCubes)[k].Y + (*mCubes)[k].Height, 
			(*mCubes)[k].Z + (*mCubes)[k].Depth);

		for (unsigned int p = 0; p < 6; p++) {
			if (points[p].x() > t_min.x() &&
				points[p].y() > t_min.y() &&
				points[p].z() > t_min.z() &&
				points[p].x() < t_max.x() &&
				points[p].y() < t_max.y() &&
				points[p].z() < t_max.z()) {
					addFace[p] = false;
					inside++;
			}
		}
	}

	/*
	for (unsigned int k = 0; k < (*mCubes).size(); k++) {
		//add the min/max vertex to the list
		Eigen::Vector3f t_min = Eigen::Vector3f(
			(*mCubes)[k].X, 
			(*mCubes)[k].Y,
			(*mCubes)[k].Z);
		Eigen::Vector3f t_max = Eigen::Vector3f(
			(*mCubes)[k].X + (*mCubes)[k].Width, 
			(*mCubes)[k].Y + (*mCubes)[k].Height, 
			(*mCubes)[k].Z + (*mCubes)[k].Depth);

		for (unsigned int p = 0; p < 6; p++) {
			if (points[p].x() > t_min.x() &&
				points[p].y() > t_min.y() &&
				points[p].z() > t_min.z() &&
				points[p].x() < t_max.x() &&
				points[p].y() < t_max.y() &&
				points[p].z() < t_max.z()) {
					addFace[p] = false;
					inside++;
			}
		}

		if (inside >= 6)
			break;
	}
	*/
	if (inside < 6) {
		lock();

		//add the min/max vertex to the list
		Eigen::Vector3f min = Eigen::Vector3f(
			(*mCubes)[i].X, 
			(*mCubes)[i].Y,
			(*mCubes)[i].Z);
		Eigen::Vector3f max = Eigen::Vector3f(
			(*mCubes)[i].X + (*mCubes)[i].Width, 
			(*mCubes)[i].Y + (*mCubes)[i].Height, 
			(*mCubes)[i].Z + (*mCubes)[i].Depth);

		// Create Vertices
		float3 verts[8] = {
			float3(min.x(), min.y(), max.z()),
			float3(max.x(), min.y(), max.z()),
			float3(max.x(), max.y(), max.z()),
			float3(min.x(), max.y(), max.z()),
			float3(min.x(), min.y(), min.z()),
//.........这里部分代码省略.........

Eigen::Vector2f multiplyPointMatrix(Eigen::MatrixXf h, float x, float y)
{
	Eigen::Vector3f p(x, y, 1.0f);
	Eigen::Vector3f r = h * p;
	return Eigen::Vector2f(r.x() / r.z(), r.y() / r.z());
}

pcl::PointCloud<pcl::PointNormal>::Ptr CloudFactory::createCylinderSection(const float _angle, const float _radius, const float _height, const Eigen::Vector3f &_center, const int _npoints)
{
	pcl::PointCloud<pcl::PointNormal>::Ptr cloud = pcl::PointCloud<pcl::PointNormal>::Ptr(new pcl::PointCloud<pcl::PointNormal>());

	float ratio = _radius / _height;
	int Nside = sqrt(_npoints * (ratio / (1 + ratio)));
	int Ncover = sqrt(_npoints * (1 / (1 + ratio)));

	double minZ = _center.z() - _height * 0.5;
	double maxZ = _center.z() + _height * 0.5;
	double stepZ = _height / Nside;

	double stepAngle = _angle / Ncover;
	double stepRadius = _radius * 0.02;

	// Generate cylinder top and bottom
	for (double angle = 0; angle < 2 * M_PI; angle += stepAngle)
	{
		for (double r = _radius; r > 0; r -= stepRadius)
		{
			cloud->push_back(PointFactory::createPointNormal(r * cos(angle) + _center.x(), r * sin(angle) + _center.y(), minZ, 0, 0, -1));
			cloud->push_back(PointFactory::createPointNormal(r * cos(angle) + _center.x(), r * sin(angle) + _center.y(), maxZ, 0, 0, 1));
		}
	}

	// Generate cylinder side
	for (double z = minZ; z <= maxZ; z += stepZ)
	{
		for (double angle = 0; angle < 2 * M_PI; angle += stepAngle)
		{
			Eigen::Vector3f normal = Eigen::Vector3f(_radius * cos(angle), _radius * sin(angle), 0).normalized();
			cloud->push_back(PointFactory::createPointNormal(_radius * cos(angle) + _center.x(), _radius * sin(angle) + _center.y(), z, normal.x(), normal.y(), normal.z()));
		}
	}

	return cloud;
}

// This algorithm comes from Sebastian O.H. Madgwick's 2010 paper:
// "An efficient orientation filter for inertial and inertial/magnetic sensor arrays"
// https://www.samba.org/tridge/UAV/madgwick_internal_report.pdf
static void 
_psmove_orientation_fusion_madgwick_marg_update(
	PSMoveOrientation *orientation_state,
	float delta_t,
	const Eigen::Vector3f &current_omega,
	const Eigen::Vector3f &current_g,
	const Eigen::Vector3f &current_m)
{
	// If there isn't a valid magnetometer or accelerometer vector, fall back to the IMU style update
	if (current_g.isZero(k_normal_epsilon) || current_m.isZero(k_normal_epsilon))
	{
		_psmove_orientation_fusion_imu_update(
			orientation_state,
			delta_t,
			current_omega,
			current_g);
		return;
	}

	PSMoveMadgwickMARGState *marg_state = &orientation_state->fusion_state.madgwick_marg_state;

	// Current orientation from earth frame to sensor frame
	Eigen::Quaternionf SEq = orientation_state->quaternion;

	// Get the direction of the magnetic fields in the identity pose.	
	// NOTE: In the original paper we converge on this vector over time automatically (See Eqn 45 & 46)
	// but since we've already done the work in calibration to get this vector, let's just use it.
	// This also removes the last assumption in this function about what 
	// the orientation of the identity-pose is (handled by the sensor transform).
	PSMove_3AxisVector identity_m= psmove_orientation_get_magnetometer_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_m_direction = Eigen::Vector3f(identity_m.x, identity_m.y, identity_m.z);

	// Get the direction of the gravitational fields in the identity pose
	PSMove_3AxisVector identity_g= psmove_orientation_get_gravity_calibration_direction(orientation_state);
	Eigen::Vector3f k_identity_g_direction = Eigen::Vector3f(identity_g.x, identity_g.y, identity_g.z);

	// Eqn 15) Applied to the gravity and magnetometer vectors
	// Fill in the 6x1 objective function matrix f(SEq, Sa, Eb, Sm) =|f_g|
	//                                                               |f_b|
	Eigen::Matrix<float, 3, 1> f_g;
	psmove_alignment_compute_objective_vector(SEq, k_identity_g_direction, current_g, f_g, NULL);

	Eigen::Matrix<float, 3, 1> f_m;
	psmove_alignment_compute_objective_vector(SEq, k_identity_m_direction, current_m, f_m, NULL);

	Eigen::Matrix<float, 6, 1> f_gb;
	f_gb.block<3, 1>(0, 0) = f_g;
	f_gb.block<3, 1>(3, 0) = f_m;

	// Eqn 21) Applied to the gravity and magnetometer vectors
	// Fill in the 4x6 objective function Jacobian matrix: J_gb(SEq, Eb)= [J_g|J_b]
	Eigen::Matrix<float, 4, 3> J_g;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_g_direction, J_g);

	Eigen::Matrix<float, 4, 3> J_m;
	psmove_alignment_compute_objective_jacobian(SEq, k_identity_m_direction, J_m);

	Eigen::Matrix<float, 4, 6> J_gb;
	J_gb.block<4, 3>(0, 0) = J_g; J_gb.block<4, 3>(0, 3) = J_m;

	// Eqn 34) gradient_F= J_gb(SEq, Eb)*f(SEq, Sa, Eb, Sm)
	// Compute the gradient of the objective function
	Eigen::Matrix<float, 4, 1> gradient_f = J_gb*f_gb;
	Eigen::Quaternionf SEqHatDot =
		Eigen::Quaternionf(gradient_f(0, 0), gradient_f(1, 0), gradient_f(2, 0), gradient_f(3, 0));

	// normalize the gradient to estimate direction of the gyroscope error
	psmove_quaternion_normalize_with_default(SEqHatDot, *k_psmove_quaternion_zero);

	// Eqn 47) omega_err= 2*SEq*SEqHatDot
	// compute angular estimated direction of the gyroscope error
	Eigen::Quaternionf omega_err = Eigen::Quaternionf(SEq.coeffs()*2.f) * SEqHatDot;

	// Eqn 48) net_omega_bias+= zeta*omega_err
	// Compute the net accumulated gyroscope bias
	marg_state->omega_bias = Eigen::Quaternionf(marg_state->omega_bias.coeffs() + omega_err.coeffs()*zeta*delta_t);
	marg_state->omega_bias.w() = 0.f; // no bias should accumulate on the w-component

	// Eqn 49) omega_corrected = omega - net_omega_bias
	Eigen::Quaternionf omega = Eigen::Quaternionf(0.f, current_omega.x(), current_omega.y(), current_omega.z());
	Eigen::Quaternionf corrected_omega = Eigen::Quaternionf(omega.coeffs() - marg_state->omega_bias.coeffs());

	// Compute the rate of change of the orientation purely from the gyroscope
	// Eqn 12) q_dot = 0.5*q*omega
	Eigen::Quaternionf SEqDot_omega = Eigen::Quaternionf(SEq.coeffs() * 0.5f) * corrected_omega;

	// Compute the estimated quaternion rate of change
	// Eqn 43) SEq_est = SEqDot_omega - beta*SEqHatDot
	Eigen::Quaternionf SEqDot_est = Eigen::Quaternionf(SEqDot_omega.coeffs() - SEqHatDot.coeffs()*beta);

	// Compute then integrate the estimated quaternion rate
	// Eqn 42) SEq_new = SEq + SEqDot_est*delta_t
	Eigen::Quaternionf SEq_new = Eigen::Quaternionf(SEq.coeffs() + SEqDot_est.coeffs()*delta_t);

	// Make sure the net quaternion is a pure rotation quaternion
	SEq_new.normalize();

//.........这里部分代码省略.........

std::vector<mavros_msgs::Mavlink> HilSensorLevelInterface::CollectData() {
  boost::mutex::scoped_lock lock(mtx_);

  ros::Time current_time = ros::Time::now();
  uint64_t time_usec = RosTimeToMicroseconds(current_time);

  mavlink_message_t mmsg;
  std::vector<mavros_msgs::Mavlink> hil_msgs;

  // Rotate gyroscope, accelerometer, and magnetometer data into NED frame
  Eigen::Vector3f gyro = R_S_B_ * hil_data_.gyro_rad_per_s;
  Eigen::Vector3f acc = R_S_B_ * hil_data_.acc_m_per_s2;
  Eigen::Vector3f mag = R_S_B_ * hil_data_.mag_G;

  // Check if we need to publish a HIL_GPS message.
  if ((current_time.nsec - last_gps_pub_time_nsec_) >= gps_interval_nsec_) {
    last_gps_pub_time_nsec_ = current_time.nsec;

    // Rotate ground speed data into NED frame
    Eigen::Vector3i gps_vel = (R_S_B_ * hil_data_.gps_vel_cm_per_s.cast<float>()).cast<int>();

    // Fill in a MAVLINK HIL_GPS message and convert it to MAVROS format.
    hil_gps_msg_.time_usec = time_usec;
    hil_gps_msg_.fix_type = hil_data_.fix_type;
    hil_gps_msg_.lat = hil_data_.lat_1e7deg;
    hil_gps_msg_.lon = hil_data_.lon_1e7deg;
    hil_gps_msg_.alt = hil_data_.alt_mm;
    hil_gps_msg_.eph = hil_data_.eph_cm;
    hil_gps_msg_.epv = hil_data_.epv_cm;
    hil_gps_msg_.vel = hil_data_.vel_1e2m_per_s;
    hil_gps_msg_.vn = gps_vel.x();
    hil_gps_msg_.ve = gps_vel.y();
    hil_gps_msg_.vd = gps_vel.z();
    hil_gps_msg_.cog = hil_data_.cog_1e2deg;
    hil_gps_msg_.satellites_visible = hil_data_.satellites_visible;

    mavlink_hil_gps_t* hil_gps_msg_ptr = &hil_gps_msg_;
    mavlink_msg_hil_gps_encode(1, 0, &mmsg, hil_gps_msg_ptr);

    mavros_msgs::MavlinkPtr rmsg_hil_gps = boost::make_shared<mavros_msgs::Mavlink>();
    rmsg_hil_gps->header.stamp.sec = current_time.sec;
    rmsg_hil_gps->header.stamp.nsec = current_time.nsec;
    mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_gps);

    hil_msgs.push_back(*rmsg_hil_gps);
  }

  // Fill in a MAVLINK HIL_SENSOR message and convert it to MAVROS format.
  hil_sensor_msg_.time_usec = time_usec;
  hil_sensor_msg_.xacc = acc.x();
  hil_sensor_msg_.yacc = acc.y();
  hil_sensor_msg_.zacc = acc.z();
  hil_sensor_msg_.xgyro = gyro.x();
  hil_sensor_msg_.ygyro = gyro.y();
  hil_sensor_msg_.zgyro = gyro.z();
  hil_sensor_msg_.xmag = mag.x();
  hil_sensor_msg_.ymag = mag.y();
  hil_sensor_msg_.zmag = mag.z();
  hil_sensor_msg_.abs_pressure = hil_data_.pressure_abs_mBar;
  hil_sensor_msg_.diff_pressure = hil_data_.pressure_diff_mBar;
  hil_sensor_msg_.pressure_alt = hil_data_.pressure_alt;
  hil_sensor_msg_.temperature = hil_data_.temperature_degC;
  hil_sensor_msg_.fields_updated = kAllFieldsUpdated;

  mavlink_hil_sensor_t* hil_sensor_msg_ptr = &hil_sensor_msg_;
  mavlink_msg_hil_sensor_encode(1, 0, &mmsg, hil_sensor_msg_ptr);

  mavros_msgs::MavlinkPtr rmsg_hil_sensor = boost::make_shared<mavros_msgs::Mavlink>();
  rmsg_hil_sensor->header.stamp.sec = current_time.sec;
  rmsg_hil_sensor->header.stamp.nsec = current_time.nsec;
  mavros_msgs::mavlink::convert(mmsg, *rmsg_hil_sensor);

  hil_msgs.push_back(*rmsg_hil_sensor);

  return hil_msgs;
}

Eigen::Matrix4f translate(const Eigen::Vector3f& v)
{
  return translate(v.x(),v.y(),v.z());
}