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

 PitchRollState()
 {
     Omega.setZero();
     GyroBias.setZero();
     AccelBias.setZero();
     Covariance.resize(ndim(), ndim());
     Covariance.setIdentity();
 };

void
cylinder_kinematics(double /*data_time*/, Eigen::Vector3d& U_com, Eigen::Vector3d& W_com, void* /*ctx*/)
{
    U_com.setZero();
    W_com.setZero();

    return;
} // cylinder_kinematics

 void mean(const std::vector<double>& weights,
         const std::vector<struct IMUOrientationMeasurement>& Chimeas)
 {
     acceleration.setZero();
     omega.setZero();
     for(const auto &&t: zip_range(weights, Chimeas))
     {
         double w = t.get<0>();
         struct IMUOrientationMeasurement m = t.get<1>();
         acceleration += w*m.acceleration;
         omega += w*m.omega;
     }
 };

// Fit a plane to 3D data
void PlaneFit::fitPlane(const Points3D& P, Eigen::Vector3d& normal, Eigen::Vector3d& mean)
{
	// Retrieve how many data points there are
	size_t N = P.size();

	// Handle trivial cases
	if(N <= 0)
	{
		normal << 0.0, 0.0, 1.0;
		mean.setZero();
		return;
	}
	else if(N == 1)
	{
		normal << 0.0, 0.0, 1.0;
		mean = P[0];
		return;
	}

	// Calculate the mean of the data points (the plane of best fit always passes through the mean)
	mean = Eigen::Vector3d::Zero();
	if(N >= 1)
		mean = std::accumulate(P.begin(), P.end(), mean) / N;

	// Construct the matrix of zero mean data points
	Eigen::MatrixXd A(3, N);
	for(size_t i = 0; i < N; i++)
		A.col(i) = P[i] - mean;

	// Perform an SVD decomposition to determine the direction with the minimum variance
	unsigned int options = (N >= 3 ? Eigen::ComputeThinU : Eigen::ComputeFullU) | Eigen::ComputeThinV;
	normal = A.jacobiSvd(options).matrixU().col(2).normalized();
}

inline void fit(const std::vector<LaserBeam> &input, Eigen::Vector3d &output, double &probability) {
    PointCloud<PointXY>::Ptr cloud( new PointCloud<PointXY> );
    for(std::vector<LaserBeam>::const_iterator it = input.begin() ; it != input.end() ; ++it) {
        PointXY p;
        p.x = it->posX();
        p.y = it->posY();
        cloud->push_back(p);
    }

    SampleConsensusModelCircle2D<PointXY>::Ptr    ransac_model(new SampleConsensusModelCircle2D<PointXY>(cloud));
    SampleConsensus<PointXY>::Ptr                 ransac(new RandomSampleConsensus<PointXY>(ransac_model));
    ransac->setDistanceThreshold(0.05);

    Eigen::VectorXf coefficients;
    ransac->computeModel();
    ransac->getModelCoefficients(coefficients);
    if(coefficients.size() > 0) {
        output.x() = coefficients[0];
        output.y() = coefficients[1];
        output.z() = coefficients[2];
        probability = ransac->getProbability();
    } else {
        output.setZero();
        probability = 0.0;
    }
}

 /** \brief Constructor
  */
 FilterState():fsm_(nullptr), transformFeatureOutputCT_(nullptr), featureOutputCov_((int)(FeatureOutput::D_),(int)(FeatureOutput::D_)){
   usePredictionMerge_ = true;
   imgTime_ = 0.0;
   imageCounter_ = 0;
   groundtruth_qCJ_.setIdentity();
   groundtruth_JrJC_.setZero();
   groundtruth_qJI_.setIdentity();
   groundtruth_IrIJ_.setZero();
   groundtruth_qCB_.setIdentity();
   groundtruth_BrBC_.setZero();
   plotGroundtruth_ = true;
   state_.initFeatureManagers(fsm_);
   fsm_.allocateMissing();
   drawPB_ = 1;
   drawPS_ = mtState::patchSize_*pow(2,mtState::nLevels_-1)+2*drawPB_;
 }

  static void testJacobian(const ExpressionTester<RotationExpression> & expressionTester) {
    auto rotExp = expressionTester.getExp();
    for (int i = 0; i < 3; i++) {
      Eigen::Vector3d p;
      p.setZero();
      p(i) = 1;
      RotationExpressionNodeFunctor functor(rotExp, p);

      sm::eigen::NumericalDiff<RotationExpressionNodeFunctor> numdiff(functor, expressionTester.getEps());

      /// Discern the size of the jacobian container
      Eigen::Matrix3d C = rotExp.toRotationMatrix();
      JacobianContainer Jc(3);
      rotExp.evaluateJacobians(Jc);
      Eigen::Matrix3d Cp_cross = sm::kinematics::crossMx(C * p);
      Jc.applyChainRule(Cp_cross);

      Eigen::VectorXd dp(Jc.cols());
      dp.setZero();
      Eigen::MatrixXd Jest = numdiff.estimateJacobian(dp);

      auto JcM = Jc.asSparseMatrix();
      sm::eigen::assertNear(Jc.asSparseMatrix(), Jest, expressionTester.getTolerance(), SM_SOURCE_FILE_POS, static_cast<std::stringstream&>(std::stringstream("Testing the RotationExpression's Jacobian (column=") << i <<")").str());
      if (expressionTester.getPrintResult()) {
        std::cout << "Jest=\n" << Jest << std::endl;
        std::cout << "Jc=\n" << JcM << std::endl;
      }
    }
  }

