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> ¤tTranslation, 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> ¢roid) { // 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; }
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]); } } }
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); } }
//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; }
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; }
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; }
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); } }
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; }
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; }
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; }
template <typename PointT, typename Scalar> inline unsigned int pcl::compute3DCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix<Scalar, 4, 1> ¢roid) { 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); } }
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; }
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; }
template <typename PointT, typename Scalar> inline void pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud, Eigen::Matrix<Scalar, Eigen::Dynamic, 1> ¢roid) { 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); }
// 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; }