i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

0.14: added gps field to Node table in database (#226). Tango: saving gps if enabled, added Rename/Remove/Share on long click in Open dialog (fixed #233)

Browse Source
This commit is contained in:
matlabbe
2017-09-21 20:59:45 -04:00
parent 114490f01e
commit bfc393a090
22 changed files with 516 additions and 103 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/jni/CameraTango.cpp
+28 -1
View File
@@ -116,7 +116,8 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
cloudStamp_(0),
tangoColorType_(0),
tangoColorStamp_(0),
colorCameraToDisplayRotation_(ROTATION_0)
colorCameraToDisplayRotation_(ROTATION_0),
lastKnownGPS_(std::vector<double>(6,0))
{
UASSERT(decimation >= 1);
}
@@ -189,6 +190,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
{
close();
lastKnownGPS_ = std::vector<double>(6,0);
TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
// Connect to Tango
@@ -513,6 +516,21 @@ std::string CameraTango::getSerial() const
return "Tango";
}
void CameraTango::setGPS(double stamp,
double longitude,
double latitude,
double altitude,
double accuracy,
double bearing)
{
lastKnownGPS_[0] = stamp;
lastKnownGPS_[1] = longitude;
lastKnownGPS_[2] = latitude;
lastKnownGPS_[3] = altitude;
lastKnownGPS_[4] = accuracy;
lastKnownGPS_[5] = bearing;
}
rtabmap::Transform CameraTango::tangoPoseToTransform(const TangoPoseData * tangoPose) const
{
UASSERT(tangoPose);
@@ -837,6 +855,15 @@ SensorData CameraTango::captureImage(CameraInfo * info)
data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
data.setGroundTruth(odom);
if(lastKnownGPS_[0] > 0.0 && rgbStamp-lastKnownGPS_[0]<2.0)
{
data.setGPS(lastKnownGPS_[0], lastKnownGPS_[1], lastKnownGPS_[2], lastKnownGPS_[3], lastKnownGPS_[4], lastKnownGPS_[5]);
}
else if(lastKnownGPS_[0]>0.0)
{
LOGD("GPS too old (current time=%f, gps time = %f)", rgbStamp, lastKnownGPS_[0]);
}
}
else
{