void sptam::sptam_node::loadCameraCalibration( const sensor_msgs::CameraInfoConstPtr& left_info, const sensor_msgs::CameraInfoConstPtr& right_info ) { // Check if a valid calibration exists if (left_info->K[0] == 0.0) { ROS_ERROR("La camara no esta calibrada"); return; } // Ponemos que el frame id de las camara info sea el mismo sensor_msgs::CameraInfoPtr left_info_copy = boost::make_shared<sensor_msgs::CameraInfo>(*left_info); sensor_msgs::CameraInfoPtr right_info_copy = boost::make_shared<sensor_msgs::CameraInfo>(*right_info); left_info_copy->header.frame_id = "stereo"; right_info_copy->header.frame_id = "stereo"; ROS_INFO_STREAM("tx: " << right_info->P[3] << " fx: " << right_info->K[0] << " baseline: " << right_info->P[3] / right_info->K[0]); // Get Stereo Camera Model from Camera Info message image_geometry::StereoCameraModel stereoCameraModel; stereoCameraModel.fromCameraInfo(left_info_copy, right_info_copy); // Get PinHole Camera Model from the Stereo Camera Model const image_geometry::PinholeCameraModel& cameraLeft = stereoCameraModel.left(); // Get rectify intrinsic Matrix (is the same for both cameras because they are rectify) cv::Mat projection = cv::Mat( cameraLeft.projectionMatrix() ); cv::Matx33d intrinsic = projection( cv::Rect(0,0,3,3) ); // Save rectify intrinsic Matrix cameraParametersLeft_.intrinsic = intrinsic; cameraParametersRight_.intrinsic = intrinsic; // Save the baseline stereo_baseline_ = stereoCameraModel.baseline(); // Compute Fild Of View (Frustum) cameraParametersLeft_.horizontalFOV = computeFOV( intrinsic(0, 0), left_info_copy->width ); cameraParametersLeft_.verticalFOV = computeFOV( intrinsic(1, 1), left_info_copy->height ); cameraParametersRight_.horizontalFOV = computeFOV( intrinsic(0, 0), right_info_copy->width ); cameraParametersRight_.verticalFOV = computeFOV( intrinsic(1, 1), right_info_copy->height ); ROS_INFO_STREAM("baseline: " << stereo_baseline_); // Create SPTAM instance sptam_ = new SPTAM( map_, cameraParametersLeft_, cameraParametersRight_, stereo_baseline_, *rowMatcher_, mapper_params_ ); }
Mat3 calc_intrinsic (double focal_x, double focal_y, double offset_x, double offset_y, double skew) { Mat3 intrinsic(Vec3(focal_x, 0, 0), Vec3(skew, focal_y, 0), Vec3(offset_x, offset_y, 1)); #if PRINT_MATRIX LOG_MESSAGE("%%Intrinsic Matrix(3x3) \n"); LOG_MESSAGE("intrinsic = [ %lf, %lf, %lf ; %lf, %lf, %lf ; %lf, %lf, %lf ] \n", intrinsic(1, 1), intrinsic(1, 2), intrinsic(1, 3), intrinsic(2, 1), intrinsic(2, 2), intrinsic(2, 3), intrinsic(3, 1), intrinsic(3, 2), intrinsic(3, 3)); #endif return intrinsic; }
void lift_pipeline(World& world) { for (auto cont : world.copy_continuations()) { auto callee = cont->callee()->isa_continuation(); // Binding to the number of arguments to avoid repeated optimization if (callee && callee->intrinsic() == Intrinsic::Pipeline && cont->num_args() == 6) { auto cont_type = world.fn_type({ world.mem_type() }); auto p_cont_type = world.fn_type({ world.mem_type(), cont_type }); auto body_type = world.fn_type({ world.mem_type(), world.type_qs32() }); auto pipe_type = world.fn_type({ world.mem_type(), world.type_qs32(), world.type_qs32(), world.type_qs32(), body_type, cont_type, p_cont_type }); // Transform: // // f(...) // pipeline(..., pipeline_body, return) // // pipeline_body(mem: mem, i: i32, ret: fn(mem)) // ret(mem) // // Into: // // f(...) // new_pipeline(..., pipeline_body, return, pipeline_continue) // // pipeline_body(mem: mem, i: i32) // continue_wrapper(mem) // // continue_wrapper(mem: mem) // pipeline_continue(mem, return) // // Note the use of 'return' as the second argument to pipeline_continue. // This is required to encode the dependence of the loop body over the call to pipeline, // so that lift_builtins can extract the correct free variables. auto pipeline_continue = world.continuation(p_cont_type, CC::C, Intrinsic::PipelineContinue, Debug("pipeline_continue")); auto continue_wrapper = world.continuation(cont_type, Debug("continue_wrapper")); auto new_pipeline = world.continuation(pipe_type, CC::C, Intrinsic::Pipeline, callee->debug()); auto old_body = cont->arg(4); auto body_cont = world.continuation(body_type, old_body->debug()); cont->jump(new_pipeline, thorin::Defs { cont->arg(0), cont->arg(1), cont->arg(2), cont->arg(3), body_cont, cont->arg(5), pipeline_continue }); Call call(4); call.callee() = old_body; call.arg(0) = body_cont->param(0); call.arg(1) = body_cont->param(1); call.arg(2) = continue_wrapper; auto target = drop(call); continue_wrapper->jump(pipeline_continue, thorin::Defs { continue_wrapper->param(0), cont->arg(5) }); body_cont->jump(target->callee(), target->args()); } } }
GetByIdVariant::GetByIdVariant( const StructureSet& structureSet, PropertyOffset offset, const ObjectPropertyConditionSet& conditionSet, std::unique_ptr<CallLinkStatus> callLinkStatus, JSFunction* intrinsicFunction) : m_structureSet(structureSet) , m_conditionSet(conditionSet) , m_offset(offset) , m_callLinkStatus(WTFMove(callLinkStatus)) , m_intrinsicFunction(intrinsicFunction) { if (!structureSet.size()) { ASSERT(offset == invalidOffset); ASSERT(conditionSet.isEmpty()); } if (intrinsicFunction) ASSERT(intrinsic() != NoIntrinsic); }
inline bool GetByIdVariant::canMergeIntrinsicStructures(const GetByIdVariant& other) const { if (m_intrinsicFunction != other.m_intrinsicFunction) return false; switch (intrinsic()) { case TypedArrayByteLengthIntrinsic: { // We can merge these sets as long as the element size of the two sets is the same. TypedArrayType thisType = (*m_structureSet.begin())->classInfo()->typedArrayStorageType; TypedArrayType otherType = (*other.m_structureSet.begin())->classInfo()->typedArrayStorageType; ASSERT(isTypedView(thisType) && isTypedView(otherType)); return logElementSize(thisType) == logElementSize(otherType); } default: return true; } RELEASE_ASSERT_NOT_REACHED(); }
/* * Note: return value indicates the amount of mana to use */ bool power_chance(power_type *x_ptr) { #if 0 // DGDGDGDG -- mana is no more hardcoded bool use_hp = FALSE; s32b diff = x_ptr->diff; /* Always true ? */ if (!x_ptr->cost) return TRUE; /* Not enough mana - use hp */ if (p_ptr->csp < x_ptr->cost) return FALSE; /* Power is not available yet */ if (p_ptr->lev < x_ptr->level) { msg_format("You need to attain level %d to use this power.", x_ptr->level); energy_use = 0; return (FALSE); } /* Too confused */ else if (intrinsic(CONFUSED)) { msg_print("You are too confused to use this power."); energy_use = 0; return (FALSE); } /* Else attempt to do it! */ if (p_ptr->lev > x_ptr->level) { s32b lev_adj = ((p_ptr->lev - x_ptr->level) / 3); if (lev_adj > 10) lev_adj = 10; diff -= lev_adj; } if (diff < 5) diff = 5; /* take time and pay the price */ p_ptr->csp -= (x_ptr->cost / 2 ) + (randint(x_ptr->cost / 2)); energy_use = get_player_energy(SPEED_POWER); /* Redraw mana and hp */ flag_bool(&p_ptr->redraw, FLAG_PR_HP); flag_bool(&p_ptr->redraw, FLAG_PR_MANA); /* Window stuff */ p_ptr->window |= (PW_PLAYER); /* Success? */ // DGDGDGDGDG this was stat_cur, needs adjusting // DGDGDGDGDG dont bother, jsut change the formula to be sane anyway if (randint(get_stat(x_ptr->stat, ind)) >= ((diff / 2) + randint(diff / 2))) { return (TRUE); } if (flush_failure) flush(); msg_print("You've failed to concentrate hard enough."); #endif return (FALSE); }
/* * Extract and set the current "lite radius" * * SWD: Experimental modification: multiple light sources have additive effect. * */ static void calc_torch(void) { /* Assume no light */ p_ptr->cur_lite = 0; /* Loop through all wielded items */ for_inventory(p_ptr, o_ptr, INVEN_PACK, INVEN_TOTAL); { /* does this item glow? */ if ((has_flag(o_ptr, FLAG_FUEL_LITE) && (get_flag(o_ptr, FLAG_FUEL) > 0)) || (!has_flag(o_ptr, FLAG_FUEL_LITE))) { p_ptr->cur_lite += get_flag(o_ptr, FLAG_LITE); } } end_inventory(); /* max radius is 5 without rewriting other code -- */ /* see cave.c:update_lite() and defines.h:LITE_MAX */ if (p_ptr->cur_lite > 5) p_ptr->cur_lite = 5; /* check if the player doesn't have a lite source, */ /* but does glow as an intrinsic. */ if (p_ptr->cur_lite == 0 && intrinsic(LITE)) p_ptr->cur_lite = 1; /* Hooked powers */ process_hooks(HOOK_CALC_LITE, "()"); /* end experimental mods */ /* Reduce lite in the small-scale wilderness map */ if (p_ptr->wild_mode) { /* Reduce the lite radius if needed */ if (p_ptr->cur_lite > WILDERNESS_SEE_RADIUS) { p_ptr->cur_lite = WILDERNESS_SEE_RADIUS; } } /* Reduce lite when running if requested */ if (running && view_reduce_lite) { /* Reduce the lite radius if needed */ if (p_ptr->cur_lite > 1) p_ptr->cur_lite = 1; } /* Notice changes in the "lite radius" */ if (p_ptr->old_lite != p_ptr->cur_lite) { /* Update the view */ p_ptr->update |= (PU_VIEW); /* Update the monsters */ p_ptr->update |= (PU_MONSTERS); /* Remember the old lite */ p_ptr->old_lite = p_ptr->cur_lite; } }
static void fix_m_list(void) { s32b i, j; /* Scan windows */ for (j = 0; j < 8; j++) { term *old = Term; s32b c = 0; /* No window */ if (!angband_term[j]) continue; /* No relevant flags */ if (!flag_exists(&window_flag[j], FLAG_PW_M_LIST)) continue; /* Activate */ Term_activate(angband_term[j]); /* Clear */ Term_clear(); /* Hallucination */ if (intrinsic(HALLUCINATE)) { c_prt(TERM_WHITE, "You can not see clearly", 0, 0); /* Fresh */ Term_fresh(); /* Restore */ Term_activate(old); return; } /* reset visible count */ for (i = 1; i < max_r_idx; i++) { monster_race *r_ptr = &r_info[i]; r_ptr->total_visible = 0; } /* Count up the number visible in each race */ for_flags(&monst_list); { i = __key; monster_type *m_ptr = get_monster(i); monster_race *r_ptr = &r_info[m_ptr->r_idx]; object_type *o_ptr = get_obj_mimic_obj_at(m_ptr->fy, m_ptr->fx); /* Skip dead monsters */ if (m_ptr->hp < 0) continue; /* Skip unseen monsters */ if (o_ptr != NULL && !o_ptr->marked) { /* Memorized objects */ if (!o_ptr->marked) continue; } else if (!m_ptr->ml) continue; /* Increase for this race */ r_ptr->total_visible++; /* Increase total Count */ c++; } end_for_flags(); /* Are monsters visible? */ if (c) { s32b w, h, num = 0; (void)Term_get_size(&w, &h); c_prt(TERM_WHITE, format("You can see %d monster%s", c, (c > 1 ? "s:" : ":")), 0, 0); for (i = 1; i < max_r_idx; i++) { monster_race *r_ptr = &r_info[i]; /* Default Colour */ byte attr = TERM_SLATE; /* Only visible monsters */ if (!r_ptr->total_visible) continue; /* Uniques */ if (has_flag(r_ptr, FLAG_UNIQUE)) { attr = TERM_L_BLUE; } /* Have we ever killed one? */ if (r_ptr->r_tkills) { if (r_ptr->level > dun_level) { attr = TERM_VIOLET; if (has_flag(r_ptr, FLAG_UNIQUE)) { attr = TERM_RED; } } } else { if (!has_flag(r_ptr, FLAG_UNIQUE)) attr = TERM_GREEN; } /* Dump the monster name */ if (r_ptr->total_visible == 1) { c_prt(attr, (r_ptr->name), (num % (h - 1)) + 1, (num / (h - 1) * 26)); } else { c_prt(attr, format("%s (x%d)", r_ptr->name, r_ptr->total_visible), (num % (h - 1)) + 1, (num / (h - 1)) * 26); } num++; } } else { c_prt(TERM_WHITE, "You see no monsters.", 0, 0); } /* Fresh */ Term_fresh(); /* Restore */ Term_activate(old); } }
void AccessCase::emitIntrinsicGetter(AccessGenerationState& state) { CCallHelpers& jit = *state.jit; JSValueRegs valueRegs = state.valueRegs; GPRReg baseGPR = state.baseGPR; GPRReg valueGPR = valueRegs.payloadGPR(); switch (intrinsic()) { case TypedArrayLengthIntrinsic: { jit.load32(MacroAssembler::Address(state.baseGPR, JSArrayBufferView::offsetOfLength()), valueGPR); jit.boxInt32(valueGPR, valueRegs, CCallHelpers::DoNotHaveTagRegisters); state.succeed(); return; } case TypedArrayByteLengthIntrinsic: { TypedArrayType type = structure()->classInfo()->typedArrayStorageType; jit.load32(MacroAssembler::Address(state.baseGPR, JSArrayBufferView::offsetOfLength()), valueGPR); if (elementSize(type) > 1) { // We can use a bitshift here since we TypedArrays cannot have byteLength that overflows an int32. jit.lshift32(valueGPR, Imm32(logElementSize(type)), valueGPR); } jit.boxInt32(valueGPR, valueRegs, CCallHelpers::DoNotHaveTagRegisters); state.succeed(); return; } case TypedArrayByteOffsetIntrinsic: { GPRReg scratchGPR = state.scratchGPR; CCallHelpers::Jump emptyByteOffset = jit.branch32( MacroAssembler::NotEqual, MacroAssembler::Address(baseGPR, JSArrayBufferView::offsetOfMode()), TrustedImm32(WastefulTypedArray)); jit.loadPtr(MacroAssembler::Address(baseGPR, JSObject::butterflyOffset()), scratchGPR); jit.loadPtr(MacroAssembler::Address(baseGPR, JSArrayBufferView::offsetOfVector()), valueGPR); jit.loadPtr(MacroAssembler::Address(scratchGPR, Butterfly::offsetOfArrayBuffer()), scratchGPR); jit.loadPtr(MacroAssembler::Address(scratchGPR, ArrayBuffer::offsetOfData()), scratchGPR); jit.subPtr(scratchGPR, valueGPR); CCallHelpers::Jump done = jit.jump(); emptyByteOffset.link(&jit); jit.move(TrustedImmPtr(0), valueGPR); done.link(&jit); jit.boxInt32(valueGPR, valueRegs, CCallHelpers::DoNotHaveTagRegisters); state.succeed(); return; } default: break; } RELEASE_ASSERT_NOT_REACHED(); }