Example #1
0
   bool Basis::push (const Vector3& p)
   {
       int i, j;
       double eps = 1e-32;
       if (m==0) {
		   q0 = p;
           c[0] = q0;
           sqr_r[0] = 0;
       } 
	   else {
          // set v_m to Q_m
          v[m] = p-q0;
   
          // compute the a_{m,i}, i< m
          for (i=1; i<m; ++i) {
               a[m].getAt(i) = 0;
               for (j=0; j<3; ++j)
                   a[m].getAt(i) += v[i].getAt(j) * v[m].getAt(j);
               a[m].getAt(i)*=(2/z[i]);
          }
   
          // update v_m to Q_m-\bar{Q}_m
          for (i=1; i<m; ++i) {
               for (j=0; j<3; ++j)
                   v[m].getAt(j) -= a[m].getAt(i)*v[i].getAt(j);
          }
   
          // compute z_m
          z[m]=0;
          z[m]+= normSquared(v[m]);
          z[m]*=2;
   
          // reject push if z_m too small
          if (z[m]<eps*current_sqr_r) {
               return false;
          }
   
          // update c, sqr_r
          double e = -sqr_r[m-1];
          e += normSquared(p-c[m-1]);
          f[m]=e/z[m];
   
          c[m] = c[m-1]+v[m]*f[m];
          sqr_r[m] = sqr_r[m-1] + e*f[m]/2;
       }
       current_c = &(c[m]);
       current_sqr_r = sqr_r[m];
       s = ++m;
       return true;
   }
Example #2
0
   double Miniball::max_excess (It t, It i, It& pivot) const
   {
       const Vector3 c = B.center();
	   const double sqr_r = B.squared_radius();
       double e, max_e = 0;
       for (It k=t; k!=i; ++k) {
           e = -sqr_r;
		   e += normSquared(*k-c);
           if (e > max_e) {
               max_e = e;
               pivot = k;
           }
       }
       return max_e;
    }
Example #3
0
Vector3 
LineicModel::findClosest(const Vector3& p, real_t* ui) const{
  real_t u0 = getFirstKnot();
  real_t u1 = getLastKnot();
  real_t deltau = (u1 - u0)/getStride();
  Vector3 p1 = getPointAt(u0);
  Vector3 res = p1;
  real_t dist = normSquared(p-res);
  Vector3 p2, pt;
  real_t lu;
  for(real_t u = u0 + deltau ; u <= u1 ; u += deltau){
    p2 = getPointAt(u);
	pt = p;
	real_t d = __closestPointToSegment(pt,p1,p2,&lu);
	if(d < dist){
	  dist = d;
	  res = pt;
      if (ui != NULL) *ui = u + deltau * (lu -1);
	}
    p1 = p2;
  }
  return res;
}
Example #4
0
 double Basis::excess (const Vector3& p) const
 {
     double e = -current_sqr_r;
     e += normSquared(p-*current_c);
     return e;
 }