void Mesh::computeFaceGradients(Eigen::MatrixXd& gradients, const Eigen::VectorXd& u) const
{
    for (FaceCIter f = faces.begin(); f != faces.end(); f++) {
        
        Eigen::Vector3d gradient;
        gradient.setZero();
        Eigen::Vector3d normal = f->normal();
        normal.normalize();
        
        HalfEdgeCIter he = f->he;
        do {
            double ui = u(he->next->next->vertex->index);
            Eigen::Vector3d ei = he->next->vertex->position - he->vertex->position;
            
            gradient += ui * normal.cross(ei);
            
            he = he->next;
        } while (he != f->he);
        
        gradient /= (2.0 * f->area());
        gradient.normalize();
        
        gradients.row(f->index) = -gradient;
    }
}

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;
}

	/**
	 * @brief Handle RAW_IMU MAVlink message.
	 * Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU
	 * @param msg		Received Mavlink msg
	 * @param imu_raw	RAW_IMU msg
	 */
	void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
	{
		ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used.");
		has_raw_imu = true;

		if (has_hr_imu || has_scaled_imu)
			return;

		auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
		auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec);

		/** @note APM send SCALED_IMU data as RAW_IMU
		 */
		auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
					Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
		auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
		auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);

		if (m_uas->is_ardupilotmega()) {
			accel_frd *= MILLIG_TO_MS2;
			accel_flu *= MILLIG_TO_MS2;
		} else if (m_uas->is_px4()) {
			accel_frd *= MILLIMS2_TO_MS2;
			accel_flu *= MILLIMS2_TO_MS2;
		}

		publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);

		if (!m_uas->is_ardupilotmega()) {
			ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
			ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
			linear_accel_vec_flu.setZero();
			linear_accel_vec_frd.setZero();
		}

		/** Magnetic field data:
		 *  @snippet src/plugins/imu.cpp mag_field
		 */
		// [mag_field]
		auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
					Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
		// [mag_field]

		publish_mag(header, mag_field);
	}

Plane::Plane(Eigen::Vector3d pos, Eigen::Vector3d rotation, Eigen::Vector2d size, double alpha, double intervall, bool minimal){
	Eigen::Quaterniond q(1,0,0,0);
	Eigen::AngleAxisd rot_x(rotation[0],Eigen::Vector3d::UnitX());
	Eigen::AngleAxisd rot_y(rotation[1],Eigen::Vector3d::UnitY());
	Eigen::AngleAxisd rot_z(rotation[2],Eigen::Vector3d::UnitZ());
	q =  q* rot_x * rot_y * rot_z;
	inertiaAxisX = q.toRotationMatrix().col(0);
	inertiaAxisY = q.toRotationMatrix().col(1);
	normalVector = q.toRotationMatrix().col(2);
	
	Eigen::Vector3d S;
	Eigen::Matrix<double,9,1> C;
	Eigen::Matrix<double,9,1> productMatrix;
	S.setZero();
	C.setZero();
	productMatrix.setZero();

	bias = 0;	

	if(!minimal){
		for(double x = -size[0]/2.0; x < +size[0]/2.0; x+=intervall){
			for(double y = -size[1]/2.0; y <+size[1]/2.0; y+=intervall){
				Eigen::Vector3d p(x,y,0);
				p = (q*p)+pos;
				points.insert(Shared_Point(new Point(p[0],p[1],p[2])));
			}	
		}
	}else{
		Eigen::Vector3d p0(-size[0]/2.0,-size[1]/2.0,0);
		Eigen::Vector3d p1(-size[0]/2.0,size[1]/2.0,0);
		Eigen::Vector3d p2(size[0]/2.0,-size[1]/2.0,0);
		Eigen::Vector3d p3(size[0]/2.0,size[1]/2.0,0);
		p0 = (q*p0)+pos;
		p1 = (q*p1)+pos;
		p2 = (q*p2)+pos;
		p3 = (q*p3)+pos;
		points.insert(Shared_Point(new Point(p0[0],p0[1],p0[2])));
		points.insert(Shared_Point(new Point(p1[0],p1[1],p1[2])));
		points.insert(Shared_Point(new Point(p2[0],p2[1],p2[2])));
		points.insert(Shared_Point(new Point(p3[0],p3[1],p3[2])));
	}
	 
	for(int i=0;i<3;i++){
		centerOfMass[i] = pos[i];
		this->S[i]=S[i];
	}
	for(int i=0;i<9;i++){
		this->C[i]=C(i,0);
		this->productMatrix[i]=productMatrix(i,0);
	}
	rectangleCalculated = false;

    alpha_=alpha;
}

