void setupRuntime() {
HiddenClass::getRoot();
object_cls = new BoxedClass(NULL, 0, sizeof(Box), false);
type_cls = new BoxedClass(object_cls, offsetof(BoxedClass, attrs), sizeof(BoxedClass), false);
type_cls->cls = type_cls;
object_cls->cls = type_cls;
none_cls = new BoxedClass(object_cls, 0, sizeof(Box), false);
None = new Box(&none_flavor, none_cls);
str_cls = new BoxedClass(object_cls, 0, sizeof(BoxedString), false);
// It wasn't safe to add __base__ attributes until object+type+str are set up, so do that now:
type_cls->giveAttr("__base__", object_cls);
str_cls->giveAttr("__base__", object_cls);
none_cls->giveAttr("__base__", object_cls);
object_cls->giveAttr("__base__", None);
tuple_cls = new BoxedClass(object_cls, 0, sizeof(BoxedTuple), false);
EmptyTuple = new BoxedTuple({});
gc::registerStaticRootObj(EmptyTuple);
module_cls = new BoxedClass(object_cls, offsetof(BoxedModule, attrs), sizeof(BoxedModule), false);
// TODO it'd be nice to be able to do these in the respective setupType methods,
// but those setup methods probably want access to these objects.
// We could have a multi-stage setup process, but that seems overkill for now.
bool_cls = new BoxedClass(object_cls, 0, sizeof(BoxedBool), false);
int_cls = new BoxedClass(object_cls, 0, sizeof(BoxedInt), false);
float_cls = new BoxedClass(object_cls, 0, sizeof(BoxedFloat), false);
function_cls = new BoxedClass(object_cls, offsetof(BoxedFunction, attrs), sizeof(BoxedFunction), false);
instancemethod_cls = new BoxedClass(object_cls, 0, sizeof(BoxedInstanceMethod), false);
list_cls = new BoxedClass(object_cls, 0, sizeof(BoxedList), false);
slice_cls = new BoxedClass(object_cls, 0, sizeof(BoxedSlice), false);
dict_cls = new BoxedClass(object_cls, 0, sizeof(BoxedDict), false);
file_cls = new BoxedClass(object_cls, 0, sizeof(BoxedFile), false);
set_cls = new BoxedClass(object_cls, 0, sizeof(BoxedSet), false);
member_cls = new BoxedClass(object_cls, 0, sizeof(BoxedMemberDescriptor), false);
STR = typeFromClass(str_cls);
BOXED_INT = typeFromClass(int_cls);
BOXED_FLOAT = typeFromClass(float_cls);
BOXED_BOOL = typeFromClass(bool_cls);
NONE = typeFromClass(none_cls);
LIST = typeFromClass(list_cls);
SLICE = typeFromClass(slice_cls);
MODULE = typeFromClass(module_cls);
DICT = typeFromClass(dict_cls);
SET = typeFromClass(set_cls);
BOXED_TUPLE = typeFromClass(tuple_cls);
object_cls->giveAttr("__name__", boxStrConstant("object"));
object_cls->giveAttr("__new__", new BoxedFunction(boxRTFunction((void*)objectNew, UNKNOWN, 1, 0, true, false)));
object_cls->freeze();
auto typeCallObj = boxRTFunction((void*)typeCall, UNKNOWN, 1, 0, true, false);
typeCallObj->internal_callable = &typeCallInternal;
type_cls->giveAttr("__call__", new BoxedFunction(typeCallObj));
type_cls->giveAttr("__name__", boxStrConstant("type"));
type_cls->giveAttr("__new__", new BoxedFunction(boxRTFunction((void*)typeNew, UNKNOWN, 2)));
type_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)typeRepr, STR, 1)));
type_cls->giveAttr("__str__", type_cls->getattr("__repr__"));
type_cls->freeze();
none_cls->giveAttr("__name__", boxStrConstant("NoneType"));
none_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)noneRepr, STR, 1)));
none_cls->giveAttr("__str__", none_cls->getattr("__repr__"));
none_cls->giveAttr("__hash__", new BoxedFunction(boxRTFunction((void*)noneHash, UNKNOWN, 1)));
none_cls->freeze();
module_cls->giveAttr("__name__", boxStrConstant("module"));
module_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)moduleRepr, STR, 1)));
module_cls->giveAttr("__str__", module_cls->getattr("__repr__"));
module_cls->freeze();
member_cls->giveAttr("__name__", boxStrConstant("member"));
member_cls->freeze();
setupBool();
setupInt();
setupFloat();
setupStr();
setupList();
setupDict();
setupSet();
setupTuple();
setupFile();
function_cls->giveAttr("__name__", boxStrConstant("function"));
function_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)functionRepr, STR, 1)));
function_cls->giveAttr("__str__", function_cls->getattr("__repr__"));
function_cls->freeze();
instancemethod_cls->giveAttr("__name__", boxStrConstant("instancemethod"));
instancemethod_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)instancemethodRepr, STR, 1)));
instancemethod_cls->freeze();
slice_cls->giveAttr("__name__", boxStrConstant("slice"));
slice_cls->giveAttr("__new__",
new BoxedFunction(boxRTFunction((void*)sliceNew, UNKNOWN, 4, 2, false, false), { NULL, None }));
slice_cls->giveAttr("__repr__", new BoxedFunction(boxRTFunction((void*)sliceRepr, STR, 1)));
slice_cls->giveAttr("__str__", slice_cls->getattr("__repr__"));
slice_cls->giveAttr("start", new BoxedMemberDescriptor(BoxedMemberDescriptor::OBJECT, SLICE_START_OFFSET));
slice_cls->giveAttr("stop", new BoxedMemberDescriptor(BoxedMemberDescriptor::OBJECT, SLICE_STOP_OFFSET));
slice_cls->giveAttr("step", new BoxedMemberDescriptor(BoxedMemberDescriptor::OBJECT, SLICE_STEP_OFFSET));
slice_cls->freeze();
// sys is the first module that needs to be set up, due to modules
// being tracked in sys.modules:
setupSys();
setupBuiltins();
setupMath();
setupTime();
setupThread();
setupCAPI();
TRACK_ALLOCATIONS = true;
}
int SysSPA2d::doSPA(int niter, double sLambda, int useCSparse, double initTol,
int maxCGiters)
{
// number of nodes
int ncams = nodes.size();
// set number of constraints
int ncons = p2cons.size();
// check for fixed frames
if (nFixed <= 0)
{
cout << "[doSPA2d] No fixed frames" << endl;
return 0;
}
for (int i=0; i<ncams; i++)
{
Node2d &nd = nodes[i];
if (i >= nFixed)
nd.isFixed = false;
else
nd.isFixed = true;
nd.setTransform(); // set up world-to-node transform for cost calculation
nd.setDr(); // always use local angles
}
// initialize vars
if (sLambda > 0.0) // do we initialize lambda?
lambda = sLambda;
double laminc = 2.0; // how much to increment lambda if we fail
double lamdec = 0.5; // how much to decrement lambda if we succeed
int iter = 0; // iterations
sqMinDelta = 1e-8 * 1e-8;
double cost = calcCost();
if (verbose)
cout << iter << " Initial squared cost: " << cost << " which is "
<< sqrt(cost/ncons) << " rms error" << endl;
int good_iter = 0;
double cumTime = 0;
for (; iter<niter; iter++) // loop at most <niter> times
{
// set up and solve linear system
// NOTE: shouldn't need to redo all calcs in setupSys if we
// got here from a bad update
long long t0, t1, t2, t3;
t0 = utime();
if (useCSparse)
setupSparseSys(lambda,iter,useCSparse); // set up sparse linear system
else
setupSys(lambda); // set up linear system
// cout << "[SPA] Solving...";
t1 = utime();
// use appropriate linear solver
if (useCSparse == SBA_BLOCK_JACOBIAN_PCG)
{
if (csp.B.rows() != 0)
{
int iters = csp.doBPCG(maxCGiters,initTol,iter);
if (verbose)
cout << "[Block PCG] " << iters << " iterations" << endl;
}
}
#ifdef SBA_DSIF
// PCG with incomplete Cholesky
else if (useCSparse == 3)
{
int res = csp.doPCG(maxCGiters);
if (res > 1)
cout << "[DoSPA] Sparse PCG failed with error " << res << endl;
}
#endif
else if (useCSparse > 0)
{
if (csp.B.rows() != 0)
{
bool ok = csp.doChol();
if (!ok)
cout << "[DoSBA] Sparse Cholesky failed!" << endl;
}
}
// Dense direct Cholesky
else
A.ldlt().solveInPlace(B); // Cholesky decomposition and solution
t2 = utime();
// cout << "solved" << endl;
// get correct result vector
VectorXd &BB = useCSparse ? csp.B : B;
// check for convergence
// this is a pretty crummy convergence measure...
double sqDiff = BB.squaredNorm();
if (sqDiff < sqMinDelta) // converged, done...
{
if (verbose)
cout << "Converged with delta: " << sqrt(sqDiff) << endl;
break;
}
// update the frames
int ci = 0;
for(int i=0; i < ncams; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.oldtrans = nd.trans; // save in case we don't improve the cost
nd.oldarot = nd.arot;
nd.trans.head<2>() += BB.segment<2>(ci);
nd.arot += BB(ci+2);
nd.normArot();
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr(); // set rotational derivatives
ci += 3; // advance B index
}
// new cost
double newcost = calcCost();
if (verbose)
cout << iter << " Updated squared cost: " << newcost << " which is "
<< sqrt(newcost/ncons) << " rms error" << endl;
// check if we did good
if (newcost < cost) // && iter != 0) // NOTE: iter==0 case is for checking
{
cost = newcost;
lambda *= lamdec; // decrease lambda
// laminc = 2.0; // reset bad lambda factor; not sure if this is a good idea...
good_iter++;
}
else
{
lambda *= laminc; // increase lambda
laminc *= 2.0; // increase the increment
// reset nodes
for(int i=0; i<ncams; i++)
{
Node2d &nd = nodes[i];
if (nd.isFixed) continue; // not to be updated
nd.trans = nd.oldtrans;
nd.arot = nd.oldarot;
nd.setTransform(); // set up projection matrix for cost calculation
nd.setDr();
}
cost = calcCost(); // need to reset errors
if (verbose)
cout << iter << " Downdated cost: " << cost << endl;
// NOTE: shouldn't need to redo all calcs in setupSys
}
t3 = utime();
if (iter == 0 && verbose)
{
printf("[SPA] Setup: %0.2f ms Solve: %0.2f ms Update: %0.2f ms\n",
0.001*(double)(t1-t0),
0.001*(double)(t2-t1),
0.001*(double)(t3-t2));
}
double dt=1e-6*(double)(t3-t0);
cumTime+=dt;
if (print_iros_stats){
cerr << "iteration= " << iter
<< "\t chi2= " << cost
<< "\t time= " << dt
<< "\t cumTime= " << cumTime
<< "\t kurtChi2= " << cost
<< endl;
}
}
// return number of iterations performed
return good_iter;
}
