void reset()
 {
   _x.setZero();
   _u.setZero();
   _K.setZero();
   _V = _Q;
 }
Esempio n. 2
0
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;
}
Esempio n. 3
0
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);
}
Esempio n. 4
0
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;
}
Esempio n. 7
0
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]);
    }
  }

}
Esempio n. 8
0
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;
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
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; 
}
Esempio n. 11
0
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();
}
Esempio n. 13
0
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;
}
Esempio n. 14
0
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;
}
Esempio n. 15
0
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; 
	  
	 
	  
}
Esempio n. 16
0
IGL_INLINE void igl::PolyVectorFieldFinder<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(2*n+1,1);
    polyCoeff[0] = 1.;
    int sign = 1;
    for (int k =0; k<n; ++k)
    {
      sign = -sign;
      int degree = 2*(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);

    Eigen::VectorXi done; done.setZero(2*n,1);

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> u(n,1);
    int ind =0;
    for (int k=0; k<2*n; ++k)
    {
      if (done[k])
        continue;
      u[ind] = roots[k];
      done[k] = 1;

      int mini = -1;
      double mind = 1e10;
      for (int l =k+1; l<2*n; ++l)
      {
        double dist = abs(roots[l]+u[ind]);
        if (dist<mind)
        {
          mind = dist;
          mini = l;
        }
      }
      done[mini] = 1;
      ind ++;
    }
    for (int k=0; k<n; ++k)
    {
      pv[k](i,0) = real(u[k]);
      pv[k](i,1) = imag(u[k]);
    }
  }

}
Esempio n. 17
0
 void copy_results()
 {
   derivatives_.setZero();
   for(size_t i=0; i<expressions_.size(); ++i)
   {
     values_(i, 0) = expressions_[i]->value();
     for(size_t j=0; j<expressions_[i]->number_of_derivatives(); ++j)
       derivatives_(i,j) = expressions_[i]->derivative(j);
   }
 }