void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
    type = "Oriented";
    orientedPoints.clear();
    
    // compute mean
    Eigen::Vector3d center;
    center.setZero();
    for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
        center += v->position;
    }
    center /= (double)vertices.size();
    
    // adjust for mean and compute covariance
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        Eigen::Vector3d pAdg = v->position - center;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)vertices.size();

    // compute eigenvectors for the covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();

    // project min and max points on each principal axis
    double min1 = INFINITY, max1 = -INFINITY;
    double min2 = INFINITY, max2 = -INFINITY;
    double min3 = INFINITY, max3 = -INFINITY;
    double d = 0.0;
    eigenVectors.transpose();
    for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
        d = eigenVectors.row(0).dot(v->position);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = eigenVectors.row(1).dot(v->position);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = eigenVectors.row(2).dot(v->position);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // add points to vector
    orientedPoints.push_back(eigenVectors.row(0) * min1);
    orientedPoints.push_back(eigenVectors.row(0) * max1);
    orientedPoints.push_back(eigenVectors.row(1) * min2);
    orientedPoints.push_back(eigenVectors.row(1) * max2);
    orientedPoints.push_back(eigenVectors.row(2) * min3);
    orientedPoints.push_back(eigenVectors.row(2) * max3);
}

void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
    // compute mean
    Eigen::Vector3d cm;
    cm.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        cm += positions[i];
    }
    cm /= (double)positions.size();
    
    // adjust for mean and compute covariance matrix
    Eigen::Matrix3d covariance;
    covariance.setZero();
    for (size_t i = 0; i < positions.size(); i++) {
        Eigen::Vector3d pAdg = positions[i] - cm;
        covariance += pAdg * pAdg.transpose();
    }
    covariance /= (double)positions.size();
    
    // compute eigenvectors for covariance matrix
    Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
    
    // set axes
    eigenVectors.transpose();
    xAxis = eigenVectors.row(0);
    yAxis = eigenVectors.row(1);
    zAxis = eigenVectors.row(2);
    
    // project min and max points on each principal axis
    double min1 = INF, max1 = -INF;
    double min2 = INF, max2 = -INF;
    double min3 = INF, max3 = -INF;
    double d = 0.0;
    for (size_t i = 0; i < positions.size(); i++) {
        d = xAxis.dot(positions[i]);
        if (min1 > d) min1 = d;
        if (max1 < d) max1 = d;
        
        d = yAxis.dot(positions[i]);
        if (min2 > d) min2 = d;
        if (max2 < d) max2 = d;
        
        d = zAxis.dot(positions[i]);
        if (min3 > d) min3 = d;
        if (max3 < d) max3 = d;
    }
    
    // set center and halflengths
    center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
    halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}

int collideSphereSphere(CollisionObject* o1, CollisionObject* o2, const double& _r0, const Eigen::Isometry3d& c0,
                        const double& _r1, const Eigen::Isometry3d& c1,
                        CollisionResult& result)
{
  double r0 = _r0;
  double r1 = _r1;
  double rsum = r0 + r1;
  Eigen::Vector3d normal = c0.translation() - c1.translation();
  double normal_sqr = normal.squaredNorm();

  if ( normal_sqr > rsum * rsum )
  {
    return 0;
  }

  r0 /= rsum;
  r1 /= rsum;

  Eigen::Vector3d point = r1 * c0.translation() + r0 * c1.translation();
  double penetration;

  if (normal_sqr < DART_COLLISION_EPS)
  {
    normal.setZero();
    penetration = rsum;

    Contact contact;
    contact.collisionObject1 = o1;
    contact.collisionObject2 = o2;
    contact.point = point;
    contact.normal = normal;
    contact.penetrationDepth = penetration;
    result.addContact(contact);
    return 1;
  }

  normal_sqr = sqrt(normal_sqr);
  normal *= (1.0/normal_sqr);
  penetration = rsum - normal_sqr;

  Contact contact;
  contact.collisionObject1 = o1;
  contact.collisionObject2 = o2;
  contact.point = point;
  contact.normal = normal;
  contact.penetrationDepth = penetration;
  result.addContact(contact);
  return 1;

}

 struct PitchRollState
 boxplus(const Eigen::VectorXd& offset) const
 {
     struct PitchRollState out;
     Eigen::Vector3d omega;
     omega.setZero();
     omega = offset.segment<3>(0);
     out.Omega = Omega + offset.segment<3>(3);
     out.GyroBias = GyroBias + offset.segment<3>(6);
     Sophus::SO3d rot=Sophus::SO3d::exp(omega);
     out.Orientation = rot*Orientation;
     out.AccelBias = AccelBias + offset.segment<3>(9);
     out.Covariance = Covariance;
     return out;
 };