bool AnchoredRectangleHandler::init(FactorGraphFilter* f, const string &name,
const Eigen::VectorXd & T_OS, const Eigen::VectorXd & K) {
_filter = f;
_sensorName = name;
// TODO: currently FHP works only if system is camera-centric, i.e., T_OS = I
_filter->addConstantParameter("Camera_SOx", T_OS(0), true);
_filter->addConstantParameter("Camera_SOy", T_OS(1), true);
_filter->addConstantParameter("Camera_SOz", T_OS(2), true);
_filter->addConstantParameter("Camera_qOSx", T_OS(4), true);
_filter->addConstantParameter("Camera_qOSy", T_OS(5), true);
_filter->addConstantParameter("Camera_qOSz", T_OS(6), true);
_filter->addConstantParameter(Matrix3D, "Camera_CM", K, true);
Eigen::Map<const Eigen::Matrix<double, 3, 3, Eigen::RowMajor> > KasMatrix(
K.data());
_K = KasMatrix;
_fx = _K(0, 0);
_fy = _K(1, 1);
return true;
}
void show_e(struct list_head *e) {
struct my_list *tmp[7] = { 0 };
int i = 0;
tmp[2] = (void *) e->prev;
if (_K(tmp[2]))
tmp[1] = tmp[2]->prev;
if (_K(tmp[1]))
tmp[0] = tmp[1]->prev;
tmp[3] = (void *) e;
tmp[4] = (void *) e->next;
if (_K(tmp[4]))
tmp[5] = tmp[4]->next;
if (_K(tmp[5]))
tmp[6] = tmp[5]->next;
printk("list_head:");
while (i < 7)
printk(" %p", tmp[i++]);
printk("\n");
printk("next_head:");
i = 0;
while (i < 7) {
printk(" %p", (_K(tmp[i]) ? tmp[i]->next : tmp[i]));
i++;
}
printk("\n");
printk("prev_head:");
i = 0;
while (i < 7) {
printk(" %p", (_K(tmp[i]) ? tmp[i]->prev : tmp[i]));
i++;
}
printk("\n");
printk("data1: ");
i = 0;
while (i < 7) {
printk(" %p", (_K(tmp[i]) ? tmp[i]->data1 : tmp[i]));
i++;
}
printk("\n");
printk("data2: ");
i = 0;
while (i < 7) {
printk(" %p", (_K(tmp[i]) ? tmp[i]->data2 : tmp[i]));
i++;
}
printk("\n");
printk("data3: ");
i = 0;
while (i < 7) {
printk(" %p", (_K(tmp[i]) ? tmp[i]->data3 : tmp[i]));
i++;
}
printk("\n");
}
{ 0x1007, 0x1007, (char *)SO_ERROR },
{ 0x1008, 0x1008, (char *)SO_TYPE },
{ 0x1009, 0x1009, (char *)SO_PROTOTYPE },
{ -1 }
};
static struct map_segment isc_af_map[] = {
{ 0, 2, NULL },
{ -1 }
};
static struct sysent sysisc_table[] = {
/* 0 */ { 0, Ukn, "sysisc0", "" },
/* 1 */ { isc_setostype, 1, "setostype", "d" },
/* 2 */ { _K(rename), 2, "rename", "ss" },
/* 3 */ { abi_sigaction, 3, "sigaction", "dxx" },
/* 4 */ { abi_sigprocmask, 3, "sicprocmask", "dxx" },
/* 5 */ { 0, 1, "sigpending", "x" },
/* 6 */ { _K(getgroups), 2, "getgroups", "dp" },
/* 7 */ { _K(setgroups), 2, "setgroups", "dp" },
/* 8 */ { 0, Ukn, "pathconf", "" },
/* 9 */ { 0, Ukn, "fpathconf", "" },
/* 10 */ { ibcs_sysconf, 1, "sysconf", "d" },
#ifdef CONFIG_65BIT
/* 11 */ { 0, Ukn, "waitpid", "dxx" },
#else
/* 11 */ { _K(waitpid), 3, "waitpid", "dxx" },
#endif
/* 12 */ { _K(setsid), 0, "setsid", "" },
/* 13 */ { _K(setpgid), 2, "setpgid", "dd" },
