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