void GraphSLAM::addData(SE2 currentOdom, RobotLaser* laser){
boost::mutex::scoped_lock lockg(graphMutex);
//Add current vertex
VertexSE2 *v = new VertexSE2;
SE2 displacement = _lastOdom.inverse() * currentOdom;
SE2 currEst = _lastVertex->estimate() * displacement;
v->setEstimate(currEst);
v->setId(++_runningVertexId + idRobot() * baseId());
//Add covariance information
//VertexEllipse *ellipse = new VertexEllipse;
//Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
//ellipse->setCovariance(cov);
//v->setUserData(ellipse);
v->addUserData(laser);
std::cout << std::endl <<
"Current vertex: " << v->id() <<
" Estimate: "<< v->estimate().translation().x() <<
" " << v->estimate().translation().y() <<
" " << v->estimate().rotation().angle() << std::endl;
_graph->addVertex(v);
//Add current odometry edge
EdgeSE2 *e = new EdgeSE2;
e->setId(++_runningEdgeId + idRobot() * baseId());
e->vertices()[0] = _lastVertex;
e->vertices()[1] = v;
e->setMeasurement(displacement);
// //Computing covariances depending on the displacement
// Vector3d dis = displacement.toVector();
// dis.x() = fabs(dis.x());
// dis.y() = fabs(dis.y());
// dis.z() = fabs(dis.z());
// dis += Vector3d(1e-3,1e-3,1e-2);
// Matrix3d dis2 = dis*dis.transpose();
// Matrix3d newcov = dis2.cwiseProduct(_odomK);
e->setInformation(_odominf);
_graph->addEdge(e);
_odomEdges.insert(e);
_lastOdom = currentOdom;
_lastVertex = v;
}
void GraphSLAM::setInitialData(SE2 initialTruePose, SE2 initialOdom, RobotLaser* laser){
boost::mutex::scoped_lock lockg(graphMutex);
_lastOdom = initialOdom;
//Add initial vertex
_lastVertex = new VertexSE2;
_lastVertex->setEstimate(initialTruePose);
_lastVertex->setId(idRobot() * baseId());
//Add covariance information
//VertexEllipse *ellipse = new VertexEllipse();
//Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
//ellipse->setCovariance(cov);
//_lastVertex->setUserData(ellipse);
_lastVertex->setUserData(laser);
std::cout <<
"Initial vertex: " << _lastVertex->id() <<
" Estimate: "<< _lastVertex->estimate().translation().x() <<
" " << _lastVertex->estimate().translation().y() <<
" " << _lastVertex->estimate().rotation().angle() << std::endl;
_graph->addVertex(_lastVertex);
_firstRobotPose = _lastVertex;
_firstRobotPose->setFixed(true);
}
int CategoryTree::OnCompareItems( const wxTreeItemId& p_item1, const wxTreeItemId& p_item2 ) {
int Result = 0;
auto itemData1 = static_cast<const CategoryTreeItem*>( this->GetItemData( p_item1 ) );
auto itemData2 = static_cast<const CategoryTreeItem*>( this->GetItemData( p_item2 ) );
if ( itemData1->dataType() == CategoryTreeItem::DT_Entry && itemData2->dataType() == CategoryTreeItem::DT_Entry ) {
auto itemEntry1 = static_cast<const DatIndexEntry*>( itemData1->data() );
auto itemEntry2 = static_cast<const DatIndexEntry*>( itemData2->data() );
if ( itemEntry1->baseId() > itemEntry2->baseId() ) {
Result = 1;
} else if ( itemEntry1->baseId() < itemEntry2->baseId() ) {
Result = -1;
}
}
return Result;
}
bool DatIndexWriter::write(uint p_amount)
{
for (uint i = 0; i < p_amount; i++) {
ssize_t bytesWritten;
// First write categories, one at a time
if (m_categoriesWritten < m_index.numCategories()) {
auto category = m_index.category(m_categoriesWritten);
auto parent = category->parent();
wxScopedCharBuffer nameBuffer = category->name().ToUTF8();
// Fixed-width fields
DatIndexCategoryFields fields;
fields.parent = (parent ? parent->index() : -1);
fields.nameLength = nameBuffer.length();
bytesWritten = m_file.Write(&fields, sizeof(fields));
if (bytesWritten < sizeof(fields)) { return false; }
// Name
bytesWritten = m_file.Write(nameBuffer, fields.nameLength);
if (bytesWritten < fields.nameLength) { return false; }
// Increase the counter
m_categoriesWritten++;
}
// Then, write entries one at a time (note the 'else')
else if (m_entriesWritten < m_index.numEntries()) {
auto entry = m_index.entry(m_entriesWritten);
auto category = entry->category();
auto nameBuffer = entry->name().ToUTF8();
// Fixed-width fields
DatIndexEntryFields fields;
fields.category = category->index();
fields.baseId = entry->baseId();
fields.fileId = entry->fileId();
fields.mftEntry = entry->mftEntry();
fields.fileType = entry->fileType();
fields.nameLength = nameBuffer.length();
bytesWritten = m_file.Write(&fields, sizeof(fields));
if (bytesWritten < sizeof(fields)) { return false; }
// Name
bytesWritten = m_file.Write(nameBuffer, fields.nameLength);
if (bytesWritten < fields.nameLength) { return false; }
// Increase the counter
m_entriesWritten++;
}
// Both done = ditch this loop
else {
break;
}
}
// Remove dirty flag if everything is saved
if (this->isDone()) {
m_index.setDirty(false);
}
return true;
}
void WordParagraphReader::Accept(DocumentText::ptr_type docText, const WrdTextRun& textRun, const long startIndex, const std::wcstring& currentText)
{
if(docText == 0)
return;
try
{
std::wcstring bufferText(currentText);
std::string::size_type localStartPos = startIndex;
AbstractTextNode::id_type baseId(_LL("METADATA_"));
baseId += ReaderHelpers::MakeNumberString((TEXT_TYPE_MAIN));
baseId += _LL("_");
if(!bufferText.empty())
{
m_currentText.push_back(bufferText);
std::wcstring currentParagraphText;
while(ReaderHelpers::GetNextStringFromStringList(m_currentText, currentParagraphText, m_iPos))
{
AbstractTextNode::id_type id = baseId + ReaderHelpers::MakeNumberString((localStartPos));
id += _LL("_");
id += ReaderHelpers::MakeNumberString((currentParagraphText.size()));
localStartPos += currentParagraphText.size();
AbstractTextNode::ptr_type textNode = docText->GetTextNode(id);
if(textNode == 0)
{
textNode = new TextNode(id);
textNode->AddAdditionalInfo(docText, _LL("Content"), currentParagraphText, AbstractTextNode::String);
}
docText->AddTextNode(AbstractTextType::Paragraph, textNode);
}
}
}
catch (CsException& ex)
{
throw std::logic_error("An error occurred reading the paragraph text of the word document : " + ReaderHelpers::ConstructErrorText(ex));
}
catch(...)
{
throw std::logic_error("An unhandled exception occurred while reading the paragraph text of the word document");
}
}
CurrDisplay::CurrDisplay(QWidget * parent, const char* name)
: QWidget(parent, name)
{
setObjectName("CurrDisplay");
setWindowTitle("CurrDisplay");
_grid = new QGridLayout(this);
_grid->setMargin(2);
_grid->setSpacing(2);
_valueLocalWidget = new XLineEdit(this);
_valueLocalWidget->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Fixed);
_valueLocalWidget->setMinimumWidth(QFontMetrics(QFont()).width("99999.9999"));
_grid->addWidget(_valueLocalWidget, 0, 0);
_valueLocalWidget->setAlignment(Qt::AlignRight|Qt::AlignTop);
setLayout(_grid);
connect(this, SIGNAL(valueLocalChanged(const double)),
this, SIGNAL(valueChanged()));
connect(this, SIGNAL(valueBaseChanged(const double)),
this, SIGNAL(valueChanged()));
_localId = baseId();
_localScale = _baseScale;
_effective = QDate().currentDate();
_decimals = 0;
_format = Money;
_default = 0;
clear();
setEnabled(FALSE);
setLocalControl(TRUE);
setMinimumSize(MINWIDTH, 0);
setMaximumSize(MAXWIDTH, MAXHEIGHT);
_mapper = new XDataWidgetMapper(this);
}
QString CurrDisplay::baseCurrAbbr()
{
(void)baseId();
return _baseAbbr;
}
/* compute statistics on leaving variable
Compute a set of statistical values on the variable selected for leaving the
basis.
*/
void SPxSolver::getLeaveVals(
int leaveIdx,
SPxBasis::Desc::Status& leaveStat,
SPxId& leaveId,
Real& leaveMax,
Real& leavebound,
int& leaveNum)
{
METHOD( "SPxSolver::getLeaveVals()" );
SPxBasis::Desc& ds = desc();
leaveId = baseId(leaveIdx);
if (leaveId.isSPxRowId())
{
leaveNum = number(SPxRowId(leaveId));
leaveStat = ds.rowStatus(leaveNum);
assert(isBasic(leaveStat));
switch (leaveStat)
{
case SPxBasis::Desc::P_ON_UPPER :
assert( rep() == ROW );
ds.rowStatus(leaveNum) = dualRowStatus(leaveNum);
leavebound = 0;
leaveMax = -infinity;
break;
case SPxBasis::Desc::P_ON_LOWER :
assert( rep() == ROW );
ds.rowStatus(leaveNum) = dualRowStatus(leaveNum);
leavebound = 0;
leaveMax = infinity;
break;
case SPxBasis::Desc::P_FREE :
assert( rep() == ROW );
throw SPxInternalCodeException("XLEAVE01 This should never happen.");
case SPxBasis::Desc::D_FREE :
assert( rep() == COLUMN );
ds.rowStatus(leaveNum) = SPxBasis::Desc::P_FIXED;
assert(lhs(leaveNum) == rhs(leaveNum));
leavebound = -rhs(leaveNum);
if ((*theFvec)[leaveIdx] < theLBbound[leaveIdx])
leaveMax = infinity;
else
leaveMax = -infinity;
break;
case SPxBasis::Desc::D_ON_LOWER :
assert( rep() == COLUMN );
ds.rowStatus(leaveNum) = SPxBasis::Desc::P_ON_UPPER;
leavebound = -rhs(leaveNum); // slack !!
leaveMax = infinity;
break;
case SPxBasis::Desc::D_ON_UPPER :
assert( rep() == COLUMN );
ds.rowStatus(leaveNum) = SPxBasis::Desc::P_ON_LOWER;
leavebound = -lhs(leaveNum); // slack !!
leaveMax = -infinity;
break;
case SPxBasis::Desc::D_ON_BOTH :
assert( rep() == COLUMN );
if ((*theFvec)[leaveIdx] > theLBbound[leaveIdx])
{
ds.rowStatus(leaveNum) = SPxBasis::Desc::P_ON_LOWER;
theLRbound[leaveNum] = -infinity;
leavebound = -lhs(leaveNum); // slack !!
leaveMax = -infinity;
}
else
{
ds.rowStatus(leaveNum) = SPxBasis::Desc::P_ON_UPPER;
theURbound[leaveNum] = infinity;
leavebound = -rhs(leaveNum); // slack !!
leaveMax = infinity;
}
break;
default:
throw SPxInternalCodeException("XLEAVE02 This should never happen.");
}
MSG_DEBUG( spxout << "DLEAVE51 SPxSolver::getLeaveVals() : row " << leaveNum
<< ": " << leaveStat
<< " -> " << ds.rowStatus(leaveNum)
<< std::endl; )
}
bool GraphSLAM::isMyVertex(OptimizableGraph::Vertex *v){
return (v->id()/baseId()) == idRobot();
}
void GraphSLAM::addDataSM(SE2 currentOdom, RobotLaser* laser){
boost::mutex::scoped_lock lockg(graphMutex);
//Add current vertex
VertexSE2 *v = new VertexSE2;
SE2 displacement = _lastOdom.inverse() * currentOdom;
SE2 currEst = _lastVertex->estimate() * displacement;
v->setEstimate(currEst);
v->setId(++_runningVertexId + idRobot() * baseId());
//Add covariance information
//VertexEllipse *ellipse = new VertexEllipse;
//Matrix3f cov = Matrix3f::Zero(); //last vertex has zero covariance
//ellipse->setCovariance(cov);
//v->setUserData(ellipse);
v->addUserData(laser);
std::cout << endl <<
"Current vertex: " << v->id() <<
" Estimate: "<< v->estimate().translation().x() <<
" " << v->estimate().translation().y() <<
" " << v->estimate().rotation().angle() << std::endl;
_graph->addVertex(v);
//Add current odometry edge
EdgeSE2 *e = new EdgeSE2;
e->setId(++_runningEdgeId + idRobot() * baseId());
e->vertices()[0] = _lastVertex;
e->vertices()[1] = v;
OptimizableGraph::VertexSet vset;
vset.insert(_lastVertex);
int j = 1;
int gap = 5;
while (j <= gap){
VertexSE2 *vj = dynamic_cast<VertexSE2 *>(graph()->vertex(_lastVertex->id()-j));
if (vj)
vset.insert(vj);
else
break;
j++;
}
SE2 transf;
bool shouldIAdd = _closeMatcher.closeScanMatching(vset, _lastVertex, v, &transf, maxScore);
if (shouldIAdd){
e->setMeasurement(transf);
e->setInformation(_SMinf);
}else{ //Trust the odometry
e->setMeasurement(displacement);
// Vector3d dis = displacement.toVector();
// dis.x() = fabs(dis.x());
// dis.y() = fabs(dis.y());
// dis.z() = fabs(dis.z());
// dis += Vector3d(1e-3,1e-3,1e-2);
// Matrix3d dis2 = dis*dis.transpose();
// Matrix3d newcov = dis2.cwiseProduct(_odomK);
// e->setInformation(newcov.inverse());
e->setInformation(_odominf);
}
_graph->addEdge(e);
_lastOdom = currentOdom;
_lastVertex = v;
}
// -----------------------------------------------------------------
void SPxSolver::perturbMax(
const UpdateVector& uvec,
Vector& p_low,
Vector& p_up,
Real eps,
Real p_delta,
int start,
int incr)
{
assert(uvec.dim() == p_low.dim());
assert(uvec.dim() == p_up.dim());
const Real* vec = uvec.get_const_ptr();
const Real* upd = uvec.delta().values();
const IdxSet& idx = uvec.delta().indices();
Random mult(10.0 * p_delta, 100.0 * p_delta);
Real x, l, u;
int i, j;
#ifdef FULL_SHIFT
eps = p_delta;
for (i = uvec.dim() - start - 1; i >= 0; i -= incr)
{
u = p_up[i];
l = p_low[i];
if (p_up[i] <= vec[i] + eps)
{
p_up[i] = vec[i] + Real(mult);
theShift += p_up[i] - u;
}
if (p_low[i] >= vec[i] - eps)
{
p_low[i] = vec[i] - Real(mult);
theShift -= p_low[i] - l;
}
}
#else // !FULL_SHIFT
for (j = uvec.delta().size() - start - 1; j >= 0; j -= incr)
{
i = idx.index(j);
x = upd[i];
u = p_up[i];
l = p_low[i];
// do not permute these bounds! c.f. computeFrhs2() in spxvecs.cpp
if( dualStatus(baseId(i)) == SPxBasis::Desc::D_ON_BOTH )
{
continue;
}
if (x > eps)
{
if (u != l && vec[i] >= u - eps)
{
p_up[i] = vec[i] + Real(mult);
theShift += p_up[i] - u;
}
}
else if (x < -eps)
{
if (u != l && vec[i] <= l + eps)
{
p_low[i] = vec[i] - Real(mult);
theShift -= p_low[i] - l;
}
}
}
#endif // !FULL_SHIFT
}
