// Finds the 3D position of a point (in reference frame B) given the inputs
TooN::Vector<3> MapMakerServerBase::ReprojectPoint(TooN::SE3<> se3AfromB, const TooN::Vector<3>& v3A,
const TooN::Vector<3>& v3B)
{
// Algorithm from Hartley & Zisserman's Multiple View Geometry book section 12.2
TooN::Matrix<3, 4> PDash;
PDash.slice<0, 0, 3, 3>() = se3AfromB.get_rotation().get_matrix();
PDash.slice<0, 3, 3, 1>() = se3AfromB.get_translation().as_col();
TooN::Matrix<4> A;
A[0][0] = -v3B[2];
A[0][1] = 0.0;
A[0][2] = v3B[0];
A[0][3] = 0.0;
A[1][0] = 0.0;
A[1][1] = -v3B[2];
A[1][2] = v3B[1];
A[1][3] = 0.0;
A[2] = v3A[0] * PDash[2] - v3A[2] * PDash[0];
A[3] = v3A[1] * PDash[2] - v3A[2] * PDash[1];
TooN::SVD<4, 4> svd(A);
TooN::Vector<4> v4Smallest = svd.get_VT()[3];
if (v4Smallest[3] == 0.0)
v4Smallest[3] = 0.00001;
return project(v4Smallest);
}
void MapView::plotCam(TooN::SE3<> droneToGlobal, bool xyCross, float thick, float len, float alpha)
{
glEnable(GL_DEPTH_TEST);
// draw cam
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScaled(0.1,0.1,0.1);
CVD::glMultMatrix(mse3ViewerFromWorld * droneToGlobal);
SetupFrustum();
glLineWidth(thick*lineWidthFactor);
if(alpha < 1)
{
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
}
else
glDisable(GL_BLEND);
glBegin(GL_LINES);
glColor4f(1,0,0,alpha);
glVertex3f(-len, 0.0f, 0.0f);
glVertex3f(len, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, -len, 0.0f);
glColor4f(0,1,0,alpha);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, len, 0.0f);
glColor4f(1,1,1,alpha);
glVertex3f(0.0f, 0.0f, 0.0f);
glVertex3f(0.0f, 0.0f, len);
glEnd();
if(xyCross)
{
glLineWidth(1*lineWidthFactor);
glColor4f(1,1,1, alpha);
SetupModelView();
TooN::Vector<2> v2CamPosXY = droneToGlobal.get_translation().slice<0,2>();
glBegin(GL_LINES);
glColor4f(1,1,1, alpha);
glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] + 0.04);
glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] - 0.04);
glVertex2d(v2CamPosXY[0] - 0.04, v2CamPosXY[1] - 0.04);
glVertex2d(v2CamPosXY[0] + 0.04, v2CamPosXY[1] + 0.04);
glEnd();
}
glDisable(GL_BLEND);
glDisable(GL_DEPTH_TEST);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
}
void EstimationNode::publishTf(TooN::SE3<> trans, ros::Time stamp, int seq, std::string system)
{
trans = trans.inverse();
tf::Matrix3x3 m;
m[0][0] = trans.get_rotation().get_matrix()(0,0);
m[0][1] = trans.get_rotation().get_matrix()(0,1);
m[0][2] = trans.get_rotation().get_matrix()(0,2);
m[1][0] = trans.get_rotation().get_matrix()(1,0);
m[1][1] = trans.get_rotation().get_matrix()(1,1);
m[1][2] = trans.get_rotation().get_matrix()(1,2);
m[2][0] = trans.get_rotation().get_matrix()(2,0);
m[2][1] = trans.get_rotation().get_matrix()(2,1);
m[2][2] = trans.get_rotation().get_matrix()(2,2);
tf::Vector3 v;
v[0] = trans.get_translation()[0];
v[1] = trans.get_translation()[1];
v[2] = trans.get_translation()[2];
tf::Transform tr = tf::Transform(m,v);
tf::StampedTransform t = tf::StampedTransform(tr,stamp,"map",system);
tf_broadcaster.sendTransform(t);
if(logfilePTAMRaw != NULL)
{
pthread_mutex_lock(&(logPTAMRaw_CS));
// log:
// - filterPosePrePTAM estimated for videoFrameTimestamp-delayVideo.
// - PTAMResulttransformed estimated for videoFrameTimestamp-delayVideo. (using imu only for last step)
// - predictedPoseSpeed estimated for lastNfoTimestamp+filter->delayControl (actually predicting)
// - predictedPoseSpeedATLASTNFO estimated for lastNfoTimestamp (using imu only)
if(logfilePTAMRaw != NULL)
(*(logfilePTAMRaw)) << seq << " " << stamp << " " << tr.getOrigin().x() << " " << tr.getOrigin().y() << " " << tr.getOrigin().z() << " " <<
tr.getRotation().x() << " " << tr.getRotation().y() << " " << tr.getRotation().z() << " " << tr.getRotation().w() << std::endl;
pthread_mutex_unlock(&(logPTAMRaw_CS));
}
}
// Draw the reference grid to give the user an idea of wether tracking is OK or not.
void PTAMWrapper::renderGrid(TooN::SE3<> camFromWorld)
{
myGLWindow->SetupViewport();
myGLWindow->SetupVideoOrtho();
myGLWindow->SetupVideoRasterPosAndZoom();
camFromWorld.get_translation() *= 1;
// The colour of the ref grid shows if the coarse stage of tracking was used
// (it's turned off when the camera is sitting still to reduce jitter.)
glColor4f(0,0,0,0.6);
// The grid is projected manually, i.e. GL receives projected 2D coords to draw.
int nHalfCells = 5;
int nTot = nHalfCells * 2 + 1;
CVD::Image<Vector<2> > imVertices(CVD::ImageRef(nTot,nTot));
for(int i=0; i<nTot; i++)
for(int j=0; j<nTot; j++)
{
Vector<3> v3;
v3[0] = (i - nHalfCells) * 1;
v3[1] = (j - nHalfCells) * 1;
v3[2] = 0.0;
Vector<3> v3Cam = camFromWorld * v3;
//v3Cam[2] *= 100;
if(v3Cam[2] < 0.001)
v3Cam = TooN::makeVector(100000*v3Cam[0],100000*v3Cam[1],0.0001);
imVertices[i][j] = mpCamera->Project(TooN::project(v3Cam))*0.5;
}
glEnable(GL_LINE_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glLineWidth(2);
for(int i=0; i<nTot; i++)
{
glBegin(GL_LINE_STRIP);
for(int j=0; j<nTot; j++)
CVD::glVertex(imVertices[i][j]);
glEnd();
glBegin(GL_LINE_STRIP);
for(int j=0; j<nTot; j++)
CVD::glVertex(imVertices[j][i]);
glEnd();
};
glLineWidth(1);
glColor3f(1,0,0);
}
