feat(slam): add rtabmap_ros
This commit is contained in:
X-lanni
@@ -0,0 +1,348 @@
|
||||
/*
|
||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||
All rights reserved.
|
||||
|
||||
Redistribution and use in source and binary forms, with or without
|
||||
modification, are permitted provided that the following conditions are met:
|
||||
* Redistributions of source code must retain the above copyright
|
||||
notice, this list of conditions and the following disclaimer.
|
||||
* Redistributions in binary form must reproduce the above copyright
|
||||
notice, this list of conditions and the following disclaimer in the
|
||||
documentation and/or other materials provided with the distribution.
|
||||
* Neither the name of the Universite de Sherbrooke nor the
|
||||
names of its contributors may be used to endorse or promote products
|
||||
derived from this software without specific prior written permission.
|
||||
|
||||
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#ifndef RTABMAP_APP_H_
|
||||
#define RTABMAP_APP_H_
|
||||
|
||||
#ifdef __ANDROID__
|
||||
#include <jni.h>
|
||||
#endif
|
||||
#include <memory>
|
||||
|
||||
#include <tango-gl/util.h>
|
||||
|
||||
#include "scene.h"
|
||||
#include "CameraMobile.h"
|
||||
#include "util.h"
|
||||
#include "ProgressionStatus.h"
|
||||
|
||||
#include <rtabmap/core/SensorCaptureThread.h>
|
||||
#include <rtabmap/core/RtabmapThread.h>
|
||||
#include <rtabmap/core/SensorEvent.h>
|
||||
#include <rtabmap/utilite/UEventsHandler.h>
|
||||
#include <boost/thread/mutex.hpp>
|
||||
#include <pcl/pcl_base.h>
|
||||
#include <pcl/TextureMesh.h>
|
||||
|
||||
#include "Measure.h"
|
||||
|
||||
// RTABMapApp handles the application lifecycle and resources.
|
||||
class RTABMapApp : public UEventsHandler {
|
||||
public:
|
||||
// Constructor and deconstructor.
|
||||
#ifdef __ANDROID__
|
||||
RTABMapApp(JNIEnv* env, jobject caller_activity);
|
||||
#else // __APPLE__
|
||||
RTABMapApp();
|
||||
void setupSwiftCallbacks(void * classPtr,
|
||||
void(*progressCallback)(void *, int, int),
|
||||
void(*initCallback)(void *, int, const char*),
|
||||
void(*statsUpdatedCallback)(void *,
|
||||
int, int, int, int,
|
||||
float,
|
||||
int, int, int, int, int ,int,
|
||||
float,
|
||||
int,
|
||||
float,
|
||||
int,
|
||||
float, float, float, float,
|
||||
int, int,
|
||||
float, float, float, float, float, float),
|
||||
void(*cameraInfoCallback)(void *, int, const char*, const char*));
|
||||
|
||||
#endif
|
||||
~RTABMapApp();
|
||||
|
||||
void setScreenRotation(int displayRotation, int cameraRotation);
|
||||
|
||||
int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
|
||||
|
||||
bool isBuiltWith(int cameraDriver) const;
|
||||
#ifdef __ANDROID__
|
||||
bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
|
||||
#else // __APPLE__
|
||||
bool startCamera();
|
||||
#endif
|
||||
// Allocate OpenGL resources for rendering, mainly for initializing the Scene.
|
||||
void InitializeGLContent();
|
||||
|
||||
// Setup the view port width and height.
|
||||
void SetViewPort(int width, int height);
|
||||
|
||||
// Main render loop.
|
||||
int Render();
|
||||
|
||||
void stopCamera();
|
||||
|
||||
// Set render camera's viewing angle, first person, third person or top down.
|
||||
//
|
||||
// @param: camera_type, camera type includes first person, third person and
|
||||
// top down
|
||||
void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
|
||||
|
||||
// Touch event passed from android activity. This function only supports two
|
||||
// touches.
|
||||
//
|
||||
// @param: touch_count, total count for touches.
|
||||
// @param: event, touch event of current touch.
|
||||
// @param: x0, normalized touch location for touch 0 on x axis.
|
||||
// @param: y0, normalized touch location for touch 0 on y axis.
|
||||
// @param: x1, normalized touch location for touch 1 on x axis.
|
||||
// @param: y1, normalized touch location for touch 1 on y axis.
|
||||
void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event,
|
||||
float x0, float y0, float x1, float y1);
|
||||
|
||||
void setPausedMapping(bool paused);
|
||||
void setOnlineBlending(bool enabled);
|
||||
void setMapCloudShown(bool shown);
|
||||
void setOdomCloudShown(bool shown);
|
||||
void setMeshRendering(bool enabled, bool withTexture);
|
||||
void setPointSize(float value);
|
||||
void setFOV(float angle);
|
||||
void setOrthoCropFactor(float value);
|
||||
void setGridRotation(float value);
|
||||
void setLighting(bool enabled);
|
||||
void setBackfaceCulling(bool enabled);
|
||||
void setWireframe(bool enabled);
|
||||
void setTextureColorSeamsHidden(bool hidden);
|
||||
void setLocalizationMode(bool enabled);
|
||||
void setTrajectoryMode(bool enabled);
|
||||
void setGraphOptimization(bool enabled);
|
||||
void setNodesFiltering(bool enabled);
|
||||
void setGraphVisible(bool visible);
|
||||
void setGridVisible(bool visible);
|
||||
void setRawScanSaved(bool enabled);
|
||||
void setCameraColor(bool enabled);
|
||||
void setFullResolution(bool enabled);
|
||||
void setSmoothing(bool enabled);
|
||||
void setDepthBleedingError(float value);
|
||||
void setDepthFromMotion(bool enabled);
|
||||
void setAppendMode(bool enabled);
|
||||
void setUpstreamRelocalizationAccThr(float value);
|
||||
void setDataRecorderMode(bool enabled);
|
||||
void setMaxCloudDepth(float value);
|
||||
void setMinCloudDepth(float value);
|
||||
void setCloudDensityLevel(int value);
|
||||
void setMeshAngleTolerance(float value);
|
||||
void setMeshDecimationFactor(float value);
|
||||
void setMeshTriangleSize(int value);
|
||||
void setClusterRatio(float value);
|
||||
void setMaxGainRadius(float value);
|
||||
void setRenderingTextureDecimation(int value);
|
||||
void setBackgroundColor(float gray);
|
||||
void setDepthConfidence(int value);
|
||||
void setExportPointCloudFormat(const std::string & format);
|
||||
int setMappingParameter(const std::string & key, const std::string & value);
|
||||
void setGPS(const rtabmap::GPS & gps);
|
||||
void addEnvSensor(int type, float value);
|
||||
|
||||
void save(const std::string & databasePath);
|
||||
bool recover(const std::string & from, const std::string & to);
|
||||
void cancelProcessing();
|
||||
bool exportMesh(
|
||||
float cloudVoxelSize,
|
||||
bool regenerateCloud,
|
||||
bool meshing,
|
||||
int textureSize,
|
||||
int textureCount,
|
||||
int normalK,
|
||||
bool optimized,
|
||||
float optimizedVoxelSize,
|
||||
int optimizedDepth,
|
||||
int optimizedMaxPolygons,
|
||||
float optimizedColorRadius,
|
||||
bool optimizedCleanWhitePolygons,
|
||||
int optimizedMinClusterSize,
|
||||
float optimizedMaxTextureDistance,
|
||||
int optimizedMinTextureClusterSize,
|
||||
int textureVertexColorPolicy,
|
||||
bool blockRendering);
|
||||
bool postExportation(bool visualize);
|
||||
bool writeExportedMesh(const std::string & directory, const std::string & name);
|
||||
int postProcessing(int approach);
|
||||
void clearMeasures();
|
||||
void showMeasures(bool x, bool y, bool z, bool custom);
|
||||
void setMeasuringMode(int mode);
|
||||
void addMeasureButtonClicked();
|
||||
void teleportButtonClicked();
|
||||
void removeMeasure();
|
||||
void setMetricSystem(bool enabled);
|
||||
void setMeasuringTextSize(float size);
|
||||
|
||||
void postOdometryEvent(
|
||||
rtabmap::Transform pose,
|
||||
float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
|
||||
float depth_fx, float depth_fy, float depth_cx, float depth_cy,
|
||||
const rtabmap::Transform & rgbFrame,
|
||||
const rtabmap::Transform & depthFrame,
|
||||
double stamp,
|
||||
double depthStamp,
|
||||
const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
|
||||
const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
|
||||
const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
|
||||
const float * points, int pointsLen, int pointsChannels,
|
||||
rtabmap::Transform viewMatrix, //view matrix
|
||||
float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
|
||||
float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
|
||||
|
||||
protected:
|
||||
virtual bool handleEvent(UEvent * event);
|
||||
|
||||
private:
|
||||
int updateMeshDecimation(int width, int height);
|
||||
rtabmap::ParametersMap getRtabmapParameters();
|
||||
void updateMeasuringState();
|
||||
bool smoothMesh(int id, rtabmap::Mesh & mesh);
|
||||
void gainCompensation(bool full = false);
|
||||
std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
|
||||
std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
|
||||
|
||||
private:
|
||||
int cameraDriver_;
|
||||
rtabmap::CameraMobile * camera_;
|
||||
rtabmap::SensorCaptureThread * sensorCaptureThread_;
|
||||
rtabmap::RtabmapThread * rtabmapThread_;
|
||||
rtabmap::Rtabmap * rtabmap_;
|
||||
rtabmap::LogHandler * logHandler_;
|
||||
|
||||
bool odomCloudShown_;
|
||||
bool graphOptimization_;
|
||||
bool nodesFiltering_;
|
||||
bool localizationMode_;
|
||||
bool trajectoryMode_;
|
||||
bool rawScanSaved_;
|
||||
bool smoothing_;
|
||||
float depthBleedingError_;
|
||||
bool depthFromMotion_;
|
||||
bool cameraColor_;
|
||||
bool fullResolution_;
|
||||
bool appendMode_;
|
||||
bool useExternalLidar_;
|
||||
float maxCloudDepth_;
|
||||
float minCloudDepth_;
|
||||
int cloudDensityLevel_;
|
||||
int meshTrianglePix_;
|
||||
float meshAngleToleranceDeg_;
|
||||
float meshDecimationFactor_;
|
||||
float clusterRatio_;
|
||||
float maxGainRadius_;
|
||||
int renderingTextureDecimation_;
|
||||
float backgroundColor_;
|
||||
unsigned char depthConfidence_;
|
||||
float upstreamRelocalizationMaxAcc_;
|
||||
std::string exportPointCloudFormat_;
|
||||
|
||||
rtabmap::ParametersMap mappingParameters_;
|
||||
|
||||
bool dataRecorderMode_;
|
||||
bool clearSceneOnNextRender_;
|
||||
bool openingDatabase_;
|
||||
bool exporting_;
|
||||
bool postProcessing_;
|
||||
bool filterPolygonsOnNextRender_;
|
||||
int gainCompensationOnNextRender_;
|
||||
bool bilateralFilteringOnNextRender_;
|
||||
bool takeScreenshotOnNextRender_;
|
||||
bool cameraJustInitialized_;
|
||||
int totalPoints_;
|
||||
int totalPolygons_;
|
||||
int lastDrawnCloudsCount_;
|
||||
float renderingTime_;
|
||||
double lastPostRenderEventTime_;
|
||||
double lastPoseEventTime_;
|
||||
std::map<std::string, float> bufferedStatsData_;
|
||||
|
||||
bool visualizingMesh_;
|
||||
bool exportedMeshUpdated_;
|
||||
pcl::TextureMesh::Ptr optTextureMesh_;
|
||||
cv::Mat optTexture_;
|
||||
rtabmap::Mesh optMesh_;
|
||||
int optRefId_;
|
||||
rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
|
||||
std::list<Measure> measures_; // In opengl frame
|
||||
bool measuresUpdated_;
|
||||
bool metricSystem_;
|
||||
float measuringTextSize_;
|
||||
float snapAxisThr_;
|
||||
std::vector<cv::Vec3f> snapAxes_;
|
||||
int measuringMode_;
|
||||
bool addMeasureClicked_;
|
||||
bool teleportClicked_;
|
||||
bool removeMeasureClicked_;
|
||||
std::vector<cv::Point3f> measuringTmpPts_; // In opengl frame
|
||||
std::vector<cv::Point3f> measuringTmpNormals_; // In opengl frame
|
||||
pcl::PointCloud<pcl::PointXYZRGB>::Ptr targetPoint_;
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr quadSample_;
|
||||
std::vector<pcl::Vertices> quadSamplePolygons_;
|
||||
// main_scene_ includes all drawable object for visualizing Tango device's
|
||||
// movement and point cloud.
|
||||
Scene main_scene_;
|
||||
|
||||
UTimer fpsTime_;
|
||||
|
||||
std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
|
||||
std::list<rtabmap::SensorEvent> sensorEvents_;
|
||||
std::list<rtabmap::Transform> poseEvents_;
|
||||
|
||||
rtabmap::Transform mapToOdom_;
|
||||
|
||||
boost::mutex cameraMutex_;
|
||||
boost::mutex rtabmapMutex_;
|
||||
boost::mutex meshesMutex_;
|
||||
boost::mutex sensorMutex_;
|
||||
boost::mutex poseMutex_;
|
||||
boost::mutex renderingMutex_;
|
||||
|
||||
USemaphore screenshotReady_;
|
||||
|
||||
std::map<int, rtabmap::Mesh> createdMeshes_;
|
||||
std::map<int, rtabmap::Transform> rawPoses_;
|
||||
|
||||
std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
|
||||
|
||||
rtabmap::ProgressionStatus progressionStatus_;
|
||||
|
||||
#ifndef __ANDROID__
|
||||
void * swiftClassPtr_;
|
||||
void(*swiftInitCallback)(void *, int, const char *);
|
||||
void(*swiftStatsUpdatedCallback)(void *,
|
||||
int, int, int, int,
|
||||
float,
|
||||
int, int, int, int, int ,int,
|
||||
float,
|
||||
int,
|
||||
float,
|
||||
int,
|
||||
float, float, float, float,
|
||||
int, int,
|
||||
float, float, float, float, float, float);
|
||||
void(*swiftCameraInfoEventCallback)(void *, int, const char *, const char *);
|
||||
|
||||
#endif
|
||||
};
|
||||
|
||||
#endif // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_
|
||||
Reference in New Issue
Block a user