int main(int argc, char** argv) 
{
    ros::init(argc,argv,"waypoint_generator");
    ros::NodeHandle node;
    ros::Rate loop_rate(50);
    
    ros::Subscriber pos_sub, joy_sub;
    pos_sub = node.subscribe("current_position",1,pos_callback);
    joy_sub = node.subscribe("joy",1,joy_callback);
    
    ros::Publisher des_pos_pub;
    des_pos_pub = node.advertise<geometry_msgs::Vector3>("desired_position",1);
    
    if ( node.getParam("timed_loop",timedLoop) ) {;}
    else
    {
        ROS_ERROR("Are waypoints timed or position based?");
        return 0;
    }
    if ( timedLoop && node.getParam("waypoint_timestep",timestep) ) {;}
    else
    {
        ROS_ERROR("What is the timing between waypoints?");
        return 0;
    }
    if ( node.getParam("robot_name",robotName) ) {;}
    else
    {
        ROS_ERROR("Which robot number is this?");
        return 0;
    }
    if ( node.getParam("trajectory_shape",shape) ) {;}
    else
    {
        ROS_ERROR("What shape to follow?");
        return 0;
    }
    
    std::vector<geometry_msgs::Vector3> desired_positions;
    
    fillPositionList(desired_positions);
    double bound = 0.05; //0.025;
    des_pos_out = desired_positions[0];

    int max = desired_positions.size()-1;
    int arg = 0;
    int count = 0;
    int countBound = 10;
    
    startWaypoints = 0;
    
    while(ros::ok())
    {
        ros::spinOnce();
        if (startControl && !startWaypoints)
        {   
            des_pos_pub.publish(desired_positions[arg]);       
        }
        else if( startControl && startWaypoints )
        {
            if(!timedLoop) // position based loop
            {
                if(normSquared(des_pos_out,curr_pos)<bound*bound)
                {
                    count++;
                }
                if(count > countBound)
                {
                    count = 0;
                    if(arg < max)
                        arg++;
                    else
                        arg = 0;
                    des_pos_out = desired_positions [arg];
                }
            }
            else // time based loop
            {
                count++;
                if( (double)count/100.0 > timestep )
                {
                    count = 0;
                    if(arg < max)
                        arg++;
                    else
                        arg = 0;
                }
            }
            des_pos_pub.publish(desired_positions[arg]);
            ros::spinOnce();
            loop_rate.sleep();
        }
        else
        {
            loop_rate.sleep();
        }
    }
}    
    OrthogonalProjections::OrthogonalProjections(const Matrix& originalVectors,
                                                 Real multiplierCutoff,
                                                 Real tolerance)
    : originalVectors_(originalVectors),
      multiplierCutoff_(multiplierCutoff),
      numberVectors_(originalVectors.rows()),
      dimension_(originalVectors.columns()),
      validVectors_(true,originalVectors.rows()), // opposite way round from vector constructor
      orthoNormalizedVectors_(originalVectors.rows(),
                              originalVectors.columns())
    {
        std::vector<Real> currentVector(dimension_);
        for (Size j=0; j < numberVectors_; ++j)
        {

            if (validVectors_[j])
            {
                for (Size k=0; k< numberVectors_; ++k) // create an orthormal basis not containing j
                {
                    for (Size m=0; m < dimension_; ++m)
                        orthoNormalizedVectors_[k][m] = originalVectors_[k][m];

                    if ( k !=j && validVectors_[k])
                    {


                        for (Size l=0; l < k; ++l)
                        {
                            if (validVectors_[l] && l !=j)
                            {
                                Real dotProduct = innerProduct(orthoNormalizedVectors_, k, orthoNormalizedVectors_,l);
                                for (Size n=0; n < dimension_; ++n)
                                    orthoNormalizedVectors_[k][n] -=  dotProduct*orthoNormalizedVectors_[l][n];
                            }

                        }

                        Real normBeforeScaling= norm(orthoNormalizedVectors_,k);

                        if (normBeforeScaling < tolerance)
                        {
                            validVectors_[k] = false;
                        }
                        else
                        {
                            Real normBeforeScalingRecip = 1.0/normBeforeScaling;
                            for (Size m=0; m < dimension_; ++m)
                                orthoNormalizedVectors_[k][m] *= normBeforeScalingRecip;

                        } // end of else (norm < tolerance)

                    } // end of if k !=j && validVectors_[k])

                }// end of  for (Size k=0; k< numberVectors_; ++k)

                // we now have an o.n. basis for everything except  j

                Real prevNormSquared = normSquared(originalVectors_, j);


                for (Size r=0; r < numberVectors_; ++r)
                    if (validVectors_[r] && r != j)
                    {
                        Real dotProduct = innerProduct(orthoNormalizedVectors_, j, orthoNormalizedVectors_,r);

                        for (Size s=0; s < dimension_; ++s)
                           orthoNormalizedVectors_[j][s] -= dotProduct*orthoNormalizedVectors_[r][s];

                    }

               Real projectionOnOriginalDirection = innerProduct(originalVectors_,j,orthoNormalizedVectors_,j);
               Real sizeMultiplier = prevNormSquared/projectionOnOriginalDirection;

               if (std::fabs(sizeMultiplier) < multiplierCutoff_)
               {
                    for (Size t=0; t < dimension_; ++t)
                        currentVector[t] = orthoNormalizedVectors_[j][t]*sizeMultiplier;

               }
               else
                   validVectors_[j] = false;


            } // end of  if (validVectors_[j])

            projectedVectors_.push_back(currentVector);


        } //end of j loop

        numberValidVectors_ =0;
        for (Size i=0; i < numberVectors_; ++i)
            numberValidVectors_ +=  validVectors_[i] ? 1 : 0;


    } // end of constructor
Example #7
0
double Vector4d::norm() const
{
    return sqrt( normSquared() );
}
Example #8
0
double vec3::norm()const{
	return sqrt(normSquared());
}
Example #9
0
T SparseVectorCompressed<T>::norm() const { return Sqrt(normSquared()); } 
Example #10
0
T SparseVectorTemplate<T>::distanceSquared(const MyT& b) const
{
  return normSquared() + b.normSquared() - Two*dot(b);
}
Example #11
0
T SparseVectorTemplate<T>::norm() const { return Sqrt(normSquared()); }
/**
 * Compute the Euclidian norm of vector \c v.
 * @copydoc normSquared()
 */
