bool extractAbsolutePrior(Eigen::Isometry3f& priorMean, Matrix6f& priorInfo,
const DrawableFrame* current){
VertexSE3* currentVertex =current->_vertex;
ImuData* imuData = 0;
OptimizableGraph::Data* d = currentVertex->userData();
while(d) {
ImuData* imuData_ = dynamic_cast<ImuData*>(d);
if (imuData_){
imuData = imuData_;
}
d=d->next();
}
if (imuData){
Eigen::Matrix3d R=imuData->getOrientation().matrix();
Eigen::Matrix3d Omega = imuData->getOrientationCovariance().inverse();
priorMean.setIdentity();
priorInfo.setZero();
for (int c = 0; c<3; c++)
for (int r = 0; r<3; r++)
priorMean.linear()(r,c)=R(r,c);
for (int c = 0; c<3; c++)
for (int r = 0; r<3; r++)
priorInfo(r+3,c+3)=Omega(r,c);
return true;
}
return false;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
initializeDrawActionsCache();
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(POSE_VERTEX_COLOR);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
drawCache(that->cacheContainer(), params_);
drawUserData(that->userData(), params_);
if(_showId && _showId->value()){
float scale = _idSize ? _idSize->value() : 1.f;
drawId(std::to_string(that->id()), scale);
}
glPopMatrix();
return this;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushMatrix();
glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),(float)that->estimate().translation().z());
AngleAxisd aa(that->estimate().rotation());
glRotatef((float)RAD2DEG(aa.angle()),(float)aa.axis().x(),(float)aa.axis().y(),(float)aa.axis().z());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
if (that->userData() && _cacheDrawActions )
(*_cacheDrawActions)(that->userData(), params_);
glPopMatrix();
return this;
}
HyperGraphElementAction* VertexSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _cacheDrawActions){
_cacheDrawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexSE3* that = static_cast<VertexSE3*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
if (_triangleX && _triangleY){
drawTriangle(_triangleX->value(), _triangleY->value());
}
CacheContainer* caches=that->cacheContainer();
if (caches){
for (CacheContainer::iterator it=caches->begin(); it!=caches->end(); it++){
Cache* c = it->second;
(*_cacheDrawActions)(c, params_);
}
}
OptimizableGraph::Data* d=that->userData();
while (d && _cacheDrawActions ){
(*_cacheDrawActions)(d, params_);
d=d->next();
}
glPopMatrix();
return this;
}