Esempio n. 18
0
//TODO msati: Optimize this portion
void ParticleManager::getInvMassMatrix( Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic >& invMassMatrix )
{
	invMassMatrix.resize( m_particles.size() * 3, m_particles.size() * 3 );
	invMassMatrix.setZero();
	for (int i = 0; i < m_particles.size(); i++)
	{
		for (int j = 0; j < 3; j++)
		{
			invMassMatrix.row(3*i+j).array()[3*i+j] = 1/(m_particles[i]->mMass);
		}
	}
}
//------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement(
    const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {

  const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion

  // Update preintegrated measurements.
  Matrix3 D_incrR_integratedOmega; // Right jacobian computed at theta_incr
  Matrix9 F_9x9; // overall Jacobian wrt preintegrated measurements (df/dx)
  Matrix93 G1,G2;
  PreintegrationBase::update(measuredAcc, measuredOmega, deltaT,
      &D_incrR_integratedOmega, &F_9x9, &G1, &G2);

  // Update preintegrated measurements covariance: as in [2] we consider a first order propagation that
  // can be seen as a prediction phase in an EKF framework. In this implementation, contrarily to [2] we
  // consider the uncertainty of the bias selection and we keep correlation between biases and preintegrated measurements
  /* ----------------------------------------------------------------------------------------------------------------------- */

  // Single Jacobians to propagate covariance
  Matrix3 H_vel_biasacc = -dRij * deltaT;
  Matrix3 H_angles_biasomega = -D_incrR_integratedOmega * deltaT;

  // overall Jacobian wrt preintegrated measurements (df/dx)
  Eigen::Matrix<double,15,15> F;
  F.setZero();
  F.block<9, 9>(0, 0) = F_9x9;
  F.block<3, 3>(0, 12) = H_angles_biasomega;
  F.block<3, 3>(6, 9) = H_vel_biasacc;
  F.block<6, 6>(9, 9) = I_6x6;

  // first order uncertainty propagation
  // Optimized matrix multiplication   (1/deltaT) * G * measurementCovariance * G.transpose()
  Eigen::Matrix<double,15,15> G_measCov_Gt;
  G_measCov_Gt.setZero(15, 15);

  // BLOCK DIAGONAL TERMS
  D_t_t(&G_measCov_Gt) = deltaT * p().integrationCovariance;
  D_v_v(&G_measCov_Gt) = (1 / deltaT) * (H_vel_biasacc)
      * (p().accelerometerCovariance + p().biasAccOmegaInit.block<3, 3>(0, 0))
      * (H_vel_biasacc.transpose());
  D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
      * (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
      * (H_angles_biasomega.transpose());
  D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance;
  D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance;

  // OFF BLOCK DIAGONAL TERMS
  Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
      * H_angles_biasomega.transpose();
  D_v_R(&G_measCov_Gt) = temp;
  D_R_v(&G_measCov_Gt) = temp.transpose();
  preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
}
Esempio n. 20
0
IGL_INLINE bool igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::
                     solve(const Eigen::VectorXi &isConstrained,
                           const Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &cfW,
                           const Eigen::VectorXi &rootsIndex,
                           Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, Eigen::Dynamic> &output)
{

  // polynomial is of the form:
  // z^(2n) +
  // -c[0]z^(2n-1) +
  // c[1]z^(2n-2) +
  // -c[2]z^(2n-3) +
  // ... +
  // (-1)^n c[n-1]

  std::vector<Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>> coeffs(n,Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1>::Zero(numF, 1));

  for (int i =0; i<n; ++i)
  {
    int degree = i+1;

    Eigen::Matrix<std::complex<typename DerivedV::Scalar>, Eigen::Dynamic,1> Ck;
    getGeneralCoeffConstraints(isConstrained,
                               cfW,
                               i,
                               rootsIndex,
                               Ck);

    Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > DD;
    computeCoefficientLaplacian(degree, DD);
    Eigen::SparseMatrix<std::complex<typename DerivedV::Scalar> > f; f.resize(numF,1);

    if (isConstrained.sum() == numF)
      coeffs[i] = Ck;
    else
      minQuadWithKnownMini(DD, f, isConstrained, Ck, coeffs[i]);
  }

  std::vector<Eigen::Matrix<typename DerivedV::Scalar, Eigen::Dynamic, 2> > pv;
  setFieldFromGeneralCoefficients(coeffs, pv);

  output.setZero(numF,3*n);
  for (int fi=0; fi<numF; ++fi)
  {
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b1 = B1.row(fi);
    const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> &b2 = B2.row(fi);
    for (int i=0; i<n; ++i)
      output.block(fi,3*i, 1, 3) = pv[i](fi,0)*b1 + pv[i](fi,1)*b2;
  }
  return true;
}
Esempio n. 21
0
void KalmanFilter::measurementUpdate(const Measurement_t &meas, double dt)
{
  Eigen::Matrix<double, n_meas, n_states> H;
  H.setZero();
  H(0, 0) = 1;
  H(1, 1) = 1;
  H(2, 2) = 1;

  const Eigen::Matrix<double, n_states, n_meas> K = P * H.transpose() *
      (H*P*H.transpose() + R).inverse();
  const Measurement_t inno = meas - H*x;
  x += K*inno;
  P = (ProcessCov_t::Identity() - K*H) * P;
}
Esempio n. 22
0
void cholesky_downdate (Eigen::Matrix<double, N, N>& L, Eigen::Matrix<double, N, 1> p) {

	L.template triangularView<Eigen::Lower>().solveInPlace(p);

	assert(p.squaredNorm() < 1); // otherwise the downdate would destroy positive definiteness.
	double rho = std::sqrt (1 - p.squaredNorm());

	Eigen::JacobiRotation<double> rot;
	Eigen::Matrix<double, N, 1> temp;
	temp.setZero();

	for (int i = N-1; i >= 0; --i) {
		rot.makeGivens(rho, p(i), &rho), p(i) = 0;
		apply_jacobi_rotation(temp, L.col(i), rot);
	}
}
Esempio n. 23
0
bool KFD_PosVelAcc::positionZObservation(double z, double error, double rejection_threshold) 
{
    Eigen::Matrix<double,1,1>  dif;
    dif(0,0) = position_world(2) - z; 
    Eigen::Matrix<double,1 ,1> cov;
    cov(0,0) = error; 
    Eigen::Matrix<double, 1, StatePosVelAcc::SIZE> H; 
    H.setZero(); 
    H(0,2) = 1; 
    bool reject =  filter->correctionChiSquare<1,1>( dif,  cov, H, rejection_threshold );
    x.vector() = filter->x;
      
    correct_state();
      
    return reject; 

}
Esempio n. 24
0
bool KFD_PosVelAcc::velocityObservation(Eigen::Vector3d velocity, Eigen::Matrix3d covariance, float reject_velocity_threshol)
{
  
      Eigen::Vector3d  velocity_diference = velocity_world - velocity; 
      //std::cout << " Vel dif " << velocity_diference << std::endl; 
      Eigen::Matrix<double, _VEL_MEASUREMENT_SIZE, StatePosVelAcc::SIZE> H; 
      H.setZero(); 
      H.block<3,3>(0,3) = Eigen::Matrix3d::Identity(); 
      //position_world = position;
      bool reject =  filter->correctionChiSquare<_VEL_MEASUREMENT_SIZE,_VEL_DEGREE_OF_FREEDOM>( velocity_diference,  covariance, H, reject_velocity_threshol );
      x.vector() = filter->x;
      
      correct_state();
      
      return reject; 
      
}
Esempio n. 25
0
Eigen::Matrix< float, 3, 3 > removeTranslationVectorFromMatrix(Eigen::Matrix<float,4,4> m)
{
  Eigen::Matrix< float, 3, 3 > result;
  result.setZero();
  result(0,0) = m(0,0);
  result(1,0) = m(1,0);
  result(2,0) = m(2,0);

  result(0,1) = m(0,1);
  result(1,1) = m(1,1);
  result(2,1) = m(2,1);

  result(0,2) = m(0,2);
  result(1,2) = m(1,2);
  result(2,2) = m(2,2);
  return result;
}
Esempio n. 26
0
template <typename PointT, typename Scalar> inline unsigned int
pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, 
                        Eigen::Matrix<Scalar, 4, 1> &centroid)
{
  if (cloud.empty ())
    return (0);

  // Initialize to 0
  centroid.setZero ();
  // For each point in the cloud
  // If the data is dense, we don't need to check for NaN
  if (cloud.is_dense)
  {
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      centroid[0] += cloud[i].x;
      centroid[1] += cloud[i].y;
      centroid[2] += cloud[i].z;
    }
    centroid[3] = 0;
    centroid /= static_cast<Scalar> (cloud.size ());

    return (static_cast<unsigned int> (cloud.size ()));
  }
  // NaN or Inf values could exist => check for them
  else
  {
    unsigned cp = 0;
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      // Check if the point is invalid
      if (!isFinite (cloud [i]))
        continue;

      centroid[0] += cloud[i].x;
      centroid[1] += cloud[i].y;
      centroid[2] += cloud[i].z;
      ++cp;
    }
    centroid[3] = 0;
    centroid /= static_cast<Scalar> (cp);

    return (cp);
  }
}
Esempio n. 27
0
Eigen::Matrix<double, 7, 1> EKF::statePrediction(float* wArray, float dt) {
	Eigen::Matrix<double, 7, 1> F = Eigen::Matrix<double, 7, 1>::Identity();
	Eigen::Matrix<double, 3, 1> w;
	w << wArray[0], wArray[1], wArray[2];

	F(4) = this->x_aposteriori(4);
	F(5) = this->x_aposteriori(5);
	F(6) = this->x_aposteriori(6);

	Eigen::Matrix<double, 4, 4> A;
	A.setZero();

	// A 1st row
	A(0, 0) = 1.0;
	A(0, 1) = -0.5 * (w(0) - F(4)) * dt;
	A(0, 2) = -0.5 * (w(1) - F(5)) * dt;
	A(0, 3) = -0.5 * (w(2) - F(6)) * dt;

	// A 2nd row
	A(1, 0) = 0.5 * (w(0) - F(4)) * dt;
	A(1, 1) = 1;
	A(1, 2) = 0.5 * (w(2) - F(6)) * dt;
	A(1, 3) = -0.5 * (w(1) - F(5)) * dt;

	// A 3rd row
	A(2, 0) = 0.5 * (w(1) - F(5)) * dt;
	A(2, 1) = -0.5 * (w(2) - F(6)) * dt;
	A(2, 2) = 1;
	A(2, 3) = 0.5 * (w(0) - F(4)) * dt;

	// A 4th row
	A(3, 0) = 0.5 * (w(2) - F(6)) * dt;
	A(3, 1) = 0.5 * (w(1) - F(5)) * dt;
	A(3, 2) = -0.5 * (w(0) - F(4)) * dt;
	A(3, 3) = 1;

	// Only (1:4)
	Eigen::Matrix<double, 4, 1> x = A * (this->x_aposteriori).block<4, 1>(0, 0);

	for (int i=0;i<4;i++)
		F(i) = x(i);
	return F;
}
Esempio n. 28
0
bool KFD_PosVelAcc::positionObservation(Eigen::Vector3d position, Eigen::Matrix3d covariance, float reject_position_threshol)
{
  
      Eigen::Vector3d  position_diference = position_world - position; 

      Eigen::Matrix<double, _POS_MEASUREMENT_SIZE, StatePosVelAcc::SIZE> H; 
      H.setZero(); 
      H.block<3,3>(0,0) = Eigen::Matrix3d::Identity(); 
      
      //position_world = position;
      bool reject =  filter->correctionChiSquare<_POS_MEASUREMENT_SIZE,_POS_DEGREE_OF_FREEDOM>( position_diference,  covariance,H, reject_position_threshol );
      
      x.vector() = filter->x;
      
       correct_state();
      
      return reject; 
      
}
Esempio n. 29
0
template <typename PointT, typename Scalar> inline void
pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, 
                        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> &centroid)
{
  typedef typename pcl::traits::fieldList<PointT>::type FieldList;

  // Get the size of the fields
  centroid.setZero (boost::mpl::size<FieldList>::value);

  if (cloud.empty ())
    return;
  // Iterate over each point
  int size = static_cast<int> (cloud.size ());
  for (int i = 0; i < size; ++i)
  {
    // Iterate over each dimension
    pcl::for_each_type<FieldList> (NdCentroidFunctor<PointT, Scalar> (cloud[i], centroid));
  }
  centroid /= static_cast<Scalar> (size);
}
Esempio n. 30
0
// Gets current projection matrix (= PerspectiveMatrix * CameraPoseMatrix)
Eigen::Matrix<double, 3, 4> get_projection(look3d::PlaneTracker& tracker) {
    std::vector<double> cam_params = tracker.GetCameraParams();
    Eigen::Matrix3d intrinsics = Eigen::Matrix3d::Identity();
    intrinsics(0, 0) = cam_params[0];
    intrinsics(1, 1) = cam_params[1];
    intrinsics(0, 2) = cam_params[2];
    intrinsics(1, 2) = cam_params[3];

    std::vector<double> m = tracker.GetCurrentPose();
    Eigen::Matrix<double, 4, 4> mtr;
        mtr << m[0], m[1], m[2], m[3],
            m[4], m[5], m[6], m[7],
            m[8], m[9], m[10], m[11],
            m[12], m[13], m[14], m[15];

    Eigen::Matrix<double, 3, 4> pose;
    pose.setZero();
    pose.block<3, 3>(0, 0) = mtr.block<3, 3>(0, 0);
    pose.block<3, 1>(0, 3) = mtr.block<3, 1>(0, 3);

    Eigen::Matrix<double, 3, 4> projection = intrinsics * pose;
    return projection;
}