void render(const FeatureBaseList& feature_list, int laser, const LandmarkBaseList& landmark_list, const Eigen::Vector3s& estimated_pose)
{
// detected corners
//std::cout << " drawCorners: " << feature_list.size() << std::endl;
std::vector<double> corner_vector;
corner_vector.reserve(2*feature_list.size());
for (auto corner : feature_list)
{
//std::cout << " corner " << corner->id() << std::endl;
corner_vector.push_back(corner->getMeasurement(0));
corner_vector.push_back(corner->getMeasurement(1));
}
myRender->drawCorners(laser == 1 ? laser1Pose : laser2Pose, corner_vector);
// landmarks
//std::cout << " drawLandmarks: " << landmark_list.size() << std::endl;
std::vector<double> landmark_vector;
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
Scalar* position_ptr = landmark->getPPtr()->getPtr();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
}
myRender->drawLandmarks(landmark_vector);
// draw localization and sensors
estimated_vehicle_pose.setPose(estimated_pose(0), estimated_pose(1), 0.2, estimated_pose(2), 0, 0);
estimated_laser_1_pose.setPose(estimated_vehicle_pose);
estimated_laser_1_pose.moveForward(laser_1_pose_(0));
estimated_laser_2_pose.setPose(estimated_vehicle_pose);
estimated_laser_2_pose.moveForward(laser_2_pose_(0));
estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + laser_2_pose_(3), estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
//Set view point and render the scene
//locate visualization view point, somewhere behind the device
// viewPoint.setPose(devicePose);
// viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() );
// viewPoint.moveForward(-5);
myRender->setViewPoint(viewPoint);
myRender->drawPoseAxis(devicePose);
myRender->drawScan(laser == 1 ? laser1Pose : laser2Pose, laser == 1 ? scan1 : scan2, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
myRender->render();
}
//function travel around
Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
else { displacement_ = 0.3; rotation_ = 0; }
devicePose.moveForward(displacement_);
devicePose.rt.setEuler(devicePose.rt.head() + rotation_, devicePose.rt.pitch(), devicePose.rt.roll());
// laser 1
laser1Pose.setPose(devicePose);
laser1Pose.moveForward(laser_1_pose_(0));
// laser 2
laser2Pose.setPose(devicePose);
laser2Pose.moveForward(laser_2_pose_(0));
laser2Pose.rt.setEuler(laser2Pose.rt.head() + laser_2_pose_(3), laser2Pose.rt.pitch(), laser2Pose.rt.roll());
devicePoses.push_back(devicePose);
ground_truth_pose_ << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head();
return ground_truth_pose_;
}
FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
laser_1_pose_(_laser_1_pose),
laser_2_pose_(_laser_2_pose)
{
devicePose.setPose(2, 8, 0.2, 0, 0, 0);
viewPoint.setPose(devicePose);
viewPoint.moveForward(10);
viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
viewPoint.moveForward(-15);
//glut initialization
faramotics::initGLUT(argc, argv);
//create a viewer for the 3D model and scan points
myRender = new CdynamicSceneRender(1200, 700, 90 * M_PI / 180, 90 * 700.0 * M_PI / (1200.0 * 180.0), 0.2, 100);
myRender->loadAssimpModel(modelFileName, true); //with wireframe
//create scanner and load 3D model
myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG); //HOKUYO_UTM30LX_180DEG or LEUZE_RS4
myScanner->loadAssimpModel(modelFileName);
}
//function travel around
void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_)
{
if (ii <= 120)
{
displacement_ = 0.1;
rotation_ = 0;
}
else if ((ii > 120) && (ii <= 170))
{
displacement_ = 0.2;
rotation_ = 1.8 * M_PI / 180;
}
else if ((ii > 170) && (ii <= 220))
{
displacement_ = 0;
rotation_ = -1.8 * M_PI / 180;
}
else if ((ii > 220) && (ii <= 310))
{
displacement_ = 0.1;
rotation_ = 0;
}
else if ((ii > 310) && (ii <= 487))
{
displacement_ = 0.1;
rotation_ = -1. * M_PI / 180;
}
else if ((ii > 487) && (ii <= 600))
{
displacement_ = 0.2;
rotation_ = 0;
}
else if ((ii > 600) && (ii <= 700))
{
displacement_ = 0.1;
rotation_ = -1. * M_PI / 180;
}
else if ((ii > 700) && (ii <= 780))
{
displacement_ = 0;
rotation_ = -1. * M_PI / 180;
}
else
{
displacement_ = 0.3;
rotation_ = 0.0 * M_PI / 180;
}
pose.moveForward(displacement_);
pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
}
int main(int argc, char** argv)
{
unsigned int ii;
CrangeScan2D *myLaserScanner;
CrangeImage *myDepthCamera;
Cpose3d pose;
vector<float> myScan;
vector<float> myImage;
timeval t1,t2;
double dTscan = 0;
double dTimage = 0;
unsigned int nTrials = 100;
//glut initialization
glutInit(&argc, argv);
//set devices
//myLaserScanner = new CrangeScan2D(LEUZE_RS4);
myLaserScanner = new CrangeScan2D(HOKUYO_UTM30LX);
//myDepthCamera = new CrangeImage(SR4000);
myDepthCamera = new CrangeImage(KINECT);
//load 3D models
//myLaserScanner->loadModel("../models/campusNordUPC.obj");
myLaserScanner->loadModel(SPHERE);
//myDepthCamera->loadModel("../models/campusNordUPC.obj");
myDepthCamera->loadModel(SPHERE);
//main loop
for (int jj=0; jj<5; jj++)
{
dTscan = 0;
dTimage = 0;
for (ii = 0 ; ii<nTrials; ii++)
{
pose.setPose(1.0+ii*1e-2, 1.0-ii*1e-2, 1.0, 30, 0.0, 0.0, inDEGREES);//just to modify a little bit the view point
myScan.clear(); //clear vector results
myImage.clear(); //clear vector results
//laser scan
gettimeofday(&t1, NULL);
myLaserScanner->computeScan(pose,myScan);
gettimeofday(&t2, NULL);
dTscan += (double) ( (t2.tv_sec + t2.tv_usec/1e6) - (t1.tv_sec + t1.tv_usec/1e6) );
//depth image
gettimeofday(&t1, NULL);
myDepthCamera->depthImage(pose,myImage);
gettimeofday(&t2, NULL);
dTimage += (double) ( (t2.tv_sec + t2.tv_usec/1e6) - (t1.tv_sec + t1.tv_usec/1e6) );
}
cout << "dTscan = " << (dTscan/nTrials)*1000 << " ms" << endl;
cout << "dTimage = " << (dTimage/nTrials)*1000 << " ms" << endl;
cout << endl;
}
//delete objects
delete myLaserScanner;
delete myDepthCamera;
return 0;
}