inline double norm(const LinAlVector& v) {
  return sqrt(normSquared(v));
}
void SecondOrderTrustRegion::doOptimize(OptimizationProblemSecond& problem) {
#ifdef NICE_USELIB_LINAL
  bool previousStepSuccessful = true;
  double previousError = problem.objective();
  problem.computeGradientAndHessian();
  double delta = computeInitialDelta(problem.gradientCached(), problem.hessianCached());
  double normOldPosition = 0.0;
  
  // iterate
  for (int iteration = 0; iteration < maxIterations; iteration++) {
//     Log::debug() << "iteration, objective: " << iteration << ", "
//                   << problem.objective() << std::endl;
    
    if (previousStepSuccessful && iteration > 0) {
      problem.computeGradientAndHessian();
    }
  
    // gradient-norm stopping condition
    if (problem.gradientNormCached() < epsilonG) {
      Log::debug() << "SecondOrderTrustRegion stopped: gradientNorm "
                   << iteration << std::endl;
      break;
    }
    
    LinAlVector gradient(problem.gradientCached().linalCol());
    LinAlVector negativeGradient(gradient);
    negativeGradient.multiply(-1.0);

    double lambda;
    int lambdaMinIndex = -1;
    // FIXME will this copy the matrix? no copy needed here!
    LinAlMatrix hessian(problem.hessianCached().linal());
    LinAlMatrix l(hessian);
    try {
      //l.CHdecompose(); // FIXME
      choleskyDecompose(hessian, l);
      
      lambda = 0.0;
    } catch (...) { //(LinAl::BLException& e) { // FIXME
      const LinAlVector& eigenValuesHessian = LinAl::eigensym(hessian);

      // find smallest eigenvalue
      lambda = std::numeric_limits<double>::infinity();
      for (unsigned int i = 0; i < problem.dimension(); i++) {
        const double eigenValue = eigenValuesHessian(i);
        if (eigenValue < lambda) {
          lambda = eigenValue;
          lambdaMinIndex = i;
        }
      }
      const double originalLambda = lambda;
      lambda = -lambda * (1.0 + epsilonLambda);
      
      l = hessian;
      for (unsigned int i = 0; i < problem.dimension(); i++) {
        l(i, i) += lambda;
      }
      try {
        //l.CHdecompose(); // FIXME
        LinAlMatrix temp(l);
        choleskyDecompose(temp, l);
      } catch (...) { // LinAl::BLException& e) { // FIXME
        /*
         * Cholesky factorization failed, which should theortically not be
         * possible (can still happen due to numeric problems,
         * also note that there seems to be a bug in CHdecompose()).
         * Try a really great lambda as last attempt.
         */        
//         lambda = -originalLambda * (1.0 + epsilonLambda * 100.0)
//                  + 2.0 * epsilonM;
        lambda = fabs(originalLambda) * (1.0 + epsilonLambda * 1E5)
                 + 1E3 * epsilonM;
//        lambda = fabs(originalLambda);// * 15.0;
        l = hessian;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          l(i, i) += lambda;
        }
        try {
          //l.CHdecompose(); // FIXME
          LinAlMatrix temp(l);
          choleskyDecompose(temp, l);
        } catch (...) { // (LinAl::BLException& e) { // FIXME
          // Cholesky factorization failed again, give up.
          l = hessian;
          for (unsigned int i = 0; i < problem.dimension(); i++) {
            l(i, i) += lambda;
          }

          const LinAlVector& eigenValuesL = LinAl::eigensym(l);
                    
          Log::detail()
               << "l.CHdecompose: exception" << std::endl
              //<< e.what() << std::endl // FIXME
               << "lambda=" << lambda << std::endl
               << "l" << std::endl << l
               << "hessian" << std::endl << hessian
               << "gradient" << std::endl << gradient
               << "eigenvalues hessian" << std::endl << eigenValuesHessian
               << "eigenvalues l" << std::endl << eigenValuesL
               << std::endl;
          return;
        }
      }
    }

    // FIXME will the copy the vector? copy is needed here
    LinAlVector step(negativeGradient);
    l.CHsubstitute(step);
    double normStepSquared = normSquared(step);
    double normStep = sqrt(normStepSquared);
    // exact: if normStep <= delta
    if (normStep - delta <= tolerance) {
      // exact: if lambda == 0 || normStep == delta
      if (std::fabs(lambda) < tolerance
          || std::fabs(normStep - delta) < tolerance) {
        // done
      } else {
        LinAlMatrix eigenVectors(problem.dimension(), problem.dimension());
        eigensym(hessian, eigenVectors);
        double a = 0.0;
        double b = 0.0;
        double c = 0.0;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          const double ui = eigenVectors(i, lambdaMinIndex);
          const double si = step(i);
          a += ui * ui;
          b += si * ui;
          c += si * si;
        }
        b *= 2.0;
        c -= delta * delta;
        const double sq = sqrt(b * b - 4.0 * a * c);
        const double root1 = 0.5 * (-b + sq) / a;
        const double root2 = 0.5 * (-b - sq) / a;
        LinAlVector step1(step);
        LinAlVector step2(step);
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          step1(i) += root1 * eigenVectors(i, lambdaMinIndex);
          step2(i) += root2 * eigenVectors(i, lambdaMinIndex);
        }
        const double psi1
          = dotProduct(gradient, step1)
            + 0.5 * productVMV(step1, hessian, step1);
        const double psi2
          = dotProduct(gradient, step2)
            + 0.5 * productVMV(step2, hessian, step2);
        if (psi1 < psi2) {
          step = step1;
        } else {
          step = step2;
        }
      }
    } else {
      for (unsigned int subIteration = 0; 
           subIteration < maxSubIterations; 
           subIteration++) {
        if (std::fabs(normStep - delta) <= kappa * delta) {
          break;
        }

        // NOTE specialized algorithm may be more effifient than solvelineq
        //      (l is lower triangle!)
        // Only lower triangle values of l are valid (other implicitly = 0.0),
        // but solvelineq doesn't know that -> explicitly set to 0.0
        for (int i = 0; i < l.rows(); i++) {
          for (int j = i + 1; j < l.cols(); j++) {
            l(i, j) = 0.0;
          }
        }
        LinAlVector y(step.size());
        try {
          y = solvelineq(l, step);
        } catch (LinAl::Exception& e) {
          // FIXME if we end up here, something is pretty wrong!
          // give up the whole thing
          Log::debug() << "SecondOrderTrustRegion stopped: solvelineq failed "
                       << iteration << std::endl;
          return;
        }
        
        lambda += (normStep - delta) / delta
                  * normStepSquared / normSquared(y);
        l = hessian;
        for (unsigned int i = 0; i < problem.dimension(); i++) {
          l(i, i) += lambda;
        }
        try {
          //l.CHdecompose(); // FIXME
          LinAlMatrix temp(l);
          choleskyDecompose(temp, l);
        } catch (...) { // (LinAl::BLException& e) { // FIXME
          Log::detail()
               << "l.CHdecompose: exception" << std::endl
               // << e.what() << std::endl // FIXME
               << "lambda=" << lambda << std::endl
               << "l" << std::endl << l
               << std::endl;
          return;
        }
        step = negativeGradient;
        l.CHsubstitute(step);
        normStepSquared = normSquared(step);
        normStep = sqrt(normStepSquared);
      }
    }
    
    // computation of step is complete, convert to NICE::Vector
    Vector stepLimun(step);
    
    // minimal change stopping condition
    if (changeIsMinimal(stepLimun, problem.position())) {
      Log::debug() << "SecondOrderTrustRegion stopped: change is minimal "
                   << iteration << std::endl;
      break;
    }
  
    if (previousStepSuccessful) {
      normOldPosition = problem.position().normL2();
    }

    // set new region parameters (to be verified later)
    problem.applyStep(stepLimun);
    
    // compute reduction rate
    const double newError = problem.objective();
