CSetOfObjects::Ptr stock_objects::Hokuyo_UTM()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
{
CBox::Ptr base = mrpt::make_aligned_shared<CBox>(
TPoint3D(-0.03, -0.03, -0.055), TPoint3D(0.03, 0.03, -0.014));
base->setColor(0, 0, 0);
ret->insert(base);
}
{
CCylinder::Ptr cyl1 =
mrpt::make_aligned_shared<CCylinder>(0.028f, 0.024f, 0.028f);
cyl1->setColor(0, 0, 0);
cyl1->setPose(CPose3D(0, 0, -0.014));
ret->insert(cyl1);
}
{
CCylinder::Ptr cyl2 =
mrpt::make_aligned_shared<CCylinder>(0.028f, 0.028f, 0.01f);
cyl2->setColor(1, 69 / 255.0, 0);
cyl2->setLocation(0, 0, 0.014);
ret->insert(cyl2);
}
{
CCylinder::Ptr cyl3 =
mrpt::make_aligned_shared<CCylinder>(0.028f, 0.028f, 0.01f);
cyl3->setColor(0, 0, 0);
cyl3->setLocation(0, 0, 0.024);
ret->insert(cyl3);
}
return ret;
}
CSetOfObjects::Ptr stock_objects::CornerXYZSimple(float scale, float lineWidth)
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
{
CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
lin->setLineWidth(lineWidth);
lin->setColor(1, 0, 0);
lin->setLineCoords(0, 0, 0, scale, 0, 0);
ret->insert(lin);
}
{
CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
lin->setLineWidth(lineWidth);
lin->setColor(0, 1, 0);
lin->setLineCoords(0, 0, 0, 0, scale, 0);
ret->insert(lin);
}
{
CSimpleLine::Ptr lin = mrpt::make_aligned_shared<CSimpleLine>();
lin->setLineWidth(lineWidth);
lin->setColor(0, 0, 1);
lin->setLineCoords(0, 0, 0, 0, 0, scale);
ret->insert(lin);
}
return ret;
}
CSetOfObjects::Ptr stock_objects::Hokuyo_URG()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
{
CBox::Ptr base = mrpt::make_aligned_shared<CBox>(
TPoint3D(-0.025, -0.025, -0.0575), TPoint3D(0.025, 0.025, -0.0185));
base->setColor(0.7, 0.7, 0.7);
ret->insert(base);
}
{
CCylinder::Ptr cyl1 =
mrpt::make_aligned_shared<CCylinder>(0.02f, 0.02f, 0.01f);
cyl1->setColor(0, 0, 0);
cyl1->setLocation(0, 0, -0.014);
ret->insert(cyl1);
}
{
CCylinder::Ptr cyl2 =
mrpt::make_aligned_shared<CCylinder>(0.02f, 0.0175f, 0.01f);
cyl2->setColor(0, 0, 0);
cyl2->setLocation(0, 0, -0.004);
ret->insert(cyl2);
}
{
CCylinder::Ptr cyl3 =
mrpt::make_aligned_shared<CCylinder>(0.0175f, 0.0175f, 0.01f);
cyl3->setColor(0, 0, 0);
cyl3->setLocation(0, 0, 0.004);
ret->insert(cyl3);
}
return ret;
}
/*---------------------------------------------------------------
CornerXYZ
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::CornerXYZ(float scale)
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
CArrow::Ptr obj = CArrow::Create(
0, 0, 0, scale, 0, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);
obj->setColor(1, 0, 0);
ret->insert(obj);
obj = CArrow::Create(
0, 0, 0, 0, scale, 0, 0.25f * scale, 0.02f * scale, 0.05f * scale);
obj->setColor(0, 1, 0);
ret->insert(obj);
obj = CArrow::Create(
0, 0, 0, 0, 0, scale, 0.25f * scale, 0.02f * scale, 0.05f * scale);
obj->setColor(0, 0, 1);
ret->insert(obj);
return ret;
}
/*---------------------------------------------------------------
BumblebeeCamera
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::BumblebeeCamera()
{
CSetOfObjects::Ptr camera =
mrpt::make_aligned_shared<opengl::CSetOfObjects>();
CPolyhedron::Ptr rect = opengl::CPolyhedron::CreateCubicPrism(
-0.02, 0.14, -0.02, 0.02, 0, -0.04);
rect->setColor(1, 0.8, 0);
camera->insert(rect);
CCylinder::Ptr lCam = mrpt::make_aligned_shared<opengl::CCylinder>(
0.01f, 0.01f, 0.003f, 10, 10);
lCam->setColor(1, 0, 0);
CCylinder::Ptr rCam = mrpt::make_aligned_shared<opengl::CCylinder>(
0.01f, 0.01f, 0.003f, 10, 10);
rCam->setPose(CPose3D(0.12, 0, 0));
rCam->setColor(0, 0, 0);
camera->insert(lCam);
camera->insert(rCam);
return camera;
}
CRobotPose::Ptr CGlWidget::removePoseFromPointsCloud(
CSetOfObjects::Ptr points, int index) const
{
auto it = points->begin() + index;
ASSERT_(it != points->end());
CRobotPose::Ptr robotPose = std::dynamic_pointer_cast<CRobotPose>(*it);
ASSERT_(robotPose);
points->removeObject(*it);
return robotPose;
}
mrpt::opengl::CSetOfObjects::Ptr framePosesVecVisualize(
const TFramePosesVec& poses, const double len, const double lineWidth)
{
mrpt::opengl::CSetOfObjects::Ptr obj =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
for (size_t i = 0; i < poses.size(); i++)
{
CSetOfObjects::Ptr corner =
opengl::stock_objects::CornerXYZSimple(len, lineWidth);
CPose3D p = poses[i];
p.x(WORLD_SCALE * p.x());
p.y(WORLD_SCALE * p.y());
p.z(WORLD_SCALE * p.z());
corner->setPose(p);
corner->setName(format("%u", (unsigned int)i));
corner->enableShowName();
obj->insert(corner);
}
return obj;
}
/**
* Generate 3 objects to work with - 1 sphere, 2 disks
*/
void generateObjects(CSetOfObjects::Ptr& world)
{
CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(0.5);
sph->setLocation(0, 0, 0);
sph->setColor(1, 0, 0);
world->insert(sph);
CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0, 0, 0, 0, DEG2RAD(5), DEG2RAD(5)));
pln->setColor(0.8, 0, 0);
world->insert(pln);
{
CDisk::Ptr pln = mrpt::make_aligned_shared<opengl::CDisk>();
pln->setDiskRadius(2);
pln->setPose(CPose3D(0, 0, 0, DEG2RAD(30), DEG2RAD(-20), DEG2RAD(-2)));
pln->setColor(0.9, 0, 0);
world->insert(pln);
}
}
/*---------------------------------------------------------------
RobotRhodon
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotRhodon()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
float height = 0;
vector<TPoint2D> level1;
level1.push_back(TPoint2D(0.31, 0));
level1.push_back(TPoint2D(0.22, 0.24));
level1.push_back(TPoint2D(-0.22, 0.24));
level1.push_back(TPoint2D(-0.31, 0));
level1.push_back(TPoint2D(-0.22, -0.24));
level1.push_back(TPoint2D(0.22, -0.24));
CPolyhedron::Ptr obj1 =
opengl::CPolyhedron::CreateCustomPrism(level1, 0.38);
obj1->setLocation(0, 0, height);
height += 0.38f;
obj1->setColor(0.6, 0.6, 0.6);
ret->insert(obj1);
vector<TPoint2D> level2;
level2.push_back(TPoint2D(0.16, 0.21));
level2.push_back(TPoint2D(-0.16, 0.21));
level2.push_back(TPoint2D(-0.16, -0.21));
level2.push_back(TPoint2D(0.16, -0.21));
CPolyhedron::Ptr obj2 =
opengl::CPolyhedron::CreateCustomPrism(level2, 0.35);
obj2->setLocation(0, 0, height);
height += 0.35f;
obj2->setColor(0.2, 0.2, 0.2);
ret->insert(obj2);
vector<TPoint2D> level3;
level3.push_back(TPoint2D(-0.12, 0.12));
level3.push_back(TPoint2D(-0.16, 0.12));
level3.push_back(TPoint2D(-0.16, -0.12));
level3.push_back(TPoint2D(-0.12, -0.12));
CPolyhedron::Ptr obj3 = opengl::CPolyhedron::CreateCustomPrism(level3, 1);
obj3->setLocation(0, 0, height);
// height+=1;
obj3->setColor(0.6, 0.6, 0.6);
ret->insert(obj3);
opengl::CCylinder::Ptr obj4 = mrpt::make_aligned_shared<opengl::CCylinder>(
0.05f, 0.05f, 0.4f, 20, 20);
obj4->setLocation(0, 0, 0.73);
obj4->setColor(0, 0, 0.9);
ret->insert(obj4);
opengl::CCylinder::Ptr obj5 = mrpt::make_aligned_shared<opengl::CCylinder>(
0.05f, 0.05f, 0.4f, 20, 20);
obj5->setPose(CPose3D(0.32, 0, 0.89, 0, -1, 0));
obj5->setColor(0, 0, 0.9);
ret->insert(obj5);
return ret;
}
CSetOfObjects::Ptr stock_objects::CornerXYZEye()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
CPose3D rotation;
CArrow::Ptr obj = CArrow::Create(0, 0, 0, 1.0, 0, 0, 0.25f, 0.02f, 0.05f);
obj->setColor(1, 0, 0);
ret->insert(obj);
obj = CArrow::Create(0, 0, 0, 0, 1.0, 0, 0.25f, 0.02f, 0.05f);
obj->setColor(0, 1, 0);
ret->insert(obj);
obj = CArrow::Create(0, 0, -1.0, 0, 0, 0, 0.25f, 0.02f, 0.05f);
obj->setColor(0, 0, 1);
ret->insert(obj);
return ret;
}
// Add objects at your will to check results
void generateObjects(CSetOfObjects::Ptr& world)
{
// create object, give it a random pose/color, insert it in the world
CDisk::Ptr dsk = mrpt::make_aligned_shared<CDisk>();
dsk->setDiskRadius(MYRANDG(5, 5), MYRANDG(5));
configRandom(dsk);
world->insert(dsk);
CSphere::Ptr sph = mrpt::make_aligned_shared<CSphere>(MYRANDG(5, 1));
configRandom(sph);
world->insert(sph);
CTexturedPlane::Ptr pln = mrpt::make_aligned_shared<CTexturedPlane>(
MYRANDG(10, -10), MYRANDG(10), MYRANDG(10, -10), MYRANDG(10));
configRandom(pln);
world->insert(pln);
for (size_t i = 0; i < 5; i++)
{
CPolyhedron::Ptr poly =
CPolyhedron::CreateRandomPolyhedron(MYRANDG(2, 2));
configRandom(poly);
world->insert(poly);
}
CCylinder::Ptr cil = mrpt::make_aligned_shared<CCylinder>(
MYRANDG(3.0, 3.0), MYRANDG(3.0, 1.0), MYRANDG(2.0f, 3.0f), 50, 1);
configRandom(cil);
world->insert(cil);
CEllipsoid::Ptr ell = mrpt::make_aligned_shared<CEllipsoid>();
CMatrixDouble md = CMatrixDouble(3, 3);
for (size_t i = 0; i < 3; i++) md(i, i) = MYRANDG(8.0, 1.0);
for (size_t i = 0; i < 3; i++)
{
size_t ii = (i + 1) % 3;
md(i, ii) = md(ii, i) = MYRANDG(sqrt(md(i, i) * md(ii, ii)));
}
ell->setCovMatrix(md);
configRandom(std::dynamic_pointer_cast<CRenderizable>(ell));
world->insert(ell);
}
// ------------------------------------------------------
// Test_Kinect
// ------------------------------------------------------
void Test_Kinect()
{
// Launch grabbing thread:
// --------------------------------------------------------
TThreadParam thrPar;
std::thread thHandle = std::thread(thread_grabbing, std::ref(thrPar));
// Wait until data stream starts so we can say for sure the sensor has been
// initialized OK:
cout << "Waiting for sensor initialization...\n";
do
{
CObservation3DRangeScan::Ptr possiblyNewObs =
std::atomic_load(&thrPar.new_obs);
if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP)
break;
else
std::this_thread::sleep_for(10ms);
} while (!thrPar.quit);
// Check error condition:
if (thrPar.quit) return;
// Feature tracking variables:
CFeatureList trackedFeats;
unsigned int step_num = 0;
bool SHOW_FEAT_IDS = true;
bool SHOW_RESPONSES = true;
CGenericFeatureTrackerAutoPtr tracker;
// "CFeatureTracker_KL" is by far the most robust implementation for now:
tracker = CGenericFeatureTrackerAutoPtr(new CFeatureTracker_KL);
tracker->enableTimeLogger(true); // Do time profiling.
// Set of parameters common to any tracker implementation:
// To see all the existing params and documentation, see
// mrpt::vision::CGenericFeatureTracker
// http://reference.mrpt.org/devel/structmrpt_1_1vision_1_1_c_generic_feature_tracker.html
tracker->extra_params["add_new_features"] =
1; // track, AND ALSO, add new features
tracker->extra_params["add_new_feat_min_separation"] = 25;
tracker->extra_params["add_new_feat_max_features"] = 150;
tracker->extra_params["add_new_feat_patch_size"] = 21;
tracker->extra_params["minimum_KLT_response_to_add"] = 40;
tracker->extra_params["check_KLT_response_every"] =
5; // Re-check the KLT-response to assure features are in good points.
tracker->extra_params["minimum_KLT_response"] =
25; // Re-check the KLT-response to assure features are in good points.
tracker->extra_params["update_patches_every"] = 0; // Update patches
// Specific params for "CFeatureTracker_KL"
tracker->extra_params["window_width"] = 25;
tracker->extra_params["window_height"] = 25;
// Global points map:
CColouredPointsMap globalPtsMap;
globalPtsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage; // Take points color from
// RGB+D observations
// globalPtsMap.colorScheme.scheme =
// CColouredPointsMap::cmFromHeightRelativeToSensorGray;
// Create window and prepare OpenGL object in the scene:
// --------------------------------------------------------
mrpt::gui::CDisplayWindow3D win3D("kinect-3d-slam 3D view", 800, 600);
win3D.setCameraAzimuthDeg(140);
win3D.setCameraElevationDeg(20);
win3D.setCameraZoom(8.0);
win3D.setFOV(90);
win3D.setCameraPointingToPoint(2.5, 0, 0);
mrpt::opengl::CPointCloudColoured::Ptr gl_points =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points->setPointSize(2.5);
mrpt::opengl::CSetOfObjects::Ptr gl_curFeats =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
mrpt::opengl::CSetOfObjects::Ptr gl_keyframes =
mrpt::make_aligned_shared<mrpt::opengl::CSetOfObjects>();
mrpt::opengl::CPointCloudColoured::Ptr gl_points_map =
mrpt::make_aligned_shared<mrpt::opengl::CPointCloudColoured>();
gl_points_map->setPointSize(2.0);
const double aspect_ratio =
480.0 / 640.0; // kinect.rows() / double( kinect.cols() );
mrpt::opengl::CSetOfObjects::Ptr gl_cur_cam_corner =
mrpt::opengl::stock_objects::CornerXYZSimple(0.4f, 4);
opengl::COpenGLViewport::Ptr viewInt;
{
mrpt::opengl::COpenGLScene::Ptr& scene = win3D.get3DSceneAndLock();
// Create the Opengl object for the point cloud:
scene->insert(gl_points_map);
scene->insert(gl_points);
scene->insert(gl_curFeats);
scene->insert(gl_keyframes);
scene->insert(mrpt::make_aligned_shared<mrpt::opengl::CGridPlaneXY>());
scene->insert(gl_cur_cam_corner);
const int VW_WIDTH =
350; // Size of the viewport into the window, in pixel units.
const int VW_HEIGHT = aspect_ratio * VW_WIDTH;
// Create the Opengl objects for the planar images each in a separate
// viewport:
viewInt = scene->createViewport("view2d_int");
viewInt->setViewportPosition(2, 2, VW_WIDTH, VW_HEIGHT);
viewInt->setTransparent(true);
win3D.unlockAccess3DScene();
win3D.repaint();
}
CImage previous_image;
map<TFeatureID, TPoint3D> lastVisibleFeats;
std::vector<TPose3D>
camera_key_frames_path; // The 6D path of the Kinect camera.
CPose3D
currentCamPose_wrt_last; // wrt last pose in "camera_key_frames_path"
bool gl_keyframes_must_refresh =
true; // Need to update gl_keyframes from camera_key_frames_path??
CObservation3DRangeScan::Ptr last_obs;
string str_status, str_status2;
while (win3D.isOpen() && !thrPar.quit)
{
CObservation3DRangeScan::Ptr possiblyNewObs =
std::atomic_load(&thrPar.new_obs);
if (possiblyNewObs && possiblyNewObs->timestamp != INVALID_TIMESTAMP &&
(!last_obs || possiblyNewObs->timestamp != last_obs->timestamp))
{
// It IS a new observation:
last_obs = possiblyNewObs;
// Feature tracking -------------------------------------------
ASSERT_(last_obs->hasIntensityImage);
CImage theImg; // The grabbed image:
theImg = last_obs->intensityImage;
// Do tracking:
if (step_num > 1) // we need "previous_image" to be valid.
{
tracker->trackFeatures(previous_image, theImg, trackedFeats);
// Remove those now out of the image plane:
CFeatureList::iterator itFeat = trackedFeats.begin();
while (itFeat != trackedFeats.end())
{
const TFeatureTrackStatus status = (*itFeat)->track_status;
bool eras =
(status_TRACKED != status && status_IDLE != status);
if (!eras)
{
// Also, check if it's too close to the image border:
const float x = (*itFeat)->x;
const float y = (*itFeat)->y;
static const float MIN_DIST_MARGIN_TO_STOP_TRACKING =
10;
if (x < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
y < MIN_DIST_MARGIN_TO_STOP_TRACKING ||
x > (last_obs->cameraParamsIntensity.ncols -
MIN_DIST_MARGIN_TO_STOP_TRACKING) ||
y > (last_obs->cameraParamsIntensity.nrows -
MIN_DIST_MARGIN_TO_STOP_TRACKING))
{
eras = true;
}
}
if (eras) // Erase or keep?
itFeat = trackedFeats.erase(itFeat);
else
++itFeat;
}
}
// Create list of 3D features in space, wrt current camera pose:
// --------------------------------------------------------------------
map<TFeatureID, TPoint3D> curVisibleFeats;
for (CFeatureList::iterator itFeat = trackedFeats.begin();
itFeat != trackedFeats.end(); ++itFeat)
{
// Pixel coordinates in the intensity image:
const int int_x = (*itFeat)->x;
const int int_y = (*itFeat)->y;
// Convert to pixel coords in the range image:
// APPROXIMATION: Assume coordinates are equal (that's not
// exact!!)
const int x = int_x;
const int y = int_y;
// Does this (x,y) have valid range data?
const float d = last_obs->rangeImage(y, x);
if (d > 0.05 && d < 10.0)
{
ASSERT_(
size_t(
last_obs->rangeImage.cols() *
last_obs->rangeImage.rows()) ==
last_obs->points3D_x.size());
const size_t nPt = last_obs->rangeImage.cols() * y + x;
curVisibleFeats[(*itFeat)->ID] = TPoint3D(
last_obs->points3D_x[nPt], last_obs->points3D_y[nPt],
last_obs->points3D_z[nPt]);
}
}
// Load local points map from 3D points + color:
CColouredPointsMap localPntsMap;
localPntsMap.colorScheme.scheme =
CColouredPointsMap::cmFromIntensityImage;
localPntsMap.loadFromRangeScan(*last_obs);
// Estimate our current camera pose from feature2feature matching:
// --------------------------------------------------------------------
if (!lastVisibleFeats.empty())
{
TMatchingPairList corrs; // pairs of correspondences
for (map<TFeatureID, TPoint3D>::const_iterator itCur =
curVisibleFeats.begin();
itCur != curVisibleFeats.end(); ++itCur)
{
map<TFeatureID, TPoint3D>::const_iterator itFound =
lastVisibleFeats.find(itCur->first);
if (itFound != lastVisibleFeats.end())
{
corrs.push_back(
TMatchingPair(
itFound->first, itCur->first, itFound->second.x,
itFound->second.y, itFound->second.z,
itCur->second.x, itCur->second.y,
itCur->second.z));
}
}
if (corrs.size() >= 3)
{
// Find matchings:
mrpt::tfest::TSE3RobustParams params;
params.ransac_minSetSize = 3;
params.ransac_maxSetSizePct = 6.0 / corrs.size();
mrpt::tfest::TSE3RobustResult results;
bool register_ok = false;
try
{
mrpt::tfest::se3_l2_robust(corrs, params, results);
register_ok = true;
}
catch (std::exception&)
{
/* Cannot find a minimum number of matches, inconsistent
* parameters due to very reduced numberof matches,etc.
*/
}
const CPose3D relativePose =
CPose3D(results.transformation);
str_status = mrpt::format(
"%d corrs | inliers: %d | rel.pose: %s ",
int(corrs.size()), int(results.inliers_idx.size()),
relativePose.asString().c_str());
str_status2 = string(
results.inliers_idx.size() == 0
? "LOST! Please, press 'r' to restart"
: "");
if (register_ok && std::abs(results.scale - 1.0) < 0.1)
{
// Seems a good match:
if ((relativePose.norm() > KEYFRAMES_MIN_DISTANCE ||
std::abs(relativePose.yaw()) > KEYFRAMES_MIN_ANG ||
std::abs(relativePose.pitch()) >
KEYFRAMES_MIN_ANG ||
std::abs(relativePose.roll()) > KEYFRAMES_MIN_ANG))
{
// Accept this as a new key-frame pose ------------
// Append new global pose of this key-frame:
const CPose3D new_keyframe_global =
CPose3D(*camera_key_frames_path.rbegin()) +
relativePose;
camera_key_frames_path.push_back(
new_keyframe_global.asTPose());
gl_keyframes_must_refresh = true;
currentCamPose_wrt_last =
CPose3D(); // It's (0,0,0) since the last
// key-frame is the current pose!
lastVisibleFeats = curVisibleFeats;
cout << "Adding new key-frame: pose="
<< new_keyframe_global << endl;
// Update global map: append another map at a given
// position:
globalPtsMap.insertObservation(
last_obs.get(), &new_keyframe_global);
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
}
else
{
currentCamPose_wrt_last = relativePose;
// cout << "cur pose: " << currentCamPose_wrt_last
// << endl;
}
}
}
}
if (camera_key_frames_path.empty() || lastVisibleFeats.empty())
{
// First iteration:
camera_key_frames_path.clear();
camera_key_frames_path.push_back(TPose3D(0, 0, 0, 0, 0, 0));
gl_keyframes_must_refresh = true;
lastVisibleFeats = curVisibleFeats;
// Update global map:
globalPtsMap.clear();
globalPtsMap.insertObservation(last_obs.get());
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
}
// Save the image for the next step:
previous_image = theImg;
// Draw marks on the RGB image:
theImg.selectTextFont("10x20");
{ // Tracked feats:
theImg.drawFeatures(
trackedFeats, TColor(0, 0, 255), SHOW_FEAT_IDS,
SHOW_RESPONSES);
theImg.textOut(
3, 22,
format("# feats: %u", (unsigned int)trackedFeats.size()),
TColor(200, 20, 20));
}
// Update visualization ---------------------------------------
// Show intensity image
win3D.get3DSceneAndLock();
viewInt->setImageView(theImg);
win3D.unlockAccess3DScene();
// Show 3D points & current visible feats, at the current camera 3D
// pose "currentCamPose_wrt_last"
// ---------------------------------------------------------------------
if (last_obs->hasPoints3D)
{
const CPose3D curGlobalPose =
CPose3D(*camera_key_frames_path.rbegin()) +
currentCamPose_wrt_last;
win3D.get3DSceneAndLock();
// All 3D points:
gl_points->loadFromPointsMap(&localPntsMap);
gl_points->setPose(curGlobalPose);
gl_cur_cam_corner->setPose(curGlobalPose);
// Current visual landmarks:
gl_curFeats->clear();
for (map<TFeatureID, TPoint3D>::const_iterator it =
curVisibleFeats.begin();
it != curVisibleFeats.end(); ++it)
{
static double D = 0.02;
mrpt::opengl::CBox::Ptr box =
mrpt::make_aligned_shared<mrpt::opengl::CBox>(
TPoint3D(-D, -D, -D), TPoint3D(D, D, D));
box->setWireframe(true);
box->setName(format("%d", int(it->first)));
box->enableShowName(true);
box->setLocation(it->second);
gl_curFeats->insert(box);
}
gl_curFeats->setPose(curGlobalPose);
win3D.unlockAccess3DScene();
win3D.repaint();
}
win3D.get3DSceneAndLock();
win3D.addTextMessage(
-100, -20, format("%.02f Hz", thrPar.Hz), TColorf(0, 1, 1), 100,
MRPT_GLUT_BITMAP_HELVETICA_18);
win3D.unlockAccess3DScene();
win3D.repaint();
step_num++;
} // end update visualization:
if (gl_keyframes_must_refresh)
{
gl_keyframes_must_refresh = false;
// cout << "Updating gl_keyframes with " <<
// camera_key_frames_path.size() << " frames.\n";
win3D.get3DSceneAndLock();
gl_keyframes->clear();
for (size_t i = 0; i < camera_key_frames_path.size(); i++)
{
CSetOfObjects::Ptr obj =
mrpt::opengl::stock_objects::CornerXYZSimple(0.3f, 3);
obj->setPose(camera_key_frames_path[i]);
gl_keyframes->insert(obj);
}
win3D.unlockAccess3DScene();
}
// Process possible keyboard commands:
// --------------------------------------
if (win3D.keyHit() && thrPar.pushed_key == 0)
{
const int key = tolower(win3D.getPushedKey());
switch (key)
{
// Some of the keys are processed in this thread:
case 'r':
lastVisibleFeats.clear();
camera_key_frames_path.clear();
gl_keyframes_must_refresh = true;
globalPtsMap.clear();
win3D.get3DSceneAndLock();
gl_points_map->loadFromPointsMap(&globalPtsMap);
win3D.unlockAccess3DScene();
break;
case 's':
{
const std::string s = "point_cloud.txt";
cout << "Dumping 3D point-cloud to: " << s << endl;
globalPtsMap.save3D_to_text_file(s);
break;
}
case 'o':
win3D.setCameraZoom(win3D.getCameraZoom() * 1.2);
win3D.repaint();
break;
case 'i':
win3D.setCameraZoom(win3D.getCameraZoom() / 1.2);
win3D.repaint();
break;
// ...and the rest in the kinect thread:
default:
thrPar.pushed_key = key;
break;
};
}
win3D.get3DSceneAndLock();
win3D.addTextMessage(
2, -30, format(
"'s':save point cloud, 'r': reset, 'o'/'i': zoom "
"out/in, mouse: orbit 3D, ESC: quit"),
TColorf(1, 1, 1), 110, MRPT_GLUT_BITMAP_HELVETICA_12);
win3D.addTextMessage(
2, -50, str_status, TColorf(1, 1, 1), 111,
MRPT_GLUT_BITMAP_HELVETICA_12);
win3D.addTextMessage(
2, -70, str_status2, TColorf(1, 1, 1), 112,
MRPT_GLUT_BITMAP_HELVETICA_18);
win3D.unlockAccess3DScene();
std::this_thread::sleep_for(1ms);
}
cout << "Waiting for grabbing thread to exit...\n";
thrPar.quit = true;
thHandle.join();
cout << "Bye!\n";
}
CSetOfObjects::Ptr stock_objects::Househam_Sprayer()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
{
CBox::Ptr cabin = mrpt::make_aligned_shared<CBox>(
TPoint3D(0.878, 0.723, -0.12), TPoint3D(-0.258, -0.723, -1.690));
cabin->setColor(0.7, 0.7, 0.7);
ret->insert(cabin);
}
{
CBox::Ptr back = mrpt::make_aligned_shared<CBox>(
TPoint3D(-0.258, 0.723, -0.72), TPoint3D(-5.938, -0.723, -1.690));
back->setColor(1, 1, 1);
ret->insert(back);
}
{
CBox::Ptr boomAxis = mrpt::make_aligned_shared<CBox>(
TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, -0.723, -1.690));
boomAxis->setColor(0, 0, 0);
ret->insert(boomAxis);
}
{
CBox::Ptr boom1 = mrpt::make_aligned_shared<CBox>(
TPoint3D(-5.938, 0.723, -1.0), TPoint3D(-6.189, 11.277, -1.620));
boom1->setColor(0, 1, 0);
ret->insert(boom1);
}
{
CBox::Ptr boom2 = mrpt::make_aligned_shared<CBox>(
TPoint3D(-5.938, -0.723, -1.0), TPoint3D(-6.189, -11.277, -1.620));
boom2->setColor(0, 1, 0);
ret->insert(boom2);
}
{
CCylinder::Ptr cyl1 =
mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
cyl1->setColor(0, 0, 0);
cyl1->setPose(CPose3D(-0.710, 0.923, -2.480, 0, 0, DEG2RAD(90)));
ret->insert(cyl1);
}
{
CCylinder::Ptr cyl2 =
mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
cyl2->setColor(0, 0, 0);
cyl2->setPose(CPose3D(-3.937, 0.923, -2.480, 0, 0, DEG2RAD(90)));
ret->insert(cyl2);
}
{
CCylinder::Ptr cyl1 =
mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
cyl1->setColor(0, 0, 0);
cyl1->setPose(CPose3D(-0.710, -0.423, -2.480, 0, 0, DEG2RAD(90)));
ret->insert(cyl1);
}
{
CCylinder::Ptr cyl2 =
mrpt::make_aligned_shared<CCylinder>(0.716f, 0.716f, 0.387f, 30);
cyl2->setColor(0, 0, 0);
cyl2->setPose(CPose3D(-3.937, -0.423, -2.480, 0, 0, DEG2RAD(90)));
ret->insert(cyl2);
}
return ret;
}
/*---------------------------------------------------------------
RobotGiraff
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotGiraff()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
float height = 0;
// Base
vector<TPoint2D> level1;
level1.push_back(TPoint2D(0.31, 0));
level1.push_back(TPoint2D(0.22, 0.24));
level1.push_back(TPoint2D(-0.22, 0.24));
level1.push_back(TPoint2D(-0.31, 0));
level1.push_back(TPoint2D(-0.22, -0.24));
level1.push_back(TPoint2D(0.22, -0.24));
CPolyhedron::Ptr obj1 =
opengl::CPolyhedron::CreateCustomPrism(level1, 0.23);
obj1->setLocation(0, 0, height);
height += 0.23f;
obj1->setColor(1.0, 0.6, 0.0);
ret->insert(obj1);
// Electronic's cage
vector<TPoint2D> level2;
level2.push_back(TPoint2D(0.13, 0.1));
level2.push_back(TPoint2D(-0.13, 0.1));
level2.push_back(TPoint2D(-0.13, -0.1));
level2.push_back(TPoint2D(0.13, -0.1));
CPolyhedron::Ptr obj2 =
opengl::CPolyhedron::CreateCustomPrism(level2, 0.45);
obj2->setLocation(0, 0, height);
height += 0.45f;
obj2->setColor(1.0, 0.6, 0.2);
ret->insert(obj2);
// Neck
vector<TPoint2D> level3;
level3.push_back(TPoint2D(0.03, 0.03));
level3.push_back(TPoint2D(-0.03, 0.03));
level3.push_back(TPoint2D(-0.03, -0.03));
level3.push_back(TPoint2D(0.03, -0.03));
CPolyhedron::Ptr obj3 =
opengl::CPolyhedron::CreateCustomPrism(level3, 0.55);
obj3->setLocation(0, 0, height);
height += 0.55f;
obj3->setColor(0.6, 0.6, 0.6);
ret->insert(obj3);
// Screen
vector<TPoint2D> level4;
level4.push_back(TPoint2D(0.03, 0.11));
level4.push_back(TPoint2D(-0.03, 0.11));
level4.push_back(TPoint2D(-0.03, -0.11));
level4.push_back(TPoint2D(0.03, -0.11));
CPolyhedron::Ptr obj4 = opengl::CPolyhedron::CreateCustomPrism(level4, 0.4);
obj4->setLocation(0, 0, height);
height += 0.4f;
obj4->setColor(1.0, 0.6, 0.0);
ret->insert(obj4);
return ret;
}
/*---------------------------------------------------------------
RobotPioneer
---------------------------------------------------------------*/
CSetOfObjects::Ptr stock_objects::RobotPioneer()
{
CSetOfObjects::Ptr ret = mrpt::make_aligned_shared<CSetOfObjects>();
ret->setName("theRobot");
CSetOfTriangles::Ptr obj = mrpt::make_aligned_shared<CSetOfTriangles>();
// Add triangles:
CSetOfTriangles::TTriangle trian;
trian.r[0] = trian.r[1] = trian.r[2] = 1;
trian.g[0] = trian.g[1] = trian.g[2] = 0;
trian.b[0] = trian.b[1] = trian.b[2] = 0;
trian.a[0] = trian.a[1] = trian.a[2] = 1;
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 0
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.20f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 1
// trian.r = 0.9f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = 0.10f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.20f;
trian.z[2] = 0.20f;
obj->insertTriangle(trian); // 2
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = 0.10f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.05f;
trian.z[2] = 0.20f;
obj->insertTriangle(trian); // 3
trian.x[0] = -0.20f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.25f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 2b
trian.x[0] = -0.20f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.05f;
trian.z[1] = 0.05f;
trian.z[2] = 0.25f;
obj->insertTriangle(trian); // 3b
// trian.r = 0.8f; trian.g = 0; trian.b = 0; trian.a = 1;
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 4
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = -0.10f;
trian.y[1] = -0.10f;
trian.y[2] = -0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.05f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 5
trian.x[0] = 0.10f;
trian.x[1] = -0.20f;
trian.x[2] = -0.20f;
trian.y[0] = 0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.25f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 6
trian.x[0] = 0.10f;
trian.x[1] = 0.10f;
trian.x[2] = -0.20f;
trian.y[0] = 0.10f;
trian.y[1] = 0.10f;
trian.y[2] = 0.10f;
trian.z[0] = 0.20f;
trian.z[1] = 0.05f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 7
trian.r[0] = trian.r[1] = trian.r[2] = 0.05f;
trian.g[0] = trian.g[1] = trian.g[2] = 0.05f;
trian.b[0] = trian.b[1] = trian.b[2] = 0.05f;
trian.a[0] = trian.a[1] = trian.a[2] = 1;
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = 0.05f;
trian.y[0] = 0.11f;
trian.y[1] = 0.11f;
trian.y[2] = 0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 8
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = -0.05f;
trian.y[0] = 0.11f;
trian.y[1] = 0.11f;
trian.y[2] = 0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 9
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = 0.05f;
trian.y[0] = -0.11f;
trian.y[1] = -0.11f;
trian.y[2] = -0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 10
trian.x[0] = 0.00f;
trian.x[1] = 0.00f;
trian.x[2] = -0.05f;
trian.y[0] = -0.11f;
trian.y[1] = -0.11f;
trian.y[2] = -0.11f;
trian.z[0] = 0.00f;
trian.z[1] = 0.10f;
trian.z[2] = 0.05f;
obj->insertTriangle(trian); // 11
ret->insert(obj);
return ret;
}
