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

 void reset()
 {
   _x.setZero();
   _u.setZero();
   _K.setZero();
   _V = _Q;
 }

void ErrorStateKF<Scalar>::predict(const ErrorStateKF<Scalar>::vec3 &wbody,
                                   const ErrorStateKF<Scalar>::mat3 &varW,
                                   const ErrorStateKF<Scalar>::vec3 &abody,
                                   const ErrorStateKF<Scalar>::mat3 &varA, 
                                   Scalar dt) {

  const vec3 ah = abody - ba_; //  corrected inputs
  const vec3 wh = wbody - bg_;
  const mat3 wRb = q_.matrix(); //  current orientation

  //  integrate state forward w/ euler equations
  const quat dq =
      q_ * quat(0, wh[0] * 0.5, wh[1] * 0.5, wh[2] * 0.5);
  q_.w() += dq.w() * dt;
  q_.x() += dq.x() * dt;
  q_.y() += dq.y() * dt;
  q_.z() += dq.z() * dt;
  q_.normalize();
  p_ += v_ * dt;
  v_ += (wRb * ah + vec3(0, 0, -g_)) * dt;

  //  construct error-state jacobian
  mat15 F;
  F.setZero();

  ///  dth = [wh] x dth - bg
  F.template block<3, 3>(0, 0) = -kr::skewSymmetric(wh);
  F.template block<3, 3>(0, 3) = -mat3::Identity();

  //  dv = -R[ah] x dth - R[ba]
  F.template block<3, 3>(6, 0) = -wRb * kr::skewSymmetric(ah);
  F.template block<3, 3>(6, 9) = -wRb;

  //  dp = dv
  F.template block<3, 3>(12, 6).setIdentity();

  //  form process covariance matrix
  Eigen::Matrix<Scalar, 12, 12> Q;
  Q.setZero();
  Q.template block<3, 3>(0, 0) = varW;
  Q.template block<3, 3>(3, 3) = Qbg_;
  Q.template block<3, 3>(6, 6) = varA;
  Q.template block<3, 3>(9, 9) = Qba_;

  Eigen::Matrix<Scalar,15,12> G;
  G.setZero();

  //  angular vel. variance on error state angle
  G.template block<3, 3>(0, 0) = mat3::Identity() * -1;
  G.template block<3, 3>(3, 3).setIdentity();
  G.template block<3, 3>(6, 6) = -wRb; //  acceleration on linear velocity
  G.template block<3, 3>(9, 9).setIdentity();

  //  integrate covariance forward
  P_ += (F * P_ + P_ * F.transpose() + G * Q * G.transpose()) * dt;
}

