/* ************************************************************************* */ 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()); }
/* ************************************************************************* */ 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)); } }
/* ************************************************************************* */ 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; }
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); }
/* ************************************************************************* */ 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)); }
/* ************************************************************************* */ 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); }
// 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); }