//Log::debug() << "newError: " << newError << std::endl;
    const double errorReduction = newError - previousError;
    const double psi = problem.gradientCached().scalarProduct(stepLimun)
                       + 0.5 * productVMV(step, hessian, step);
    double rho;
    if (std::fabs(psi) <= epsilonRho
        && std::fabs(errorReduction) <= epsilonRho) {
      rho = 1.0;
    } else {
      rho = errorReduction / psi;
    }

    // NOTE psi and errorReduction checks added to the algorithm 
    //      described in Ferid Bajramovic's Diplomarbeit
    if (rho < eta1 || psi >= 0.0 || errorReduction > 0.0) {
      previousStepSuccessful = false;
      problem.unapplyStep(stepLimun);
      delta = alpha2 * normStep;
    } else {
      previousStepSuccessful = true;
      previousError = newError;
      if (rho >= eta2) {
        const double newDelta = alpha1 * normStep;
        if (newDelta > delta) {
          delta = newDelta;
        }
      } // else: don't change delta
    }
    
    // delta stopping condition
    if (delta < epsilonDelta * normOldPosition) {
      Log::debug() << "SecondOrderTrustRegion stopped: delta too small "
                   << iteration << std::endl;
      break;
    }
  }
#else // no linal
  fthrow(Exception,
         "SecondOrderTrustRegion needs LinAl. Please recompile with LinAl");
#endif
}