bool ROSMain_features() {
CoSLAM& coSLAM = MyApp::coSLAM;
/////////////////////////1.GPU initilization/////////////////////////
//initialization for CG;
glutInitDisplayMode(GLUT_RGB | GLUT_DEPTH | GLUT_DOUBLE);
glutCreateWindow(" ");
glutHideWindow();
glewInit();
V3D_GPU::Cg_ProgramBase::initializeCg();
//////////////////////////2.read video information//////////////////
try {
// for(int c = 0; c < coSLAM.numCams; c++){
// coSLAM.slam[c].videoReader = &MyApp::aviReader[c];
// }
coSLAM.init(false);
MyApp::bInitSucc = true;
logInfo("Loading video sequences.. OK!\n");
} catch (SL_Exception& e) {
logInfo(e.what());
#ifdef WIN32
wxMessageBox(e.what());
#endif
return 0;
}
//notify the GUI thread to create GUIs
MyApp::broadcastCreateGUI();
//wait for the accomplishment of creating GUIs
MyApp::waitCreateGUI();
for (int i = 0; i < coSLAM.numCams; i++){
MyApp::videoWnd[i]->setSLAMData(i, &coSLAM);
vector<float> reprojErrStatic, reprojErrDynamic;
vector<int> frameNumber;
MyApp::s_reprojErrDynamic.push_back(reprojErrStatic);
MyApp::s_reprojErrStatic.push_back(reprojErrDynamic);
MyApp::s_frameNumber.push_back(frameNumber);
}
MyApp::modelWnd1->setSLAMData(&coSLAM);
MyApp::modelWnd2->setSLAMData(&coSLAM);
MyApp::broadcastCreateGUI();
//for measuring the timings
Timings timingsPerStep;
Timings timingsReadFrame;
Timings timingsNewMapPonits;
/* start the SLAM process*/
try {
coSLAM.readFrame();
//copy the data to buffers for display
updateDisplayData();
//initialise the map points
for (int i = 0; i < coSLAM.numCams; i++) {
printf("slam[%d].m_camPos.size(): %d\n", i, coSLAM.slam[i].m_camPos.size());
}
// tic();
// coSLAM.initMap();
// toc();
cout <<"debug\n";
// redis->setPose(1, 2, 3.6);
while (!MyApp::bExit){
pthread_mutex_lock(&MyApp::_mutexImg);
if (!coSLAM.grabReadFrame())
pthread_cond_wait(&MyApp::_condImg, &MyApp::_mutexImg);
pthread_mutex_unlock(&MyApp::_mutexImg);
// //copy the data to buffers for display
updateDisplayData();
//printf("current frame: %d\n", coSLAM.curFrame);
if (MyApp::bStartInit){
MyApp::publishMapInit();
printf("Start initializing map...\n");
coSLAM.curFrame = 0;
//initialise the map points
if (coSLAM.initMap()){
if (MyApp::doCalibration) {
// Need this in real test
coSLAM.calibGlobal2Cam();
}
for (int i = 0; i < coSLAM.numCams; i++)
coSLAM.state[i] = SLAM_STATE_NORMAL;
printf("Init map success!\n");
break;
}
else{
MyApp::bStartInit = false;
MyApp::publishMapInit();
}
}
}
updateDisplayData();
// coSLAM.pause();
for (int i = 0; i < coSLAM.numCams; i++) {
printf("slam[%d].m_camPos.size(): %d\n", i, coSLAM.slam[i].m_camPos.size());
coSLAM.state[i] = SLAM_STATE_NORMAL;
}
// return 0;
// coSLAM.pause();
int endFrame = SLAMParam::nTotalFrame - SLAMParam::nSkipFrame
- SLAMParam::nInitFrame - 10;
// int endFrame = 500;
// endFrame = 1500;
int i = 0;
// init estimation flag
bool bEstPose[SLAM_MAX_NUM];
for (int i = 0; i < SLAM_MAX_NUM; i++){
bEstPose[i] = false;
}
vector<double> tmStepVec;
vector<double> poseSent[SLAM_MAX_NUM];
// vector<double> rosTime;
while (!MyApp::bExit) {
// while (MyApp::bStop) {/*stop*/
// }
// if (MyApp::_mergeable){
// if (MyApp::_imgReady[0] && MyApp::_imgReady[1]){
// coSLAM.grabReadFrame();
// MyApp::_imgAvailableForMerge = true;
// }
// else
// continue;
// }
i++;
TimeMeasurer tmPerStep;
tmPerStep.tic();
if (!receiveFeatures){
coSLAM.grabReadFrame();
coSLAM.featureTracking();
}
else{
coSLAM.featureReceiving();
coSLAM.virtualReadFrame();
}
for (int i = 0; i < coSLAM.numCams; i++){
cv::Mat cvImg(coSLAM.slam[i].m_img.rows, coSLAM.slam[i].m_img.cols, CV_8UC1,
coSLAM.slam[i].m_img.data);
MyApp::s_camFrames[i].push_back(cvImg.clone());
CamPoseItem* cam = coSLAM.slam[i].m_camPos.current();
double ts = cam->ts;
MyApp::s_camFramesTS[i].push_back(ts);
}
if(!coSLAM.poseUpdate(bEstPose))
break;
//Use redis to send over the poses
// for (int i = 0; i < coSLAM.numCams; i++){
// double org[3];
//// getCamCenter(coSLAM.slam[i].m_camPos.current(), org);
// coSLAM.transformCamPose2Global(coSLAM.slam[i].m_camPos.current(), org);
//
// MyApp::redis[i]->setPose(org[0], org[1], org[2]);
//
// double ts = coSLAM.slam[i].m_camPos.current()->ts;
// poseSent[i].push_back(ts);
// poseSent[i].push_back(org[0]);
// poseSent[i].push_back(org[1]);
// poseSent[i].push_back(org[2]);
// rosTime.push_back(ts);
// }
// if (MyApp::bStartMove){
// MyApp::redis_start->setCommand("go");
// MyApp::bStartMove = false;
// }
//coSLAM.pause();
coSLAM.cameraGrouping();
//existing 3D to 2D points robust
coSLAM.activeMapPointsRegister(Const::PIXEL_ERR_VAR);
TimeMeasurer tmNewMapPoints;
tmNewMapPoints.tic();
if (receiveFeatures){
coSLAM.genNewMapPoints_new();
if (MyApp::_mergeable)
MyApp::triggerClients();
}
else
{
bool merge = false;
coSLAM.genNewMapPoints(merge);
}
//
// coSLAM.m_tmNewMapPoints = tmNewMapPoints.toc();
// cout << "coSLAM.m_tmNewMapPoints" << coSLAM.m_tmNewMapPoints << endl;
//point registration
coSLAM.currentMapPointsRegister(Const::PIXEL_ERR_VAR,
i % 50 == 0 ? true : false);
coSLAM.storeDynamicPoints();
updateDisplayData();
redrawAllViews();
// Sleep(50);
if (i % 500 == 0) {
//coSLAM.releaseFeatPts(coSLAM.curFrame - 500);
coSLAM.releaseKeyPoseImgs(coSLAM.curFrame - 500);
coSLAM.m_lastReleaseFrm = coSLAM.curFrame;
}
coSLAM.m_tmPerStep = tmPerStep.toc();
tmStepVec.push_back(coSLAM.m_tmPerStep);
//Send pose
for (int i = 0; i < coSLAM.numCams; i++)
{
if (coSLAM.slam[i].m_camPos.size() > 0){
double org[3], rpy[3];
CamPoseItem* cam = coSLAM.slam[i].m_camPos.current();
double ts = cam->ts;
getCamCenter(cam, org);
coSLAM.transformCamPose2Global(cam, org, rpy);
double targetPos[3];
targetPos[0] = targetPos[1] = targetPos[2] = 0;
double theta = 0;
if(coSLAM.dynObjPresent){
coSLAM.transformTargetPos2Global(cam->currDynPos, targetPos);
//printf("targetPos: %lf %lf %lf\n", targetPos[0], targetPos[1], targetPos[2]);
coSLAM.slam[i].projectTargetToCam(cam, cam->currDynPos);
theta = atan2(coSLAM.slam[i]._targetPosInCam[0], coSLAM.slam[i]._targetPosInCam[2]);
// double H = targetPos[2] * 2;
// double Z = coSLAM.slam[i]._targetPosInCam[2];
// Publish target pose
MyApp::transformStampedMsg.transform.translation.x = targetPos[0];
MyApp::transformStampedMsg.transform.translation.y = targetPos[1];
//MyApp::transformStampedMsg.transform.translation.z = H;
MyApp::transformStampedMsg.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, theta);
MyApp::transformStampedMsg.header.stamp.sec = ts;
MyApp::targetPosePub.publish(MyApp::transformStampedMsg);
//MyApp::redis[i]->setPoseTarget(ts, 1, theta, org[0], org[1], rpy[2], targetPos[0], targetPos[1], 0.9);
// MyApp::redis_dynObj->setDynObj(ts, targetPos[0], targetPos[1], 0.9);
// printf("currDynPos: %lf %lf %lf\n", cam->currDynPos[0], cam->currDynPos[1], cam->currDynPos[2]);
}
// Publish camera pose
MyApp::transformStampedMsg.transform.translation.x = org[0];
MyApp::transformStampedMsg.transform.translation.y = org[1];
//MyApp::transformStampedMsg.transform.translation.z = org[2];
MyApp::transformStampedMsg.transform.rotation = tf::createQuaternionMsgFromRollPitchYaw(rpy[0], rpy[1], rpy[2]);
MyApp::transformStampedMsg.header.stamp.sec = ts;
MyApp::posePubs[i].publish(MyApp::transformStampedMsg);
// else {
// MyApp::redis[i]->setPoseTarget(ts, 0, theta, org[0], org[1], rpy[2], targetPos[0], targetPos[1], 0.9);
// }
// (Jacob) Huh?
//MyApp::redis_dynObj->setDynObj(ts, 0.5, 2, 0.9);
printf("targetPos: %lf %lf %lf\n", targetPos[0], targetPos[1], targetPos[2]);
printf("org[2]: %lf %lf %lf\n", org[0], org[1], rpy[2]);
}
}
}
cout << " the result is saved at " << MyApp::timeStr << endl;
coSLAM.exportResults(MyApp::timeStr);
FILE* fid = fopen("/home/rui/coslamTime.txt","w");
for (int i = 0; i < tmStepVec.size(); i++){
fprintf(fid, "%lf\n", tmStepVec[i]);
}
fclose(fid);
// FILE* fid = fopen("slam_timing.txt","w");
// for (int i = 0; i < tmStepVec.size(); i++)
// fprintf(fid, "%f\n", tmStepVec[i]);
// fclose(fid);
//
// fid = fopen("poseSent0.txt","w");
// for (int i = 0; i < poseSent[0].size(); i = i + 4)
// fprintf(fid, "%lf %lf %lf %lf\n", poseSent[0][i],
// poseSent[0][i+1], poseSent[0][i+2], poseSent[0][i+3]);
// fclose(fid);
//
// fid = fopen("poseSent1.txt","w");
// for (int i = 0; i < poseSent[1].size(); i = i + 4)
// fprintf(fid, "%lf %lf %lf %lf\n", poseSent[1][i],
// poseSent[1][i+1], poseSent[1][i+2], poseSent[1][i+3]);
// fclose(fid);
//
// FILE* fid = fopen("rosTime.txt","w");
// for (int i = 0; i < rosTime.size(); i = i + coSLAM.numCams + 1){
// for (int j = 0; j <= coSLAM.numCams; j++){
// fprintf(fid, "%lf ", rosTime[i+j]);
// }
// fprintf(fid, "\n");
// }
// fclose(fid);
logInfo("slam finished\n");
} catch (SL_Exception& e) {
logInfo(e.what());
} catch (std::exception& e) {
#ifdef WIN32
wxMessageBox(e.what());
#endif
logInfo("%s\n", e.what());
logInfo("slam failed!\n");
#ifdef WIN32
wxMessageBox(e.what());
#endif
}
logInfo("\nslam stopped!\n");
return 0;
}
void GLScenePane::drawCameras() {
//draw camera trajectory
for (int i = 0; i < m_pSLAM->numCams; i++) {
const CamPoseItem* cam = m_pSLAM->slam[i].m_camPos.first();
while (cam->f < m_pSLAM->m_lastFrmBundle)
cam = cam->next;
//glColor3f(CAMERA_COLORS[3 * i], CAMERA_COLORS[3 * i + 1], CAMERA_COLORS[3 * i + 2]);
glLineWidth(2.0f);
glColor3f(0.5f, 0.5f, 0.5f);
glBegin(GL_LINE_STRIP);
while (cam) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
//if (cam->f >= m_pSLAM->curFrame - 200)
{
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
glPointSize(2.0);
glBegin(GL_LINE_STRIP);
cam = m_pSLAM->slam[i].m_camPos.first();
while (cam) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
//if (cam->f >= m_pSLAM->curFrame - 200)
{
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
cam = m_pSLAM->slam[i].m_camPos.first();
glColor3f(0.5f, 0.5f, 0.5f);
glLineWidth(2.0f);
glBegin(GL_LINE_STRIP);
while (cam && cam->f <= m_pSLAM->m_lastFrmBundle) {
// drawCamCenter(cam, PT_COLOR + 3 * i);
if (cam->f >= m_pSLAM->m_firstFrmBundle) {
double org[3];
getCamCenter(cam, org);
glVertex3d(org[0], org[1], org[2]);
}
cam = cam->next;
}
glEnd();
drawCamera(m_pSLAM->slam[i].videoReader->_w,
m_pSLAM->slam[i].videoReader->_h, m_pSLAM->slam[i].K.data,
m_pSLAM->slam[i].m_camPos.current(), m_camSize,
CAMERA_COLORS + 3 * i, 100);
}
//draw key camera poses
for (int i = 0; i < m_pSLAM->numCams; i++) {
for (size_t n = 0; n < m_pSLAM->slam[i].m_selfKeyPose.size(); ++n) {
const KeyPose* keyPose = m_pSLAM->slam[i].m_selfKeyPose[n];
drawCameraPose(keyPose->cam, m_camSize, CAMERA_COLORS + 3 * i, 4);
}
const KeyPose* keyPose = m_pSLAM->slam[i].m_keyPose.first();
int n = 0;
while (keyPose) {
drawCameraPose(keyPose->cam, m_camSize * 0.5,
CAMERA_COLORS + 3 * i);
n++;
keyPose = keyPose->next;
}
}
}
