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;
}
typename ExponentialPlusPiecewisePolynomial<CoefficientType>::ValueType ExponentialPlusPiecewisePolynomial<CoefficientType>::value(double t) const
{
  int segment_index = getSegmentIndex(t);

  Eigen::Matrix<double, Eigen::Dynamic, 1> ret = piecewise_polynomial_part.value(t);
  double tj = getStartTime(segment_index);
  auto exponential = (A * (t - tj)).eval().exp().eval();
  ret.noalias() += K * exponential * alpha.col(segment_index);
  return ret;
}
Example #3
0
void IDIM::computeY(const MultiBody& mb, const MultiBodyConfig& mbc)
{
	const std::vector<Body>& bodies = mb.bodies();
	const std::vector<Joint>& joints = mb.joints();
	const std::vector<int>& pred = mb.predecessors();

	Eigen::Matrix<double, 6, 10> bodyFPhi;
	for(int i = static_cast<int>(bodies.size()) - 1; i >= 0; --i)
	{
		const sva::MotionVecd& vb_i = mbc.bodyVelB[i];
		Eigen::Matrix<double, 6, 10> vb_i_Phi(IMPhi(vb_i));

		bodyFPhi.noalias() = IMPhi(mbc.bodyAccB[i]);
		// bodyFPhi += vb_i x* IMPhi(vb_i)
		// is faster to convert each col in a ForceVecd
		// than using sva::vector6ToCrossDualMatrix
		for(int c = 0; c < 10; ++c)
		{
			bodyFPhi.col(c).noalias() +=
				(vb_i.crossDual(sva::ForceVecd(vb_i_Phi.col(c)))).vector();
		}

		int iDofPos = mb.jointPosInDof(i);

		Y_.block(iDofPos, i*10, joints[i].dof(), 10).noalias() =
			mbc.motionSubspace[i].transpose()*bodyFPhi;

		int j = i;
		while(pred[j] != -1)
		{
			const sva::PTransformd& X_p_j = mbc.parentToSon[j];
			// bodyFPhi = X_p_j^T bodyFPhi
			// is faster to convert each col in a ForceVecd
			// than using X_p_j.inv().dualMatrix()
			for(int c = 0; c < 10; ++c)
			{
				bodyFPhi.col(c) =
					X_p_j.transMul(sva::ForceVecd(bodyFPhi.col(c))).vector();
			}
			j = pred[j];

			int jDofPos = mb.jointPosInDof(j);
			if(joints[j].dof() != 0)
			{
				Y_.block(jDofPos, i*10, joints[j].dof(), 10).noalias() =
					mbc.motionSubspace[j].transpose()*bodyFPhi;
			}
		}
	}
}
Example #4
0
template<int N> void calculateGaussPdf(Eigen::Matrix<double,Eigen::Dynamic,N> X, Eigen::Matrix<double,1,N> mu, Eigen::Matrix<double,N,N> C, double* result){
    Eigen::Matrix<double,N,N> L = C.llt().matrixL().transpose(); // cholesky decomposition
    Eigen::Matrix<double,N,N> Linv = L.inverse();
    
    double det = L.diagonal().prod(); //determinant of L is equal to square rooot of determinant of C
	double lognormconst = -log(2 * M_PI)*X.cols()/2 - log(fabs(det));

    Eigen::Matrix<double,1,N> x = mu;
    Eigen::Matrix<double,1,N> tmp = x;
    for (int i=0; i<X.rows(); i++){
        x.noalias() = X.row(i) - mu;
        tmp.noalias() = x*Linv;
        double exponent = -0.5 * (tmp.cwiseProduct(tmp)).sum();
        result[i] = lognormconst+exponent;
    }
    
    /*
	Eigen::Matrix<double,Eigen::Dynamic,N> X0 = (X.rowwise() - mu)*Linv;
	Eigen::Map<Eigen::Matrix<double,Eigen::Dynamic,1> > resultMap(result, X.rows());
	resultMap = (X0.rowwise().squaredNorm()).array() * (-0.5) + lognormconst;
    */
    
    fmath::expd_v(result, X.rows());
}
bool ClosedFormCalibration::calibrate(const MotionInformationVector& measurements, SE2& laserOffset, Eigen::Vector3d& odomParams)
{
  std::vector<VelocityMeasurement, Eigen::aligned_allocator<VelocityMeasurement> > velMeasurements;
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& odomMotion = measurements[i].odomMotion;
    const double& timeInterval = measurements[i].timeInterval;
    MotionMeasurement mm(odomMotion.translation().x(), odomMotion.translation().y(), odomMotion.rotation().angle(), timeInterval);
    VelocityMeasurement velMeas = OdomConvert::convertToVelocity(mm);
    velMeasurements.push_back(velMeas);
  }

  double J_21, J_22;
  {
    Eigen::MatrixXd A(measurements.size(), 2);
    Eigen::VectorXd x(measurements.size());
    for (size_t i = 0; i < measurements.size(); ++i) {
      const SE2& laserMotion = measurements[i].laserMotion;
      const double& timeInterval = measurements[i].timeInterval;
      const VelocityMeasurement& velMeas = velMeasurements[i];
      A(i, 0) = velMeas.vl() * timeInterval;
      A(i, 1) = velMeas.vr() * timeInterval;
      x(i) = laserMotion.rotation().angle();
    }
    // (J_21, J_22) = (-r_l / b, r_r / b)
    Eigen::Vector2d linearSolution = (A.transpose() * A).inverse() * A.transpose() * x;
    //std::cout << linearSolution.transpose() << std::endl;
    J_21 = linearSolution(0);
    J_22 = linearSolution(1);
  }

  // construct M
  Eigen::Matrix<double, 5, 5> M;
  M.setZero();
  for (size_t i = 0; i < measurements.size(); ++i) {
    const SE2& laserMotion = measurements[i].laserMotion;
    const double& timeInterval = measurements[i].timeInterval;
    const VelocityMeasurement& velMeas = velMeasurements[i];
    Eigen::Matrix<double, 2, 5> L;
    double omega_o_k = J_21 * velMeas.vl() + J_22 * velMeas.vr();
    double o_theta_k = omega_o_k * timeInterval;
    double sx = 1.;
    double sy = 0.;
    if (fabs(o_theta_k) > std::numeric_limits<double>::epsilon()) {
      sx = sin(o_theta_k)       / o_theta_k;
      sy = (1 - cos(o_theta_k)) / o_theta_k;
    }
    double c_x = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sx;
    double c_y = 0.5 * timeInterval * (-J_21 * velMeas.vl() + J_22 * velMeas.vr()) * sy;
    L(0, 0) = -c_x;
    L(0, 1) = 1 - cos(o_theta_k);
    L(0, 2) = sin(o_theta_k);
    L(0, 3) = laserMotion.translation().x(); 
    L(0, 4) = -laserMotion.translation().y();
    L(1, 0) = -c_y;
    L(1, 1) = - sin(o_theta_k);
    L(1, 2) = 1 - cos(o_theta_k);
    L(1, 3) = laserMotion.translation().y(); 
    L(1, 4) = laserMotion.translation().x();
    M.noalias() += L.transpose() * L;
  }
  //std::cout << M << std::endl;

  // compute lagrange multiplier
  // and solve the constrained least squares problem
  double m11 = M(0,0);
  double m13 = M(0,2);
  double m14 = M(0,3);
  double m15 = M(0,4);
  double m22 = M(1,1);
  double m34 = M(2,3);
  double m35 = M(2,4);
  double m44 = M(3,3); 

  double a = m11 * SQR(m22) - m22 * SQR(m13);
  double b = 2*m11 * SQR(m22) * m44 - SQR(m22) * SQR(m14) - 2*m22 * SQR(m13) * m44 - 2*m11 * m22 * SQR(m34) 
    - 2*m11 * m22 * SQR(m35) - SQR(m22) * SQR(m15) + 2*m13 * m22 * m34 * m14 + SQR(m13) * SQR(m34) 
    + 2*m13 * m22 * m35 * m15 + SQR(m13) * SQR(m35);
  double c = - 2*m13 * CUBE(m35) * m15 - m22 * SQR(m13) * SQR(m44) + m11 * SQR(m22) * SQR(m44) + SQR(m13) * SQR(m35) * m44
    + 2*m13 * m22 * m34 * m14 * m44 + SQR(m13) * SQR(m34) * m44 - 2*m11 * m22 * SQR(m34) * m44 - 2 * m13 * CUBE(m34) * m14
    - 2*m11 * m22 * SQR(m35) * m44 + 2*m11 * SQR(m35) * SQR(m34) + m22 * SQR(m14) * SQR(m35) - 2*m13 * SQR(m35) * m34 * m14
    - 2*m13 * SQR(m34) * m35 * m15 + m11 * std::pow(m34, 4) + m22 * SQR(m15) * SQR(m34) + m22 * SQR(m35) * SQR(m15)
    + m11 * std::pow(m35, 4) - SQR(m22) * SQR(m14) * m44 + 2*m13 * m22 * m35 * m15 * m44 + m22 * SQR(m34) * SQR(m14)
    - SQR(m22) * SQR(m15) * m44;

  // solve the quadratic equation
  double lambda1, lambda2;
  if(a < std::numeric_limits<double>::epsilon()) {
    if(b <= std::numeric_limits<double>::epsilon())
      return false;
    lambda1 = lambda2 = -c/b;
  } else {
    double delta = b*b - 4*a*c;
    if (delta < 0)
      return false;
    lambda1 = 0.5 * (-b-sqrt(delta)) / a;
    lambda2 = 0.5 * (-b+sqrt(delta)) / a;
  }

  Eigen::VectorXd x1 = solveLagrange(M, lambda1);
  Eigen::VectorXd x2 = solveLagrange(M, lambda2);
  double err1 = x1.dot(M * x1);
  double err2 = x2.dot(M * x2);

  const Eigen::VectorXd& calibrationResult = err1 < err2 ? x1 : x2;
  odomParams(0) = - calibrationResult(0) * J_21;
  odomParams(1) = calibrationResult(0) * J_22;
  odomParams(2) = calibrationResult(0);

  laserOffset = SE2(calibrationResult(1), calibrationResult(2), atan2(calibrationResult(4), calibrationResult(3)));

  return true;
}