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