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; }