Example #1
0
	/* ************************************************************************* */
	double AllDiff::operator()(const Values& values) const {
		std::set < size_t > taken; // record values taken by keys
		BOOST_FOREACH(Index dkey, keys_) {
			size_t value = values.at(dkey); // get the value for that key
			if (taken.count(value)) return 0.0;// check if value alreday taken
			taken.insert(value);// if not, record it as taken and keep checking
		}
void LIRGenerator::do_NewMultiArray(NewMultiArray* x) {
  // make sure registers are spilled
  spill_values_on_stack(x->state());
  if (x->state_before() != NULL) {
    spill_values_on_stack(x->state_before());
  }
  Values* dims = x->dims();
  int i = dims->length();
  LIRItemList* items = new LIRItemList();
  while (i-- > 0) {
    LIRItem* size = new LIRItem(dims->at(i), this);
    items->at_put(i, size);
    assert(!size->is_register() || x->state_before() == NULL, "shouldn't be since it was spilled above");
  }

  // need to get the info before, as the items may become invalid through item_free

  CodeEmitInfo* patching_info = NULL;
  if (!x->klass()->is_loaded() || PatchALot) {
    patching_info = state_for(x, x->state_before());
  }
  i = dims->length();
  while (i-- > 0) {
    LIRItem* size = items->at(i);
    size->load_item();

    emit()->store_stack_parameter(size->result(), i);
  }
  RInfo reg = set_with_result_register(x)->rinfo();

  CodeEmitInfo* info = state_for(x, x->state());
  emit()->new_multi_array(reg, x->klass(), x->rank(), norinfo, info, patching_info);
}
void LIRGenerator::do_NewMultiArray(NewMultiArray* x) {
  Values* dims = x->dims();
  int i = dims->length();
  LIRItemList* items = new LIRItemList(dims->length(), NULL);
  while (i-- > 0) {
    LIRItem* size = new LIRItem(dims->at(i), this);
    items->at_put(i, size);
  }

  // need to get the info before, as the items may become invalid through item_free
  CodeEmitInfo* patching_info = NULL;
  if (!x->klass()->is_loaded() || PatchALot) {
    patching_info = state_for(x, x->state_before());

    // cannot re-use same xhandlers for multiple CodeEmitInfos, so
    // clone all handlers
    x->set_exception_handlers(new XHandlers(x->exception_handlers()));
  }

  i = dims->length();
  while (i-- > 0) {
    LIRItem* size = items->at(i);
    // if a patching_info was generated above then debug information for the state before
    // the call is going to be emitted.  The LIRGenerator calls above may have left some values
    // in registers and that's been recorded in the CodeEmitInfo.  In that case the items
    // for those values can't simply be freed if they are registers because the values
    // might be destroyed by store_stack_parameter.  So in the case of patching, delay the
    // freeing of the items that already were in registers
    size->load_item();
    store_stack_parameter (size->result(),
                           in_ByteSize(STACK_BIAS +
                                       frame::memory_parameter_word_sp_offset * wordSize +
                                       i * sizeof(jint)));
  }

  // This instruction can be deoptimized in the slow path : use
  // O0 as result register.
  const LIR_Opr reg = result_register_for(x->type());
  CodeEmitInfo* info = state_for(x, x->state());

  jobject2reg_with_patching(reg, x->klass(), patching_info);
  LIR_Opr rank = FrameMap::O1_opr;
  __ move(LIR_OprFact::intConst(x->rank()), rank);
  LIR_Opr varargs = FrameMap::as_pointer_opr(O2);
  int offset_from_sp = (frame::memory_parameter_word_sp_offset * wordSize) + STACK_BIAS;
  __ add(FrameMap::SP_opr,
         LIR_OprFact::intptrConst(offset_from_sp),
         varargs);
  LIR_OprList* args = new LIR_OprList(3);
  args->append(reg);
  args->append(rank);
  args->append(varargs);
  __ call_runtime(Runtime1::entry_for(Runtime1::new_multi_array_id),
                  LIR_OprFact::illegalOpr,
                  reg, args, info);

  LIR_Opr result = rlock_result(x);
  __ move(reg, result);
}
void InstructionPrinter::do_NewMultiArray(NewMultiArray* x) {
  tty->print("new multi array [");
  Values* dims = x->dims();
  for (int i = 0; i < dims->length(); i++) {
    if (i > 0) tty->print(", ");
    print_value(dims->at(i));
  }
  tty->print("] ");
  print_klass(x->klass());
}
Example #5
0
/* ************************************************************************* */
LinearizedGaussianFactor::LinearizedGaussianFactor(
    const GaussianFactor::shared_ptr& gaussian, const Values& lin_points)
: NonlinearFactor(gaussian->keys())
{
  // Extract the keys and linearization points
  for(const Key& key: gaussian->keys()) {
    // extract linearization point
    assert(lin_points.exists(key));
    this->lin_points_.insert(key, lin_points.at(key));
  }
}
Example #6
0
/* ************************************************************************* */
Vector LinearizedJacobianFactor::error_vector(const Values& c) const {

  Vector errorVector = -b();

  for(Key key: this->keys()) {
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    Vector d = linPt.localCoordinates_(newPt);
    const constABlock A = this->A(key);
    errorVector += A * d;
  }

  return errorVector;
}
Example #7
0
void LIRGenerator::do_NewMultiArray(NewMultiArray* x) {
  Values* dims = x->dims();
  int i = dims->length();
  LIRItemList* items = new LIRItemList(dims->length(), NULL);
  while (i-- > 0) {
    LIRItem* size = new LIRItem(dims->at(i), this);
    items->at_put(i, size);
  }

  // Evaluate state_for early since it may emit code.
  CodeEmitInfo* patching_info = NULL;
  if (!x->klass()->is_loaded() || PatchALot) {
    patching_info = state_for(x, x->state_before());

    // cannot re-use same xhandlers for multiple CodeEmitInfos, so
    // clone all handlers.  This is handled transparently in other
    // places by the CodeEmitInfo cloning logic but is handled
    // specially here because a stub isn't being used.
    x->set_exception_handlers(new XHandlers(x->exception_handlers()));
  }
  CodeEmitInfo* info = state_for(x, x->state());

  i = dims->length();
  while (i-- > 0) {
    LIRItem* size = items->at(i);
    size->load_nonconstant();

    store_stack_parameter(size->result(), in_ByteSize(i*4));
  }

  LIR_Opr reg = result_register_for(x->type());
  jobject2reg_with_patching(reg, x->klass(), patching_info);

  LIR_Opr rank = FrameMap::rbx_opr;
  __ move(LIR_OprFact::intConst(x->rank()), rank);
  LIR_Opr varargs = FrameMap::rcx_opr;
  __ move(FrameMap::rsp_opr, varargs);
  LIR_OprList* args = new LIR_OprList(3);
  args->append(reg);
  args->append(rank);
  args->append(varargs);
  __ call_runtime(Runtime1::entry_for(Runtime1::new_multi_array_id),
                  LIR_OprFact::illegalOpr,
                  reg, args, info);

  LIR_Opr result = rlock_result(x);
  __ move(reg, result);
}
Example #8
0
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor>
LinearizedHessianFactor::linearize(const Values& c) const {

  // Construct an error vector in key-order from the Values
  Vector dx = Vector::Zero(dim());
  size_t index = 0;
  for(unsigned int i = 0; i < this->size(); ++i){
    Key key = this->keys()[i];
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
    index += linPt.dim();
  }

  // f2 = f1 - 2*dx'*g1 + dx'*G1*dx
  //newInfo(this->size(), this->size())(0,0) += -2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm().selfadjointView<Eigen::Upper>() * dx;
  double f = constantTerm() - 2*dx.dot(linearTerm()) + dx.transpose() * squaredTerm() * dx;

  // g2 = g1 - G1*dx
  //newInfo.rangeColumn(0, this->size(), this->size(), 0) -= squaredTerm().selfadjointView<Eigen::Upper>() * dx;
  Vector g = linearTerm() - squaredTerm() * dx;
  std::vector<Vector> gs;
  std::size_t offset = 0;
  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
    const std::size_t dim = info_.getDim(i);
    gs.push_back(g.segment(offset, dim));
    offset += dim;
  }

  // G2 = G1
  // Do Nothing
  std::vector<Matrix> Gs;
  for(DenseIndex i = 0; i < info_.nBlocks()-1; ++i) {
    Gs.push_back(info_.diagonalBlock(i));
    for(DenseIndex j = i + 1; j < info_.nBlocks()-1; ++j) {
      Gs.push_back(info_.aboveDiagonalBlock(i, j));
    }
  }

  // Create a Hessian Factor from the modified info matrix
  //return boost::shared_ptr<GaussianFactor>(new HessianFactor(js, newInfo));
  return boost::shared_ptr<GaussianFactor>(new HessianFactor(keys(), Gs, gs, f));
}
Example #9
0
/* ************************************************************************* */
double LinearizedHessianFactor::error(const Values& c) const {

  // Construct an error vector in key-order from the Values
  Vector dx = Vector::Zero(dim());
  size_t index = 0;
  for(unsigned int i = 0; i < this->size(); ++i){
    Key key = this->keys()[i];
    const Value& newPt = c.at(key);
    const Value& linPt = lin_points_.at(key);
    dx.segment(index, linPt.dim()) = linPt.localCoordinates_(newPt);
    index += linPt.dim();
  }

  // error 0.5*(f - 2*x'*g + x'*G*x)
  double f = constantTerm();
  double xtg = dx.dot(linearTerm());
  double xGx = dx.transpose() * squaredTerm() * dx;

  return 0.5 * (f - 2.0 * xtg +  xGx);
}
Example #10
0
// main
int main(int argc, char** argv) {

  // load Plaza1 data
  list<TimedOdometry> odometry = readOdometry();
//  size_t M = odometry.size();

  vector<RangeTriple> triples = readTriples();
  size_t K = triples.size();

  // parameters
  size_t start = 220, end=3000;
  size_t minK = 100; // first batch of smart factors
  size_t incK = 50; // minimum number of range measurements to process after
  bool robust = true;
  bool smart = true;

  // Set Noise parameters
  Vector priorSigmas = Vector3(1, 1, M_PI);
  Vector odoSigmas = Vector3(0.05, 0.01, 0.2);
  double sigmaR = 100; // range standard deviation
  const NM::Base::shared_ptr // all same type
  priorNoise = NM::Diagonal::Sigmas(priorSigmas), //prior
  odoNoise = NM::Diagonal::Sigmas(odoSigmas), // odometry
  gaussian = NM::Isotropic::Sigma(1, sigmaR), // non-robust
  tukey = NM::Robust::Create(NM::mEstimator::Tukey::Create(15), gaussian), //robust
  rangeNoise = robust ? tukey : gaussian;

  // Initialize iSAM
  ISAM2 isam;

  // Add prior on first pose
  Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
      M_PI - 2.02108900000000);
  NonlinearFactorGraph newFactors;
  newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));

  ofstream os2(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultLM.txt");
  ofstream os3(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResultSR.txt");

  //  initialize points (Gaussian)
  Values initial;
  if (!smart) {
    initial.insert(symbol('L', 1), Point2(-68.9265, 18.3778));
    initial.insert(symbol('L', 6), Point2(-37.5805, 69.2278));
    initial.insert(symbol('L', 0), Point2(-33.6205, 26.9678));
    initial.insert(symbol('L', 5), Point2(1.7095, -5.8122));
  }
  Values landmarkEstimates = initial; // copy landmarks
  initial.insert(0, pose0);

  //  initialize smart range factors
  size_t ids[] = { 1, 6, 0, 5 };
  typedef boost::shared_ptr<SmartRangeFactor> SmartPtr;
  map<size_t, SmartPtr> smartFactors;
  if (smart) {
    for(size_t jj: ids) {
      smartFactors[jj] = SmartPtr(new SmartRangeFactor(sigmaR));
      newFactors.push_back(smartFactors[jj]);
    }
  }

  // set some loop variables
  size_t i = 1; // step counter
  size_t k = 0; // range measurement counter
  Pose2 lastPose = pose0;
  size_t countK = 0, totalCount=0;

  // Loop over odometry
  gttic_(iSAM);
  for(const TimedOdometry& timedOdometry: odometry) {
    //--------------------------------- odometry loop -----------------------------------------
    double t;
    Pose2 odometry;
    boost::tie(t, odometry) = timedOdometry;

    // add odometry factor
    newFactors.push_back(
        BetweenFactor<Pose2>(i - 1, i, odometry,
            NM::Diagonal::Sigmas(odoSigmas)));

    // predict pose and add as initial estimate
    Pose2 predictedPose = lastPose.compose(odometry);
    lastPose = predictedPose;
    initial.insert(i, predictedPose);
    landmarkEstimates.insert(i, predictedPose);

    // Check if there are range factors to be added
    while (k < K && t >= boost::get<0>(triples[k])) {
      size_t j = boost::get<1>(triples[k]);
      double range = boost::get<2>(triples[k]);
      if (i > start) {
        if (smart && totalCount < minK) {
          smartFactors[j]->addRange(i, range);
          printf("adding range %g for %d on %d",range,(int)j,(int)i);cout << endl;
        }
        else {
          RangeFactor<Pose2, Point2> factor(i, symbol('L', j), range,
              rangeNoise);
          // Throw out obvious outliers based on current landmark estimates
          Vector error = factor.unwhitenedError(landmarkEstimates);
          if (k <= 200 || fabs(error[0]) < 5)
            newFactors.push_back(factor);
        }
        totalCount += 1;
      }
      k = k + 1;
      countK = countK + 1;
    }

    // Check whether to update iSAM 2
    if (k >= minK && countK >= incK) {
      gttic_(update);
      isam.update(newFactors, initial);
      gttoc_(update);
      gttic_(calculateEstimate);
      Values result = isam.calculateEstimate();
      gttoc_(calculateEstimate);
      lastPose = result.at<Pose2>(i);
      bool hasLandmarks = result.exists(symbol('L', ids[0]));
      if (hasLandmarks) {
        // update landmark estimates
        landmarkEstimates = Values();
        for(size_t jj: ids)
          landmarkEstimates.insert(symbol('L', jj), result.at(symbol('L', jj)));
      }
      newFactors = NonlinearFactorGraph();
      initial = Values();
      if (smart && !hasLandmarks) {
        cout << "initialize from smart landmarks" << endl;
        for(size_t jj: ids) {
          Point2 landmark = smartFactors[jj]->triangulate(result);
          initial.insert(symbol('L', jj), landmark);
          landmarkEstimates.insert(symbol('L', jj), landmark);
        }
      }
      countK = 0;
      for(const Values::ConstFiltered<Point2>::KeyValuePair& it: result.filter<Point2>())
        os2 << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t1"
            << endl;
      if (smart) {
        for(size_t jj: ids) {
          Point2 landmark = smartFactors[jj]->triangulate(result);
          os3 << jj << "\t" << landmark.x() << "\t" << landmark.y() << "\t1"
              << endl;
        }
      }
    }
    i += 1;
    if (i>end) break;
    //--------------------------------- odometry loop -----------------------------------------
  } // end for
  gttoc_(iSAM);

  // Print timings
  tictoc_print_();

  // Write result to file
  Values result = isam.calculateEstimate();
  ofstream os(
      "/Users/dellaert/borg/gtsam/gtsam_unstable/examples/rangeResult.txt");
  for(const Values::ConstFiltered<Pose2>::KeyValuePair& it: result.filter<Pose2>())
    os << it.key << "\t" << it.value.x() << "\t" << it.value.y() << "\t"
        << it.value.theta() << endl;
  exit(0);
}