diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index e22f1945..4e2e888c 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -116,8 +116,7 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, cloudStamp_(0), tangoColorType_(0), tangoColorStamp_(0), - colorCameraToDisplayRotation_(ROTATION_0), - lastKnownGPS_(std::vector(6,0)) + colorCameraToDisplayRotation_(ROTATION_0) { UASSERT(decimation >= 1); } @@ -190,7 +189,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string { close(); - lastKnownGPS_ = std::vector(6,0); + lastKnownGPS_ = GPS(); TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics); @@ -516,19 +515,9 @@ std::string CameraTango::getSerial() const return "Tango"; } -void CameraTango::setGPS(double stamp, - double longitude, - double latitude, - double altitude, - double accuracy, - double bearing) +void CameraTango::setGPS(const GPS & gps) { - lastKnownGPS_[0] = stamp; - lastKnownGPS_[1] = longitude; - lastKnownGPS_[2] = latitude; - lastKnownGPS_[3] = altitude; - lastKnownGPS_[4] = accuracy; - lastKnownGPS_[5] = bearing; + lastKnownGPS_ = gps; } rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const @@ -856,13 +845,13 @@ SensorData CameraTango::captureImage(CameraInfo * info) } data.setGroundTruth(odom); - if(lastKnownGPS_[0] > 0.0 && rgbStamp-lastKnownGPS_[0]<2.0) + if(lastKnownGPS_.stamp() > 0.0 && rgbStamp-lastKnownGPS_.stamp()<2.0) { - data.setGPS(lastKnownGPS_[0], lastKnownGPS_[1], lastKnownGPS_[2], lastKnownGPS_[3], lastKnownGPS_[4], lastKnownGPS_[5]); + data.setGPS(lastKnownGPS_); } - else if(lastKnownGPS_[0]>0.0) + else if(lastKnownGPS_.stamp()>0.0) { - LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_[0]); + LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_.stamp()); } } else diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h index a0f876e0..6d7275d9 100644 --- a/app/android/jni/CameraTango.h +++ b/app/android/jni/CameraTango.h @@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define CAMERATANGO_H_ #include +#include #include #include #include @@ -89,12 +90,7 @@ public: void setSmoothing(bool enabled) {smoothing_ = enabled;} void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;} void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;} - void setGPS(double stamp, - double longitude, - double latitude, - double altitude, - double accuracy, - double bearing); + void setGPS(const GPS & gps); void cloudReceived(const cv::Mat & cloud, double timestamp); void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp); @@ -132,7 +128,7 @@ private: TangoSupportRotation colorCameraToDisplayRotation_; cv::Mat fisheyeRectifyMapX_; cv::Mat fisheyeRectifyMapY_; - std::vector lastKnownGPS_; + GPS lastKnownGPS_; }; } /* namespace rtabmap */ diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index bfc158a2..5893483d 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -1975,16 +1975,11 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string & } } -void RTABMapApp::setGPS(double stamp, - double longitude, - double latitude, - double altitude, - double accuracy, - double bearing) +void RTABMapApp::setGPS(const rtabmap::GPS & gps) { if(camera_) { - camera_->setGPS(stamp, longitude, latitude, altitude, accuracy, bearing); + camera_->setGPS(gps); } } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 31074c53..c4b72291 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -147,12 +147,7 @@ class RTABMapApp : public UEventsHandler { void setRenderingTextureDecimation(int value); void setBackgroundColor(float gray); int setMappingParameter(const std::string & key, const std::string & value); - void setGPS(double stamp, - double longitude, - double latitude, - double altitude, - double accuracy, - double bearing); + void setGPS(const rtabmap::GPS & gps); void resetMapping(); void save(const std::string & databasePath); diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index 07e0bdb6..5801cb0b 100644 --- a/app/android/jni/jni_interface.cpp +++ b/app/android/jni/jni_interface.cpp @@ -349,12 +349,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setGPS( double accuracy, double bearing) { - return app.setGPS(stamp, + return app.setGPS(GPS(stamp, longitude, latitude, altitude, accuracy, - bearing); + bearing)); } JNIEXPORT void JNICALL diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 6ffa80b3..8a5041ca 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -159,7 +159,7 @@ public: void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const; bool getCalibration(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const; bool getLaserScanInfo(int signatureId, LaserScanInfo & info) const; - bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, std::vector & gps) const; + bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps) const; void loadLinks(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; void getWeight(int signatureId, int & weight) const; void getAllNodeIds(std::set & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const; @@ -253,7 +253,7 @@ private: virtual void loadNodeDataQuery(std::list & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0; virtual bool getCalibrationQuery(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const = 0; virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const = 0; - virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, std::vector & gps) const = 0; + virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const = 0; virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0; diff --git a/corelib/include/rtabmap/core/GeodeticCoords.h b/corelib/include/rtabmap/core/GeodeticCoords.h index 720d76b8..4f4f76e2 100644 --- a/corelib/include/rtabmap/core/GeodeticCoords.h +++ b/corelib/include/rtabmap/core/GeodeticCoords.h @@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +/* + * The methods in this file were modified from the originals of the MRPT toolkit (see notice below): + * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp + */ + +/* +---------------------------------------------------------------------------+ + | Mobile Robot Programming Toolkit (MRPT) | + | http://www.mrpt.org/ | + | | + | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file | + | See: http://www.mrpt.org/Authors - All rights reserved. | + | Released under BSD License. See details in http://www.mrpt.org/License | + +---------------------------------------------------------------------------+ */ + #ifndef GEODETICCOORDS_H_ #define GEODETICCOORDS_H_ @@ -52,12 +66,58 @@ public: cv::Point3d toGeocentric_WGS84() const; cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y + void fromGeocentric_WGS84(const cv::Point3d& geocentric); + void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin); + + static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin); + private: double latitude_; // deg double longitude_; // deg double altitude_; // m }; +class GPS +{ +public: + GPS(): + stamp_(0.0), + longitude_(0.0), + latitude_(0.0), + altitude_(0.0), + error_(0.0), + bearing_(0.0) + {} + GPS(const double & stamp, + const double & longitude, + const double & latitude, + const double & altitude, + const double & error, + const double & bearing): + stamp_(stamp), + longitude_(longitude), + latitude_(latitude), + altitude_(altitude), + error_(error), + bearing_(bearing) + {} + const double & stamp() const {return stamp_;} + const double & longitude() const {return longitude_;} + const double & latitude() const {return latitude_;} + const double & altitude() const {return altitude_;} + const double & error() const {return error_;} + const double & bearing() const {return bearing_;} + + GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);} +private: + double stamp_; // in sec + double longitude_; // DD + double latitude_; // DD + double altitude_; // m + double error_; // m + double bearing_; // deg (North 0->360 clockwise) +}; + } #endif /* GEODETICCOORDS_H_ */ diff --git a/corelib/include/rtabmap/core/Graph.h b/corelib/include/rtabmap/core/Graph.h index 7e8ce2df..adb28a9f 100644 --- a/corelib/include/rtabmap/core/Graph.h +++ b/corelib/include/rtabmap/core/Graph.h @@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include namespace rtabmap { class Memory; @@ -58,6 +59,11 @@ bool RTABMAP_EXP importPoses( std::multimap * constraints = 0, // optional for formats 3 and 4 std::map * stamps = 0); // optional for format 1 +bool RTABMAP_EXP exportGPS( + const std::string & filePath, + const std::map & gpsValues, + unsigned int rgba = 0xFFFFFFFF); + /** * Compute translation and rotation errors for KITTI datasets. * See http://www.cvlibs.net/datasets/kitti/eval_odometry.php. diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index e616277b..0705b5b0 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -177,7 +177,7 @@ public: double & stamp, Transform & groundTruth, std::vector & velocity, - std::vector & gps, + GPS & gps, bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; SensorData getNodeData(int nodeId, bool uncompressedData = false) const; @@ -301,6 +301,7 @@ private: bool _memoryChanged; // False by default, become true only when Memory::update() is called. bool _linksChanged; // False by default, become true when links are modified. int _signaturesAdded; + GPS _gpsOrigin; std::map _signatures; // TODO : check if a signature is already added? although it is not supposed to occur... std::set _stMem; // id diff --git a/corelib/include/rtabmap/core/OdometryORBSLAM2.h b/corelib/include/rtabmap/core/OdometryORBSLAM2.h index 01626b5b..221454cc 100644 --- a/corelib/include/rtabmap/core/OdometryORBSLAM2.h +++ b/corelib/include/rtabmap/core/OdometryORBSLAM2.h @@ -53,7 +53,6 @@ private: private: #ifdef RTABMAP_ORB_SLAM2 ORBSLAM2System * orbslam2_; - ORB_SLAM2::System * system_; bool firstFrame_; #endif Transform originLocalTransform_; diff --git a/corelib/include/rtabmap/core/Optimizer.h b/corelib/include/rtabmap/core/Optimizer.h index acf6a881..06468a21 100644 --- a/corelib/include/rtabmap/core/Optimizer.h +++ b/corelib/include/rtabmap/core/Optimizer.h @@ -75,6 +75,7 @@ public: bool isCovarianceIgnored() const {return covarianceIgnored_;} double epsilon() const {return epsilon_;} bool isRobust() const {return robust_;} + bool priorsIgnored() const {return priorsIgnored_;} // setters void setIterations(int iterations) {iterations_ = iterations;} @@ -82,6 +83,7 @@ public: void setCovarianceIgnored(bool enabled) {covarianceIgnored_ = enabled;} void setEpsilon(double epsilon) {epsilon_ = epsilon;} void setRobust(bool enabled) {robust_ = enabled;} + void setPriorsIgnored(bool enabled) {priorsIgnored_ = enabled;} virtual void parseParameters(const ParametersMap & parameters); @@ -128,7 +130,8 @@ protected: bool slam2d = Parameters::defaultRegForce3DoF(), bool covarianceIgnored = Parameters::defaultOptimizerVarianceIgnored(), double epsilon = Parameters::defaultOptimizerEpsilon(), - bool robust = Parameters::defaultOptimizerRobust()); + bool robust = Parameters::defaultOptimizerRobust(), + bool priorsIgnored = Parameters::defaultOptimizerPriorsIgnored()); Optimizer(const ParametersMap & parameters); private: @@ -137,6 +140,7 @@ private: bool covarianceIgnored_; double epsilon_; bool robust_; + bool priorsIgnored_; }; } /* namespace rtabmap */ diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 59a30a99..feac4934 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -362,6 +362,7 @@ class RTABMAP_EXP Parameters #endif RTABMAP_PARAM(Optimizer, VarianceIgnored, bool, false, "Ignore constraints' variance. If checked, identity information matrix is used for each constraint. Otherwise, an information matrix is generated from the variance saved in the links."); RTABMAP_PARAM(Optimizer, Robust, bool, false, uFormat("Robust graph optimization using Vertigo (only work for g2o and GTSAM optimization strategies). Not compatible with \"%s\" if enabled.", kRGBDOptimizeMaxError().c_str())); + RTABMAP_PARAM(Optimizer, PriorsIgnored, bool, true, "Ignore prior constraints (global pose or GPS) while optimizing. Currently only g2o and gtsam optimization supports this."); RTABMAP_PARAM(g2o, Solver, int, 0, "0=csparse 1=pcg 2=cholmod 3=Eigen"); RTABMAP_PARAM(g2o, Optimizer, int, 0, "0=Levenberg 1=GaussNewton"); diff --git a/corelib/include/rtabmap/core/SensorData.h b/corelib/include/rtabmap/core/SensorData.h index a694d9c9..265f2868 100644 --- a/corelib/include/rtabmap/core/SensorData.h +++ b/corelib/include/rtabmap/core/SensorData.h @@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -225,17 +226,11 @@ public: const Transform & globalPose() const {return globalPose_;} const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;} - void setGPS(double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing) + void setGPS(const GPS & gps) { - gps_ = std::vector(6,0.0); - gps_[0]=stamp; - gps_[1]=longitude; - gps_[2]=latitude; - gps_[3]=altitude; - gps_[4]=accuracy; - gps_[5]=bearing; + gps_ = gps; } - const std::vector & gps() const {return gps_;} + const GPS & gps() const {return gps_;} long getMemoryUsed() const; // Return memory usage in Bytes @@ -278,7 +273,7 @@ private: Transform globalPose_; cv::Mat globalPoseCovariance_; // 6x6 double - std::vector gps_; + GPS gps_; }; } diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index d4bafaea..04b2442b 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -713,7 +713,7 @@ bool DBDriver::getNodeInfo( double & stamp, Transform & groundTruthPose, std::vector & velocity, - std::vector & gps) const + GPS & gps) const { bool found = false; // look in the trash diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 702a5753..da9758ab 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -1756,7 +1756,7 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, double & stamp, Transform & groundTruthPose, std::vector & velocity, - std::vector & gps) const + GPS & gps) const { bool found = false; if(_ppDb && signatureId) @@ -1854,12 +1854,13 @@ bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, if(uStrNumCmp(_version, "0.14.0") >= 0) { - gps.resize(6,0); + std::vector gpsV(6,0); data = sqlite3_column_blob(ppStmt, index); // velocity dataSize = sqlite3_column_bytes(ppStmt, index++); - if((unsigned int)dataSize == gps.size()*sizeof(double) && data) + if((unsigned int)dataSize == gpsV.size()*sizeof(double) && data) { - memcpy(gps.data(), data, dataSize); + memcpy(gpsV.data(), data, dataSize); + gps = GPS(gpsV[0], gpsV[1], gpsV[2], gpsV[3], gpsV[4], gpsV[5]); } } } @@ -2395,7 +2396,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list & ids, std::list< } if(gps.size() == 6) { - s->sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]); + s->sensorData().setGPS(GPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5])); } s->setSaved(true); nodes.push_back(s); @@ -4320,6 +4321,7 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const } } + std::vector gps; if(uStrNumCmp(_version, "0.10.1") >= 0) { // ignore user_data @@ -4345,14 +4347,21 @@ void DBDriverSqlite3::stepNode(sqlite3_stmt * ppStmt, const Signature * s) const if(uStrNumCmp(_version, "0.14.0") >= 0) { - if(s->sensorData().gps().empty()) + if(s->sensorData().gps().stamp() <= 0.0) { rc = sqlite3_bind_null(ppStmt, index++); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); } else { - rc = sqlite3_bind_blob(ppStmt, index++, s->sensorData().gps().data(), s->sensorData().gps().size()*sizeof(double), SQLITE_STATIC); + gps.resize(6,0.0); + gps[0] = s->sensorData().gps().stamp(); + gps[1] = s->sensorData().gps().longitude(); + gps[2] = s->sensorData().gps().latitude(); + gps[3] = s->sensorData().gps().altitude(); + gps[4] = s->sensorData().gps().error(); + gps[5] = s->sensorData().gps().bearing(); + rc = sqlite3_bind_blob(ppStmt, index++, gps.data(), gps.size()*sizeof(double), SQLITE_STATIC); UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); } } diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index 02031b99..998dcb3e 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -127,7 +127,7 @@ private: virtual void loadNodeDataQuery(std::list & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const; virtual bool getCalibrationQuery(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const; virtual bool getLaserScanInfoQuery(int signatureId, LaserScanInfo & info) const; - virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, std::vector & gps) const; + virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector & velocity, GPS & gps) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures) const; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const; virtual void getLastIdQuery(const std::string & tableName, int & id) const; diff --git a/corelib/src/DBReader.cpp b/corelib/src/DBReader.cpp index b696f000..31d347e8 100644 --- a/corelib/src/DBReader.cpp +++ b/corelib/src/DBReader.cpp @@ -268,7 +268,7 @@ SensorData DBReader::captureImage(CameraInfo * info) int mapId; Transform localTransform, pose, groundTruth; std::vector velocity; - std::vector gps; + GPS gps; _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps); if(previousStamp && stamp && stamp > previousStamp) { @@ -323,7 +323,7 @@ SensorData DBReader::getNextData(CameraInfo * info) double stamp; Transform groundTruth; std::vector velocity; - std::vector gps; + GPS gps; _dbDriver->getNodeInfo(*_currentId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps); cv::Mat infMatrix = cv::Mat::eye(6,6,CV_64FC1); @@ -437,6 +437,7 @@ SensorData DBReader::getNextData(CameraInfo * info) data.setId(seq); data.setStamp(stamp); data.setGroundTruth(groundTruth); + data.setGPS(gps); UDEBUG("Laser=%d RGB/Left=%d Depth/Right=%d, UserData=%d", data.laserScanRaw().empty()?0:1, data.imageRaw().empty()?0:1, diff --git a/corelib/src/GeodeticCoords.cpp b/corelib/src/GeodeticCoords.cpp index fa40b748..9ffa5c1d 100644 --- a/corelib/src/GeodeticCoords.cpp +++ b/corelib/src/GeodeticCoords.cpp @@ -51,8 +51,24 @@ namespace rtabmap { inline double DEG2RAD(const double x) { return x*M_PI/180.0;} +inline double RAD2DEG(const double x) { return x*180.0/M_PI;} inline double square(const double & value) {return value*value;} +GeodeticCoords::GeodeticCoords() : + latitude_(0.0), + longitude_(0.0), + altitude_(0.0) +{ + +} +GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) : + latitude_(latitude), + longitude_(longitude), + altitude_(altitude) +{ + +} + //*--------------------------------------------------------------- // geodeticToGeocentric_WGS84 // ---------------------------------------------------------------*/ @@ -123,19 +139,72 @@ cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const return out; } -GeodeticCoords::GeodeticCoords() : - latitude_(0.0), - longitude_(0.0), - altitude_(0.0) +void GeodeticCoords::fromGeocentric_WGS84(const cv::Point3d& geocentric) { + static const double a = 6378137; // Semi-major axis of the Earth (meters) + static const double b = 6356752.3142; // Semi-minor axis: + const double sa2 = a*a; + const double sb2 = b*b; + + const double e2 = (sa2 - sb2) / sa2; + const double ep2 = (sa2 - sb2) / sb2; + const double p = std::sqrt(geocentric.x * geocentric.x + geocentric.y * geocentric.y); + const double theta = atan2(geocentric.z * a, p * b); + + longitude_ = atan2(geocentric.y, geocentric.x); + latitude_ = atan2( + geocentric.z + ep2 * b * sin(theta) * sin(theta) * sin(theta), + p - e2 * a * cos(theta) * cos(theta) * cos(theta)); + + const double clat = cos(latitude_); + const double slat = sin(latitude_); + const double N = sa2 / std::sqrt(sa2 * clat * clat + sb2 * slat * slat); + + altitude_ = p / clat - N; + longitude_ = RAD2DEG(longitude_); + latitude_ = RAD2DEG(latitude_); } -GeodeticCoords::GeodeticCoords(double latitude, double longitude, double altitude) : - latitude_(latitude), - longitude_(longitude), - altitude_(altitude) + +void GeodeticCoords::fromENU_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin) { + fromGeocentric_WGS84(ENU_WGS84ToGeocentric_WGS84(enu, origin)); +} +cv::Point3d GeodeticCoords::ENU_WGS84ToGeocentric_WGS84(const cv::Point3d& enu, const GeodeticCoords& origin) +{ + // Generate reference 3D point: + cv::Point3f originGeocentric; + originGeocentric = origin.toGeocentric_WGS84(); + + cv::Vec3d P_ref(originGeocentric.x, originGeocentric.y, originGeocentric.z); + + // Z axis -> In direction out-ward the center of the Earth: + cv::Vec3d REF_X, REF_Y, REF_Z; + REF_Z = cv::normalize(P_ref); + + // 1st column: Starting at the reference point, move in the tangent + // direction + // east-ward: I compute this as the derivative of P_ref wrt "longitude": + // A_east[0] =-(N+in_height_meters)*cos(lat)*sin(lon); --> -Z[1] + // A_east[1] = (N+in_height_meters)*cos(lat)*cos(lon); --> Z[0] + // A_east[2] = 0; --> 0 + // --------------------------------------------------------------------------- + cv::Vec3d AUX_X(-REF_Z[1], REF_Z[0], 0); + REF_X = cv::normalize(AUX_X); + + // 2nd column: The cross product: + REF_Y = REF_Z.cross(REF_X); + + cv::Point3d out_coords; + out_coords.x = + REF_X[0] * enu.x + REF_Y[0] * enu.y + REF_Z[0] * enu.z + originGeocentric.x; + out_coords.y = + REF_X[1] * enu.x + REF_Y[1] * enu.y + REF_Z[1] * enu.z + originGeocentric.y; + out_coords.z = + REF_X[2] * enu.x + REF_Y[2] * enu.y + REF_Z[2] * enu.z + originGeocentric.z; + + return out_coords; } } diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp index 433fa900..7c031dcb 100644 --- a/corelib/src/Graph.cpp +++ b/corelib/src/Graph.cpp @@ -425,6 +425,105 @@ bool importPoses( return false; } +bool exportGPS( + const std::string & filePath, + const std::map & gpsValues, + unsigned int rgba) +{ + UDEBUG("%s", filePath.c_str()); + std::string tmpPath = filePath; + + std::string ext = UFile::getExtension(filePath); + + if(ext.compare("kml")!=0 && ext.compare("txt")!=0) + { + UERROR("Only txt and kml formats are supported!"); + return false; + } + + FILE* fout = 0; +#ifdef _MSC_VER + fopen_s(&fout, tmpPath.c_str(), "w"); +#else + fout = fopen(tmpPath.c_str(), "w"); +#endif + if(fout) + { + if(ext.compare("kml")==0) + { + std::string values; + for(std::map::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter) + { + values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude()); + } + + // switch argb (Qt format) -> abgr + unsigned int abgr = 0xFF << 24 | (rgba & 0xFF) << 16 | (rgba & 0xFF00) | ((rgba >> 16) &0xFF); + + std::string colorHexa = uFormat("%08x", abgr); + + fprintf(fout, "\n"); + fprintf(fout, "\n"); + fprintf(fout, "\n" + " %s\n", tmpPath.c_str()); + fprintf(fout, " \n" + " \n" + " normal\n" + " #sn_ylw-pushpin\n" + " \n" + " \n" + " highlight\n" + " #sh_ylw-pushpin\n" + " \n" + " \n" + " \n" + " \n", colorHexa.c_str(), colorHexa.c_str()); + fprintf(fout, " \n" + " %s\n" + " #msn_ylw-pushpin" + " \n" + " \n" + " %s\n" + " \n" + " \n" + " \n" + "\n" + "\n", + uSplit(tmpPath, '.').front().c_str(), + values.c_str()); + } + else + { + fprintf(fout, "# stamp longitude latitude altitude error bearing\n"); + for(std::map::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter) + { + fprintf(fout, "%f %f %f %f %f %f\n", + iter->second.stamp(), + iter->second.longitude(), + iter->second.latitude(), + iter->second.altitude(), + iter->second.error(), + iter->second.bearing()); + } + } + + fclose(fout); + return true; + } + return false; +} + // KITTI evaluation float lengths[] = {100,200,300,400,500,600,700,800}; int32_t num_lengths = 8; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 32964de5..9278abf7 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -1401,6 +1401,7 @@ void Memory::clear() _idMapCount = kIdStart; _memoryChanged = false; _linksChanged = false; + _gpsOrigin = GPS(); if(_dbDriver) { @@ -3051,7 +3052,7 @@ Transform Memory::getOdomPose(int signatureId, bool lookInDatabase) const std::string label; double stamp; std::vector velocity; - std::vector gps; + GPS gps; getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase); return pose; } @@ -3063,7 +3064,7 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const std::string label; double stamp; std::vector velocity; - std::vector gps; + GPS gps; getNodeInfo(signatureId, pose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase); return groundTruth; } @@ -3076,7 +3077,7 @@ bool Memory::getNodeInfo(int signatureId, double & stamp, Transform & groundTruth, std::vector & velocity, - std::vector & gps, + GPS & gps, bool lookInDatabase) const { const Signature * s = this->getSignature(signatureId); @@ -3967,10 +3968,7 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p s->sensorData().setUserDataRaw(data.userDataRaw()); s->sensorData().setGroundTruth(data.groundTruth()); - if(!data.gps().empty()) - { - s->sensorData().setGPS(data.gps()[0], data.gps()[1], data.gps()[2], data.gps()[3], data.gps()[4], data.gps()[5]); - } + s->sensorData().setGPS(data.gps()); t = timer.ticks(); if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f); @@ -3996,9 +3994,39 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p s->sensorData().setOccupancyGrid(ground, obstacles, cellSize, viewPoint); // prior - if(!isIntermediateNode && !data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1) + if(!isIntermediateNode) { - s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv())); + if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1) + { + s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv())); + + /*if(data.gps().stamp() > 0.0) + { + UWARN("GPS constraint ignored as global pose is also set."); + }*/ + } + else if(data.gps().stamp() > 0.0) + { + // TODO: What kind of covariance should we set to have decent gtsam and g2o results!? + /*if(_gpsOrigin.stamp() <= 0.0) + { + _gpsOrigin = data.gps(); + } + cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords()); + Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI); + cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)*0.00000001; + if(data.gps().error() > 0.0) + { + // only set x, y as we don't know variance for other degrees of freedom. + gpsInfMatrix.at(0,0) = gpsInfMatrix.at(1,1) = 0.1; + gpsInfMatrix.at(2,2) = 100000; + s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix)); + } + else + { + UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error()); + }*/ + } } return s; diff --git a/corelib/src/OdometryORBSLAM2.cpp b/corelib/src/OdometryORBSLAM2.cpp index 0be97494..675bc450 100644 --- a/corelib/src/OdometryORBSLAM2.cpp +++ b/corelib/src/OdometryORBSLAM2.cpp @@ -749,7 +749,6 @@ OdometryORBSLAM2::OdometryORBSLAM2(const ParametersMap & parameters) : #ifdef RTABMAP_ORB_SLAM2 , orbslam2_(0), - system_(0), firstFrame_(true) #endif { diff --git a/corelib/src/Optimizer.cpp b/corelib/src/Optimizer.cpp index bcdf72d9..7c24d059 100644 --- a/corelib/src/Optimizer.cpp +++ b/corelib/src/Optimizer.cpp @@ -181,7 +181,10 @@ void Optimizer::getConnectedGraph( uFormat("Input links should be unique between two poses (%d->%d).", iter->second.from(), iter->second.to()).c_str()); biLinks.insert(std::make_pair(iter->second.from(), iter->second.to())); - biLinks.insert(std::make_pair(iter->second.to(), iter->second.from())); + if(iter->second.from() != iter->second.to()) + { + biLinks.insert(std::make_pair(iter->second.to(), iter->second.from())); + } } while((depth == 0 || d < depth) && nextDepth.size()) @@ -199,18 +202,26 @@ void Optimizer::getConnectedGraph( for(std::multimap::const_iterator iter=biLinks.find(*jter); iter!=biLinks.end() && iter->first==*jter; ++iter) { int nextId = iter->second; - if(ids.find(nextId) == ids.end() && uContains(posesIn, nextId)) + if(uContains(posesIn, nextId)) { - nextDepth.insert(nextId); + if(ids.find(nextId) == ids.end()) + { + nextDepth.insert(nextId); - std::multimap::const_iterator kter = graph::findLink(linksIn, *jter, nextId); - if(depth == 0 || d < depth-1) - { - linksOut.insert(*kter); + std::multimap::const_iterator kter = graph::findLink(linksIn, *jter, nextId); + if(depth == 0 || d < depth-1) + { + linksOut.insert(*kter); + } + else if(curentDepth.find(nextId) != curentDepth.end() || + ids.find(nextId) != ids.end()) + { + linksOut.insert(*kter); + } } - else if(curentDepth.find(nextId) != curentDepth.end() || - ids.find(nextId) != ids.end()) + else if(*jter == nextId) { + std::multimap::const_iterator kter = graph::findLink(linksIn, *jter, nextId); linksOut.insert(*kter); } } @@ -221,12 +232,13 @@ void Optimizer::getConnectedGraph( } } -Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust) : +Optimizer::Optimizer(int iterations, bool slam2d, bool covarianceIgnored, double epsilon, bool robust, bool priorsIgnored) : iterations_(iterations), slam2d_(slam2d), covarianceIgnored_(covarianceIgnored), epsilon_(epsilon), - robust_(robust) + robust_(robust), + priorsIgnored_(priorsIgnored) { } @@ -235,7 +247,8 @@ Optimizer::Optimizer(const ParametersMap & parameters) : slam2d_(Parameters::defaultRegForce3DoF()), covarianceIgnored_(Parameters::defaultOptimizerVarianceIgnored()), epsilon_(Parameters::defaultOptimizerEpsilon()), - robust_(Parameters::defaultOptimizerRobust()) + robust_(Parameters::defaultOptimizerRobust()), + priorsIgnored_(Parameters::defaultOptimizerPriorsIgnored()) { parseParameters(parameters); } @@ -247,6 +260,7 @@ void Optimizer::parseParameters(const ParametersMap & parameters) Parameters::parse(parameters, Parameters::kRegForce3DoF(), slam2d_); Parameters::parse(parameters, Parameters::kOptimizerEpsilon(), epsilon_); Parameters::parse(parameters, Parameters::kOptimizerRobust(), robust_); + Parameters::parse(parameters, Parameters::kOptimizerPriorsIgnored(), priorsIgnored_); } std::map Optimizer::optimize( diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp index 646d607d..938639a0 100644 --- a/corelib/src/OptimizerG2O.cpp +++ b/corelib/src/OptimizerG2O.cpp @@ -235,17 +235,19 @@ std::map OptimizerG2O::optimize( } // detect if there is a global pose prior set, if so remove rootId - for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + if(!priorsIgnored()) { - if(iter->second.from() == iter->second.to()) + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) { - rootId = 0; - break; + if(iter->second.from() == iter->second.to()) + { + rootId = 0; + break; + } } } UDEBUG("fill poses to g2o..."); - std::map > geoPoses; // pose / information matrix for(std::map::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter) { UASSERT(!iter->second.isNull()); @@ -292,47 +294,50 @@ std::map OptimizerG2O::optimize( if(id1 == id2) { - if(isSlam2d()) + if(!priorsIgnored()) { - g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior(); - g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); - priorEdge->setVertex(0, v1); - priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); - priorEdge->setParameterId(0, PARAM_OFFSET); - Eigen::Matrix information = Eigen::Matrix::Identity(); - if(!isCovarianceIgnored()) + if(isSlam2d()) { - information(0,0) = iter->second.infMatrix().at(0,0); // x-x - information(0,1) = iter->second.infMatrix().at(0,1); // x-y - information(0,2) = iter->second.infMatrix().at(0,5); // x-theta - information(1,0) = iter->second.infMatrix().at(1,0); // y-x - information(1,1) = iter->second.infMatrix().at(1,1); // y-y - information(1,2) = iter->second.infMatrix().at(1,5); // y-theta - information(2,0) = iter->second.infMatrix().at(5,0); // theta-x - information(2,1) = iter->second.infMatrix().at(5,1); // theta-y - information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta + g2o::EdgeSE2Prior * priorEdge = new g2o::EdgeSE2Prior(); + g2o::VertexSE2* v1 = (g2o::VertexSE2*)optimizer.vertex(id1); + priorEdge->setVertex(0, v1); + priorEdge->setMeasurement(g2o::SE2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta())); + priorEdge->setParameterId(0, PARAM_OFFSET); + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + information(0,0) = iter->second.infMatrix().at(0,0); // x-x + information(0,1) = iter->second.infMatrix().at(0,1); // x-y + information(0,2) = iter->second.infMatrix().at(0,5); // x-theta + information(1,0) = iter->second.infMatrix().at(1,0); // y-x + information(1,1) = iter->second.infMatrix().at(1,1); // y-y + information(1,2) = iter->second.infMatrix().at(1,5); // y-theta + information(2,0) = iter->second.infMatrix().at(5,0); // theta-x + information(2,1) = iter->second.infMatrix().at(5,1); // theta-y + information(2,2) = iter->second.infMatrix().at(5,5); // theta-theta + } + priorEdge->setInformation(information); + edge = priorEdge; } - priorEdge->setInformation(information); - edge = priorEdge; - } - else - { - g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior(); - g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); - priorEdge->setVertex(0, v1); - Eigen::Affine3d a = iter->second.transform().toEigen3d(); - Eigen::Isometry3d pose; - pose = a.rotation(); - pose.translation() = a.translation(); - priorEdge->setMeasurement(pose); - priorEdge->setParameterId(0, PARAM_OFFSET); - Eigen::Matrix information = Eigen::Matrix::Identity(); - if(!isCovarianceIgnored()) + else { - memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + g2o::EdgeSE3Prior * priorEdge = new g2o::EdgeSE3Prior(); + g2o::VertexSE3* v1 = (g2o::VertexSE3*)optimizer.vertex(id1); + priorEdge->setVertex(0, v1); + Eigen::Affine3d a = iter->second.transform().toEigen3d(); + Eigen::Isometry3d pose; + pose = a.rotation(); + pose.translation() = a.translation(); + priorEdge->setMeasurement(pose); + priorEdge->setParameterId(0, PARAM_OFFSET); + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + } + priorEdge->setInformation(information); + edge = priorEdge; } - priorEdge->setInformation(information); - edge = priorEdge; } } else @@ -462,7 +467,7 @@ std::map OptimizerG2O::optimize( } } - if (!optimizer.addEdge(edge)) + if (edge && !optimizer.addEdge(edge)) { delete edge; UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2); @@ -767,34 +772,37 @@ std::map OptimizerG2O::optimizeBA( int id1 = iter->second.from(); int id2 = iter->second.to(); - UASSERT(!iter->second.transform().isNull()); - - Eigen::Matrix information = Eigen::Matrix::Identity(); - memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); - - // between cameras, not base_link - Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform(); - UDEBUG("added edge %d->%d (in cam frame=%s)", - id1, - id2, - camLink.prettyPrint().c_str()); - Eigen::Affine3d a = camLink.toEigen3d(); - - g2o::EdgeSBACam * e = new g2o::EdgeSBACam(); - g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1); - g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2); - UASSERT(v1 != 0); - UASSERT(v2 != 0); - e->setVertex(0, v1); - e->setVertex(1, v2); - e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation())); - e->setInformation(information); - - if (!optimizer.addEdge(e)) + if(id1 != id2) // not supporting prior { - delete e; - UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2); - return optimizedPoses; + UASSERT(!iter->second.transform().isNull()); + + Eigen::Matrix information = Eigen::Matrix::Identity(); + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + + // between cameras, not base_link + Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform(); + UDEBUG("added edge %d->%d (in cam frame=%s)", + id1, + id2, + camLink.prettyPrint().c_str()); + Eigen::Affine3d a = camLink.toEigen3d(); + + g2o::EdgeSBACam * e = new g2o::EdgeSBACam(); + g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1); + g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2); + UASSERT(v1 != 0); + UASSERT(v2 != 0); + e->setVertex(0, v1); + e->setVertex(1, v2); + e->setMeasurement(g2o::SE3Quat(a.rotation(), a.translation())); + e->setInformation(information); + + if (!optimizer.addEdge(e)) + { + delete e; + UERROR("Map: Failed adding constraint between %d and %d, skipping", id1, id2); + return optimizedPoses; + } } } } diff --git a/corelib/src/OptimizerGTSAM.cpp b/corelib/src/OptimizerGTSAM.cpp index be6802b1..da0bd57d 100644 --- a/corelib/src/OptimizerGTSAM.cpp +++ b/corelib/src/OptimizerGTSAM.cpp @@ -99,18 +99,34 @@ std::map OptimizerGTSAM::optimize( { gtsam::NonlinearFactorGraph graph; - //prior first pose - UASSERT(uContains(poses, rootId)); - const Transform & initialPose = poses.at(rootId); - if(isSlam2d()) + // detect if there is a global pose prior set, if so remove rootId + if(!priorsIgnored()) { - gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01)); - graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + for(std::multimap::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter) + { + if(iter->second.from() == iter->second.to()) + { + rootId = 0; + break; + } + } } - else + + //prior first pose + if(rootId != 0) { - gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); - graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + UASSERT(uContains(poses, rootId)); + const Transform & initialPose = poses.at(rootId); + if(isSlam2d()) + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.01, 0.01, 0.01)); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose2(initialPose.x(), initialPose.y(), initialPose.theta()), priorNoise)); + } + else + { + gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished()); + graph.add(gtsam::PriorFactor(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise)); + } } UDEBUG("fill poses to gtsam..."); @@ -134,93 +150,130 @@ std::map OptimizerGTSAM::optimize( { int id1 = iter->second.from(); int id2 = iter->second.to(); + UASSERT(!iter->second.transform().isNull()); if(id1 == id2) { - // not supporting pose prior - continue; - } - - UASSERT(!iter->second.transform().isNull()); - -#ifdef RTABMAP_VERTIGO - if(this->isRobust() && - iter->second.type()!=Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged) - { - // create new switch variable - // Sunderhauf IROS 2012: - // "Since it is reasonable to initially accept all loop closure constraints, - // a proper and convenient initial value for all switch variables would be - // sij = 1 when using the linear switch function" - double prior = 1.0; - initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); - - // create switch prior factor - // "If the front-end is not able to assign sound individual values - // for Ξij , it is save to set all Ξij = 1, since this value is close - // to the individual optimal choice of Ξij for a large range of - // outliers." - gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); - graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); - } -#endif - - if(isSlam2d()) - { - Eigen::Matrix information = Eigen::Matrix::Identity(); - if(!isCovarianceIgnored()) + if(!priorsIgnored()) { - // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) - information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x - information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y - information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta - information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x - information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y - information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta - information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x - information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y - information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta - } - gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + if(isSlam2d()) + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x + information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y + information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta + information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x + information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y + information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta + information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x + information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y + information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta + } -#ifdef RTABMAP_VERTIGO - if(this->isRobust() && - iter->second.type()!=Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged) - { - // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); - } - else -#endif - { - graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + graph.add(gtsam::PriorFactor(id1, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + else + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information = information / 1000.0; + } + + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + graph.add(gtsam::PriorFactor(id1, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } } } else { - Eigen::Matrix information = Eigen::Matrix::Identity(); - if(!isCovarianceIgnored()) - { - memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); - // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) - information = information / 1000.0; - } - - gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); - #ifdef RTABMAP_VERTIGO if(this->isRobust() && - iter->second.type()!=Link::kNeighbor && - iter->second.type() != Link::kNeighborMerged) + iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged && + iter->second.type() != Link::kPosePrior) { - // create switchable edge factor - graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + // create new switch variable + // Sunderhauf IROS 2012: + // "Since it is reasonable to initially accept all loop closure constraints, + // a proper and convenient initial value for all switch variables would be + // sij = 1 when using the linear switch function" + double prior = 1.0; + initialEstimate.insert(gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior)); + + // create switch prior factor + // "If the front-end is not able to assign sound individual values + // for Ξij , it is save to set all Ξij = 1, since this value is close + // to the individual optimal choice of Ξij for a large range of + // outliers." + gtsam::noiseModel::Diagonal::shared_ptr switchPriorModel = gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector1(1.0)); + graph.add(gtsam::PriorFactor (gtsam::Symbol('s',switchCounter), vertigo::SwitchVariableLinear(prior), switchPriorModel)); + } +#endif + + if(isSlam2d()) + { + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information(0,0) = iter->second.infMatrix().at(0,0)/1000.0; // x-x + information(0,1) = iter->second.infMatrix().at(0,1)/1000.0; // x-y + information(0,2) = iter->second.infMatrix().at(0,5)/1000.0; // x-theta + information(1,0) = iter->second.infMatrix().at(1,0)/1000.0; // y-x + information(1,1) = iter->second.infMatrix().at(1,1)/1000.0; // y-y + information(1,2) = iter->second.infMatrix().at(1,5)/1000.0; // y-theta + information(2,0) = iter->second.infMatrix().at(5,0)/1000.0; // theta-x + information(2,1) = iter->second.infMatrix().at(5,1)/1000.0; // theta-y + information(2,2) = iter->second.infMatrix().at(5,5)/1000.0; // theta-theta + } + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + +#ifdef RTABMAP_VERTIGO + if(this->isRobust() && + iter->second.type()!=Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } + else +#endif + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose2(iter->second.transform().x(), iter->second.transform().y(), iter->second.transform().theta()), model)); + } } else -#endif { - graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + Eigen::Matrix information = Eigen::Matrix::Identity(); + if(!isCovarianceIgnored()) + { + memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double)); + // For some reasons, dividing by 1000 avoids some exceptions (maybe too large numbers on optimization) + information = information / 1000.0; + } + + gtsam::noiseModel::Gaussian::shared_ptr model = gtsam::noiseModel::Gaussian::Information(information); + +#ifdef RTABMAP_VERTIGO + if(this->isRobust() && + iter->second.type() != Link::kNeighbor && + iter->second.type() != Link::kNeighborMerged && + iter->second.type() != Link::kPosePrior) + { + // create switchable edge factor + graph.add(vertigo::BetweenFactorSwitchableLinear(id1, id2, gtsam::Symbol('s', switchCounter++), gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } + else +#endif + { + graph.add(gtsam::BetweenFactor(id1, id2, gtsam::Pose3(iter->second.transform().toEigen4d()), model)); + } } } } diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index fc4ce9fe..fbca7490 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -761,7 +761,7 @@ void Rtabmap::exportPoses(const std::string & path, bool optimized, bool global, std::string l; double stamp = 0.0; std::vector v; - std::vector gps; + GPS gps; _memory->getNodeInfo(iter->first, o, m, w, l, stamp, g, v, gps, true); stamps.insert(std::make_pair(iter->first, stamp)); } @@ -2651,7 +2651,7 @@ bool Rtabmap::process( double stamp = 0; Transform groundTruth; std::vector velocity; - std::vector gps; + GPS gps; _memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, false); signatures.insert(std::make_pair(iter->first, Signature(iter->first, @@ -2665,10 +2665,7 @@ bool Rtabmap::process( { signatures.at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]); } - if(!gps.empty()) - { - signatures.at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]); - } + signatures.at(iter->first).sensorData().setGPS(gps); } localGraphSize = (int)poses.size(); if(!lastSignatureLocalizedPose.isNull()) @@ -3346,7 +3343,7 @@ void Rtabmap::get3DMap( double stamp = 0; Transform groundTruth; std::vector velocity; - std::vector gps; + GPS gps; _memory->getNodeInfo(*iter, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, true); SensorData data = _memory->getNodeData(*iter); data.setId(*iter); @@ -3370,10 +3367,7 @@ void Rtabmap::get3DMap( { signatures.at(*iter).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]); } - if(!gps.empty()) - { - signatures.at(*iter).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]); - } + signatures.at(*iter).sensorData().setGPS(gps); } } else if(_memory && (_memory->getStMem().size() || _memory->getWorkingMem().size() > 1)) @@ -3426,7 +3420,7 @@ void Rtabmap::getGraph( double stamp = 0; Transform groundTruth; std::vector velocity; - std::vector gps; + GPS gps; _memory->getNodeInfo(iter->first, odomPoseLocal, mapId, weight, label, stamp, groundTruth, velocity, gps, global); signatures->insert(std::make_pair(iter->first, Signature(iter->first, @@ -3455,10 +3449,7 @@ void Rtabmap::getGraph( { signatures->at(iter->first).setVelocity(velocity[0], velocity[1], velocity[2], velocity[3], velocity[4], velocity[5]); } - if(!gps.empty()) - { - signatures->at(iter->first).sensorData().setGPS(gps[0], gps[1], gps[2], gps[3], gps[4], gps[5]); - } + signatures->at(iter->first).sensorData().setGPS(gps); } } } diff --git a/guilib/include/rtabmap/gui/DatabaseViewer.h b/guilib/include/rtabmap/gui/DatabaseViewer.h index d03108b9..c54ae3c9 100644 --- a/guilib/include/rtabmap/gui/DatabaseViewer.h +++ b/guilib/include/rtabmap/gui/DatabaseViewer.h @@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include @@ -94,6 +95,9 @@ private slots: void exportPosesKITTI(); void exportPosesTORO(); void exportPosesG2O(); + void exportPosesKML(); + void exportGPS_TXT(); + void exportGPS_KML(); void generateLocalGraph(); void regenerateLocalMaps(); void regenerateCurrentLocalMaps(); @@ -162,6 +166,7 @@ private: void refineConstraint(int from, int to, bool silent); bool addConstraint(int from, int to, bool silent); void exportPoses(int format); + void exportGPS(int format); private: Ui_DatabaseViewer * ui_; @@ -181,6 +186,8 @@ private: std::multimap graphLinks_; std::map poses_; std::map groundTruthPoses_; + std::map gpsPoses_; + std::map gpsValues_; std::multimap links_; std::multimap linksRefined_; std::multimap linksAdded_; diff --git a/guilib/include/rtabmap/gui/GraphViewer.h b/guilib/include/rtabmap/gui/GraphViewer.h index 08f7eb0a..1f5a25be 100644 --- a/guilib/include/rtabmap/gui/GraphViewer.h +++ b/guilib/include/rtabmap/gui/GraphViewer.h @@ -34,8 +34,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include +#include class QGraphicsItem; class QGraphicsPixmapItem; @@ -58,6 +60,9 @@ public: const std::multimap & constraints, const std::map & mapIds); void updateGTGraph(const std::map & poses); + void updateGPSGraph( + const std::map & gpsMapPoses, + const std::map & gpsValues); void updateReferentialPosition(const Transform & t); void updateMap(const cv::Mat & map8U, float resolution, float xMin, float yMin); void updatePosterior(const std::map & posterior); @@ -89,6 +94,7 @@ public: const QColor & getLocalPathColor() const {return _localPathColor;} const QColor & getGlobalPathColor() const {return _globalPathColor;} const QColor & getGTColor() const {return _gtPathColor;} + const QColor & getGPSColor() const {return _gpsPathColor;} const QColor & getIntraSessionLoopColor() const {return _loopIntraSessionColor;} const QColor & getInterSessionLoopColor() const {return _loopInterSessionColor;} bool isIntraInterSessionColorsEnabled() const {return _intraInterSessionColors;} @@ -102,6 +108,8 @@ public: bool isGlobalPathVisible() const; bool isLocalPathVisible() const; bool isGtGraphVisible() const; + bool isGPSGraphVisible() const; + bool isOrientationENU() const; // setters void setWorkingDirectory(const QString & path); @@ -119,6 +127,7 @@ public: void setLocalPathColor(const QColor & color); void setGlobalPathColor(const QColor & color); void setGTColor(const QColor & color); + void setGPSColor(const QColor & color); void setIntraSessionLoopColor(const QColor & color); void setInterSessionLoopColor(const QColor & color); void setIntraInterSessionColorsEnabled(bool enabled); @@ -132,6 +141,8 @@ public: void setGlobalPathVisible(bool visible); void setLocalPathVisible(bool visible); void setGtGraphVisible(bool visible); + void setGPSGraphVisible(bool visible); + void setOrientationENU(bool enabled); signals: void configChanged(); @@ -158,6 +169,7 @@ private: QColor _localPathColor; QColor _globalPathColor; QColor _gtPathColor; + QColor _gpsPathColor; QColor _loopIntraSessionColor; QColor _loopInterSessionColor; bool _intraInterSessionColors; @@ -166,10 +178,13 @@ private: QGraphicsItem * _globalPathRoot; QGraphicsItem * _localPathRoot; QGraphicsItem * _gtGraphRoot; + QGraphicsItem * _gpsGraphRoot; QMap _nodeItems; QMultiMap _linkItems; QMap _gtNodeItems; + QMap _gpsNodeItems; QMultiMap _gtLinkItems; + QMultiMap _gpsLinkItems; QMultiMap _localPathLinkItems; QMultiMap _globalPathLinkItems; float _nodeRadius; @@ -181,6 +196,7 @@ private: QGraphicsEllipseItem * _localRadius; float _loopClosureOutlierThr; float _maxLinkLength; + bool _orientationENU; }; } /* namespace rtabmap */ diff --git a/guilib/src/DatabaseViewer.cpp b/guilib/src/DatabaseViewer.cpp index ad869bbd..e55e926c 100644 --- a/guilib/src/DatabaseViewer.cpp +++ b/guilib/src/DatabaseViewer.cpp @@ -68,6 +68,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "rtabmap/core/RegistrationVis.h" #include "rtabmap/core/RegistrationIcp.h" #include "rtabmap/core/OccupancyGrid.h" +#include "rtabmap/core/GeodeticCoords.h" #include "rtabmap/gui/DataRecorder.h" #include "ExportCloudsDialog.h" #include "EditDepthArea.h" @@ -229,6 +230,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI())); connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO())); connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O())); + connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML())); + connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT())); + connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML())); connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap())); connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap())); connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures())); @@ -250,6 +254,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) : ui_->pushButton_reject->setEnabled(false); ui_->menuExport_poses->setEnabled(false); + ui_->menuExport_GPS->setEnabled(false); + ui_->actionPoses_KML->setEnabled(false); ui_->horizontalSlider_A->setTracking(false); ui_->horizontalSlider_B->setTracking(false); @@ -698,6 +704,8 @@ bool DatabaseViewer::openDatabase(const QString & path) graphLinks_.clear(); poses_.clear(); groundTruthPoses_.clear(); + gpsPoses_.clear(); + gpsValues_.clear(); mapIds_.clear(); links_.clear(); linksAdded_.clear(); @@ -710,6 +718,8 @@ bool DatabaseViewer::openDatabase(const QString & path) ui_->graphViewer->clearAll(); occupancyGridViewer_->clear(); ui_->menuExport_poses->setEnabled(false); + ui_->menuExport_GPS->setEnabled(false); + ui_->actionPoses_KML->setEnabled(false); ui_->checkBox_showOptimized->setEnabled(false); ui_->toolBox_statistics->clear(); databaseFileName_.clear(); @@ -1011,6 +1021,7 @@ void DatabaseViewer::exportDatabase() std::map poses; std::map stamps; std::map groundTruths; + std::map gpsValues; for(int i=0; i velocity; - std::vector gps; + GPS gps; if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps)) { if(frameRate == 0 || @@ -1040,6 +1051,10 @@ void DatabaseViewer::exportDatabase() poses.insert(std::make_pair(ids_[i], odomPose)); stamps.insert(std::make_pair(ids_[i], stamp)); groundTruths.insert(std::make_pair(ids_[i], groundTruth)); + if(gps.stamp() > 0.0) + { + gpsValues.insert(std::make_pair(ids_[i], gps)); + } } } if(sessionExported >= 0 && mapId > sessionExported) @@ -1115,7 +1130,14 @@ void DatabaseViewer::exportDatabase() stamps.at(id), userData); } - sensorData.setGroundTruth(groundTruths.at(id)); + if(groundTruths.find(id)!=groundTruths.end()) + { + sensorData.setGroundTruth(groundTruths.at(id)); + } + if(gpsValues.find(id)!=gpsValues.end()) + { + sensorData.setGPS(gpsValues.at(id)); + } recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance); @@ -1294,8 +1316,12 @@ void DatabaseViewer::updateIds() mapIds_.clear(); poses_.clear(); groundTruthPoses_.clear(); + gpsPoses_.clear(); + gpsValues_.clear(); ui_->checkBox_alignPosesWithGroundTruth->setVisible(false); ui_->label_alignPosesWithGroundTruth->setVisible(false); + ui_->menuExport_GPS->setEnabled(false); + ui_->actionPoses_KML->setEnabled(false); links_.clear(); linksAdded_.clear(); linksRefined_.clear(); @@ -1325,10 +1351,9 @@ void DatabaseViewer::updateIds() double s; int mapId; std::vector v; - std::vector gps; + GPS gps; dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps); mapIds_.insert(std::make_pair(ids_[i], mapId)); - if(i>0) { if(mapIds_.at(ids_[i-1]) == mapId) @@ -1392,6 +1417,20 @@ void DatabaseViewer::updateIds() { groundTruthPoses_.insert(std::make_pair(ids_[i], g)); } + if(gps.stamp() > 0.0) + { + gpsValues_.insert(std::make_pair(ids_[i], gps)); + + cv::Point3f p(0.0f,0.0f,0.0f); + if(!gpsPoses_.empty()) + { + GeodeticCoords coords = gps.toGeodeticCoords(); + GPS originGPS = gpsValues_.begin()->second; + p = coords.toENU_WGS84(originGPS.toGeodeticCoords()); + } + Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*180.0/M_PI)); + gpsPoses_.insert(std::make_pair(ids_[i], pose)); + } } if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end()) @@ -1403,10 +1442,23 @@ void DatabaseViewer::updateIds() } } } - if(!groundTruthPoses_.empty()) + if(!groundTruthPoses_.empty() || !gpsPoses_.empty()) { ui_->checkBox_alignPosesWithGroundTruth->setVisible(true); ui_->label_alignPosesWithGroundTruth->setVisible(true); + if(!groundTruthPoses_.empty()) + { + ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth")); + } + else + { + ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS")); + } + } + if(!gpsValues_.empty()) + { + ui_->menuExport_GPS->setEnabled(true); + ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty()); } UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size()); @@ -1434,6 +1486,7 @@ void DatabaseViewer::updateIds() ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize())); ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size())); ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size())); + ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size())); ui_->textEdit_info->append(""); long total = 0; long dbSize = UFile::length(dbDriver_->getUrl()); @@ -1663,6 +1716,10 @@ void DatabaseViewer::exportPosesG2O() { exportPoses(4); } +void DatabaseViewer::exportPosesKML() +{ + exportPoses(5); +} void DatabaseViewer::exportPoses(int format) { @@ -1676,6 +1733,104 @@ void DatabaseViewer::exportPoses(int format) } } + if(format == 5) + { + if(gpsValues_.empty() || gpsPoses_.empty()) + { + QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!")); + } + else + { + std::map graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); + + //align with ground truth for more meaningful results + pcl::PointCloud cloud1, cloud2; + cloud1.resize(graph.size()); + cloud2.resize(graph.size()); + int oi = 0; + int idFirst = 0; + for(std::map::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter) + { + std::map::iterator iter2 = graph.find(iter->first); + if(iter2!=graph.end()) + { + if(oi==0) + { + idFirst = iter->first; + } + cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z()); + cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z()); + } + } + + Transform t = Transform::getIdentity(); + if(oi>5) + { + cloud1.resize(oi); + cloud2.resize(oi); + + t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1); + } + else if(idFirst) + { + t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse(); + } + + std::map values; + GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords(); + for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) + { + iter->second = t * iter->second; + + GeodeticCoords coord; + coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin); + double bearing = -(iter->second.theta()*180.0/M_PI-90.0); + if(bearing < 0) + { + bearing += 360; + } + + Transform p, g; + int w; + std::string l; + double stamp=0.0; + int mapId; + std::vector v; + GPS gps; + dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps); + values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0))); + } + + QString output = pathDatabase_ + QDir::separator() + "poses.kml"; + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + output, + tr("Google Earth file (*.kml)")); + + if(!path.isEmpty()) + { + bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba()); + + if(saved) + { + QMessageBox::information(this, + tr("Export poses..."), + tr("GPS coordinates saved to \"%1\".") + .arg(path)); + } + else + { + QMessageBox::information(this, + tr("Export poses..."), + tr("Failed to save GPS coordinates to \"%1\"!") + .arg(path)); + } + } + } + return; + } + std::map optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value()); if(optimizedPoses.size()) @@ -1796,7 +1951,7 @@ void DatabaseViewer::exportPoses(int format) double stamp=0.0; int mapId; std::vector v; - std::vector gps; + GPS gps; if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps)) { stamps.insert(std::make_pair(iter->first, stamp)); @@ -1842,6 +1997,48 @@ void DatabaseViewer::exportPoses(int format) } } +void DatabaseViewer::exportGPS_TXT() +{ + exportGPS(0); +} +void DatabaseViewer::exportGPS_KML() +{ + exportGPS(1); +} + +void DatabaseViewer::exportGPS(int format) +{ + if(!gpsValues_.empty()) + { + QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml"); + QString path = QFileDialog::getSaveFileName( + this, + tr("Save File"), + output, + format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)")); + + if(!path.isEmpty()) + { + bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba()); + + if(saved) + { + QMessageBox::information(this, + tr("Export poses..."), + tr("GPS coordinates saved to \"%1\".") + .arg(path)); + } + else + { + QMessageBox::information(this, + tr("Export poses..."), + tr("Failed to save GPS coordinates to \"%1\"!") + .arg(path)); + } + } + } +} + void DatabaseViewer::generateGraph() { if(!dbDriver_) @@ -1989,7 +2186,7 @@ void DatabaseViewer::regenerateLocalMaps() double stamp; QString msg; std::vector velocity; - std::vector gps; + GPS gps; if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps)) { Signature s = data; @@ -2067,7 +2264,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps() double stamp; QString msg; std::vector velocity; - std::vector gps; + GPS gps; if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps)) { Signature s = data; @@ -2469,7 +2666,7 @@ void DatabaseViewer::update(int value, std::string l; double s; std::vector v; - std::vector gps; + GPS gps; dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps); weight->setNum(w); @@ -2482,10 +2679,10 @@ void DatabaseViewer::update(int value, stamp->setText(QString::number(s, 'f')); stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); } - if(gps.size()) + if(gps.stamp()>0.0) { - labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps[0], 'f')).arg(gps[1]).arg(gps[2]).arg(gps[3]).arg(gps[4]).arg(gps[5])); - labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps[0]*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); + labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing())); + labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz")); } if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection()) { @@ -3511,7 +3708,7 @@ void DatabaseViewer::updateConstraintView( double s; Transform p,g; std::vector v; - std::vector gps; + GPS gps; dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps); if(!p.isNull()) { @@ -3914,16 +4111,22 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) { std::map graph = uValueAt(graphes_, value); + std::map refPoses = groundTruthPoses_; + if(refPoses.empty()) + { + refPoses = gpsPoses_; + } + // Log ground truth statistics (in TUM's RGBD-SLAM format) - if(groundTruthPoses_.size()) + if(refPoses.size()) { // compute KITTI statistics before aligning the poses float length = graph::computePathLength(graph); - if(groundTruthPoses_.size() == graph.size() && length >= 100.0f) + if(refPoses.size() == graph.size() && length >= 100.0f) { float t_err = 0.0f; float r_err = 0.0f; - graph::calcKittiSequenceErrors(uValues(groundTruthPoses_), uValues(graph), t_err, r_err); + graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err); UINFO("KITTI t_err = %f %%", t_err); UINFO("KITTI r_err = %f deg/m", r_err); ui_->toolBox_statistics->updateStat("GT/kitti_t_err/%", t_err, false); @@ -3938,7 +4141,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) cloud2.resize(graph.size()); int oi = 0; int idFirst = 0; - for(std::map::const_iterator iter=groundTruthPoses_.begin(); iter!=groundTruthPoses_.end(); ++iter) + for(std::map::const_iterator iter=refPoses.begin(); iter!=refPoses.end(); ++iter) { std::map::iterator iter2 = graph.find(iter->first); if(iter2!=graph.end()) @@ -3962,7 +4165,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } else if(idFirst) { - t = groundTruthPoses_.at(idFirst) * graph.at(idFirst).inverse(); + t = refPoses.at(idFirst) * graph.at(idFirst).inverse(); } if(!t.isIdentity()) { @@ -3987,8 +4190,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) int oi=0; for(std::map::iterator iter=graph.begin(); iter!=graph.end(); ++iter) { - std::map::const_iterator jter = groundTruthPoses_.find(iter->first); - if(jter!=groundTruthPoses_.end()) + std::map::const_iterator jter = refPoses.find(iter->first); + if(jter!=refPoses.end()) { Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0); @@ -4142,6 +4345,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value) } ui_->graphViewer->updateGTGraph(groundTruthPoses_); + ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_); ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_); ui_->graphViewer->clearMap(); occupancyGridViewer_->clear(); @@ -4428,6 +4632,7 @@ void DatabaseViewer::updateGraphView() int totalLocalTime = 0; int totalLocalSpace = 0; int totalUser = 0; + int totalPriors = 0; for(std::multimap::iterator iter=links.begin(); iter!=links.end();) { if(iter->second.type() == Link::kNeighbor) @@ -4474,15 +4679,20 @@ void DatabaseViewer::updateGraphView() } ++totalUser; } + else if(iter->second.type() == Link::kPosePrior) + { + ++totalPriors; + } ++iter; } - ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6)") + ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)") .arg(totalNeighbor) .arg(totalNeighborMerged) .arg(totalGlobal) .arg(totalLocalSpace) .arg(totalLocalTime) - .arg(totalUser)); + .arg(totalUser) + .arg(totalPriors)); Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters()); diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index 416556bb..78c846cb 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -2089,7 +2089,7 @@ bool ExportCloudsDialog::getExportedClouds( int m,w; std::string l; double s; - std::vector gps; + GPS gps; _dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps); } } @@ -2122,7 +2122,7 @@ bool ExportCloudsDialog::getExportedClouds( int m,w; std::string l; double s; - std::vector gps; + GPS gps; _dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps); } } diff --git a/guilib/src/GraphViewer.cpp b/guilib/src/GraphViewer.cpp index 2380ccdf..44a6e2b7 100644 --- a/guilib/src/GraphViewer.cpp +++ b/guilib/src/GraphViewer.cpp @@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include #include @@ -81,6 +82,8 @@ public: this->setBrush(b); } + int id() const {return _id;}; + int mapId() const {return _mapId;} const Transform & pose() const {return _pose;} void setPose(const Transform & pose) {this->setPos(-pose.y()*100.0f,-pose.x()*100.0f); _pose=pose;} @@ -104,6 +107,29 @@ private: Transform _pose; }; +class NodeGPSItem: public NodeItem +{ +public: + NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps) : + NodeItem(id, mapId, pose, radius), + _gps(gps) + { + } + virtual ~NodeGPSItem() {} +protected: + virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event ) + { + this->setToolTip(QString("%1 [%2] %3\n" + "longitude=%4 latitude=%5 altitude=%6m error=%7m bearing=%8deg") + .arg(id()).arg(mapId()).arg(pose().prettyPrint().c_str()) + .arg(_gps.longitude()).arg(_gps.latitude()).arg(_gps.altitude()).arg(_gps.error()).arg(_gps.bearing())); + this->setScale(2); + QGraphicsEllipseItem::hoverEnterEvent(event); + } +private: + GPS _gps; +}; + class LinkItem: public QGraphicsLineItem { public: @@ -195,6 +221,7 @@ GraphViewer::GraphViewer(QWidget * parent) : _localPathColor(Qt::cyan), _globalPathColor(Qt::darkMagenta), _gtPathColor(Qt::gray), + _gpsPathColor(Qt::darkCyan), _loopIntraSessionColor(Qt::red), _loopInterSessionColor(Qt::green), _intraInterSessionColors(false), @@ -209,7 +236,8 @@ GraphViewer::GraphViewer(QWidget * parent) : _gridCellSize(0.0f), _localRadius(0), _loopClosureOutlierThr(0), - _maxLinkLength(0.02f) + _maxLinkLength(0.02f), + _orientationENU(false) { this->setScene(new QGraphicsScene(this)); this->setDragMode(QGraphicsView::ScrollHandDrag); @@ -268,6 +296,10 @@ GraphViewer::GraphViewer(QWidget * parent) : _gtGraphRoot->setZValue(2); _gtGraphRoot->setParentItem(_root); + _gpsGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001)); + _gpsGraphRoot->setZValue(2); + _gpsGraphRoot->setParentItem(_root); + this->restoreDefaults(); this->fitInView(this->sceneRect(), Qt::KeepAspectRatio); @@ -622,6 +654,135 @@ void GraphViewer::updateGTGraph(const std::map & poses) UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks()); } +void GraphViewer::updateGPSGraph( + const std::map & poses, + const std::map & gpsValues) +{ + UTimer timer; + bool wasVisible = _gpsGraphRoot->isVisible(); + _gpsGraphRoot->show(); + bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0; + UDEBUG("poses=%d", (int)poses.size()); + //Hide nodes and links + for(QMap::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter) + { + iter.value()->hide(); + iter.value()->setColor(_gpsPathColor); // reset color + } + for(QMultiMap::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter) + { + iter.value()->hide(); + } + + for(std::map::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(!iter->second.isNull()) + { + QMap::iterator itemIter = _gpsNodeItems.find(iter->first); + if(itemIter != _gpsNodeItems.end()) + { + itemIter.value()->setPose(iter->second); + itemIter.value()->show(); + } + else + { + // create node item + const Transform & pose = iter->second; + UASSERT(gpsValues.find(iter->first) != gpsValues.end()); + NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first)); + this->scene()->addItem(item); + item->setZValue(20); + item->setColor(_gpsPathColor); + item->setParentItem(_gpsGraphRoot); + _gpsNodeItems.insert(iter->first, item); + } + + if(iter!=poses.begin()) + { + std::map::const_iterator iterPrevious = iter; + --iterPrevious; + Transform previousPose = iterPrevious->second; + Transform currentPose = iter->second; + + LinkItem * linkItem = 0; + QMultiMap::iterator linkIter = _gpsLinkItems.end(); + if(_gpsLinkItems.contains(iterPrevious->first)) + { + linkIter = _gpsLinkItems.find(iter->first); + while(linkIter.key() == iterPrevious->first && linkIter != _gpsLinkItems.end()) + { + if(linkIter.value()->to() == iter->first) + { + linkIter.value()->setPoses(previousPose, currentPose); + linkIter.value()->show(); + linkItem = linkIter.value(); + break; + } + ++linkIter; + } + } + if(linkItem == 0) + { + //create a link item + linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1); + QPen p = linkItem->pen(); + p.setWidthF(_linkWidth*100.0f); + linkItem->setPen(p); + linkItem->setZValue(10); + this->scene()->addItem(linkItem); + linkItem->setParentItem(_gpsGraphRoot); + _gpsLinkItems.insert(iterPrevious->first, linkItem); + } + if(linkItem) + { + linkItem->setColor(_gpsPathColor); + } + } + } + } + + //remove not used nodes and links + for(QMap::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end();) + { + if(!iter.value()->isVisible()) + { + delete iter.value(); + iter = _gpsNodeItems.erase(iter); + } + else + { + ++iter; + } + } + for(QMultiMap::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end();) + { + if(!iter.value()->isVisible()) + { + delete iter.value(); + iter = _gpsLinkItems.erase(iter); + } + else + { + ++iter; + } + } + + if(_gpsNodeItems.size() || _gpsLinkItems.size()) + { + this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents + + if(wasEmpty) + { + QRectF rect = this->scene()->itemsBoundingRect(); + this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio); + } + } + + _gpsGraphRoot->setVisible(wasVisible); + + UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks()); +} + void GraphViewer::updateReferentialPosition(const Transform & t) { QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24()*100.0f, -t.o14()*100.0f); @@ -820,6 +981,10 @@ void GraphViewer::clearGraph() _gtNodeItems.clear(); qDeleteAll(_gtLinkItems); _gtLinkItems.clear(); + qDeleteAll(_gpsNodeItems); + _gpsNodeItems.clear(); + qDeleteAll(_gpsLinkItems); + _gpsLinkItems.clear(); _referential->resetTransform(); _localRadius->resetTransform(); @@ -867,6 +1032,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("local_path_color", this->getLocalPathColor()); settings.setValue("global_path_color", this->getGlobalPathColor()); settings.setValue("gt_color", this->getGTColor()); + settings.setValue("gps_color", this->getGPSColor()); settings.setValue("intra_session_color", this->getIntraSessionLoopColor()); settings.setValue("inter_session_color", this->getInterSessionLoopColor()); settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled()); @@ -880,6 +1046,8 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons settings.setValue("global_path_visible", this->isGlobalPathVisible()); settings.setValue("local_path_visible", this->isLocalPathVisible()); settings.setValue("gt_graph_visible", this->isGtGraphVisible()); + settings.setValue("gps_graph_visible", this->isGPSGraphVisible()); + settings.setValue("orientation_ENU", this->isOrientationENU()); if(!group.isEmpty()) { settings.endGroup(); @@ -906,6 +1074,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value()); this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value()); this->setGTColor(settings.value("gt_color", this->getGTColor()).value()); + this->setGPSColor(settings.value("gps_color", this->getGPSColor()).value()); this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value()); this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value()); this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool()); @@ -919,6 +1088,8 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group) this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool()); this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool()); this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool()); + this->setGPSGraphVisible(settings.value("gps_graph_visible", this->isGPSGraphVisible()).toBool()); + this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool()); if(!group.isEmpty()) { settings.endGroup(); @@ -957,6 +1128,14 @@ bool GraphViewer::isGtGraphVisible() const { return _gtGraphRoot->isVisible(); } +bool GraphViewer::isGPSGraphVisible() const +{ + return _gpsGraphRoot->isVisible(); +} +bool GraphViewer::isOrientationENU() const +{ + return _orientationENU; +} void GraphViewer::setWorkingDirectory(const QString & path) { @@ -973,6 +1152,10 @@ void GraphViewer::setNodeRadius(float radius) { iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f); } + for(QMap::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter) + { + iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f); + } } void GraphViewer::setLinkWidth(float width) { @@ -1100,6 +1283,18 @@ void GraphViewer::setGTColor(const QColor & color) iter.value()->setColor(_gtPathColor); } } +void GraphViewer::setGPSColor(const QColor & color) +{ + _gpsPathColor = color; + for(QMap::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter) + { + iter.value()->setColor(_gpsPathColor); + } + for(QMultiMap::iterator iter=_gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter) + { + iter.value()->setColor(_gpsPathColor); + } +} void GraphViewer::setIntraSessionLoopColor(const QColor & color) { _loopIntraSessionColor = color; @@ -1192,6 +1387,18 @@ void GraphViewer::setGtGraphVisible(bool visible) { _gtGraphRoot->setVisible(visible); } +void GraphViewer::setGPSGraphVisible(bool visible) +{ + _gpsGraphRoot->setVisible(visible); +} +void GraphViewer::setOrientationENU(bool enabled) +{ + if(_orientationENU!=enabled) + { + _orientationENU = enabled; + this->rotate(_orientationENU?90:270); + } +} void GraphViewer::restoreDefaults() { @@ -1258,6 +1465,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path")); QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path")); QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth")); + QAction * aChangeGPSColor = menuLink->addAction(tr("GPS")); menuLink->addSeparator(); QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors")); QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure")); @@ -1272,6 +1480,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeLocalPathColor->setIcon(createIcon(_localPathColor)); aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor)); aChangeGTColor->setIcon(createIcon(_gtPathColor)); + aChangeGPSColor->setIcon(createIcon(_gpsPathColor)); aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor)); aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor)); aChangeNeighborColor->setIconVisibleInMenu(true); @@ -1284,6 +1493,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) aChangeLocalPathColor->setIconVisibleInMenu(true); aChangeGlobalPathColor->setIconVisibleInMenu(true); aChangeGTColor->setIconVisibleInMenu(true); + aChangeGPSColor->setIconVisibleInMenu(true); aChangeIntraSessionLoopColor->setIconVisibleInMenu(true); aChangeInterSessionLoopColor->setIconVisibleInMenu(true); aSetIntraInterSessionColors->setCheckable(true); @@ -1302,6 +1512,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) QAction * aShowHideGlobalPath; QAction * aShowHideLocalPath; QAction * aShowHideGtGraph; + QAction * aShowHideGPSGraph; + QAction * aOrientationENU; if(_gridMap->isVisible()) { aShowHideGridMap = menu.addAction(tr("Hide grid map")); @@ -1366,10 +1578,22 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { aShowHideGtGraph = menu.addAction(tr("Show ground truth graph")); } + if(_gpsGraphRoot->isVisible()) + { + aShowHideGPSGraph = menu.addAction(tr("Hide GPS graph")); + } + else + { + aShowHideGPSGraph = menu.addAction(tr("Show GPS graph")); + } + aOrientationENU = menu.addAction(tr("ENU Orientation")); + aOrientationENU->setCheckable(true); + aOrientationENU->setChecked(_orientationENU); aShowHideGraph->setEnabled(_nodeItems.size()); aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size()); aShowHideLocalPath->setEnabled(_localPathLinkItems.size()); aShowHideGtGraph->setEnabled(_gtNodeItems.size()); + aShowHideGPSGraph->setEnabled(_gpsNodeItems.size()); menu.addSeparator(); QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults")); @@ -1487,6 +1711,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) r == aChangeLocalPathColor || r == aChangeGlobalPathColor || r == aChangeGTColor || + r == aChangeGPSColor || r == aChangeIntraSessionLoopColor || r == aChangeInterSessionLoopColor) { @@ -1535,6 +1760,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { color = _gtPathColor; } + else if(r == aChangeGPSColor) + { + color = _gpsPathColor; + } else if(r == aChangeIntraSessionLoopColor) { color = _loopIntraSessionColor; @@ -1595,6 +1824,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { this->setGTColor(color); } + else if(r == aChangeGPSColor) + { + this->setGPSColor(color); + } else if(r == aChangeIntraSessionLoopColor) { this->setIntraSessionLoopColor(color); @@ -1671,6 +1904,15 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event) { this->setGtGraphVisible(!this->isGtGraphVisible()); } + else if(r == aShowHideGPSGraph) + { + this->setGPSGraphVisible(!this->isGPSGraphVisible()); + } + else if(r == aOrientationENU) + { + this->setOrientationENU(!this->isOrientationENU()); + } + if(r) { emit configChanged(); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index a16c0a60..1df50752 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -792,6 +792,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : _ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str()); _ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str()); _ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str()); + _ui->graphOptimization_priorsIgnored->setObjectName(Parameters::kOptimizerPriorsIgnored().c_str()); _ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str()); _ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str()); diff --git a/guilib/src/ui/DatabaseViewer.ui b/guilib/src/ui/DatabaseViewer.ui index c0255d2c..954ec4af 100644 --- a/guilib/src/ui/DatabaseViewer.ui +++ b/guilib/src/ui/DatabaseViewer.ui @@ -51,8 +51,8 @@ 0 - -24 - 393 + 0 + 408 232 @@ -226,8 +226,8 @@ 0 - -24 - 393 + 0 + 408 232 @@ -534,6 +534,14 @@ + + + + + Export GPS... + + + @@ -543,6 +551,7 @@ + @@ -898,7 +907,7 @@ - Links (N, NM, G, LS, LT, U) + Links (N, NM, G, LS, LT, U, P) @@ -1187,8 +1196,8 @@ 0 0 - 280 - 666 + 309 + 650 @@ -1550,8 +1559,8 @@ 0 0 - 201 - 126 + 324 + 168 @@ -1650,8 +1659,8 @@ 0 0 - 186 - 496 + 309 + 256 @@ -2263,6 +2272,21 @@ g2o (*.g2o) + + + Google Earth (*.kml) + + + + + Raw format (*.txt) + + + + + Google Earth (*.kml) + + diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 576f4aad..bfde8119 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,25 +63,16 @@ 0 - -629 - 678 - 2739 + 0 + 673 + 2749 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -95,7 +86,7 @@ QFrame::Raised - 18 + 14 @@ -4570,16 +4561,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki Directory of images (optional settings) - - 0 - - - 0 - - - 0 - - + 0 @@ -8489,21 +8471,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + - + Qt::Horizontal - + Optimize graph from the newest node. @@ -8516,7 +8498,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures. @@ -8529,7 +8511,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - + -If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation). @@ -8542,6 +8524,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag + + + + Ignore pose priors. + + + true + + + Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse + + + + + + + + + + @@ -12653,16 +12655,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag - - 0 - - - 0 - - - 0 - - + 0 @@ -12802,16 +12795,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare - - 0 - - - 0 - - - 0 - - + 0 @@ -12969,16 +12953,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13058,16 +13033,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 @@ -13179,16 +13145,7 @@ Lower the ratio -> higher the precision. 0 means disabled, matching the neare 0 - - 0 - - - 0 - - - 0 - - + 0 diff --git a/tools/KittiDataset/main.cpp b/tools/KittiDataset/main.cpp index bbc89114..aafb4dbc 100644 --- a/tools/KittiDataset/main.cpp +++ b/tools/KittiDataset/main.cpp @@ -480,7 +480,7 @@ int main(int argc, char * argv[]) std::string l; double s; std::vector v; - std::vector gps; + GPS gps; rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true); if(!gtPose.isNull()) { diff --git a/tools/RgbdDataset/main.cpp b/tools/RgbdDataset/main.cpp index 37b67575..8376ca17 100644 --- a/tools/RgbdDataset/main.cpp +++ b/tools/RgbdDataset/main.cpp @@ -318,7 +318,7 @@ int main(int argc, char * argv[]) std::string l; double s; std::vector v; - std::vector gps; + GPS gps; rtabmap.getMemory()->getNodeInfo(iter->first, o, m, w, l, s, gtPose, v, gps, true); if(!gtPose.isNull()) {