template<typename PointSource, typename PointTarget> double
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeDerivatives (Eigen::Matrix<double, 6, 1> &score_gradient,
                                                                                 Eigen::Matrix<double, 6, 6> &hessian,
                                                                                 PointCloudSource &trans_cloud,
                                                                                 Eigen::Matrix<double, 6, 1> &p,
                                                                                 bool compute_hessian)
{
  // Original Point and Transformed Point
  PointSource x_pt, x_trans_pt;
  // Original Point and Transformed Point (for math)
  Eigen::Vector3d x, x_trans;
  // Occupied Voxel
  TargetGridLeafConstPtr cell;
  // Inverse Covariance of Occupied Voxel
  Eigen::Matrix3d c_inv;

  score_gradient.setZero ();
  hessian.setZero ();
  double score = 0;

  // Precompute Angular Derivatives (eq. 6.19 and 6.21)[Magnusson 2009]
  computeAngleDerivatives (p);

  // Update gradient and hessian for each point, line 17 in Algorithm 2 [Magnusson 2009]
  for (size_t idx = 0; idx < input_->points.size (); idx++)
  {
    x_trans_pt = trans_cloud.points[idx];

    // Find nieghbors (Radius search has been experimentally faster than direct neighbor checking.
    std::vector<TargetGridLeafConstPtr> neighborhood;
    std::vector<float> distances;
    target_cells_.radiusSearch (x_trans_pt, resolution_, neighborhood, distances);

    for (typename std::vector<TargetGridLeafConstPtr>::iterator neighborhood_it = neighborhood.begin (); neighborhood_it != neighborhood.end (); neighborhood_it++)
    {
      cell = *neighborhood_it;
      x_pt = input_->points[idx];
      x = Eigen::Vector3d (x_pt.x, x_pt.y, x_pt.z);

      x_trans = Eigen::Vector3d (x_trans_pt.x, x_trans_pt.y, x_trans_pt.z);

      // Denorm point, x_k' in Equations 6.12 and 6.13 [Magnusson 2009]
      x_trans -= cell->getMean ();
      // Uses precomputed covariance for speed.
      c_inv = cell->getInverseCov ();

      // Compute derivative of transform function w.r.t. transform vector, J_E and H_E in Equations 6.18 and 6.20 [Magnusson 2009]
      computePointDerivatives (x);
      // Update score, gradient and hessian, lines 19-21 in Algorithm 2, according to Equations 6.10, 6.12 and 6.13, respectively [Magnusson 2009]
      score += updateDerivatives (score_gradient, hessian, x_trans, c_inv, compute_hessian);

    }
  }
  return (score);
}

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 singleArmTranslationConstraint:: constraintUpdate(const Eigen::Matrix<double, 3, 1> &currentTranslation,
						       const Eigen::Matrix<double, 3, 1> &desiredTranslation,
						       const Eigen::Matrix<double, 3, 1> &timeDerivative,
						       const Eigen::Matrix<double, 6, DOF> &gradient
						       ){

  // update function measure
  Eigen::Matrix<double, 3, 1> measure;
  measure.setZero();
  measure = currentTranslation - desiredTranslation;
  


  measureNorm_ = measure.norm();

  // update the constraint gradient
  Eigen::Matrix<double, 3, DOF> tempGradient;
  tempGradient.setZero();
  tempGradient = gradient.block<3,DOF>(0,0);
    
  // set up the gradient
  for(int j = 0; j<dim_; j++){
    for(int i = 0; i<DOF; i++){
      // int k = i + sIndex_;
      modelPointer_->chgCoeff(
			      *constrPArray_[j],
			      *(*jointVVarPArrayPointer_)[i],
			      tempGradient(j, i)
			      // gradient(j, i)
			      );	
    }// finish one constraint
  }// finish all constraints


  Eigen::Matrix<double, 3, 1> rhs;
  rhs.setZero();
  // update the rhs
  for(int j = 0; j<dim_; j++){
    
    rhs(j) =  - gain_*( currentTranslation(j) - desiredTranslation(j) ); 

    constrPArray_[j]->set(GRB_DoubleAttr_RHS, 
			  rhs(j)
			  );

  }

  // std::cout<<"error   X: "<<measure(0)           <<" error   Y: "<<measure(1)           <<" error   Z: "<<measure(2)<<std::endl;
  // std::cout<<"error   norm: "<<measureNorm_<<std::endl;
  // std::cout<<"seperation line:---------------------------- "<<std::endl;

}

BaseSensorDataNode* SyncSensorDataNodeMaker::makeNode(MapManager* manager, BaseSensorData* data) {
    SynchronizedSensorData* sdata = dynamic_cast<SynchronizedSensorData*>(data);
    if (! sdata)
        return 0;
    SyncSensorDataNode* snode = new SyncSensorDataNode(manager, sdata);
    for (size_t i = 0; i<sdata->sensorDatas.size(); i++) {
        IMUData* imu = dynamic_cast<IMUData*>(sdata->sensorDatas[i]);
        if (imu) {
            MapNodeUnaryRelation* imuRel = new MapNodeUnaryRelation(manager);
            imuRel->nodes()[0] = snode;
            Eigen::Isometry3d t;
            t.setIdentity();
            t.linear() = imu->orientation().toRotationMatrix();
            Eigen::Matrix<double, 6, 6> info;
            info.setZero();
            info.block<3,3>(3,3) = imu->orientationCovariance().inverse();
            //info.block<3,3>(3,3).setIdentity();
            imuRel->setTransform(t);
            imuRel->setInformationMatrix(info);
            snode->setImu(imuRel);
            break;
        }
    }
    return snode;
}

IGL_INLINE void igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::setFieldFromGeneralCoefficients(const  std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>> &coeffs,
                                                            std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2>> &pv)
{
  pv.assign(n, Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2>::Zero(numF, 2));
  for (int i = 0; i <numF; ++i)
  {

    //    poly coefficients: 1, 0, -Acoeff, 0, Bcoeff
    //    matlab code from roots (given there are no trailing zeros in the polynomial coefficients)
    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> polyCoeff;
    polyCoeff.setZero(n+1,1);
    polyCoeff[0] = 1.;
    int sign = 1;
    for (int k =0; k<n; ++k)
    {
      sign = -sign;
      int degree = k+1;
      polyCoeff[degree] = (1.*sign)*coeffs[k](i);
    }

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> roots;
    igl::polyRoots<std::complex<typename DerivedV::Scalar>, typename DerivedV::Scalar >(polyCoeff,roots);
    for (int k=0; k<n; ++k)
    {
      pv[k](i,0) = real(roots[k]);
      pv[k](i,1) = imag(roots[k]);
    }
  }

}

