HyperGraphElementAction* VertexPointXYZDrawAction::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;
VertexPointXYZ* that = static_cast<VertexPointXYZ*>(element);
glPushMatrix();
glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT);
glDisable(GL_LIGHTING);
glColor3f(LANDMARK_VERTEX_COLOR);
float ps = _pointSize ? _pointSize->value() : 1.f;
glTranslatef((float)that->estimate()(0),(float)that->estimate()(1),(float)that->estimate()(2));
opengl::drawPoint(ps);
glPopAttrib();
drawCache(that->cacheContainer(), params);
drawUserData(that->userData(), params);
glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE2* e = static_cast<EdgeSE2*>(element);
VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1));
glColor3f(0.5,0.5,0.8);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f(fromEdge->estimate().translation().x(),fromEdge->estimate().translation().y(),0.);
glVertex3f(toEdge->estimate().translation().x(),toEdge->estimate().translation().y(),0.);
glEnd();
glPopAttrib();
return this;
}
HyperGraphElementAction* EdgeSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3* e = static_cast<EdgeSE3*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexSE3* toEdge = static_cast<VertexSE3*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
glColor3f(POSE_EDGE_COLOR);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),(float)fromEdge->estimate().translation().z());
glVertex3f((float)toEdge->estimate().translation().x(),(float)toEdge->estimate().translation().y(),(float)toEdge->estimate().translation().z());
glEnd();
glPopAttrib();
return this;
}
HyperGraphElementAction *VertexPlane3dDrawAction::operator()(HyperGraph::HyperGraphElement *element,
HyperGraphElementAction::Parameters *params)
{
if (typeid(*element).name() != _typeName)
return 0;
refreshPropertyPtrs(params);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
VertexPlane3d *that = static_cast<VertexPlane3d *>(element);
glPushAttrib(GL_ENABLE_BIT | GL_POINT_BIT);
glDisable(GL_LIGHTING);
glColor3f(0.8f, 0.5f, 0.3f);
if (_pointSize)
glPointSize(_pointSize->value());
// glBegin(GL_POINTS);
glBegin(GL_LINES);
glVertex3f((float)that->estimate()(0), (float)that->estimate()(1), (float)that->estimate()(2));
glEnd();
glPopAttrib();
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.5,0.5,0.8);
glPushMatrix();
glTranslatef(that->estimate().translation().x(),that->estimate().translation().y(),that->estimate().translation().z());
AngleAxisd aa(that->estimate().rotation());
glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),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_);
}
}
glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeProjectDisparityDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_ ){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3PointXYZDisparity* e = static_cast<EdgeSE3PointXYZDisparity*>(element);
VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
VertexPointXYZ* toEdge = static_cast<VertexPointXYZ*>(e->vertices()[1]);
if (! fromEdge || ! toEdge)
return this;
Eigen::Isometry3d fromTransform=fromEdge->estimate() * e->cameraParameter()->offset();
glColor3f(LANDMARK_EDGE_COLOR);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),(float)fromTransform.translation().z());
glVertex3f((float)toEdge->estimate().x(),(float)toEdge->estimate().y(),(float)toEdge->estimate().z());
glEnd();
glPopAttrib();
return this;
}
HyperGraphElementAction* EdgeSE3PlaneSensorCalibDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_)
{
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE3PlaneSensorCalib* that = dynamic_cast<EdgeSE3PlaneSensorCalib*>(element);
if (! that)
return this;
const VertexSE3* robot = dynamic_cast<const VertexSE3*>(that->vertex(0));
const VertexSE3* sensor = dynamic_cast<const VertexSE3*>(that->vertex(2));
if (! robot|| ! sensor)
return 0;
double d=that->measurement().distance();
double azimuth=Plane3D::azimuth(that->measurement().normal());
double elevation=Plane3D::elevation(that->measurement().normal());
glColor3f(float(that->color(0)), float(that->color(1)), float(that->color(2)));
glPushMatrix();
Isometry3D robotAndSensor = robot->estimate() * sensor->estimate();
glMultMatrixd(robotAndSensor.matrix().data());
glRotatef(float(RAD2DEG(azimuth)), 0.f, 0.f, 1.f);
glRotatef(float(RAD2DEG(elevation)), 0.f, -1.f, 0.f);
glTranslatef(float(d), 0.f, 0.f);
float planeWidth = 0.5f;
float planeHeight = 0.5f;
if (0) {
planeWidth = _planeWidth->value();
planeHeight = _planeHeight->value();
}
if (_planeWidth && _planeHeight){
glBegin(GL_QUADS);
glNormal3f(-1,0,0);
glVertex3f(0,-planeWidth, -planeHeight);
glVertex3f(0, planeWidth, -planeHeight);
glVertex3f(0, planeWidth, planeHeight);
glVertex3f(0,-planeWidth, planeHeight);
glEnd();
}
glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE2* e = static_cast<EdgeSE2*>(element);
VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0));
VertexSE2* to = static_cast<VertexSE2*>(e->vertex(1));
if (! from && ! to)
return this;
SE2 fromTransform;
SE2 toTransform;
glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING | GL_COLOR);
glDisable(GL_LIGHTING);
if (! from) {
glColor3f(POSE_EDGE_GHOST_COLOR);
toTransform = to->estimate();
fromTransform = to->estimate()*e->measurement().inverse();
// DRAW THE FROM EDGE AS AN ARROW
glPushMatrix();
glTranslatef((float)fromTransform.translation().x(), (float)fromTransform.translation().y(),0.f);
glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f);
opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
glPopMatrix();
} else if (! to){
glColor3f(POSE_EDGE_GHOST_COLOR);
fromTransform = from->estimate();
toTransform = from->estimate()*e->measurement();
// DRAW THE TO EDGE AS AN ARROW
glPushMatrix();
glTranslatef(toTransform.translation().x(),toTransform.translation().y(),0.f);
glRotatef((float)RAD2DEG(toTransform.rotation().angle()),0.f,0.f,1.f);
opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
glPopMatrix();
} else {
glColor3f(POSE_EDGE_COLOR);
fromTransform = from->estimate();
toTransform = to->estimate();
}
glBegin(GL_LINES);
glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),0.f);
glVertex3f((float)toTransform.translation().x(),(float)toTransform.translation().y(),0.f);
glEnd();
glPopAttrib();
return this;
}
g2o::HyperGraphElementAction* EdgePoseLandmarkReprojectDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Get data
const EdgePoseLandmarkReproject* reproj_edge_ptr = static_cast<EdgePoseLandmarkReproject*>(element);
const VertexPose * w_T_cam
= static_cast<const VertexPose*>(reproj_edge_ptr->vertices()[0]);
const VertexLandmarkXYZ * P_world
= static_cast<const VertexLandmarkXYZ*>(reproj_edge_ptr->vertices()[1]);
Eigen::Vector3d camera = w_T_cam->estimate().translation();
Eigen::Vector3d landmark = P_world->estimate();
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Draw points
// gl setup
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
// draw 3D will draw rays for omni pixels, and 3D points for stereo
glLineWidth(0.3);
glColor3f(1.0f,0.0f,0.0f);
glBegin(GL_LINES);
glVertex3f(camera[0], camera[1], camera[2]);
glVertex3f(landmark[0], landmark[1], landmark[2]);
glEnd();
// cleanup opengl settings
glEnable(GL_LIGHTING);
glPopAttrib();
return this;
}
HyperGraphElementAction* VertexSE2DrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
if (! _drawActions){
_drawActions = HyperGraphActionLibrary::instance()->actionByName("draw");
}
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
VertexSE2* that = static_cast<VertexSE2*>(element);
glColor3f(0.5f,0.5f,0.8f);
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
glPushMatrix();
glTranslatef((float)that->estimate().translation().x(),(float)that->estimate().translation().y(),0.f);
glRotatef((float)RAD2DEG(that->estimate().rotation().angle()),0.f,0.f,1.f);
if (_show && _show->value()) {
float tx=0.1f, ty=0.05f;
if (_triangleX && _triangleY){
tx=_triangleX->value();
ty=_triangleY->value();
}
glBegin(GL_TRIANGLE_FAN);
glVertex3f( tx ,0.f ,0.f);
glVertex3f(-tx ,-ty, 0.f);
glVertex3f(-tx , ty, 0.f);
glEnd();
}
OptimizableGraph::Data* d=that->userData();
while (d && _drawActions ){
(*_drawActions)(d, params_);
d=d->next();
}
glPopMatrix();
glPopAttrib();
return this;
}
HyperGraphElementAction* CacheSE3OffsetDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
//CacheSE3Offset* that = static_cast<CacheSE3Offset*>(element);
//glPushMatrix();
//glMultMatrixd(that->offsetParam()->offset().matrix().data());
// if (_cubeSide)
// drawMyPyramid(_cubeSide->value(), _cubeSide->value());
//glPopMatrix();
return this;
}
HyperGraphElementAction* EdgeSE2DistanceOrientationDrawAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE2DistanceOrientation* e = static_cast<EdgeSE2DistanceOrientation*>(element);
VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0));
VertexPointXY* to = static_cast<VertexPointXY*>(e->vertex(1));
if (! from)
return this;
double guessRange=5;
double theta = e->measurement();
Vector2D p(cos(theta)*guessRange, sin(theta)*guessRange);
glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR);
glDisable(GL_LIGHTING);
if (!to){
p=from->estimate()*p;
glColor3f(LANDMARK_EDGE_GHOST_COLOR);
glPushAttrib(GL_POINT_SIZE);
glPointSize(3);
glBegin(GL_POINTS);
glVertex3f((float)p.x(),(float)p.y(),0.f);
glEnd();
glPopAttrib();
} else {
p=to->estimate();
glColor3f(LANDMARK_EDGE_COLOR);
}
glBegin(GL_LINES);
glVertex3f((float)from->estimate().translation().x(),(float)from->estimate().translation().y(),0.f);
glVertex3f((float)p.x(),(float)p.y(),0.f);
glEnd();
glPopAttrib();
return this;
}
HyperGraphElementAction* CacheSE3OffsetDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
if (typeid(*element).name()!=_typeName)
return 0;
CacheSE3Offset* that = static_cast<CacheSE3Offset*>(element);
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
float cs = _cubeSide ? _cubeSide->value() : 1.0f;
glPushAttrib(GL_COLOR);
glColor3f(POSE_PARAMETER_COLOR);
glPushMatrix();
glMultMatrixd(that->offsetParam()->offset().data());
opengl::drawBox(cs,cs,cs);
glPopMatrix();
glPopAttrib();
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;
}
HyperGraphElementAction* CacheSE3OffsetDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params){
if (typeid(*element).name()!=_typeName)
return 0;
CacheSE3Offset* that = static_cast<CacheSE3Offset*>(element);
refreshPropertyPtrs(params);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
glPushMatrix();
const Vector3d& offsetT=that->offsetParam()->offset().translation();
AngleAxisd aa(that->offsetParam()->offset().rotation());
glTranslatef(offsetT.x(), offsetT.y(), offsetT.z());
glRotatef(RAD2DEG(aa.angle()),aa.axis().x(),aa.axis().y(),aa.axis().z());
// if (_cubeSide)
// drawMyPyramid(_cubeSide->value(), _cubeSide->value());
glPopMatrix();
return this;
}
HyperGraphElementAction* VertexSE3PriorDrawAction::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;
VertexSE3Prior* that = static_cast<VertexSE3Prior*>(element);
glColor3f(POSE_VERTEX_PRIOR_COLOR);
glPushMatrix();
glMultMatrixd(that->estimate().matrix().data());
opengl::drawArrow2D(_triangleX->value(), _triangleY->value(), _triangleX->value()*.3f);
drawCache(that->cacheContainer(), params_);
drawUserData(that->userData(), params_);
glPopMatrix();
return this;
}
CacheSE3OffsetDrawAction::CacheSE3OffsetDrawAction(): DrawAction(typeid(CacheSE3Offset).name()){
_previousParams = (DrawAction::Parameters*)0x42;
refreshPropertyPtrs(0);
}
HyperGraphElementAction* EdgeSE2Landmark_malcolmDrawAction::operator()(HyperGraph::HyperGraphElement* element,
HyperGraphElementAction::Parameters* params_){
// std::cout << "DRAW up :D" << std::endl;exit(0);
if (typeid(*element).name()!=_typeName){
std::cout << "Wrong name :(" <<std::endl;;
return 0;
}
// refreshPropertyPtrs(params_);
// if (! _previousParams)
// return this;
//
// if (_show && !_show->value())
// return this;
//
// EdgeSE2Prior* e = static_cast<EdgeSE2Prior*>(element);
// VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
// VertexSE2* toEdge = static_cast<VertexSE2*>(e->vertex(1));
// // EdgeSE2PointXY* e = static_cast<EdgeSE2PointXY*>(element);
// // VertexSE2* fromEdge = static_cast<VertexSE2*>(e->vertex(0));
// // VertexPointXY* toEdge = static_cast<VertexPointXY*>(e->vertex(1));
// if (! fromEdge)
// return this;
// Vector3D p3d=e->measurement().toVector();
// Vector2D p;
// p << p3d(0), p3d(1);
// glPushAttrib(GL_ENABLE_BIT|GL_LIGHTING|GL_COLOR);
// glDisable(GL_LIGHTING);
// if (!toEdge){
// p=fromEdge->estimate()/*.toVector()*/*p;
// glColor3f(LANDMARK_EDGE_GHOST_COLOR);
// glPushAttrib(GL_POINT_SIZE);
// glPointSize(3);
// glBegin(GL_POINTS);
// glVertex3f((float)p.x(),(float)p.y(),0.f);
// glEnd();
// glPopAttrib();
// } else {
// p=toEdge->estimate().toVector();
// glColor3f(LANDMARK_EDGE_COLOR);
// }
// glBegin(GL_LINES);
// glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),0.f);
// glVertex3f((float)p.x(),(float)p.y(),0.f);
// glEnd();
// glPopAttrib();
// return this;
refreshPropertyPtrs(params_);
if (! _previousParams)
return this;
if (_show && !_show->value())
return this;
EdgeSE2Landmark_malcolm* e = static_cast<EdgeSE2Landmark_malcolm*>(element);
VertexSE2* from = static_cast<VertexSE2*>(e->vertex(0));
VertexSE2* to = static_cast<VertexSE2*>(e->vertex(1));
if (! from && ! to)
return this;
SE2 fromTransform;
SE2 toTransform;
glPushAttrib(GL_ENABLE_BIT | GL_LIGHTING | GL_COLOR);
glDisable(GL_LIGHTING);
if (! from) {
glColor3f(POSE_EDGE_LANDMARK_MALCOLM_GHOST_COLOR);
toTransform = to->estimate();
fromTransform = to->estimate()*e->measurement().inverse();
// DRAW THE FROM EDGE AS AN ARROW
glPushMatrix();
glTranslatef((float)fromTransform.translation().x(), (float)fromTransform.translation().y(),0.f);
glRotatef((float)RAD2DEG(fromTransform.rotation().angle()),0.f,0.f,1.f);
opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
glPopMatrix();
} else if (! to){
glColor3f(POSE_EDGE_LANDMARK_MALCOLM_GHOST_COLOR);
fromTransform = from->estimate();
toTransform = from->estimate()*e->measurement();
// DRAW THE TO EDGE AS AN ARROW
glPushMatrix();
glTranslatef(toTransform.translation().x(),toTransform.translation().y(),0.f);
glRotatef((float)RAD2DEG(toTransform.rotation().angle()),0.f,0.f,1.f);
opengl::drawArrow2D((float)_triangleX->value(), (float)_triangleY->value(), (float)_triangleX->value()*.3f);
glPopMatrix();
} else {
glColor3f(POSE_EDGE_LANDMARK_MALCOLM_COLOR);
fromTransform = from->estimate();
toTransform = to->estimate();
}
glBegin(GL_LINES);
glVertex3f((float)fromTransform.translation().x(),(float)fromTransform.translation().y(),0.f);
glVertex3f((float)toTransform.translation().x(),(float)toTransform.translation().y(),0.f);
glEnd();
glPopAttrib();
return this;
}
g2o::HyperGraphElementAction* EdgePoseLandmarkReprojectBVDrawAction::operator()(g2o::HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_) {
if (typeid(*element).name()!=_typeName) {
return 0;
}
refreshPropertyPtrs(params_);
if (! _previousParams) {
return this;
}
if (_show && !_show->value()) {
return this;
}
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Get data
const EdgePoseLandmarkReprojectBV* reproj_edge_ptr = static_cast<EdgePoseLandmarkReprojectBV*>(element);
const VertexPose* w_T_cam = static_cast<const VertexPose*>(reproj_edge_ptr->vertices()[0]);
const VertexLandmarkXYZ* P_world = static_cast<const VertexLandmarkXYZ*>(reproj_edge_ptr->vertices()[1]);
Eigen::Vector3d camera = w_T_cam->estimate().translation();
Eigen::Vector3d landmark = P_world->estimate();
Eigen::Affine3d world2cam = w_T_cam->estimate();
// get bearing vector in world coords
Eigen::Vector2d px = reproj_edge_ptr->measurement();
Eigen::Vector3d bv = world2cam* Eigen::Vector3d(px[0], px[1], 1.0);
//get bearing vector cam->point in cam frame, then put on image plane
Eigen::Vector3d bvP = world2cam.inverse()*landmark; // vector cam -> point cam frame
bvP = Eigen::Vector3d(bvP[0]/bvP[2], bvP[1]/bvP[2], 1.0);
Eigen::Vector3d pxP = world2cam * bvP;
////////////////////////////////////////////////////////////////////////////////////////////////////
/// Draw points
// gl setup
glPushAttrib(GL_ENABLE_BIT);
glDisable(GL_LIGHTING);
// draw 3D will draw rays for omni pixels, and 3D points for stereo
glLineWidth(0.1);
glColor3f(0.0f,1.0f,0.0f);
glBegin(GL_LINES);
glVertex3f(camera[0], camera[1], camera[2]);
glVertex3f(landmark[0], landmark[1], landmark[2]);
glEnd();
// draw bearing vector to image plane
glLineWidth(2);
glColor3f(0.0f,1.0f,1.0f);
glBegin(GL_LINES);
glVertex3f(camera[0], camera[1], camera[2]);
glVertex3f(bv[0], bv[1], bv[2]);
glEnd();
// draw error on image plane
glLineWidth(1);
glColor3f(1.0f, 0.0f, 0.0f);
glBegin(GL_LINES);
glVertex3f(pxP[0], pxP[1], pxP[2]);
glVertex3f(bv[0], bv[1], bv[2]);
glEnd();
// cleanup opengl settings
glEnable(GL_LIGHTING);
glPopAttrib();
return this;
}
