bool Bitmap::loadFromFile(const wxString& filepath)
{
static bool init = false;
if (!init)
{
wxInitAllImageHandlers();
// wxImage::AddHandler(new wxPNGHandler);
// wxImage::AddHandler(new wxJPEGHandler);
// wxImage::AddHandler(new wxBMPHandler);
init = true;
}
wxImage image;
image.LoadFile(filepath);
m_scale = computeScale(image.GetWidth());
int w = image.GetWidth() * m_scale;
int h = image.GetHeight() * m_scale;
if (w > 1 && h > 1)
m_bitmap = new wxBitmap(image.Scale(w, h));
else
m_bitmap = new wxBitmap(image.Scale(image.GetWidth(), image.GetHeight()));
return true;
}
void
Battery::updateBatteryStatus(hrt_abstime timestamp, float voltage_v, float current_a,
bool connected, bool selected_source, int priority,
float throttle_normalized,
bool armed, battery_status_s *battery_status)
{
reset(battery_status);
battery_status->timestamp = timestamp;
filterVoltage(voltage_v);
filterCurrent(current_a);
sumDischarged(timestamp, current_a);
estimateRemaining(voltage_v, current_a, throttle_normalized, armed);
determineWarning();
computeScale();
if (_voltage_filtered_v > 2.1f) {
battery_status->voltage_v = voltage_v;
battery_status->voltage_filtered_v = _voltage_filtered_v;
battery_status->scale = _scale;
battery_status->current_a = current_a;
battery_status->current_filtered_a = _current_filtered_a;
battery_status->discharged_mah = _discharged_mah;
battery_status->warning = _warning;
battery_status->remaining = _remaining;
battery_status->connected = connected;
battery_status->system_source = selected_source;
battery_status->priority = priority;
}
}
void ScaleItem::setUnits(enum Units units)
{
prepareGeometryChange();
_units = units;
computeScale();
updateBoundingRect();
update();
}
void ScaleItem::setZoom(int z)
{
prepareGeometryChange();
_zoom = z;
computeScale();
updateBoundingRect();
update();
}
void ScaleItem::setZoom(int z, qreal lat)
{
prepareGeometryChange();
_zoom = z;
_lat = lat;
computeScale();
updateBoundingRect();
update();
}
Real SPxScaler::computeScalingVecs(
const SVSet* vecset,
const DataArray<Real>& coScaleval,
DataArray<Real>& scaleval)
{
METHOD( "SPxScaler::computeScalingVecs()" );
Real pmax = 0.0;
for(int i = 0; i < vecset->num(); ++i )
{
const SVector& vec = (*vecset)[i];
Real maxi = 0.0;
Real mini = infinity;
for( int j = 0; j < vec.size(); ++j)
{
Real x = fabs(vec.value(j) * coScaleval[vec.index(j)]);
if (!isZero(x))
{
if (x > maxi)
maxi = x;
if (x < mini)
mini = x;
}
}
// empty rows/cols are possible
if (mini == infinity || maxi == 0.0)
{
mini = 1.0;
maxi = 1.0;
}
assert(mini < infinity);
assert(maxi > 0.0);
scaleval[i] = 1.0 / computeScale(mini, maxi);
Real p = maxi / mini;
if (p > pmax)
pmax = p;
}
return pmax;
}
OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)
{
assert(_optimizer && "_optimizer not set");
assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
bool ok = _solver->buildStructure();
if (! ok) {
cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
return OptimizationAlgorithm::Fail;
}
}
double t=get_monotonic_time();
_optimizer->computeActiveErrors();
G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
if (globalStats) {
globalStats->timeResiduals = get_monotonic_time()-t;
t=get_monotonic_time();
}
double currentChi = _optimizer->activeRobustChi2();
double tempChi=currentChi;
_solver->buildSystem();
if (globalStats) {
globalStats->timeQuadraticForm = get_monotonic_time()-t;
}
// core part of the Levenbarg algorithm
if (iteration == 0) {
_currentLambda = computeLambdaInit();
_ni = 2;
}
double rho=0;
int& qmax = _levenbergIterations;
qmax = 0;
do {
_optimizer->push();
if (globalStats) {
globalStats->levenbergIterations++;
t=get_monotonic_time();
}
// update the diagonal of the system matrix
_solver->setLambda(_currentLambda);
bool ok2 = _solver->solve();
if (globalStats) {
globalStats->timeLinearSolution+=get_monotonic_time()-t;
t=get_monotonic_time();
}
_optimizer->update(_solver->x());
if (globalStats) {
globalStats->timeUpdate = get_monotonic_time()-t;
}
// restore the diagonal
_solver->setLambda(- _currentLambda);
_optimizer->computeActiveErrors();
tempChi = _optimizer->activeRobustChi2();
if (! ok2)
tempChi=std::numeric_limits<double>::max();
rho = (currentChi-tempChi);
double scale = computeScale();
scale += 1e-3; // make sure it's non-zero :)
rho /= scale;
if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
double alpha = 1.-pow((2*rho-1),3);
// crop lambda between minimum and maximum factors
alpha = (std::min)(alpha, _goodStepUpperScale);
double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
_currentLambda *= scaleFactor;
_ni = 2;
currentChi=tempChi;
_optimizer->discardTop();
} else {
_currentLambda*=_ni;
_ni*=2;
_optimizer->pop(); // restore the last state before trying to optimize
}
qmax++;
} while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
if (qmax == _maxTrialsAfterFailure->value() || rho==0)
return Terminate;
return OK;
}
/* scAdjust:
* Scale the layout.
* equal > 0 => scale uniformly in x and y to remove overlaps
* equal = 0 => scale separately in x and y to remove overlaps
* equal < 0 => scale down uniformly in x and y to remove excess space
* The last assumes there are no overlaps at present.
* Based on Marriott, Stuckey, Tam and He,
* "Removing Node Overlapping in Graph Layout Using Constrained Optimization",
* Constraints,8(2):143--172, 2003.
*/
int scAdjust(graph_t * g, int equal)
{
int nnodes = agnnodes(g);
info *nlist = N_GNEW(nnodes, info);
info *p = nlist;
node_t *n;
pointf s;
int i;
expand_t margin;
pointf *aarr;
int m;
margin = sepFactor (g);
if (margin.doAdd) {
/* we use inches below */
margin.x = PS2INCH(margin.x);
margin.y = PS2INCH(margin.y);
}
for (n = agfstnode(g); n; n = agnxtnode(g, n)) {
double w2, h2;
if (margin.doAdd) {
w2 = (ND_width(n) / 2.0) + margin.x;
h2 = (ND_height(n) / 2.0) + margin.y;
}
else {
w2 = margin.x * ND_width(n) / 2.0;
h2 = margin.y * ND_height(n) / 2.0;
}
p->pos.x = ND_pos(n)[0];
p->pos.y = ND_pos(n)[1];
p->bb.LL.x = p->pos.x - w2;
p->bb.LL.y = p->pos.y - h2;
p->bb.UR.x = p->pos.x + w2;
p->bb.UR.y = p->pos.y + h2;
p->wd2 = w2;
p->ht2 = h2;
p->np = n;
p++;
}
if (equal < 0) {
s.x = s.y = compress(nlist, nnodes);
if (s.x == 0) { /* overlaps exist */
free(nlist);
return 0;
}
fprintf(stderr, "compress %g \n", s.x);
} else {
aarr = mkOverlapSet(nlist, nnodes, &m);
if (m == 0) { /* no overlaps */
free(aarr);
free(nlist);
return 0;
}
if (equal) {
s.x = s.y = computeScale(aarr, m);
} else {
s = computeScaleXY(aarr, m);
}
free(aarr);
}
p = nlist;
for (i = 0; i < nnodes; i++) {
ND_pos(p->np)[0] = s.x * p->pos.x;
ND_pos(p->np)[1] = s.y * p->pos.y;
p++;
}
free(nlist);
return 1;
}