Eigen::VectorXd ClosedFormCalibration::solveLagrange(const Eigen::Matrix<double,5,5>& M, double lambda)
{
  // A = M * lambda*W (see paper)
  Eigen::Matrix<double,5,5> A;
  A.setZero();
  A(3,3) = A(4,4) = lambda;
  A.noalias() += M;

  // compute the kernel of A by SVD
  Eigen::JacobiSVD< Eigen::Matrix<double,5,5> > svd(A, ComputeFullV);
  Eigen::VectorXd result = svd.matrixV().col(4);
  //for (int i = 0; i < 5; ++i)
  //std::cout << "singular value " << i << " "  << svd.singularValues()(i) << std::endl;
  //std::cout << "kernel base " << result << std::endl;

  // enforce the conditions
  // x_1 > 0
  if (result(0) < 0.)
    result *= -1;
  // x_4^2 + x_5^2 = 1
  double scale = sqrt(pow(result(3), 2) + pow(result(4), 2));
  result /= scale;

  return result;
}

template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (ConstCloudIterator<PointT> &cloud_iterator,
                        Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  // Initialize to 0
  centroid.setZero ();
  
  unsigned cp = 0;

  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  while (cloud_iterator.isValid ())
  {
    // Check if the point is invalid
    if (!pcl_isfinite (cloud_iterator->x) ||
        !pcl_isfinite (cloud_iterator->y) ||
        !pcl_isfinite (cloud_iterator->z))
      continue;
    centroid[0] += cloud_iterator->x;
    centroid[1] += cloud_iterator->y;
    centroid[2] += cloud_iterator->z;
    ++cp;
    ++cloud_iterator;
  }
  centroid[3] = 0;
  centroid /= static_cast<Scalar> (cp);
  return (cp);
}

void LipmVarHeightPlanner::getAB(const double x0[6], const double u[3], double z0, Eigen::Matrix<double,6,6> &A, Eigen::Matrix<double,6,3> &B) const
{
  A.setIdentity();
  B.setZero();
 
  double x = x0[XX];
  double y = x0[YY];
  double z = x0[ZZ] - z0;

  double px = u[XX];
  double py = u[YY];
  double F = u[ZZ];

  // A
  A(0, 3) = _dt;
  A(1, 4) = _dt;
  A(2, 5) = _dt;

  A(3, 0) = F*_dt/(_mass*z);
  A(3, 2) = F*_dt*(px-x)/(_mass*z*z);

  A(4, 1) = F*_dt/(_mass*z);
  A(4, 2) = F*_dt*(py-y)/(_mass*z*z);
 
  // B
  B(3, 0) = -F*_dt/(_mass*z); 
  B(3, 2) = -(px-x)*_dt/(_mass*z); 
  
  B(4, 1) = -F*_dt/(_mass*z); 
  B(4, 2) = -(py-y)*_dt/(_mass*z);

  B(5, 2) = _dt/_mass; 
}

void GIPCam::SetParams(float fx, float fy, float cx, float cy, uint width, uint height) {
	// TODO: NOTE: WE IGNORE THE SKEW TERM IN THE GIVEN K MATRIX

	const float scale_x = 240.f; //      // width*( 1.f/(MAX_X-MIN_X) ); // width*0.5.
	const float scale_y = 240.f; // height*( 1.f/(MAX_Y-MIN_Y) ); // height*0.75*0.5.
	
	const float new_fx = scale_x * fx;
	const float new_fy = scale_y * fy;

	const int new_cx = (int)(scale_x*cx + 180.f); // (float)width/2;
	const int new_cy = (int)(scale_y*cy + 240.f); //(float)height/2;

	//m_params.m_height = height;
	//m_params.m_width = width;
	m_params.m_intrinsic.Set(-new_fx, -new_fy, new_cx, new_cy);

	Eigen::Matrix<float, 3, 3, Eigen::RowMajor> intrinsic;
	intrinsic.setZero();
	intrinsic(0,0) = m_params.m_intrinsic.m_fx;
	intrinsic(1,1) = m_params.m_intrinsic.m_fy;
	intrinsic(2,2) = 1.f;
	intrinsic(0,2) = m_params.m_intrinsic.m_cx;
	intrinsic(1,2) = m_params.m_intrinsic.m_cy;

	Eigen::Matrix<float, 3, 3, Eigen::RowMajor> invIntrinsic = intrinsic.inverse();

	m_params.m_invIntrinsic.Set(invIntrinsic(0,0), invIntrinsic(1,1), invIntrinsic(0,2), invIntrinsic(1,2));
}

