diff --git a/corelib/include/rtabmap/core/camera/CameraSeerSense.h b/corelib/include/rtabmap/core/camera/CameraSeerSense.h index c4821bb6..f4c6595b 100644 --- a/corelib/include/rtabmap/core/camera/CameraSeerSense.h +++ b/corelib/include/rtabmap/core/camera/CameraSeerSense.h @@ -19,16 +19,17 @@ public: public: CameraSeerSense( - float imageRate=0.0f, + bool computeOdometry = false, + float imageRate = 0.0f, const Transform & localTransform = Transform::getIdentity() ); virtual ~CameraSeerSense(); - void setIMU(bool imuPublished, bool publishInterIMU); - virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; + virtual bool odomProvided() const; + virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0); protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); @@ -36,8 +37,7 @@ protected: private: #ifdef RTABMAP_XVSDK CameraModel cameraModel_; - bool imuPublished_; - bool publishInterIMU_; + bool computeOdometry_; int imuId_; int tofId_; std::shared_ptr device_; diff --git a/corelib/src/camera/CameraSeerSense.cpp b/corelib/src/camera/CameraSeerSense.cpp index 06afed55..ee445d3b 100644 --- a/corelib/src/camera/CameraSeerSense.cpp +++ b/corelib/src/camera/CameraSeerSense.cpp @@ -12,12 +12,11 @@ bool CameraSeerSense::available() #endif } -CameraSeerSense::CameraSeerSense(float imageRate, const Transform & localTransform) : +CameraSeerSense::CameraSeerSense(bool computeOdometry, float imageRate, const Transform & localTransform) : Camera(imageRate, localTransform) #ifdef RTABMAP_XVSDK , - imuPublished_(true), - publishInterIMU_(false), + computeOdometry_(computeOdometry), imuId_(0), tofId_(0) #endif @@ -35,6 +34,9 @@ CameraSeerSense::~CameraSeerSense() if(tofId_) device_->tofCamera()->unregisterColorDepthImageCallback(tofId_); + if(device_->slam()) + device_->slam()->stop(); + if(device_->imuSensor()) device_->imuSensor()->stop(); @@ -47,16 +49,6 @@ CameraSeerSense::~CameraSeerSense() dataReady_.release(); } -void CameraSeerSense::setIMU(bool imuPublished, bool publishInterIMU) -{ -#ifdef RTABMAP_XVSDK - imuPublished_ = imuPublished; - publishInterIMU_ = publishInterIMU; -#else - UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!"); -#endif -} - bool CameraSeerSense::init(const std::string & calibrationFolder, const std::string & cameraName) { UDEBUG(""); @@ -72,14 +64,14 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str UASSERT(device_->imuSensor()); device_->imuSensor()->start(); imuId_ = device_->imuSensor()->registerCallback([this](const xv::Imu & xvImu) { - if(imuPublished_ && xvImu.hostTimestamp > 0) + if(xvImu.hostTimestamp > 0) { - if(publishInterIMU_) + if(isInterIMUPublishing()) { IMU imu(cv::Vec3d(xvImu.gyro[0], xvImu.gyro[1], xvImu.gyro[2]), cv::Mat::eye(3,3,CV_64FC1), cv::Vec3d(xvImu.accel[0], xvImu.accel[1], xvImu.accel[2]), cv::Mat::eye(3,3,CV_64FC1), this->getLocalTransform()); - UEventsManager::post(new IMUEvent(imu, xvImu.hostTimestamp)); + this->postInterIMU(imu, xvImu.hostTimestamp); } else { @@ -90,6 +82,12 @@ bool CameraSeerSense::init(const std::string & calibrationFolder, const std::str } }); + if(computeOdometry_) + { + UASSERT(device_->slam()); + device_->slam()->start(xv::Slam::Mode::Mixed); + } + auto frameRate = xv::TofCamera::Framerate::FPS_30; if(this->getImageRate() > 25) frameRate = xv::TofCamera::Framerate::FPS_30; @@ -172,6 +170,34 @@ std::string CameraSeerSense::getSerial() const return ""; } +bool CameraSeerSense::odomProvided() const +{ +#ifdef RTABMAP_XVSDK + return computeOdometry_; +#else + return false; +#endif +} + +bool CameraSeerSense::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime) +{ +#ifdef RTABMAP_XVSDK + xv::Pose xvPose; + if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, stamp)) + { + pose = this->getLocalTransform() * + Transform( + xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0], + xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1], + xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) * + this->getLocalTransform().inverse(); + covariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005; + return true; + } +#endif + return false; +} + SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info) { SensorData data; @@ -190,7 +216,7 @@ SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info) lastData_ = std::pair>(); dataMutex_.unlock(); - if(imuPublished_ && !publishInterIMU_) + if(!isInterIMUPublishing()) { cv::Vec3d gyro, acc; std::map>::const_iterator iterA, iterB; @@ -223,6 +249,18 @@ SensorData CameraSeerSense::captureImage(SensorCaptureInfo * info) imuMutex_.unlock(); data.setIMU(IMU(gyro, cv::Mat::eye(3, 3, CV_64FC1), acc, cv::Mat::eye(3, 3, CV_64FC1), this->getLocalTransform())); } + + xv::Pose xvPose; + if(computeOdometry_ && device_->slam()->getPoseAt(xvPose, data.stamp())) + { + info->odomPose = this->getLocalTransform() * + Transform( + xvPose.transform().rotation()[0], xvPose.transform().rotation()[1], xvPose.transform().rotation()[2], xvPose.transform().translation()[0], + xvPose.transform().rotation()[3], xvPose.transform().rotation()[4], xvPose.transform().rotation()[5], xvPose.transform().translation()[1], + xvPose.transform().rotation()[6], xvPose.transform().rotation()[7], xvPose.transform().rotation()[8], xvPose.transform().translation()[2]) * + this->getLocalTransform().inverse(); + info->odomCovariance = cv::Mat::eye(6, 6, CV_64FC1) * 0.0005; + } #else UERROR("CameraSeerSense: RTAB-Map is not built with XVisio SDK support!"); #endif diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 3dbd48fd..b37ff99a 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -425,6 +425,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) : if (!CameraSeerSense::available()) { _ui->comboBox_cameraRGBD->setItemData(kSrcSeerSense - kSrcRGBD, 0, Qt::UserRole - 1); + _ui->comboBox_odom_sensor->setItemData(3, 0, Qt::UserRole - 1); } _ui->openni2_exposure->setEnabled(CameraOpenNI2::exposureGainAvailable()); _ui->openni2_gain->setEnabled(CameraOpenNI2::exposureGainAvailable()); @@ -6240,6 +6241,11 @@ PreferencesDialog::Src PreferencesDialog::getOdomSourceDriver() const //Zed SDK return kSrcStereoZed; } + else if(_ui->comboBox_odom_sensor->currentIndex() == 3) + { + //XVisio SDK + return kSrcSeerSense; + } else if(_ui->comboBox_odom_sensor->currentIndex() != 0) { UERROR("Not implemented!"); @@ -6408,7 +6414,7 @@ Camera * PreferencesDialog::createCamera( bool odomOnly, bool odomSensorExtrinsicsCalib) { - if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed)) + if(odomOnly && !(driver == kSrcStereoRealSense2 || driver == kSrcStereoZed || driver == kSrcSeerSense)) { QMessageBox::warning(this, tr("Odometry Sensor"), tr("Driver %1 cannot support odometry only mode.").arg(driver), QMessageBox::Ok); @@ -6807,9 +6813,13 @@ Camera * PreferencesDialog::createCamera( { UDEBUG("SeerSense"); camera = new CameraSeerSense( + getOdomSourceDriver() == kSrcSeerSense, this->getGeneralInputRate(), this->getSourceLocalTransform()); - ((CameraSeerSense*)camera)->setIMU(true, _ui->checkbox_publishInterIMU->isChecked()); + camera->setInterIMUPublishing( + _ui->checkbox_publishInterIMU->isChecked(), + _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? + IMUFilter::create((IMUFilter::Type)(getIMUFilteringStrategy()-1), this->getAllParameters()):0); } else if(driver == kSrcUsbDevice) { diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index 3f46bb02..95dfebeb 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -8144,6 +8144,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki ZED sdk + + + SeerSense + + @@ -8741,7 +8746,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki 1 - + @@ -11892,7 +11897,7 @@ see Sqlite3 doc 'PRAGMA synchronous'. - + Likelihood ratio for VP.