X-lanni
349 lines
13 KiB
C++
349 lines
13 KiB
C++
/*
|
|
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_
|