void SlamSystem::createNewCurrentKeyframe(std::shared_ptr<Frame> newKeyframeCandidate)
{
	if(enablePrintDebugInfo && printThreadingInfo)
		printf("CREATE NEW KF %d from %d\n", newKeyframeCandidate->id(), currentKeyFrame->id());


	if(SLAMEnabled)
	{
		// add NEW keyframe to id-lookup
		keyFrameGraph->idToKeyFrameMutex.lock();
		keyFrameGraph->idToKeyFrame.insert(std::make_pair(newKeyframeCandidate->id(), newKeyframeCandidate));
		keyFrameGraph->idToKeyFrameMutex.unlock();
	}

	// propagate & make new.
	map->createKeyFrame(newKeyframeCandidate.get());

	if(printPropagationStatistics)
	{

		Eigen::Matrix<float, 20, 1> data;
		data.setZero();
		data[0] = runningStats.num_prop_attempts / ((float)width*height);
		data[1] = (runningStats.num_prop_created + runningStats.num_prop_merged) / (float)runningStats.num_prop_attempts;
		data[2] = runningStats.num_prop_removed_colorDiff / (float)runningStats.num_prop_attempts;

        //outputWrapper->publishDebugInfo(data);
	}

    currentKeyFrameMutex.lock();
	currentKeyFrame = newKeyframeCandidate;
	currentKeyFrameMutex.unlock();
}

inline Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns>
List(const impl::RowVector<Scalar, Columns>& head, Tail... tail)
{
    Eigen::Matrix<Scalar, sizeof...(Tail) + 1, Columns> matrix;
    matrix.setZero();
    impl::fillMatrix(matrix, 0, head, tail...);
    return matrix;
}

Eigen::Matrix<double, TDim, TDim> NuTo::ContinuumElementIGA<TDim>::CalculateJacobianParametricSpaceIGA() const
{
    Eigen::Matrix<double, TDim, TDim> jac;
    jac.setZero(TDim, TDim);
    for (int i = 0; i < TDim; i++)
        jac(i, i) = 0.5 * (mKnots(i, 1) - mKnots(i, 0));

    return jac;
}

void KFD_PosVelAcc::predict(Eigen::Vector3d acc_intertial, double dt, Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> process_noise)
{
 	  
	  /** Inertial Navigation */ 
	  
	  //gravity correction 
	  Eigen::Vector3d gravity_world = Eigen::Vector3d::Zero(); 
	  gravity_world.z() = 9.871; 
	  
	  //graviy in inertial frame  
	  Eigen::Vector3d gravity_inertial =  R_input_to_world.inverse() * gravity_world; 

	  //gravity correction 
	  acc_intertial = acc_intertial - gravity_inertial; 
	  
	  //inertial navigation 
	  velocity_world = velocity_world +   acc_intertial * dt; 
	  position_world = position_world  + R_input_to_world * velocity_world * dt; 
	  
	  /** Kalman Filter */ 
	  
	  //sets the transition matrix 
	  Eigen::Matrix<double, StatePosVelAcc::SIZE, StatePosVelAcc::SIZE> F;
	  F.setZero();
	  
// 	  F.block<3,3>(0,3) =  Eigen::Matrix3d::Identity(); 
// 	  F.block<3,3>(3,6) =  Eigen::Matrix3d( R_input_to_world ); 
	  F.block<3,3>(0,3) =  Eigen::Matrix3d( R_input_to_world );
	  F.block<3,3>(3,6) =  Eigen::Matrix3d::Identity(); 
	  
	  F(6,6) = - 0.00136142583242962/dt;
	  F(7,7) = - 0.00143792179010407/dt;
	  F(8,8) = - 0.000155846795306766/dt;

	 //std::cout << "F \n" << F << std::endl; 
	/* std::cout 
	      << "0 0 0 1 0 0 0 0 0 \n" 
	      << "0 0 0 0 1 0 0 0 0 \n"  
	      << "0 0 0 0 0 1 0 0 0 \n"  
	      << "0 0 0 0 0 0 R R R \n" 
	      << "0 0 0 0 0 0 R R R \n"  
	      << "0 0 0 0 0 0 R R R \n"  
	      << "0 0 0 0 0 0 0 0 0 \n" 
	      << "0 0 0 0 0 0 0 0 0 \n" 
	      << "0 0 0 0 0 0 0 0 0 \n" 
	      <<std::endl;*/
	  
	  //updates the Kalman Filter 
	  filter->predictionDiscrete( F, process_noise, dt); 

	  //get the updated values 
	  x.vector()=filter->x; 
	  
	 
	  
}