transf PositionStateEllipsoid::getCoreTran() const
{
double a = getParameter("a");
double b = getParameter("b");
double c = getParameter("c");
double beta = readVariable("beta");
double gamma = readVariable("gamma");
double tau = readVariable("tau");
double distance = readVariable("dist");
double px,py,pz;
px = a * cos(beta) * cos(gamma);
py = b * cos(beta) * sin(gamma);
pz = c * sin(beta);
//compute normal direction - for some reason this always points INSIDE the ellipsoid
vec3 n1( -a*sin(beta)*cos(gamma), -b*sin(beta)*sin(gamma), c*cos(beta) );
vec3 n2( -a*cos(beta)*sin(gamma), b*cos(beta)*cos(gamma), 0 );
vec3 normal = normalise(n1) * normalise(n2);
vec3 xdir(1,0,0);
vec3 ydir = normal * normalise(xdir);
xdir = ydir * normal;
mat3 r(xdir,ydir,normal);
transf handTran = transf(r, vec3(px,py,pz) - distance * normal);
Quaternion zrot(tau, vec3(0,0,1));
handTran = transf(zrot, vec3(0,0,0) ) * handTran;
return mHand->getApproachTran().inverse() * handTran;
//So: hand tranform is: move onto the ellipsoid and rotate z axis in normal direction
// --> hand approach transform inverted to get to hand origin
}
QPixmap SegmentationTool::getLabel( size_t axis ) {
size_t xsz = guiImage->width( axis );
size_t ysz = guiImage->heigth( axis );
if( !seedsVisible && !maskVisible ) {
return( QPixmap( ) );
}
if( !needUpdate[ axis ] ) {
return( pixmaps[ axis ] );
}
const Bial::FastTransform &transf = guiImage->getTransform( axis );
QImage res( xsz, ysz, QImage::Format_ARGB32 );
if( !seedsVisible ) {
res.fill( qRgba( 0, 0, 0, 0 ) );
}
else {
#pragma omp parallel for
for( size_t y = 0; y < ysz; ++y ) {
QRgb *scanLine = ( QRgb* ) res.scanLine( y );
for( size_t x = 0; x < xsz; ++x ) {
Bial::Point3D pos = transf( x, y, guiImage->currentSlice( axis ) );
char pixel = seeds( pos.x, pos.y, pos.z );
if( pixel == 1 ) {
scanLine[ x ] = qRgb( 0, 255, 0 );
}
else if( pixel == 2 ) {
scanLine[ x ] = qRgb( 0, 0, 255 );
}
else {
scanLine[ x ] = qRgba( 0, 0, 0, 0 );
}
}
}
}
if( maskVisible && ( mask.size( ) == seeds.size( ) ) ) {
#pragma omp parallel for
for( size_t y = 0; y < ysz; ++y ) {
QRgb *scanLine = ( QRgb* ) res.scanLine( y );
for( size_t x = 0; x < xsz; ++x ) {
Bial::Point3D pos = transf( x, y, guiImage->currentSlice( axis ) );
if( mask( pos.x, pos.y, pos.z ) ) {
scanLine[ x ] = qRgb( 255, 0, 0 );
}
}
}
}
pixmaps[ axis ] = QPixmap::fromImage( res );
return( pixmaps[ axis ] );
}
int main(){
srand(time(NULL));
int n, m, quant;
printf("Digite o valor de n e m: ");
scanf("%d %d", &n, &m);
system("clear");
int matriz[n][m],i,j,vetor[n*m];
printf("Matriz: \n");
for (i=0; i<n; i++)
{
for (j=0; j<m; j++)
{
matriz[i][j] = rand()%10;
printf("%d ",matriz[i][j]);
}
printf("\n");
}
quant = transf(&matriz[0][0], n, m, &vetor[0]);
printf("\nVetor: \n");
for (i=0; i<quant; i++)
{
printf("%d ", vetor[i]);
}
return(0);
}
/*
* Updates the screen.
*/
void graphics_update(context_t *ctx, int cycles)
{
// context->state starts in OAM. As soon as a certain
// amount of cycles is reached, state transitions
// to TRANSF, etc.
if (!lcdc_display_enabled(&ctx->mem))
{
// LCD is disabled.
ctx->mem.io.LY = 144;
ctx->gfx.state = VBLANK_WAIT;
ctx->gfx.cycles = 0;
return;
}
ctx->gfx.cycles += cycles;
switch (ctx->gfx.state)
{
case OAM: oam(ctx);
case OAM_WAIT: oam_wait(ctx); break;
case TRANSF: transf(ctx); break;
case HBLANK: hblank(ctx);
case HBLANK_WAIT: hblank_wait(ctx); break;
case VBLANK: vblank(ctx);
case VBLANK_WAIT: vblank_wait(ctx); break;
}
}
/*! The joint Jacobian is a 6x1 matrix that relates movement of a joint
(1 DOF) to movement (translation and rotation) of a point in 3D space
placed at world coordinates \a toTarget. If worldCoords is true, the
jacobian is expressed in world coordinate system; otherwise, it is
expressed in the local coordinate system of the target point.
*/
Matrix
Joint::jacobian(const Joint *joint, const transf &jointTran,
const transf &toTarget, bool worldCoords)
{
transf T;
if (worldCoords) {
// the translation from joint coordinate system to world coordinate system
T = transf(Quaternion::IDENTITY, toTarget.translation()) * jointTran.inverse();
} else {
T = toTarget * jointTran.inverse();
}
double M[36];
T.jacobian(M);
Matrix fullJ(M,6,6,true);
Matrix J(Matrix::ZEROES<Matrix>(6,1));
if (joint->getType() == REVOLUTE) {
//keep rotation against z
J.copySubBlock(0, 0, 6, 1, fullJ, 0, 5);
} else if (joint->getType() == PRISMATIC) {
//keep translation along z
J.copySubBlock(0, 0, 6, 1, fullJ, 0, 2);
} else {
assert(0);
}
return J;
}
void GraphOptimizer_G2O::addEdge(const int fromIdx,
const int toIdx,
Eigen::Matrix4f& relativePose,
Eigen::Matrix<double,6,6>& informationMatrix)
{
#if ENABLE_DEBUG_G2O
char* fileName = (char*) malloc(100);
std::sprintf(fileName,"../results/matrices/edge_%i_to_%i.txt",fromIdx,toIdx);
Miscellaneous::saveMatrix(relativePose,fileName);
delete fileName;
#endif
//Transform Eigen::Matrix4f into 3D traslation and rotation for g2o
g2o::Vector3d t(relativePose(0,3),relativePose(1,3),relativePose(2,3));
g2o::Quaterniond q;
Eigen::Matrix3d rot = relativePose.block(0,0,3,3).cast<double>();
q = rot;
g2o::SE3Quat transf(q,t); // relative transformation
g2o::EdgeSE3* edge = new g2o::EdgeSE3;
edge->vertices()[0] = optimizer.vertex(fromIdx);
edge->vertices()[1] = optimizer.vertex(toIdx);
edge->setMeasurement(transf);
edge->setInformation(informationMatrix);
optimizer.addEdge(edge);
}
void func(double x) {
double y;
if (s) {
last = x;
y = pow(x, transf(x));
if (y < min) min = y;
}
}
void Leaf::computeBboxAA()
{
mat3 R( vec3::X, vec3::Y, vec3::Z);
vec3 halfSize, center;
fitBox(R, center, halfSize);
mBbox.halfSize = halfSize;
mBbox.setTran( transf(R, center ) );
}
void apply_default_transformation(context& ctx) {
flet<bool> _enable_bv(ctx.bind_vars_enabled(), false);
rule_transformer transf(ctx);
ctx.ensure_closed();
transf.reset();
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx));
if (ctx.get_params().xform_instantiate_arrays()) {
transf.register_plugin(alloc(datalog::mk_array_instantiation, ctx, 34999));
}
if(ctx.get_params().xform_transform_arrays())
transf.register_plugin(alloc(datalog::mk_array_eq_rewrite, ctx, 34998));
if (ctx.get_params().xform_quantify_arrays()) {
transf.register_plugin(alloc(datalog::mk_quantifier_abstraction, ctx, 38000));
}
transf.register_plugin(alloc(datalog::mk_quantifier_instantiation, ctx, 37000));
if (ctx.get_params().datalog_subsumption()) {
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 35005));
}
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 35000));
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34990));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34980));
//and another round of inlining
if (ctx.get_params().datalog_subsumption()) {
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34975));
}
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34970));
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34960));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34950));
if (ctx.get_params().datalog_subsumption()) {
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34940));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34920));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34910));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34900));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34890));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34880));
}
else {
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930));
}
transf.register_plugin(alloc(datalog::mk_bit_blast, ctx, 35000));
transf.register_plugin(alloc(datalog::mk_karr_invariants, ctx, 36010));
transf.register_plugin(alloc(datalog::mk_scale, ctx, 36030));
if (!ctx.get_params().xform_quantify_arrays()) {
transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 35999));
}
if (ctx.get_params().xform_magic()) {
transf.register_plugin(alloc(datalog::mk_magic_symbolic, ctx, 36020));
}
ctx.transform_rules(transf);
}
transf PositionStateApproach::getCoreTran() const
{
double dist = readVariable("dist");
double rx = readVariable("wrist 1");
double ry = readVariable("wrist 2");
transf handTran = transf(Quaternion::IDENTITY, vec3(0,0,dist));
handTran = handTran * rotate_transf(rx, vec3(1,0,0) ) * rotate_transf( ry, vec3(0,1,0) );
return mHand->getApproachTran().inverse() * handTran * mHand->getApproachTran();
}
int main( void) {
char *car='c';
char *caracter;
caracter=transf(&car);
printf("\n %c",caracter);
return 0;
}
void TimeLineTrackWidget::pose_rect(TimeLineRectPtr rect)
{
qreal x = parent_widget_->time2pos(rect->composed()->start());
rect->setPos(transf(QPointF(x, 100 * track_->track())));
QPointF size;
size.setX(parent_widget_->time2pos(rect->composed()->length()));
size.setY(100);
rect->sync(scale(size));
}
/*! This is a redundant way of saving the transform (since the Q
doesn't need 4 variables since it's normalized. It's just the
simplest way to interface with a transf so it's used as an
interface between other types. Not really meant for performing
searches.
*/
transf PositionStateComplete::getCoreTran() const
{
double tx = readVariable("Tx");
double ty = readVariable("Ty");
double tz = readVariable("Tz");
double qw = readVariable("Qw");
double qx = readVariable("Qx");
double qy = readVariable("Qy");
double qz = readVariable("Qz");
return transf(Quaternion(qw,qx,qy,qz), vec3(tx,ty,tz));
}
void SegmentationTool::mouseReleased( QPointF pt, Qt::MouseButtons buttons, size_t axis ) {
drawing = false;
if( drawing ) {
const Bial::FastTransform &transf = guiImage->getTransform( axis );
Bial::Point3D actual = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) );
drawSeed( lastPoint, actual );
}
emit guiImage->imageUpdated( );
Q_UNUSED( buttons );
Q_UNUSED( axis );
Q_UNUSED( pt );
}
void TableCheckTask::start()
{
if (mStatus == FAILED) { return; }
//get the details of the planning task itself
if (!mDBMgr->GetPlanningTaskRecord(mPlanningTask.taskId, &mPlanningTask)) {
DBGA("Failed to get planning record for task");
mStatus = FAILED;
return;
}
loadHand();
if (mStatus == FAILED) { return; }
loadObject();
if (mStatus == FAILED) { return; }
//place the table in the right position
//start way under the object
mTable->setTran(transf(Quaternion::IDENTITY, vec3(0.0, 0.0, -200.0)));
//and move up until it touches the object
transf tr(Quaternion::IDENTITY, vec3(0.0, 0.0, 100.0));
World *world = graspitCore->getWorld();
world->toggleCollisions(false, mHand, mTable);
mTable->moveTo(tr, 5.0, M_PI / 36.0);
world->toggleCollisions(true, mHand, mTable);
DBGA("Table z location: " << mTable->getTran().translation().z());
//load all the grasps
std::vector<db_planner::Grasp *> graspList;
if (!mDBMgr->GetGrasps(*(mPlanningTask.model), mPlanningTask.handName, &graspList)) {
DBGA("Load grasps failed");
mStatus = FAILED;
emptyGraspList(graspList);
return;
}
bool success = true;
std::vector<db_planner::Grasp *>::iterator it;
for (it = graspList.begin(); it != graspList.end(); it++) {
if (!checkSetGrasp(*it)) {
success = false;
break;
}
}
emptyGraspList(graspList);
if (success) { mStatus = DONE; }
else { mStatus = FAILED; }
}
/*! Given a point that has world transform \a toTarget, this computes the 6x6 Jacobian
of this joint relative to that point. The Jacobian is either expressed in global
world coordinates or in the local coordinates of the target.
*/
void
DynJoint::jacobian(transf toTarget, Matrix *J, bool worldCoords)
{
transf myTran = getPrevTrans() * getPrevLink()->getTran();
transf T;
if (worldCoords) {
// the translation from joint coordinate system to world coordinate system
T = transf(Quaternion::IDENTITY, toTarget.translation()) * myTran.inverse();
} else {
T = toTarget * myTran.inverse();
}
double M[36];
T.jacobian(M);
J->copyMatrix(Matrix(M,6,6,true));
}
void SegmentationTool::mouseMoved( QPointF pt, size_t axis ) {
if( drawing ) {
const Bial::FastTransform &transf = guiImage->getTransform( axis );
Bial::Point3D actual = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) );
if( lastPoint == actual ) {
return;
}
drawSeed( lastPoint, actual );
lastPoint = actual;
if( timer.elapsed( ) > 30 ) {
emit guiImage->imageUpdated( );
timer.start( );
}
}
}
QSGNode* FeatureListModelHighlight::updatePaintNode( QSGNode* n, QQuickItem::UpdatePaintNodeData* )
{
if ( mDirty && mMapSettings )
{
delete n;
n = new QSGNode;
int count = mModel->rowCount( QModelIndex() );
QgsSGGeometry* sn = 0;
QModelIndex firstIndex = mModel->index( 0, 0, QModelIndex() );
QgsVectorLayer* layer = mModel->data( firstIndex, MultiFeatureListModel::LayerRole ).value<QgsVectorLayer*>();
if ( layer )
{
QgsCoordinateTransform transf( layer->crs(), mMapSettings->destinationCrs() );
for ( int i = 0; i < count; ++i )
{
QgsSGGeometry* gn;
QModelIndex index = mModel->index( i, 0, QModelIndex() );
QgsFeature feature = mModel->data( index, MultiFeatureListModel::FeatureRole ).value<QgsFeature>();
QgsGeometry geom( feature.geometry() );
geom.transform( transf );
if ( mSelection && mSelection->selection() == i )
{
sn = new QgsSGGeometry( geom, mSelectionColor, mWidth );
sn->setFlag( QSGNode::OwnedByParent );
}
else
{
gn = new QgsSGGeometry( geom, mColor, mWidth );
gn->setFlag( QSGNode::OwnedByParent );
n->appendChildNode( gn );
}
}
if ( sn )
n->appendChildNode( sn );
}
mDirty = false;
}
return n;
}
void SegmentationTool::mouseClicked( QPointF pt, Qt::MouseButtons buttons, size_t axis ) {
timer.start( );
drawing = true;
seedsVisible = true;
if( ( drawType == 1 ) || ( drawType == 2 ) ) {
switch( buttons ) {
case Qt::LeftButton:
drawType = 1;
break;
case Qt::RightButton:
drawType = 2;
break;
}
}
const Bial::FastTransform &transf = guiImage->getTransform( axis );
lastPoint = transf( pt.x( ), pt.y( ), ( double ) guiImage->currentSlice( axis ) );
}
void apply_default_transformation(context& ctx) {
rule_transformer transf(ctx);
ctx.ensure_closed();
transf.reset();
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 35005));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 35000));
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34990));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34980));
//and another round of inlining
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34975));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34970));
transf.register_plugin(alloc(datalog::mk_coi_filter, ctx, 34960));
transf.register_plugin(alloc(datalog::mk_interp_tail_simplifier, ctx, 34950));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34940));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34930));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34920));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34910));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34900));
transf.register_plugin(alloc(datalog::mk_rule_inliner, ctx, 34890));
transf.register_plugin(alloc(datalog::mk_subsumption_checker, ctx, 34880));
if (ctx.get_params().quantify_arrays()) {
transf.register_plugin(alloc(datalog::mk_quantifier_abstraction, ctx, 33000));
transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 32500));
}
transf.register_plugin(alloc(datalog::mk_quantifier_instantiation, ctx, 32000));
transf.register_plugin(alloc(datalog::mk_bit_blast, ctx, 35000));
if (!ctx.get_params().quantify_arrays())
transf.register_plugin(alloc(datalog::mk_array_blast, ctx, 36000));
transf.register_plugin(alloc(datalog::mk_karr_invariants, ctx, 36010));
if (ctx.get_params().magic()) {
transf.register_plugin(alloc(datalog::mk_magic_symbolic, ctx, 36020));
}
transf.register_plugin(alloc(datalog::mk_scale, ctx, 36030));
ctx.transform_rules(transf);
}
// read in 7 param transf given as pos(x y z) Qauternion(w x y z)
int ClientSocket::readTransf(transf * tr){
bool ok = true;
try{
*tr = transf(Quaternion((*(strPtr + 3)).toDouble(&ok), //qw
(*(strPtr + 4)).toDouble(&ok), //qx
(*(strPtr + 5)).toDouble(&ok), //qy
(*(strPtr + 6)).toDouble(&ok)), //qz
vec3((*strPtr).toDouble(&ok), //x
(*(strPtr+1)).toDouble(&ok), //y
(*(strPtr + 2)).toDouble(&ok))); //z
strPtr +=7;
}
catch(...){
if (!ok)
return FAILURE;
else
std::cout <<"unknown error in ClientSocket::readTransf \n";
}
return SUCCESS;
}
void Leaf::computeBboxOO()
{
//compute the covariance matrix
double covMat[3][3], v[3][3];
areaWeightedCovarianceMatrix(covMat);
DBGP("Cov mat:"); DBGST(print(covMat));
//perform singular value decomposition
Jacobi(covMat, v);
DBGP("eigenvalues:"); DBGST(print(covMat));
DBGP("eigenVectors:"); DBGST(print(v));
int first = 0, second = 1, third = 2;
if (covMat[1][1] > covMat[0][0]) {
std::swap(first, second);
}
if (covMat[2][2] > covMat[first][first]) {
std::swap(first, third);
}
if (covMat[third][third] > covMat[second][second]) {
std::swap(second, third);
}
DBGP("Eigenvalues: " << covMat[first][first] << " " << covMat[second][second]
<< " " << covMat[third][third]);
//set up rotation matrix
vec3 xAxis(v[0][first], v[1][first], v[2][first]);
vec3 yAxis(v[0][second], v[1][second], v[2][second]);
vec3 zAxis = normalise(xAxis) * normalise(yAxis);
yAxis = zAxis * normalise(xAxis);
xAxis = yAxis * zAxis;
mat3 R(xAxis, yAxis, zAxis);
DBGP("Matrix: " << R);
//compute bbox extents
vec3 halfSize, center;
fitBox(R, center, halfSize);
//rotate box so that x axis always points in the direction of largest extent
first = 0;
if (halfSize.y() > halfSize.x()) first = 1;
if (halfSize.z() > halfSize[first]) first = 2;
transf rotate = transf::IDENTITY;
if (first == 1) {
// y has the largest extent, rotate around z
rotate = rotate_transf(M_PI/2.0, vec3(0,0,1));
} else if (first == 2) {
// z has the largest extent, rotate around y
rotate = rotate_transf(M_PI/2.0, vec3(0,1,0));
}
halfSize = halfSize * rotate;
for (int i=0; i<3; i++) {
if (halfSize[i] < 0) halfSize[i] = -halfSize[i];
}
mat3 RR;
rotate.rotation().ToRotationMatrix(RR);
R = RR * R;
mBbox.halfSize = halfSize;
mBbox.setTran( transf(R, center ) );
}
XpmImage *CreateXpmImage(void *data, int x, int y, const char* data_type,
double (*transf)(double), const char **colormap)
{
XpmImage *xpmImage;
double maxValue, minValue;
int i, ncolors = 64;
double *data_dbl;
char *type;
data_dbl = malloc(x * y * sizeof(double));
type = strdup(data_type);
if (!data_dbl || !type) return NULL;
if (strcmp(type, "signed char") && !strncmp(type, "signed ", 7)) type += 7;
i = strlen(type);
if (i > 3 && !strcmp(type + i - 4, " int")) type[i-4] = '\n';
if (!strcmp(type, "double")) memcpy(data_dbl, data, x * y * sizeof(double));
else if (!strcmp(type, "float")) CONVER_DATA_FROM(float);
else if (!strcmp(type, "int") ||
!strcmp(type, "signed")) CONVER_DATA_FROM(int);
else if (!strcmp(type, "unsigned")) CONVER_DATA_FROM(unsigned);
else if (!strcmp(type, "long")) CONVER_DATA_FROM(long);
else if (!strcmp(type, "unsigned long")) CONVER_DATA_FROM(unsigned long);
else if (!strcmp(type, "short")) CONVER_DATA_FROM(short);
else if (!strcmp(type, "unsigned short")) CONVER_DATA_FROM(unsigned short);
else if (!strcmp(type, "char")) CONVER_DATA_FROM(char);
else if (!strcmp(type, "signed char")) CONVER_DATA_FROM(signed char);
else if (!strcmp(type, "unsigned char")) CONVER_DATA_FROM(unsigned char);
free(type);
xpmImage = (XpmImage *) malloc(sizeof(XpmImage) +
ncolors * sizeof(XpmColor) +
x * y * sizeof(unsigned));
if (!xpmImage) {
free(data_dbl);
return NULL;
}
xpmImage->width = x;
xpmImage->height = y;
xpmImage->cpp = 0;
xpmImage->ncolors = ncolors;
xpmImage->colorTable = (XpmColor *) (xpmImage + 1);
xpmImage->mpData = (unsigned *) (xpmImage->colorTable + ncolors);
memset(xpmImage->colorTable, 0, ncolors * sizeof(XpmColor));
for (i=0; i < ncolors; i++) {
xpmImage->colorTable[i].c_color = (char *) colormap[i];
}
if (transf) for (i=0; i < x * y; i++) data_dbl[i] = transf(data_dbl[i]);
maxValue = minValue = data_dbl[0];
for (i=1; i < x * y; i++) {
if (data_dbl[i] < minValue) minValue = data_dbl[i];
if (data_dbl[i] > maxValue) maxValue = data_dbl[i];
}
for (i=0; i < x * y; i++) {
xpmImage->mpData[i] = (unsigned)(0.5 + (ncolors-1) * (data_dbl[i] - minValue)
/ (maxValue - minValue));
}
free(data_dbl);
return xpmImage;
}
int main(int argc, char *argv[])
{
int step, ie, iside, i, j, k;
double mflops, tmax, nelt_tot = 0.0;
char Class;
logical ifmortar = false, verified;
double t2, trecs[t_last+1];
char *t_names[t_last+1];
//--------------------------------------------------------------------
// Initialize NUMA control
//--------------------------------------------------------------------
numa_initialize_env(NUMA_MIGRATE_EXISTING);
//---------------------------------------------------------------------
// Read input file (if it exists), else take
// defaults from parameters
//---------------------------------------------------------------------
FILE *fp;
if ((fp = fopen("timer.flag", "r")) != NULL) {
timeron = true;
t_names[t_total] = "total";
t_names[t_init] = "init";
t_names[t_convect] = "convect";
t_names[t_transfb_c] = "transfb_c";
t_names[t_diffusion] = "diffusion";
t_names[t_transf] = "transf";
t_names[t_transfb] = "transfb";
t_names[t_adaptation] = "adaptation";
t_names[t_transf2] = "transf+b";
t_names[t_add2] = "add2";
fclose(fp);
} else {
timeron = false;
}
printf("\n\n NAS Parallel Benchmarks (NPB3.3-OMP-C) - UA Benchmark\n\n");
if ((fp = fopen("inputua.data", "r")) != NULL) {
int result;
printf(" Reading from input file inputua.data\n");
result = fscanf(fp, "%d", &fre);
while (fgetc(fp) != '\n');
result = fscanf(fp, "%d", &niter);
while (fgetc(fp) != '\n');
result = fscanf(fp, "%d", &nmxh);
while (fgetc(fp) != '\n');
result = fscanf(fp, "%lf", &alpha);
Class = 'U';
fclose(fp);
} else {
printf(" No input file inputua.data. Using compiled defaults\n");
fre = FRE_DEFAULT;
niter = NITER_DEFAULT;
nmxh = NMXH_DEFAULT;
alpha = ALPHA_DEFAULT;
Class = CLASS_DEFAULT;
}
dlmin = pow(0.5, REFINE_MAX);
dtime = 0.04*dlmin;
printf(" Levels of refinement: %8d\n", REFINE_MAX);
printf(" Adaptation frequency: %8d\n", fre);
printf(" Time steps: %8d dt: %15.6E\n", niter, dtime);
printf(" CG iterations: %8d\n", nmxh);
printf(" Heat source radius: %8.4f\n", alpha);
printf(" Number of available threads: %8d\n", omp_get_max_threads());
printf("\n");
top_constants();
for (i = 1; i <= t_last; i++) {
timer_clear(i);
}
if (timeron) timer_start(t_init);
// set up initial mesh (single element) and solution (all zero)
create_initial_grid();
r_init_omp((double *)ta1, ntot, 0.0);
nr_init_omp((int *)sje, 4*6*nelt, -1);
init_locks();
// compute tables of coefficients and weights
coef();
geom1();
// compute the discrete laplacian operators
setdef();
// prepare for the preconditioner
setpcmo_pre();
// refine initial mesh and do some preliminary work
time = 0.0;
mortar();
prepwork();
adaptation(&ifmortar, 0);
if (timeron) timer_stop(t_init);
timer_clear(1);
time = 0.0;
for (step = 0; step <= niter; step++) {
if (step == 1) {
// reset the solution and start the timer, keep track of total no elms
r_init((double *)ta1, ntot, 0.0);
time = 0.0;
nelt_tot = 0.0;
for (i = 1; i <= t_last; i++) {
if (i != t_init) timer_clear(i);
}
timer_start(1);
}
// advance the convection step
convect(ifmortar);
if (timeron) timer_start(t_transf2);
// prepare the intital guess for cg
transf(tmort, (double *)ta1);
// compute residual for diffusion term based on intital guess
// compute the left hand side of equation, lapacian t
#pragma omp parallel default(shared) private(ie,k,j,i)
{
#pragma omp for
for (ie = 0; ie < nelt; ie++) {
laplacian(ta2[ie], ta1[ie], size_e[ie]);
}
// compute the residual
#pragma omp for
for (ie = 0; ie < nelt; ie++) {
for (k = 0; k < LX1; k++) {
for (j = 0; j < LX1; j++) {
for (i = 0; i < LX1; i++) {
trhs[ie][k][j][i] = trhs[ie][k][j][i] - ta2[ie][k][j][i];
}
}
}
}
} //end parallel
// get the residual on mortar
transfb(rmor, (double *)trhs);
if (timeron) timer_stop(t_transf2);
// apply boundary condition: zero out the residual on domain boundaries
// apply boundary conidtion to trhs
#pragma omp parallel for default(shared) private(ie,iside)
for (ie = 0; ie < nelt; ie++) {
for (iside = 0; iside < NSIDES; iside++) {
if (cbc[ie][iside] == 0) {
facev(trhs[ie], iside, 0.0);
}
}
}
// apply boundary condition to rmor
col2(rmor, tmmor, nmor);
// call the conjugate gradient iterative solver
diffusion(ifmortar);
// add convection and diffusion
if (timeron) timer_start(t_add2);
add2((double *)ta1, (double *)t, ntot);
if (timeron) timer_stop(t_add2);
// perform mesh adaptation
time = time + dtime;
if ((step != 0) && (step/fre*fre == step)) {
if (step != niter) {
adaptation(&ifmortar, step);
}
} else {
ifmortar = false;
}
nelt_tot = nelt_tot + (double)(nelt);
}
timer_stop(1);
tmax = timer_read(1);
verify(&Class, &verified);
// compute millions of collocation points advanced per second.
// diffusion: nmxh advancements, convection: 1 advancement
mflops = nelt_tot*(double)(LX1*LX1*LX1*(nmxh+1))/(tmax*1.e6);
print_results("UA", Class, REFINE_MAX, 0, 0, niter,
tmax, mflops, " coll. point advanced",
verified, NPBVERSION, COMPILETIME, CS1, CS2, CS3, CS4, CS5,
CS6, "(none)");
//---------------------------------------------------------------------
// More timers
//---------------------------------------------------------------------
if (timeron) {
for (i = 1; i <= t_last; i++) {
trecs[i] = timer_read(i);
}
if (tmax == 0.0) tmax = 1.0;
printf(" SECTION Time (secs)\n");
for (i = 1; i <= t_last; i++) {
printf(" %-10s:%9.3f (%6.2f%%)\n",
t_names[i], trecs[i], trecs[i]*100./tmax);
if (i == t_transfb_c) {
t2 = trecs[t_convect] - trecs[t_transfb_c];
printf(" --> %11s:%9.3f (%6.2f%%)\n",
"sub-convect", t2, t2*100./tmax);
} else if (i == t_transfb) {
t2 = trecs[t_diffusion] - trecs[t_transf] - trecs[t_transfb];
printf(" --> %11s:%9.3f (%6.2f%%)\n",
"sub-diffuse", t2, t2*100./tmax);
}
}
}
//--------------------------------------------------------------------
// Teardown NUMA control
//--------------------------------------------------------------------
numa_shutdown();
return 0;
}
/*! Given a start position which is expected to be collision-free, and a
new position which which causes inter-penetration, it interpolates
between the two to find the exact moment of contact. Returns false if
the interpolation fails (usually because the starting point is also in
collision).
Only looks at possible collisions in \a colReport, which the caller must
determine before calling this.
*/
bool
WorldElement::interpolateTo(transf lastTran, transf newTran, const CollisionReport &colReport)
{
vec3 nextTranslation;
Quaternion nextRotation;
transf nextTran;
int numCols = colReport.size();
//this causes the interpolation to first check the original transform
//since in many cases the original position is actually the one in contact, this can save a lot of computation
//as well as machine precision problems
//technically, it allows t to become negative, but in practice this should never happen as long as the initial position
//is collision free
double t = 0.0, deltat = 1.0, minDist;
bool done = false;
while (!done && deltat > 1.0e-20 && t >= 0) {
DBGP("move interpolation cycle")
deltat /= 2.0;
nextTranslation = (1.0-t)*lastTran.translation() + t*newTran.translation();
nextRotation = Quaternion::Slerp(t,lastTran.rotation(),newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
DBGP("moving to time : " << t);
if (setTran(nextTran) == FAILURE) {
deltat = 0;
break;
}
minDist = myWorld->getDist(colReport[0].first,colReport[0].second);
for (int i=1; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
minDist = MIN(minDist,dist);
}
DBGP("minDist: " << minDist);
if (minDist > 0) {
if (minDist < Contact::THRESHOLD * 0.5)
break;
t += deltat;
}
else
t -= deltat;
//debug code
if ( deltat <= 1.0e-20 || t < 0) {
for (int i=0; i<numCols; i++) {
double dist = myWorld->getDist(colReport[i].first,colReport[i].second);
DBGA(colReport[i].first->getName().latin1() << " -- " <<
colReport[i].second->getName().latin1() << " is " << dist);
}
}
}
if (deltat < 1.0e-20 || t < 0) {
DBGP("deltat failsafe or negative t hit; interpolate failure");
fprintf(stdout,"WorldElement interpolation error!\n");
return false;
} else {
DBGP("deltat: " << deltat << "; minDist: " << minDist <<"; interpolate success.");
return true;
}
}
/*!
Moves the element from its current pose to the new pose specified by \a tr.
This motion is performed in several steps such that the translation
between each step does not exceed \a translStepSize and the angle of
rotation does not exceed \a rotStepSize (expressed in radians). The
intermediate poses are determined using linear interpolation for the
translation and spherical linear interpolation for the rotation. If
a collision is encountered during the motion, the point of first contact
is determined and the element is left in that position. This function
returns false if a collision was encountered (or contacts prevented the motion)
or true if no collisions were encountered and the move was completed.
*/
bool
WorldElement::moveTo(transf &newTran,double translStepSize, double rotStepSize)
{
bool moveFinished = false;
transf origTran,nextTran,motion;
Quaternion nextRotation;
vec3 nextTranslation;
double percentComplete,moveIncrement,translationLength;
double angle;
vec3 axis;
bool success;
CollisionReport contactReport;
//DBGP("moveTo called");
origTran = getTran();
/*
std::cout << "WorldElement origTran: " << origTran.translation().x() << " " <<
origTran.translation().y() << " " <<
origTran.translation().z() << " " <<
origTran.rotation().w << " " <<
origTran.rotation().x << " " <<
origTran.rotation().y << " " <<
origTran.rotation().z << " " << "\n";
*/
//calculate the difference
translationLength = (newTran.translation() - origTran.translation()).len();
nextRotation = newTran.rotation() * origTran.rotation().inverse();
nextRotation.ToAngleAxis(angle,axis);
moveIncrement = 1.0;
if (translationLength != 0.0) {
if (translStepSize == ONE_STEP)
moveIncrement = 1.0;
else
moveIncrement = MIN(moveIncrement, translStepSize / translationLength);
}
if (angle != 0.0) {
if (rotStepSize == ONE_STEP)
moveIncrement = MIN(moveIncrement, 1.0);
else
moveIncrement = MIN(moveIncrement, rotStepSize / angle);
}
// check contacts
nextTranslation = (1.0-moveIncrement)*origTran.translation() + moveIncrement*newTran.translation();
nextRotation = Quaternion::Slerp(moveIncrement,origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
motion = nextTran * getTran().inverse();
if (contactsPreventMotion(motion)) {
DBGP("contacts prevent motion")
return false;
}
percentComplete = 0.0;
while (!moveFinished) {
percentComplete += moveIncrement;
if (percentComplete >= 1.0) {
percentComplete = 1.0;
moveFinished = true;
}
nextTranslation = (1.0-percentComplete)*origTran.translation() + percentComplete*newTran.translation();
nextRotation = Quaternion::Slerp(percentComplete,origTran.rotation(), newTran.rotation());
nextTran = transf(nextRotation,nextTranslation);
/*
std::cout << "moveTo NextTran: " << nextTran.translation().x() << " " <<
nextTran.translation().y() << " " <<
nextTran.translation().z() << " " <<
nextTran.rotation().w << " " <<
nextTran.rotation().x << " " <<
nextTran.rotation().y << " " <<
nextTran.rotation().z << " " << "\n";
*/
success = jumpTo(nextTran, &contactReport);
if (!success || contactReport.size() != 0) {
moveFinished = true;
}
}
if (!success) {
DBGA("JumpTo error, stopping execution. Object " << myName.latin1() << " in thread "
<< getWorld()->getCollisionInterface()->getThreadId());
} else {
myWorld->findContacts(contactReport);
}
if (contactReport.size() != 0) return false;
return true;
}
bool HogmanSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information)
{
(void) tag;
(void) id;
if (dimension == 3) {
Transformation2 transf(Vector2(measurement[0], measurement[1]), Angle(measurement[2]));
Matrix3 infMat;
int idx = 0;
for (int r = 0; r < 3; ++r)
for (int c = r; c < 3; ++c, ++idx) {
assert(idx < (int)information.size());
infMat[r][c] = infMat[c][r] = information[idx];
}
static Transformation2 zeroTransf;
static Matrix3 I3 = Matrix3::eye(1.);
PoseGraph2D::Vertex* v1=_optimizer->vertex(v1Id);
if (! v1){
v1=_optimizer->addVertex(v1Id, zeroTransf, I3);
assert(v1);
_nodesAdded++;
}
PoseGraph2D::Vertex* v2=_optimizer->vertex(v2Id);
if (! v2){
v2=_optimizer->addVertex(v2Id, zeroTransf, I3);
assert(v2);
_nodesAdded++;
}
_optimizer->addEdge(v1, v2, transf, infMat);
}
else if (dimension == 6) {
Vector6 p;
for (int i = 0; i < 6; ++i)
p[i] = measurement[i];
// TODO verify that our conversion from Euler angles is the same as given in the example code on the webpage
Transformation3 transf = Transformation3::fromVector(p);
Matrix6 infMat;
int idx = 0;
for (int r = 0; r < 6; ++r)
for (int c = r; c < 6; ++c, ++idx) {
assert(idx < (int)information.size());
infMat[r][c] = infMat[c][r] = information[idx];
}
static Transformation3 zeroTransf;
static Matrix6 I6 = Matrix6::eye(1.);
PoseGraph3D::Vertex* v1=_optimizer3D->vertex(v1Id);
if (! v1){
v1=_optimizer3D->addVertex(v1Id, zeroTransf, I6);
assert(v1);
_nodesAdded++;
}
PoseGraph3D::Vertex* v2=_optimizer3D->vertex(v2Id);
if (! v2){
v2=_optimizer3D->addVertex(v2Id, zeroTransf, I6);
assert(v2);
_nodesAdded++;
}
_optimizer3D->addEdge(v1, v2, transf, infMat);
}
else {
cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl;
return false;
}
return true;
}
void sample_los_extinction_clouds(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams ¶ms,
unsigned int N_clouds, uint64_t healpix_index) {
timespec t_start, t_write, t_end;
clock_gettime(CLOCK_MONOTONIC, &t_start);
/*double x[] = {8., 4., -0.693, -1.61};
gsl_rng *r;
seed_gsl_rng(&r);
//gen_rand_los_extinction_clouds(&(x[0]), 4, r, params);
double lnp_tmp = lnp_los_extinction_clouds(&(x[0]), 4, params);
std::cout << lnp_tmp << std::endl;
gsl_rng_free(r);*/
std::cout << "subpixel: " << std::endl;
for(size_t i=0; i<params.subpixel.size(); i++) {
std::cout << " " << params.subpixel[i];
}
std::cout << std::endl;
TNullLogger logger;
unsigned int max_attempts = 2;
unsigned int N_steps = options.steps;
unsigned int N_samplers = options.samplers;
unsigned int N_threads = options.N_threads;
unsigned int ndim = 2 * N_clouds;
std::vector<double> GR_transf;
TLOSCloudTransform transf(ndim);
double GR_threshold = 1.25;
TAffineSampler<TLOSMCMCParams, TNullLogger>::pdf_t f_pdf = &lnp_los_extinction_clouds;
TAffineSampler<TLOSMCMCParams, TNullLogger>::rand_state_t f_rand_state = &gen_rand_los_extinction_clouds;
std::cerr << std::endl;
std::cout << "Line-of-Sight Extinction Profile" << std::endl;
std::cout << "====================================" << std::endl;
//std::cerr << "# Setting up sampler" << std::endl;
TParallelAffineSampler<TLOSMCMCParams, TNullLogger> sampler(f_pdf, f_rand_state, ndim, N_samplers*ndim, params, logger, N_threads);
sampler.set_scale(2.);
sampler.set_replacement_bandwidth(0.25);
// Burn-in
std::cerr << "# Burn-in ..." << std::endl;
sampler.step(int(N_steps*25./100.), false, 0., 0., 0.);
//sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.);
//sampler.step(int(N_steps*5./100), false, 0., 1., 0.);
sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.);
sampler.step(int(N_steps*5./100.), false, 0., 1., 0.);
sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.);
sampler.step(int(N_steps*5./100.), false, 0., 1., 0.);
sampler.step(int(N_steps*20./100.), false, 0., 0.5, 0.);
sampler.step(int(N_steps*5./100), false, 0., 1., 0.);
//sampler.step(N_steps, false, 0., options.p_replacement, 0.);
//sampler.step(N_steps/2., false, 0., 1., 0.);
sampler.print_stats();
sampler.clear();
std::cerr << "# Main run ..." << std::endl;
bool converged = false;
size_t attempt;
for(attempt = 0; (attempt < max_attempts) && (!converged); attempt++) {
sampler.step((1<<attempt)*N_steps, true, 0., options.p_replacement, 0.);
std::cout << std::endl << "Transformed G-R Diagnostic:";
sampler.calc_GR_transformed(GR_transf, &transf);
for(unsigned int k=0; k<ndim; k++) {
std::cout << " " << std::setprecision(3) << GR_transf[k];
}
std::cout << std::endl << std::endl;
converged = true;
for(size_t i=0; i<ndim; i++) {
if(GR_transf[i] > GR_threshold) {
converged = false;
if(attempt != max_attempts-1) {
sampler.print_stats();
std::cerr << "# Extending run ..." << std::endl;
sampler.step(int(N_steps*1./5.), false, 0., 1., 0.);
sampler.clear();
//logger.clear();
}
break;
}
}
}
clock_gettime(CLOCK_MONOTONIC, &t_write);
//std::stringstream group_name;
//group_name << "/pixel " << healpix_index;
//group_name << "/los clouds";
//chain.save(out_fname, group_name.str(), 0, "Delta mu, Delta E(B-V)", 3, 100, converged);
std::stringstream group_name;
group_name << "/pixel " << healpix_index;
TChain chain = sampler.get_chain();
TChainWriteBuffer writeBuffer(ndim, 100, 1);
writeBuffer.add(chain, converged);
writeBuffer.write(out_fname, group_name.str(), "clouds");
clock_gettime(CLOCK_MONOTONIC, &t_end);
sampler.print_stats();
std::cout << std::endl;
if(!converged) {
std::cerr << "# Failed to converge." << std::endl;
}
std::cerr << "# Number of steps: " << (1<<(attempt-1))*N_steps << std::endl;
std::cerr << "# Time elapsed: " << std::setprecision(2) << (t_end.tv_sec - t_start.tv_sec) + 1.e-9*(t_end.tv_nsec - t_start.tv_nsec) << " s" << std::endl;
std::cerr << "# Sample time: " << std::setprecision(2) << (t_write.tv_sec - t_start.tv_sec) + 1.e-9*(t_write.tv_nsec - t_start.tv_nsec) << " s" << std::endl;
std::cerr << "# Write time: " << std::setprecision(2) << (t_end.tv_sec - t_write.tv_sec) + 1.e-9*(t_end.tv_nsec - t_write.tv_nsec) << " s" << std::endl << std::endl;
}
bool G2oSlamInterface::addEdge(const std::string& tag, int id, int dimension, int v1Id, int v2Id, const std::vector<double>& measurement, const std::vector<double>& information)
{
(void) tag;
(void) id;
size_t oldEdgesSize = _optimizer->edges().size();
if (dimension == 3) {
SE2 transf(measurement[0], measurement[1], measurement[2]);
Eigen::Matrix3d infMat;
int idx = 0;
for (int r = 0; r < 3; ++r)
for (int c = r; c < 3; ++c, ++idx) {
assert(idx < (int)information.size());
infMat(r,c) = infMat(c,r) = information[idx];
}
//cerr << PVAR(infMat) << endl;
int doInit = 0;
SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
if (! v1) {
OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
_verticesAdded.insert(v);
doInit = 1;
++_nodesAdded;
}
if (! v2) {
OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
_verticesAdded.insert(v);
doInit = 2;
++_nodesAdded;
}
if (_optimizer->edges().size() == 0) {
cerr << "FIRST EDGE ";
if (v1->id() < v2->id()) {
cerr << "fixing " << v1->id() << endl;
v1->setFixed(true);
}
else {
cerr << "fixing " << v2->id() << endl;
v2->setFixed(true);
}
}
OnlineEdgeSE2* e = new OnlineEdgeSE2;
e->vertices()[0] = v1;
e->vertices()[1] = v2;
e->setMeasurement(transf);
e->setInformation(infMat);
_optimizer->addEdge(e);
_edgesAdded.insert(e);
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
e->initialEstimate(toSet, from);
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
e->initialEstimate(fromSet, to);
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
else if (dimension == 6) {
Eigen::Isometry3d transf;
Matrix<double, 6, 6> infMat;
if (measurement.size() == 7) { // measurement is a Quaternion
Vector7d meas;
for (int i=0; i<7; ++i)
meas(i) = measurement[i];
// normalize the quaternion to recover numerical precision lost by storing as human readable text
Vector4d::MapType(meas.data()+3).normalize();
transf = internal::fromVectorQT(meas);
for (int i = 0, idx = 0; i < infMat.rows(); ++i)
for (int j = i; j < infMat.cols(); ++j){
infMat(i,j) = information[idx++];
if (i != j)
infMat(j,i)=infMat(i,j);
}
} else { // measurement consists of Euler angles
Vector6d aux;
aux << measurement[0], measurement[1], measurement[2],measurement[3], measurement[4], measurement[5];
transf = internal::fromVectorET(aux);
Matrix<double, 6, 6> infMatEuler;
int idx = 0;
for (int r = 0; r < 6; ++r)
for (int c = r; c < 6; ++c, ++idx) {
assert(idx < (int)information.size());
infMatEuler(r,c) = infMatEuler(c,r) = information[idx];
}
// convert information matrix to our internal representation
Matrix<double, 6, 6> J;
SE3Quat transfAsSe3(transf.matrix().topLeftCorner<3,3>(), transf.translation());
jac_quat3_euler3(J, transfAsSe3);
infMat.noalias() = J.transpose() * infMatEuler * J;
//cerr << PVAR(transf.matrix()) << endl;
//cerr << PVAR(infMat) << endl;
}
int doInit = 0;
SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
if (! v1) {
OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
_verticesAdded.insert(v);
doInit = 1;
++_nodesAdded;
}
if (! v2) {
OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
_verticesAdded.insert(v);
doInit = 2;
++_nodesAdded;
}
if (_optimizer->edges().size() == 0) {
cerr << "FIRST EDGE ";
if (v1->id() < v2->id()) {
cerr << "fixing " << v1->id() << endl;
v1->setFixed(true);
}
else {
cerr << "fixing " << v2->id() << endl;
v2->setFixed(true);
}
}
OnlineEdgeSE3* e = new OnlineEdgeSE3;
e->vertices()[0] = v1;
e->vertices()[1] = v2;
e->setMeasurement(transf);
e->setInformation(infMat);
_optimizer->addEdge(e);
_edgesAdded.insert(e);
if (doInit) {
OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
switch (doInit){
case 1: // initialize v1 from v2
{
HyperGraph::VertexSet toSet;
toSet.insert(to);
if (e->initialEstimatePossible(toSet, from) > 0.) {
e->initialEstimate(toSet, from);
}
break;
}
case 2:
{
HyperGraph::VertexSet fromSet;
fromSet.insert(from);
if (e->initialEstimatePossible(fromSet, to) > 0.) {
e->initialEstimate(fromSet, to);
}
break;
}
default: cerr << "doInit wrong value\n";
}
}
}
else {
cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl;
return false;
}
if (oldEdgesSize == 0) {
_optimizer->jacobianWorkspace().allocate();
}
return true;
}
void sample_los_extinction(std::string out_fname, TMCMCOptions &options, TLOSMCMCParams ¶ms,
unsigned int N_regions, uint64_t healpix_index) {
timespec t_start, t_write, t_end;
clock_gettime(CLOCK_MONOTONIC, &t_start);
std::cout << "guess of EBV max = " << params.EBV_guess_max << std::endl;
guess_EBV_profile(options, params, N_regions);
//monotonic_guess(img_stack, N_regions, params.EBV_prof_guess, options);
TNullLogger logger;
unsigned int max_attempts = 2;
unsigned int N_steps = options.steps;
unsigned int N_samplers = options.samplers;
unsigned int N_threads = options.N_threads;
unsigned int ndim = N_regions + 1;
std::vector<double> GR_transf;
TLOSTransform transf(ndim);
double GR_threshold = 1.25;
TAffineSampler<TLOSMCMCParams, TNullLogger>::pdf_t f_pdf = &lnp_los_extinction;
TAffineSampler<TLOSMCMCParams, TNullLogger>::rand_state_t f_rand_state = &gen_rand_los_extinction_from_guess;
std::cerr << std::endl;
std::cout << "Line-of-Sight Extinction Profile" << std::endl;
std::cout << "====================================" << std::endl;
TParallelAffineSampler<TLOSMCMCParams, TNullLogger> sampler(f_pdf, f_rand_state, ndim, N_samplers*ndim, params, logger, N_threads);
sampler.set_scale(1.2);
sampler.set_replacement_bandwidth(0.50);
// Burn-in
std::cerr << "# Burn-in ..." << std::endl;
sampler.step(int(N_steps*40./100.), false, 0., 0.4, 0.);
sampler.step(int(N_steps*10./100), false, 0., 1.0, 0., false);
sampler.step(int(N_steps*40./100.), false, 0., 0.4, 0.);
sampler.step(int(N_steps*10./100), false, 0., 0.8, 0.);
//sampler.step(N_steps, false, 0., options.p_replacement, 0.);
//sampler.step(N_steps/2., false, 0., 1., 0.);
sampler.print_stats();
sampler.clear();
std::cerr << "# Main run ..." << std::endl;
bool converged = false;
size_t attempt;
for(attempt = 0; (attempt < max_attempts) && (!converged); attempt++) {
sampler.step((1<<attempt)*N_steps, true, 0., options.p_replacement, 0.);
std::cout << std::endl << "Transformed G-R Diagnostic:";
sampler.calc_GR_transformed(GR_transf, &transf);
for(unsigned int k=0; k<ndim; k++) {
std::cout << " " << std::setprecision(3) << GR_transf[k];
}
std::cout << std::endl << std::endl;
converged = true;
for(size_t i=0; i<ndim; i++) {
if(GR_transf[i] > GR_threshold) {
converged = false;
if(attempt != max_attempts-1) {
sampler.print_stats();
std::cerr << "# Extending run ..." << std::endl;
sampler.step(int(N_steps*1./5.), false, 0., 1., 0.);
sampler.clear();
//logger.clear();
}
break;
}
}
}
clock_gettime(CLOCK_MONOTONIC, &t_write);
std::stringstream group_name;
group_name << "/pixel " << healpix_index;
TChain chain = sampler.get_chain();
TChainWriteBuffer writeBuffer(ndim, 500, 1);
writeBuffer.add(chain, converged);
writeBuffer.write(out_fname, group_name.str(), "los");
std::stringstream los_group_name;
los_group_name << group_name.str() << "/los";
H5Utils::add_watermark<double>(out_fname, los_group_name.str(), "DM_min", params.img_stack->rect->min[0]);
H5Utils::add_watermark<double>(out_fname, los_group_name.str(), "DM_max", params.img_stack->rect->max[0]);
clock_gettime(CLOCK_MONOTONIC, &t_end);
sampler.print_stats();
std::cout << std::endl;
if(!converged) {
std::cerr << "# Failed to converge." << std::endl;
}
std::cerr << "# Number of steps: " << (1<<(attempt-1))*N_steps << std::endl;
std::cerr << "# Time elapsed: " << std::setprecision(2) << (t_end.tv_sec - t_start.tv_sec) + 1.e-9*(t_end.tv_nsec - t_start.tv_nsec) << " s" << std::endl;
std::cerr << "# Sample time: " << std::setprecision(2) << (t_write.tv_sec - t_start.tv_sec) + 1.e-9*(t_write.tv_nsec - t_start.tv_nsec) << " s" << std::endl;
std::cerr << "# Write time: " << std::setprecision(2) << (t_end.tv_sec - t_write.tv_sec) + 1.e-9*(t_end.tv_nsec - t_write.tv_nsec) << " s" << std::endl << std::endl;
}