i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 12:30:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

LiDAR capture support in standalone library (#1264)

Browse Source 
* Working rtabmap_lidar-mapping example (live and pcap)

* finalizing merge, added some deprecated

* fixed build

* Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI.

* backward compatibility

* fixed some not used variable warnings, fixed qt build for lidar mapping example

* Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8

* ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera  inputs.

* Working external lidar on iOS

* util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup

* fixed winsock include order

* reverted camera tool

* disable imu filtering when odom sensor is used

* Updated package version

* fixed windows build

* fixing more windows build erros
This commit is contained in:
matlabbe
2024-04-14 19:06:04 -07:00
committed by GitHub
parent 6a6913c939
commit 700704bec9
131 changed files with 10585 additions and 7476 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1 -1
View File
@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
#######################
SET(RTABMAP_MAJOR_VERSION 0)
SET(RTABMAP_MINOR_VERSION 21)
SET(RTABMAP_PATCH_VERSION 4)
SET(RTABMAP_PATCH_VERSION 5)
SET(RTABMAP_VERSION
${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
app/android/jni/CameraARCore.cpp
+17 -145
View File
@@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
}
}
SensorData CameraARCore::captureImage(CameraInfo * info)
SensorData CameraARCore::updateDataOnRender(Transform & pose)
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
pose.setNull();
SensorData data;
if(!arSession_)
{
@@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
if (geometry_changed != 0 || !uvs_initialized_) {
ArFrame_transformCoordinates2d(
arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
transformed_uvs_);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
@@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
{
@@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
float pose_raw[7];
ArCamera_getPose(arSession_, ar_camera, arPose_);
ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
poseArCore = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
Transform poseArCore = pose;
if(pose.isNull())
if(poseArCore.isNull())
{
LOGE("CameraARCore: Pose is null");
}
else
{
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
// Get calibration parameters
float fx,fy, cx, cy;
@@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
data.setFeatures(kpts, kpts3, cv::Mat());
if(!poseArCore.isNull())
{
pose = poseArCore;
this->poseReceived(pose, stamp);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
}
}
else
@@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
ArCamera_release(ar_camera);
return data;
}
void CameraARCore::capturePoseOnly()
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
if(!arSession_)
{
return;
}
if(textureId_ != 0)
{
glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
ArSession_setCameraTextureName(arSession_, textureId_);
}
// Update session to get current frame and render camera background.
if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
return;
}
// If display rotation changed (also includes view size change), we need to
// re-query the uv coordinates for the on-screen portion of the camera image.
int32_t geometry_changed = 0;
ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
if (geometry_changed != 0 || !uvs_initialized_) {
ArFrame_transformCoordinates2d(
arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
transformed_uvs_);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
}
ArCamera* ar_camera;
ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
ArCamera_getProjectionMatrix(arSession_, ar_camera,
/*near=*/0.1f, /*far=*/100.f,
glm::value_ptr(projectionMatrix_));
// adjust origin
if(!getOriginOffset().isNull())
{
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
ArTrackingState camera_tracking_state;
ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
ArCamera_getPose(arSession_, ar_camera, arPose_);
ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(!pose.isNull())
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
int32_t is_depth_supported = 0;
ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
if(is_depth_supported)
{
LOGD("Acquire depth image!");
ArImage * depthImage = nullptr;
ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
ArImageFormat format;
ArImage_getFormat(arSession_, depthImage, &format);
if(format == AR_IMAGE_FORMAT_DEPTH16)
{
LOGD("Depth format detected!");
int planeCount;
ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
LOGD("planeCount=%d", planeCount);
UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
const uint8_t *data = nullptr;
int len = 0;
int stride;
int width;
int height;
ArImage_getWidth(arSession_, depthImage, &width);
ArImage_getHeight(arSession_, depthImage, &height);
ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
float fx,fy, cx, cy;
int32_t rgb_width, rgb_height;
ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
float scaleX = (float)width / (float)rgb_width;
float scaleY = (float)height / (float)rgb_height;
CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
this->setOcclusionImage(occlusionImage, occlusionModel);
}
ArImage_release(depthImage);
}
}
ArCamera_release(ar_camera);
}
} /* namespace rtabmap */
app/android/jni/CameraARCore.h
+2 -11
View File
@@ -63,23 +63,14 @@ public:
CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
virtual ~CameraARCore();
bool uvsInitialized() const {return uvs_initialized_;}
const float* uvsTransformed() const {return transformed_uvs_;}
void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
void setupGL();
virtual void close(); // close Tango connection
virtual void close(); // close ARCore connection
virtual std::string getSerial() const;
GLuint getTextureId() const {return textureId_;}
void imageCallback(AImageReader *reader);
protected:
virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
virtual void capturePoseOnly();
virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);
app/android/jni/CameraAREngine.cpp
+77 -69
View File
@@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri
deviceTColorCamera_ = opticalRotation;
// Required as ArSession_update does some off-screen OpenGL stuff...
HwArSession_setCameraTextureName(arSession_, textureId_);
if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
{
UERROR("Cannot resume camera!");
@@ -169,38 +166,87 @@ void CameraAREngine::close()
CameraMobile::close();
}
SensorData CameraAREngine::captureImage(CameraInfo * info)
void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
{
CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
if(arSession_)
{
int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
if (ret > 3) {
ret -= 4;
}
HwArSession_setDisplayGeometry(arSession_, ret, width, height);
}
}
SensorData CameraAREngine::updateDataOnRender(Transform & pose)
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
pose.setNull();
SensorData data;
if(!arSession_)
{
return data;
}
if(textureId_ == 0)
{
glGenTextures(1, &textureId_);
glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
}
if(textureId_!=0)
HwArSession_setCameraTextureName(arSession_, textureId_);
// Update session to get current frame and render camera background.
if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
LOGE("CameraAREngine::captureImage() ArSession_update error");
return data;
}
// If display rotation changed (also includes view size change), we need to
// re-query the uv coordinates for the on-screen portion of the camera image.
int32_t geometry_changed = 0;
HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
if (geometry_changed != 0 || !uvs_initialized_) {
HwArFrame_transformDisplayUvCoords(
arSession_, arFrame_,
BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
transformed_uvs_);
UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)",
transformed_uvs_[0], transformed_uvs_[1],
transformed_uvs_[2], transformed_uvs_[3],
transformed_uvs_[4], transformed_uvs_[5],
transformed_uvs_[6], transformed_uvs_[7]);
UASSERT(transformed_uvs_);
uvs_initialized_ = true;
}
HwArCamera* ar_camera;
HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
HwArCamera_getProjectionMatrix(arSession_, ar_camera,
/*near=*/0.1f, /*far=*/100.f,
glm::value_ptr(projectionMatrix_));
// adjust origin
if(!getOriginOffset().isNull())
{
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
HwArTrackingState camera_tracking_state;
HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
// Get calibration parameters
// FIXME: Hard-coded as getting intrinsics with the api fails
float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
@@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
double stamp = double(timestamp_ns)/10e8;
CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
data = SensorData(outputRGB, outputDepth, model, 0, stamp);
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(pose.isNull())
{
LOGE("CameraAREngine: Pose is null");
}
else
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose, stamp);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
}
}
}
else
@@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
}
HwArCamera_release(ar_camera);
if(pose.isNull())
{
LOGE("CameraAREngine: Pose is null");
}
else
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
return data;
}
void CameraAREngine::capturePoseOnly()
{
UScopeMutex lock(arSessionMutex_);
//LOGI("Capturing image...");
SensorData data;
if(!arSession_)
{
return;
}
// Update session to get current frame and render camera background.
if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
LOGE("CameraARCore::captureImage() ArSession_update error");
return;
}
HwArCamera* ar_camera;
HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
HwArTrackingState camera_tracking_state;
HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
Transform pose;
CameraModel model;
if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
{
// pose in OpenGL coordinates
float pose_raw[7];
HwArCamera_getPose(arSession_, ar_camera, arPose_);
HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
if(!pose.isNull())
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
}
}
HwArCamera_release(ar_camera);
}
} /* namespace rtabmap */
app/android/jni/CameraAREngine.h
+5 -4
View File
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEvent.h>
#include <rtabmap/utilite/UTimer.h>
#include <boost/thread/mutex.hpp>
#include <background_renderer.h>
#include <huawei_arengine_interface.h>
@@ -48,13 +49,14 @@ public:
CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
virtual ~CameraAREngine();
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual void close(); // close Tango connection
virtual void close(); // close AREngine connection
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual void capturePoseOnly();
virtual SensorData updateDataOnRender(Transform & pose);
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -69,7 +71,6 @@ private:
HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
HwArPose * arPose_ = nullptr;
bool arInstallRequested_;
GLuint textureId_;
UMutex arSessionMutex_;
};
app/android/jni/CameraMobile.cpp
+188 -132
View File
@@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
CameraMobile::CameraMobile(bool smoothing) :
Camera(10),
deviceTColorCamera_(Transform::getIdentity()),
spinOncePreviousStamp_(0.0),
textureId_(0),
uvs_initialized_(false),
previousStamp_(0.0),
stampEpochOffset_(0.0),
smoothing_(smoothing),
colorCameraToDisplayRotation_(ROTATION_0),
@@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &)
void CameraMobile::close()
{
previousPose_.setNull();
previousStamp_ = 0.0;
firstFrame_ = true;
lastKnownGPS_ = GPS();
lastEnvSensors_.clear();
originOffset_ = Transform();
originUpdate_ = false;
pose_ = Transform();
dataPose_ = Transform();
data_ = SensorData();
if(textureId_ != 0)
@@ -97,35 +94,107 @@ void CameraMobile::close()
void CameraMobile::resetOrigin()
{
previousPose_.setNull();
previousStamp_ = 0.0;
firstFrame_ = true;
lastKnownGPS_ = GPS();
lastEnvSensors_.clear();
pose_ = Transform();
dataPose_ = Transform();
data_ = SensorData();
originUpdate_ = true;
}
void CameraMobile::poseReceived(const Transform & pose)
bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
{
pose.setNull();
int maxWaitTimeMs = maxWaitTime * 1000;
// Interpolate pose
if(!poseBuffer_.empty())
{
poseMutex_.lock();
int waitTry = 0;
while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
{
poseMutex_.unlock();
++waitTry;
uSleep(1);
poseMutex_.lock();
}
if(poseBuffer_.rbegin()->first < stamp)
{
if(maxWaitTimeMs > 0)
{
UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
}
else
{
UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
}
}
else
{
std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
std::map<double, Transform>::const_iterator iterA = iterB;
if(iterA != poseBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == poseBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && stamp == iterA->first)
{
pose = iterA->second;
}
else if(stamp >= iterA->first && stamp <= iterB->first)
{
pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else // stamp < iterA->first
{
UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
}
}
poseMutex_.unlock();
}
return !pose.isNull();
}
void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
{
if(!pose.isNull())
{
// send pose of the camera (without optical rotation)
Transform p = pose*deviceTColorCamera_;
Transform p = pose;
if(originUpdate_)
{
originOffset_ = p.translation().inverse();
originUpdate_ = false;
}
if(stampEpochOffset_ == 0.0)
{
stampEpochOffset_ = UTimer::now() - deviceStamp;
}
double epochStamp = stampEpochOffset_ + deviceStamp;
if(!originOffset_.isNull())
{
this->post(new PoseEvent(originOffset_*p));
p = originOffset_*p;
}
else
{
this->post(new PoseEvent(p));
UScopeMutex lock(poseMutex_);
poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
}
// send pose of the camera (with optical rotation)
this->post(new PoseEvent(p * deviceTColorCamera_));
}
}
@@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps)
lastKnownGPS_ = gps;
}
void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
void CameraMobile::addEnvSensor(int type, float value)
{
LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
}
void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
{
UScopeMutex lock(dataMutex_);
bool notify = !data_.isValid();
LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
data_ = data;
pose_ = pose;
dataPose_ = pose;
viewMatrix_ = viewMatrix;
projectionMatrix_ = projectionMatrix;
@@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
// adjust origin
if(!originOffset_.isNull())
{
pose_ = originOffset_ * pose_;
dataPose_ = originOffset_ * dataPose_;
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
@@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
uvs_initialized_ = true;
}
LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
LOGD("CameraMobile::update textureId_=%d", (int)textureId_);
if(textureId_ != 0 && texCoord != 0)
{
@@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
return;
}
}
}
void CameraMobile::addEnvSensor(int type, float value)
{
lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
}
void CameraMobile::spinOnce()
{
if(!this->isRunning())
postUpdate();
if(notify)
{
bool ignoreFrame = false;
//float rate = 10.0f; // maximum 10 FPS for image data
double now = UTimer::now();
/*if(rate>0.0f)
{
if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
{
ignoreFrame = true;
}
}*/
dataReady_.release();
}
}
if(!ignoreFrame)
void CameraMobile::updateOnRender()
{
UScopeMutex lock(dataMutex_);
bool notify = !data_.isValid();
data_ = updateDataOnRender(dataPose_);
if(data_.isValid())
{
postUpdate();
if(notify)
{
spinOnceFrameRateTimer_.start();
spinOncePreviousStamp_ = now;
mainLoop();
}
else
{
// just send pose
capturePoseOnly();
dataReady_.release();
}
}
}
void CameraMobile::mainLoopBegin()
SensorData CameraMobile::updateDataOnRender(Transform & pose)
{
double t = cameraStartedTime_.elapsed();
if(t < 5.0)
{
uSleep((5.0-t)*1000); // just to make sure that the camera is started
}
LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
"should be overridden by inherited classes. Returning empty data!\n");
return SensorData();
}
void CameraMobile::mainLoop()
void CameraMobile::postUpdate()
{
CameraInfo info;
SensorData data = this->captureImage(&info);
if(data.isValid() && !info.odomPose.isNull())
if(data_.isValid())
{
if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
{
data.setGPS(lastKnownGPS_);
data_.setGPS(lastKnownGPS_);
}
else if(lastKnownGPS_.stamp()>0.0)
{
LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
}
if(lastEnvSensors_.size())
{
data.setEnvSensors(lastEnvSensors_);
data_.setEnvSensors(lastEnvSensors_);
lastEnvSensors_.clear();
}
if(smoothing_ && !data.depthRaw().empty())
if(smoothing_ && !data_.depthRaw().empty())
{
//UTimer t;
data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
//LOGD("Bilateral filtering, time=%fs", t.ticks());
}
@@ -273,15 +336,15 @@ void CameraMobile::mainLoop()
{
UDEBUG("ROTATION_90");
cv::Mat rgb, depth;
cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
cv::flip(data.imageRaw(),rgb,1);
cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::flip(data_.imageRaw(),rgb,1);
cv::transpose(rgb,rgbt);
rgb = rgbt;
cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
cv::flip(data.depthRaw(),depth,1);
cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
cv::flip(data_.depthRaw(),depth,1);
cv::transpose(depth,deptht);
depth = deptht;
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
@@ -290,25 +353,25 @@ void CameraMobile::mainLoop()
model.cx()>0?model.imageWidth()-model.cx():0,
model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = data.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
keypoints[i].pt.x = data_.keypoints()[i].pt.y;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_180)
{
UDEBUG("ROTATION_180");
cv::Mat rgb, depth;
cv::flip(data.imageRaw(),rgb,1);
cv::flip(data_.imageRaw(),rgb,1);
cv::flip(rgb,rgb,0);
cv::flip(data.depthOrRightRaw(),depth,1);
cv::flip(data_.depthOrRightRaw(),depth,1);
cv::flip(depth,depth,0);
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageWidth(), model.imageHeight());
model = CameraModel(
model.fx(),
@@ -317,26 +380,26 @@ void CameraMobile::mainLoop()
model.cy()>0?model.imageHeight()-model.cy():0,
model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
else if(colorCameraToDisplayRotation_ == ROTATION_270)
{
UDEBUG("ROTATION_270");
cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
cv::transpose(data.imageRaw(),rgb);
cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
cv::transpose(data_.imageRaw(),rgb);
cv::flip(rgb,rgb,1);
cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
cv::transpose(data.depthOrRightRaw(),depth);
cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
cv::transpose(data_.depthOrRightRaw(),depth);
cv::flip(depth,depth,1);
CameraModel model = data.cameraModels()[0];
CameraModel model = data_.cameraModels()[0];
cv::Size sizet(model.imageHeight(), model.imageWidth());
model = CameraModel(
model.fy(),
@@ -345,61 +408,54 @@ void CameraMobile::mainLoop()
model.cx(),
model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
model.setImageSize(sizet);
data.setRGBDImage(rgb, depth, model);
data_.setRGBDImage(rgb, depth, model);
std::vector<cv::KeyPoint> keypoints = data.keypoints();
std::vector<cv::KeyPoint> keypoints = data_.keypoints();
for(size_t i=0; i<keypoints.size(); ++i)
{
keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
keypoints[i].pt.y = data.keypoints()[i].pt.x;
keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
keypoints[i].pt.y = data_.keypoints()[i].pt.x;
}
data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
}
rtabmap::Transform pose = info.odomPose;
data.setGroundTruth(Transform());
// convert stamp to epoch
bool firstFrame = previousPose_.isNull();
if(firstFrame)
{
stampEpochOffset_ = UTimer::now()-data.stamp();
}
data.setStamp(stampEpochOffset_ + data.stamp());
OdometryInfo info;
if(!firstFrame)
{
info.interval = data.stamp()-previousStamp_;
info.transform = previousPose_.inverse() * pose;
}
// linear cov = 0.0001
info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
if(!firstFrame)
{
// angular cov = 0.000001
info.reg.covariance.at<double>(3,3) *= 0.01;
info.reg.covariance.at<double>(4,4) *= 0.01;
info.reg.covariance.at<double>(5,5) *= 0.01;
}
LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
this->post(new OdometryEvent(data, pose, info));
previousPose_ = pose;
previousStamp_ = data.stamp();
}
else if(!this->isKilled() && info.odomPose.isNull())
{
LOGW("Odometry lost");
this->post(new OdometryEvent());
}
}
SensorData CameraMobile::captureImage(CameraInfo * info)
SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
{
if(info)
SensorData data;
if(dataReady_.acquire(1, 5000))
{
info->odomPose = pose_;
UScopeMutex lock(dataMutex_);
data = data_;
data_ = SensorData();
}
return data_;
if(data.isValid())
{
data.setGroundTruth(Transform());
data.setStamp(stampEpochOffset_ + data.stamp());
if(info)
{
// linear cov = 0.0001
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
if(!firstFrame_)
{
// angular cov = 0.000001
info->odomCovariance.at<double>(3,3) *= 0.01;
info->odomCovariance.at<double>(4,4) *= 0.01;
info->odomCovariance.at<double>(5,5) *= 0.01;
}
info->odomPose = dataPose_;
}
firstFrame_ = false;
}
else
{
UWARN("CameraMobile::captureImage() invalid data!");
}
return data;
}
LaserScan CameraMobile::scanFromPointCloudData(
app/android/jni/CameraMobile.h
+20 -16
View File
@@ -68,7 +68,7 @@ private:
Transform pose_;
};
class CameraMobile : public Camera, public UThread, public UEventsSender {
class CameraMobile : public Camera, public UEventsSender {
public:
static const float bilateralFilteringSigmaS;
static const float bilateralFilteringSigmaR;
@@ -93,14 +93,20 @@ public:
// abstract functions
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
virtual void close(); // inherited classes should call its parent in their close().
virtual void close(); // inherited classes should call its parent at the end of their close().
virtual std::string getSerial() const {return "CameraMobile";}
void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void updateOnRender();
const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
void resetOrigin();
virtual bool isCalibrated() const;
void poseReceived(const Transform & pose); // in rtabmap frame
virtual bool odomProvided() const { return true; }
virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
double getStampEpochOffset() const {return stampEpochOffset_;}
const CameraModel & getCameraModel() const {return model_;}
const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
@@ -108,10 +114,7 @@ public:
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
void setGPS(const GPS & gps);
void addEnvSensor(int type, float value);
void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
GLuint getTextureId() {return textureId_;}
bool uvsInitialized() const {return uvs_initialized_;}
const float* uvsTransformed() const {return transformed_uvs_;}
@@ -122,17 +125,15 @@ public:
const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual void capturePoseOnly() {}
virtual SensorData updateDataOnRender(Transform & pose);
virtual void mainLoopBegin();
virtual void mainLoop();
private:
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
void postUpdate(); // Should be called while being protected by dataMutex_
protected:
CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
UTimer spinOnceFrameRateTimer_;
double spinOncePreviousStamp_;
GLuint textureId_;
glm::mat4 viewMatrix_;
@@ -141,9 +142,7 @@ protected:
bool uvs_initialized_ = false;
private:
Transform previousPose_;
double previousStamp_;
UTimer cameraStartedTime_;
bool firstFrame_;
double stampEpochOffset_;
bool smoothing_;
ScreenRotation colorCameraToDisplayRotation_;
@@ -152,8 +151,13 @@ private:
Transform originOffset_;
bool originUpdate_;
USemaphore dataReady_;
UMutex dataMutex_;
SensorData data_;
Transform pose_;
Transform dataPose_;
UMutex poseMutex_;
std::map<double, Transform> poseBuffer_; // <stamp, Pose>
cv::Mat occlusionImage_;
CameraModel occlusionModel_;
app/android/jni/CameraTango.cpp
+19 -24
View File
@@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
if(pose->status_code == TANGO_POSE_VALID)
{
CameraTango* app = static_cast<CameraTango*>(context);
app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
}
}
@@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
UASSERT(cloud.type() == CV_32FC4);
boost::mutex::scoped_lock lock(dataMutex_);
boost::mutex::scoped_lock lock(tangoDataMutex_);
// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
// "In the current version of Project Tango Tablet RGB IR camera
@@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
if(dt >= 0.0 && dt < 0.5)
{
bool notify = !data_.isValid();
bool notify = !tangoData_.isValid();
cv::Mat tangoImage = tangoColor_;
cv::Mat rgb;
@@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
else
{
LOGE("Not supported color format : %d.", tangoColorType);
data_ = SensorData();
tangoData_ = SensorData();
return;
}
@@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
if(rawScanPublished_)
{
data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
else
{
data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
}
data_.setGroundTruth(odom);
tangoData_.setGroundTruth(odom);
}
else
{
LOGE("Could not get depth and rgb images!?!");
data_ = SensorData();
tangoData_ = SensorData();
return;
}
if(notify)
{
dataReady_.release();
tangoDataReady_.release();
}
LOGD("process cloud received %fs", timer.ticks());
}
@@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
{
//LOGD("RGB received! %fs", timestamp);
boost::mutex::scoped_lock lock(dataMutex_);
boost::mutex::scoped_lock lock(tangoDataMutex_);
tangoColor_ = tangoImage.clone();
tangoColorStamp_ = timestamp;
@@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
return pose;
}
SensorData CameraTango::captureImage(CameraInfo * info)
SensorData CameraTango::updateDataOnRender(Transform & pose)
{
//LOGI("Capturing image...");
pose.setNull();
if(textureId_ == 0)
{
glGenTextures(1, &textureId_);
@@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
if (status == TANGO_SUCCESS)
{
if(info)
{
info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
}
pose = getPoseAtTimestamp(video_overlay_timestamp);
int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
if (rotation > 3) {
@@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
}
SensorData data;
if(dataReady_.acquireTry(1))
if(tangoDataReady_.acquireTry(1))
{
boost::mutex::scoped_lock lock(dataMutex_);
data = data_;
data_ = SensorData();
if(info)
{
info->odomPose = data.groundTruth();
data.setGroundTruth(Transform());
}
boost::mutex::scoped_lock lock(tangoDataMutex_);
data = tangoData_;
tangoData_ = SensorData();
pose = data.groundTruth();
data.setGroundTruth(Transform());
}
return data;
app/android/jni/CameraTango.h
+4 -5
View File
@@ -52,7 +52,6 @@ public:
virtual void close(); // close Tango connection
virtual std::string getSerial() const;
rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
void setDecimation(int value) {decimation_ = value;}
void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
@@ -61,7 +60,7 @@ public:
void tangoEventReceived(int type, const char * key, const char * value);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData updateDataOnRender(Transform & pose);
private:
rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -71,12 +70,12 @@ private:
bool colorCamera_;
int decimation_;
bool rawScanPublished_;
SensorData data_;
SensorData tangoData_;
cv::Mat tangoColor_;
int tangoColorType_;
double tangoColorStamp_;
boost::mutex dataMutex_;
USemaphore dataReady_;
boost::mutex tangoDataMutex_;
USemaphore tangoDataReady_;
cv::Mat fisheyeRectifyMapX_;
cv::Mat fisheyeRectifyMapY_;
};
app/android/jni/RTABMapApp.cpp
+108 -135
View File
@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/GainCompensator.h>
#include <rtabmap/core/DBDriver.h>
#include <rtabmap/core/Recovery.h>
#include <rtabmap/core/lidar/LidarVLP16.h>
#include <pcl/common/common.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
@@ -202,6 +203,7 @@ RTABMapApp::RTABMapApp() :
#endif
cameraDriver_(0),
camera_(0),
sensorCaptureThread_(0),
rtabmapThread_(0),
rtabmap_(0),
logHandler_(0),
@@ -216,6 +218,7 @@ RTABMapApp::RTABMapApp() :
cameraColor_(true),
fullResolution_(false),
appendMode_(true),
useExternalLidar_(false),
maxCloudDepth_(2.5),
minCloudDepth_(0.0),
cloudDensityLevel_(1),
@@ -537,7 +540,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
@@ -885,7 +888,7 @@ bool RTABMapApp::startCamera()
#endif
LOGW("startCamera() camera driver=%d", cameraDriver_);
boost::mutex::scoped_lock lock(cameraMutex_);
if(cameraDriver_ == 0) // Tango
{
#ifdef RTABMAP_TANGO
@@ -937,6 +940,19 @@ bool RTABMapApp::startCamera()
LOGI("Start camera thread");
cameraJustInitialized_ = true;
if(useExternalLidar_)
{
rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
lidar->init();
camera_->setImageRate(0); // if lidar, to get close camera synchronization
sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
}
else
{
sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
}
sensorCaptureThread_->start();
return true;
}
UERROR("Failed camera initialization!");
@@ -948,13 +964,12 @@ void RTABMapApp::stopCamera()
LOGI("stopCamera()");
{
boost::mutex::scoped_lock lock(cameraMutex_);
if(camera_!=0)
if(sensorCaptureThread_!=0)
{
camera_->join(true);
camera_->close();
delete camera_;
sensorCaptureThread_->join(true);
delete sensorCaptureThread_; // camera_ is closed and deleted inside
sensorCaptureThread_ = 0;
camera_ = 0;
poseBuffer_.clear();
}
}
{
@@ -1241,7 +1256,7 @@ int RTABMapApp::Render()
std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
try
{
if(camera_ == 0)
if(sensorCaptureThread_ == 0)
{
// We are not doing continous drawing, just measure single draw
fpsTime_.restart();
@@ -1272,49 +1287,45 @@ int RTABMapApp::Render()
{
if(cameraDriver_ <= 2)
{
camera_->spinOnce();
camera_->updateOnRender();
}
#ifdef DEBUG_RENDERING_PERFORMANCE
LOGW("Camera spinOnce %fs", time.ticks());
LOGW("Camera updateOnRender %fs", time.ticks());
#endif
if(cameraDriver_ != 2)
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
{
if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
}
if(camera_->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
if(graphOptimization_ && !mapToOdom_.isIdentity())
{
main_scene_.background_renderer_ = new BackgroundRenderer();
main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
}
if(camera_->uvsInitialized())
{
uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
if(graphOptimization_ && !mapToOdom_.isIdentity())
{
rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
}
}
if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
{
rtabmap::CameraModel occlusionModel;
cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
}
if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
{
rtabmap::CameraModel occlusionModel;
cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
occlusionMesh.indices = indices;
occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
}
else if(!occlusionImage.empty())
{
UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
}
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
occlusionMesh.indices = indices;
occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
}
else if(!occlusionImage.empty())
{
UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
}
}
#ifdef DEBUG_RENDERING_PERFORMANCE
@@ -1334,14 +1345,14 @@ int RTABMapApp::Render()
}
}
rtabmap::OdometryEvent odomEvent;
rtabmap::SensorEvent sensorEvent;
{
boost::mutex::scoped_lock lock(odomMutex_);
if(odomEvents_.size())
boost::mutex::scoped_lock lock(sensorMutex_);
if(sensorEvents_.size())
{
LOGI("Process odom events");
odomEvent = odomEvents_.back();
odomEvents_.clear();
LOGI("Process sensor events");
sensorEvent = sensorEvents_.back();
sensorEvents_.clear();
if(cameraJustInitialized_)
{
notifyCameraStarted = true;
@@ -1361,7 +1372,7 @@ int RTABMapApp::Render()
{
main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
}
if(camera_!=0 && cameraJustInitialized_)
if(sensorCaptureThread_!=0 && cameraJustInitialized_)
{
notifyCameraStarted = true;
cameraJustInitialized_ = false;
@@ -1562,9 +1573,9 @@ int RTABMapApp::Render()
if(clearSceneOnNextRender_)
{
LOGI("Clearing all rendering data...");
odomMutex_.lock();
odomEvents_.clear();
odomMutex_.unlock();
sensorMutex_.lock();
sensorEvents_.clear();
sensorMutex_.unlock();
poseMutex_.lock();
poseEvents_.clear();
@@ -1800,7 +1811,7 @@ int RTABMapApp::Render()
// Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2004,26 +2015,26 @@ int RTABMapApp::Render()
}
else
{
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0);
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
//just process the last one
if(!odomEvent.pose().isNull())
if(!sensorEvent.info().odomPose.isNull())
{
if(odomCloudShown_ && !trajectoryMode_)
{
if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty())
if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()))
if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
{
int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
}
else
{
//scan
cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
indices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
@@ -2034,10 +2045,10 @@ int RTABMapApp::Render()
if(cloud->size() && indices->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
main_scene_.setCloudVisible(-1, true);
}
else
@@ -2127,7 +2138,7 @@ int RTABMapApp::Render()
lastPostRenderEventTime_ = UTimer::now();
if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
{
UERROR("TangoPoseEventNotReceived");
UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
@@ -2319,7 +2330,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
void RTABMapApp::setGraphOptimization(bool enabled)
{
graphOptimization_ = enabled;
if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
{
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
@@ -3709,19 +3720,12 @@ void RTABMapApp::postCameraPoseEvent(
if(qx==0 && qy==0 && qz==0 && qw==0)
{
// Lost! clear buffer
poseBuffer_.clear();
camera_->resetOrigin(); // we are lost, create new session on next valid frame
return;
}
rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
camera_->poseReceived(pose);
poseBuffer_.insert(std::make_pair(stamp, pose));
if(poseBuffer_.size() > 1000)
{
poseBuffer_.erase(poseBuffer_.begin());
}
camera_->poseReceived(pose, stamp);
}
}
@@ -3833,66 +3837,41 @@ void RTABMapApp::postOdometryEvent(
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
rtabmap::Transform poseWithOriginOffset = pose;
if(!camera_->getOriginOffset().isNull())
{
poseWithOriginOffset = camera_->getOriginOffset() * pose;
}
// Registration depth to rgb
if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
{
UTimer time;
rtabmap::Transform motion = rtabmap::Transform::getIdentity();
if(depthStamp != stamp && !poseBuffer_.empty())
if(depthStamp != stamp)
{
// Interpolate pose
if(!poseBuffer_.empty())
rtabmap::Transform poseDepth;
cv::Mat cov;
if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
{
UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
}
else
{
if(poseBuffer_.rbegin()->first < depthStamp)
{
UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
}
else
{
std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
rtabmap::Transform poseDepth;
if(iterA != poseBuffer_.begin())
{
iterA = --iterA;
}
if(iterB == poseBuffer_.end())
{
iterB = --iterB;
}
if(iterA == iterB && depthStamp == iterA->first)
{
poseDepth = iterA->second;
}
else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
{
poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
}
else if(depthStamp < iterA->first)
{
UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
}
else
{
UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
}
if(!poseDepth.isNull())
{
#ifndef DISABLE_LOG
UDEBUG("poseRGB =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
#endif
motion = pose.inverse()*poseDepth;
// transform in camera frame
motion = poseWithOriginOffset.inverse()*poseDepth;
// transform in camera frame
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
#ifndef DISABLE_LOG
UDEBUG("motion=%s", motion.prettyPrint().c_str());
UDEBUG("motion=%s", motion.prettyPrint().c_str());
#endif
}
}
}
}
rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
@@ -3941,11 +3920,6 @@ void RTABMapApp::postOdometryEvent(
if(!outputDepth.empty())
{
rtabmap::Transform poseWithOriginOffset = pose;
if(!camera_->getOriginOffset().isNull())
{
poseWithOriginOffset = camera_->getOriginOffset() * pose;
}
rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
camera_->setOcclusionImage(outputDepth, depthModel);
@@ -3971,8 +3945,7 @@ void RTABMapApp::postOdometryEvent(
texCoords[5] = t5;
texCoords[6] = t6;
texCoords[7] = t7;
camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
camera_->spinOnce();
camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
}
}
}
@@ -3989,17 +3962,17 @@ void RTABMapApp::postOdometryEvent(
bool RTABMapApp::handleEvent(UEvent * event)
{
if(camera_!=0)
if(sensorCaptureThread_!=0)
{
// called from events manager thread, so protect the data
if(event->getClassName().compare("OdometryEvent") == 0)
if(event->getClassName().compare("SensorEvent") == 0)
{
LOGI("Received OdometryEvent!");
if(odomMutex_.try_lock())
LOGI("Received SensorEvent!");
if(sensorMutex_.try_lock())
{
odomEvents_.clear();
odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
odomMutex_.unlock();
sensorEvents_.clear();
sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
sensorMutex_.unlock();
}
}
if(event->getClassName().compare("RtabmapEvent") == 0)
app/android/jni/RTABMapApp.h
+6 -3
View File
@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "util.h"
#include "ProgressionStatus.h"
#include <rtabmap/core/SensorCaptureThread.h>
#include <rtabmap/core/RtabmapThread.h>
#include <rtabmap/core/SensorEvent.h>
#include <rtabmap/utilite/UEventsHandler.h>
#include <boost/thread/mutex.hpp>
#include <pcl/pcl_base.h>
@@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler {
private:
int cameraDriver_;
rtabmap::CameraMobile * camera_;
rtabmap::SensorCaptureThread * sensorCaptureThread_;
rtabmap::RtabmapThread * rtabmapThread_;
rtabmap::Rtabmap * rtabmap_;
rtabmap::LogHandler * logHandler_;
@@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler {
bool cameraColor_;
bool fullResolution_;
bool appendMode_;
bool useExternalLidar_;
float maxCloudDepth_;
float minCloudDepth_;
int cloudDensityLevel_;
@@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler {
UTimer fpsTime_;
std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
std::list<rtabmap::OdometryEvent> odomEvents_;
std::list<rtabmap::SensorEvent> sensorEvents_;
std::list<rtabmap::Transform> poseEvents_;
std::map<double, rtabmap::Transform> poseBuffer_;
rtabmap::Transform mapToOdom_;
boost::mutex cameraMutex_;
boost::mutex rtabmapMutex_;
boost::mutex meshesMutex_;
boost::mutex odomMutex_;
boost::mutex sensorMutex_;
boost::mutex poseMutex_;
boost::mutex renderingMutex_;
app/android/jni/background_renderer.cc
+3 -3
View File
@@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
}
void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");
GLuint program = shaderPrograms_[depthTexture>0?1:0];
@@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
else
#endif
glBindTexture(GL_TEXTURE_2D, texture_id_);
if(depthTexture>0)
{
// Texture activate unit 1
@@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
glEnableVertexAttribArray(attributeVertices);
app/android/jni/background_renderer.h
+7 -1
View File
@@ -28,9 +28,15 @@
#include "util.h"
static const GLfloat BackgroundRenderer_kVertices[] = {
static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
};
//static const GLfloat BackgroundRenderer_kVerticesView[] = {
// 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
//};
static const GLfloat BackgroundRenderer_kVerticesView[] = {
0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
};
static const GLfloat BackgroundRenderer_kTexCoord[] = {
1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
};
app/android/libs/.gitignore
+4 -1
View File
@@ -1 +1,4 @@
*.jar
# Ignore everything in this directory
*
# Except this file
!.gitignore
app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java
+15 -3
View File
@@ -115,6 +115,9 @@ public class ARCoreSharedCamera {
// Image reader that continuously processes CPU images.
public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
private boolean mTOFAvailable = false;
ByteBuffer mPreviousDepth = null;
double mPreviousDepthStamp = 0.0;
public boolean isDepthSupported() {return mTOFAvailable;}
@@ -757,7 +760,6 @@ public class ARCoreSharedCamera {
double stamp = (double)frame.getTimestamp()/10e8;
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
CameraIntrinsics intrinsics = camera.getImageIntrinsics();
try{
Image image = frame.acquireCameraImage();
@@ -813,6 +815,12 @@ public class ARCoreSharedCamera {
depth = mTOFImageReader.depth16_raw;
depthStamp = (double)mTOFImageReader.timestamp/10e8;
}
if(mPreviousDepth == null)
{
mPreviousDepth = depth;
mPreviousDepthStamp = depthStamp;
}
if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
@@ -825,13 +833,17 @@ public class ARCoreSharedCamera {
rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
stamp,
depthStamp,
depthStamp>stamp?mPreviousDepthStamp:depthStamp,
y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(),
depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
points, points.limit()/4,
viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
p[0], p[5], p[8], p[9], p[10], p[11], p[14],
texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
mPreviousDepthStamp = depthStamp;
mPreviousDepth = depth;
}
else
{
app/ios/RTABMapApp/NativeWrapper.cpp
+2 -2
View File
@@ -284,11 +284,11 @@ void setCameraNative(const void *object, int type) {
}
void postCameraPoseEventNative(const void *object,
float x, float y, float z, float qx, float qy, float qz, float qw)
float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
{
if(object)
{
native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0);
native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp);
}
else
{
app/ios/RTABMapApp/NativeWrapper.hpp
+1 -1
View File
@@ -67,7 +67,7 @@ bool startCameraNative(const void *object);
void stopCameraNative(const void *object);
void setCameraNative(const void *object, int type);
void postCameraPoseEventNative(const void *object,
float x, float y, float z, float qx, float qy, float qz, float qw);
float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
void postOdometryEventNative(const void *object,
float x, float y, float z, float qx, float qy, float qz, float qw,
float fx, float fy, float cx, float cy,
app/ios/RTABMapApp/RTABMap.swift
+4 -4
View File
@@ -216,18 +216,18 @@ class RTABMap {
setCameraNative(native_rtabmap, Int32(type))
}
func postCameraPoseEvent(pose: simd_float4x4) {
func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) {
let rotation = GLKMatrix3(
m: (pose[0,0], pose[0,1], pose[0,2],
pose[1,0], pose[1,1], pose[1,2],
pose[2,0], pose[2,1], pose[2,2]))
let quat = GLKQuaternionMakeWithMatrix3(rotation)
postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp)
}
func notifyLost() {
// a null transform will make rtabmap creating a new session
postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0)
postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0)
}
func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
@@ -239,7 +239,7 @@ class RTABMap {
let quat = GLKQuaternionMakeWithMatrix3(rotation)
postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp)
let confMap = frame.sceneDepth?.confidenceMap
let depthMap = frame.sceneDepth?.depthMap
corelib/include/rtabmap/core/Camera.h
+19 -37
View File
@@ -1,5 +1,5 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
@@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/CameraInfo.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
class UTimer;
#include <rtabmap/core/SensorCapture.h>
#include <rtabmap/core/IMU.h>
namespace rtabmap
{
class IMUFilter;
/**
* Class Camera
*
*/
class RTABMAP_CORE_EXPORT Camera
class RTABMAP_CORE_EXPORT Camera : public SensorCapture
{
public:
virtual ~Camera();
SensorData takeImage(CameraInfo * info = 0);
SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
float getImageRate() const {return getFrameRate();}
void setImageRate(float imageRate) {setFrameRate(imageRate);}
void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
bool isInterIMUPublishing() const {return publishInterIMU_;}
bool initFromFile(const std::string & calibrationPath);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
virtual bool isCalibrated() const = 0;
virtual std::string getSerial() const = 0;
virtual bool odomProvided() const { return false; }
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
//getters
float getImageRate() const {return _imageRate;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setImageRate(float imageRate) {_imageRate = imageRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void resetTimer();
protected:
/**
* Constructor
@@ -78,19 +63,16 @@ protected:
*/
Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage(CameraInfo * info = 0) = 0;
virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;
int getNextSeqID() {return ++_seq;}
void postInterIMU(const IMU & imu, double stamp);
private:
float _imageRate;
Transform _localTransform;
cv::Size _targetImageSize;
UTimer * _frameRateTimer;
int _seq;
virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);}
private:
IMUFilter * imuFilter_;
bool publishInterIMU_;
};
corelib/include/rtabmap/core/CameraEvent.h
+1 -62
View File
@@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <rtabmap/utilite/UEvent.h>
#include "rtabmap/core/SensorData.h"
#include "rtabmap/core/CameraInfo.h"
namespace rtabmap
{
class CameraEvent :
public UEvent
{
public:
enum Code {
kCodeData,
kCodeNoMoreImages
};
public:
CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
UEvent(kCodeData),
data_(image, seq, stamp)
{
cameraInfo_.cameraName = cameraName;
}
CameraEvent() :
UEvent(kCodeNoMoreImages)
{
}
CameraEvent(const SensorData & data) :
UEvent(kCodeData),
data_(data)
{
}
CameraEvent(const SensorData & data, const std::string & cameraName) :
UEvent(kCodeData),
data_(data)
{
cameraInfo_.cameraName = cameraName;
}
CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
UEvent(kCodeData),
data_(data),
cameraInfo_(cameraInfo)
{
}
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return cameraInfo_.cameraName;}
const CameraInfo & info() const {return cameraInfo_;}
virtual ~CameraEvent() {}
virtual std::string getClassName() const {return std::string("CameraEvent");}
private:
SensorData data_;
CameraInfo cameraInfo_;
};
} // namespace rtabmap
#include "rtabmap/core/SensorEvent.h"
corelib/include/rtabmap/core/CameraInfo.h
+1 -47
View File
@@ -27,50 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include <string>
namespace rtabmap
{
class CameraInfo
{
public:
CameraInfo() :
cameraName(""),
id(0),
stamp(0.0),
timeCapture(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f),
timeHistogramEqualization(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeBilateralFiltering(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
virtual ~CameraInfo() {}
std::string cameraName;
int id;
double stamp;
float timeCapture;
float timeDisparity;
float timeMirroring;
float timeStereoExposureCompensation;
float timeImageDecimation;
float timeHistogramEqualization;
float timeScanFromDepth;
float timeUndistortDepth;
float timeBilateralFiltering;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
std::vector<float> odomVelocity;
};
} // namespace rtabmap
#include "rtabmap/core/SensorCaptureInfo.h"
corelib/include/rtabmap/core/CameraThread.h
+1 -133
View File
@@ -27,136 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
class Camera;
class CameraInfo;
class SensorData;
class StereoDense;
class IMUFilter;
class Feature2D;
/**
* Class CameraThread
*
*/
class RTABMAP_CORE_EXPORT CameraThread :
public UThread,
public UEventsSender
{
public:
// ownership transferred
CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
/**
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose
* @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
*/
CameraThread(Camera * camera,
Camera * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
bool odomAsGt = false,
const ParametersMap & parameters = ParametersMap());
CameraThread(Camera * camera,
bool odomAsGt,
const ParametersMap & parameters = ParametersMap());
virtual ~CameraThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setImageRate(float imageRate);
void setDistortionModel(const std::string & path);
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
void disableIMUFiltering();
void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
void disableFeatureDetection();
// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
RTABMAP_DEPRECATED void setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
bool forceGroundNormalsUp);
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f,
float groundNormalsUp = 0.0f);
void postUpdate(SensorData * data, CameraInfo * info = 0) const;
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
bool odomProvided() const;
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
private:
Camera * _camera;
Camera * _odomSensor;
Transform _extrinsicsOdomToCamera;
bool _odomAsGt;
double _poseTimeOffset;
float _poseScaleFactor;
bool _mirroring;
bool _stereoExposureCompensation;
bool _colorOnly;
int _imageDecimation;
int _histogramMethod;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
float _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
Feature2D * _featureDetector;
bool _depthAsMask;
};
} // namespace rtabmap
#include "rtabmap/core/SensorCaptureThread.h"
corelib/include/rtabmap/core/DBReader.h
+2 -2
View File
@@ -86,10 +86,10 @@ public:
const DBDriver * driver() const {return _dbDriver;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
SensorData getNextData(CameraInfo * info = 0);
SensorData getNextData(SensorCaptureInfo * info = 0);
private:
std::list<std::string> _paths;
corelib/include/rtabmap/core/IMUFilter.h
+2 -1
View File
@@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <Eigen/Geometry>
namespace rtabmap {
class IMUFilter
class RTABMAP_CORE_EXPORT IMUFilter
{
public:
enum Type {
corelib/include/rtabmap/core/LaserScan.h
+9 -2
View File
@@ -47,7 +47,8 @@ public:
kXYZRGB=7,
kXYZNormal=8,
kXYZINormal=9,
kXYZRGBNormal=10};
kXYZRGBNormal=10,
kXYZIT=11};
static std::string formatName(const Format & format);
static int channels(const Format & format);
@@ -55,6 +56,7 @@ public:
static bool isScanHasNormals(const Format & format);
static bool isScanHasRGB(const Format & format);
static bool isScanHasIntensity(const Format & format);
static bool isScanHasTime(const Format & format);
static LaserScan backwardCompatibility(
const cv::Mat & oldScanFormat,
int maxPoints = 0,
@@ -121,22 +123,27 @@ public:
float angleMin() const {return angleMin_;}
float angleMax() const {return angleMax_;}
float angleIncrement() const {return angleIncrement_;}
void setLocalTransform(const Transform & t) {localTransform_ = t;}
Transform localTransform() const {return localTransform_;}
bool empty() const {return data_.empty();}
bool isEmpty() const {return data_.empty();}
int size() const {return data_.cols;}
int size() const {return data_.total();}
int dataType() const {return data_.type();}
bool is2d() const {return isScan2d(format_);}
bool hasNormals() const {return isScanHasNormals(format_);}
bool hasRGB() const {return isScanHasRGB(format_);}
bool hasIntensity() const {return isScanHasIntensity(format_);}
bool hasTime() const {return isScanHasTime(format_);}
bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
bool isOrganized() const {return data_.rows > 1;}
LaserScan clone() const;
LaserScan densify() const;
int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
int getTimeOffset() const {return hasTime()?4:-1;}
float & field(unsigned int pointIndex, unsigned int channelOffset);
corelib/include/rtabmap/core/Lidar.h
+57
View File
@@ -0,0 +1,57 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/SensorCapture.h>
namespace rtabmap
{
/**
* Class Lidar
*
*/
class RTABMAP_CORE_EXPORT Lidar : public SensorCapture
{
public:
virtual ~Lidar() {}
protected:
/**
* Constructor
*
* @param lidarRate the frame rate (Hz), 0 for fast as the lidar can
* @param localTransform the transform from base frame to lidar frame
*/
Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
SensorCapture(lidarRate, localTransform) {}
};
} // namespace rtabmap
corelib/include/rtabmap/core/Odometry.h
+1
View File
@@ -111,6 +111,7 @@ private:
bool _alignWithGround;
bool _publishRAMUsage;
bool _imagesAlreadyRectified;
bool _deskewing;
Transform _pose;
int _resetCurrentCount;
double previousStamp_;
corelib/include/rtabmap/core/OdometryInfo.h
+3
View File
@@ -50,6 +50,7 @@ public:
localBundleConstraints(0),
localBundleTime(0),
keyFrameAdded(false),
timeDeskewing(0.0f),
timeEstimation(0.0f),
timeParticleFiltering(0.0f),
stamp(0),
@@ -76,6 +77,7 @@ public:
output.localBundlePoses = localBundlePoses;
output.localBundleModels = localBundleModels;
output.keyFrameAdded = keyFrameAdded;
output.timeDeskewing = timeDeskewing;
output.timeEstimation = timeEstimation;
output.timeParticleFiltering = timeParticleFiltering;
output.stamp = stamp;
@@ -105,6 +107,7 @@ public:
std::map<int, Transform> localBundlePoses;
std::map<int, std::vector<CameraModel> > localBundleModels;
bool keyFrameAdded;
float timeDeskewing;
float timeEstimation;
float timeParticleFiltering;
double stamp;
corelib/include/rtabmap/core/Parameters.h
+3
View File
@@ -463,6 +463,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Odom, ScanKeyFrameThr, float, 0.9, "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
RTABMAP_PARAM(Odom, ImageDecimation, unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
RTABMAP_PARAM(Odom, AlignWithGround, bool, false, "Align odometry with the ground on initialization.");
RTABMAP_PARAM(Odom, Deskewing, bool, true, "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");
// Odometry Frame-to-Map
RTABMAP_PARAM(OdomF2M, MaxSize, int, 2000, "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -752,6 +753,7 @@ class RTABMAP_CORE_EXPORT Parameters
RTABMAP_PARAM(Icp, Epsilon, float, 0, "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
RTABMAP_PARAM(Icp, CorrespondenceRatio, float, 0.1, "Ratio of matching correspondences to accept the transform.");
RTABMAP_PARAM(Icp, Force4DoF, bool, false, uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
RTABMAP_PARAM(Icp, FiltersEnabled, int, 3, "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
#ifdef RTABMAP_POINTMATCHER
RTABMAP_PARAM(Icp, PointToPlane, bool, true, "Use point to plane ICP.");
#else
@@ -931,6 +933,7 @@ public:
static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);
static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false);
static void writeINI(const std::string & configFile, const ParametersMap & parameters);
/**
corelib/include/rtabmap/core/RegistrationIcp.h
+1
View File
@@ -69,6 +69,7 @@ private:
float _epsilon;
float _correspondenceRatio;
bool _force4DoF;
int _filtersEnabled;
bool _pointToPlane;
int _pointToPlaneK;
float _pointToPlaneRadius;
corelib/include/rtabmap/core/SensorCapture.h
+93
View File
@@ -0,0 +1,93 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/core/SensorCaptureInfo.h>
#include "rtabmap/core/SensorData.h"
#include <set>
#include <stack>
#include <list>
#include <vector>
class UDirectory;
class UTimer;
namespace rtabmap
{
/**
* Class Camera
*
*/
class RTABMAP_CORE_EXPORT SensorCapture
{
public:
virtual ~SensorCapture();
SensorData takeData(SensorCaptureInfo * info = 0);
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
virtual std::string getSerial() const = 0;
virtual bool odomProvided() const { return false; }
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
//getters
float getFrameRate() const {return _frameRate;}
const Transform & getLocalTransform() const {return _localTransform;}
//setters
void setFrameRate(float frameRate) {_frameRate = frameRate;}
void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
void resetTimer();
protected:
/**
* Constructor
*
* @param frameRate the frame rate (Hz), 0 for fast as the sensor can
* @param localTransform the transform from base frame to sensor frame
*/
SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
int getNextSeqID() {return ++_seq;}
private:
float _frameRate;
Transform _localTransform;
UTimer * _frameRateTimer;
int _seq;
};
} // namespace rtabmap
corelib/include/rtabmap/core/SensorCaptureInfo.h
+82
View File
@@ -0,0 +1,82 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/Transform.h"
#include <string>
namespace rtabmap
{
class SensorCaptureInfo
{
public:
SensorCaptureInfo() :
cameraName(""),
id(0),
stamp(0.0),
timeCapture(0.0f),
timeDeskewing(0.0f),
timeDisparity(0.0f),
timeMirroring(0.0f),
timeStereoExposureCompensation(0.0f),
timeImageDecimation(0.0f),
timeHistogramEqualization(0.0f),
timeScanFromDepth(0.0f),
timeUndistortDepth(0.0f),
timeBilateralFiltering(0.0f),
timeTotal(0.0f),
odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
{
}
virtual ~SensorCaptureInfo() {}
std::string cameraName;
int id;
double stamp;
float timeCapture;
float timeDeskewing;
float timeDisparity;
float timeMirroring;
float timeStereoExposureCompensation;
float timeImageDecimation;
float timeHistogramEqualization;
float timeScanFromDepth;
float timeUndistortDepth;
float timeBilateralFiltering;
float timeTotal;
Transform odomPose;
cv::Mat odomCovariance;
std::vector<float> odomVelocity;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
} // namespace rtabmap
corelib/include/rtabmap/core/SensorCaptureThread.h
+216
View File
@@ -0,0 +1,216 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
#include <rtabmap/core/Parameters.h>
#include <rtabmap/core/Transform.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace clams
{
class DiscreteDepthDistortionModel;
}
namespace rtabmap
{
class Camera;
class Lidar;
class SensorCapture;
class SensorCaptureInfo;
class SensorData;
class StereoDense;
class IMUFilter;
class Feature2D;
/**
* Class CameraThread
*
*/
class RTABMAP_CORE_EXPORT SensorCaptureThread :
public UThread,
public UEventsSender
{
public:
// ownership transferred
SensorCaptureThread(
Camera * camera,
const ParametersMap & parameters = ParametersMap());
/**
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose (can be again the camera)
* @param odomAsGt set odometry sensor pose as ground truth instead of odometry
* @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame (without optical rotation)
*/
SensorCaptureThread(
Camera * camera,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
*/
SensorCaptureThread(
Lidar * lidar,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
*/
SensorCaptureThread(
Lidar * lidar,
Camera * camera,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
*/
SensorCaptureThread(
Lidar * lidar,
SensorCapture * odomSensor,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
/**
* @param lidar the lidar to take scans from
* @param camera the camera to take images from
* @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
* @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
*/
SensorCaptureThread(
Lidar * lidar,
Camera * camera,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset = 0.0,
float poseScaleFactor = 1.0f,
double poseWaitTime = 0.1,
const ParametersMap & parameters = ParametersMap());
virtual ~SensorCaptureThread();
void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
void setImageDecimation(int decimation) {_imageDecimation = decimation;}
void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
void setFrameRate(float frameRate);
RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
void setDistortionModel(const std::string & path);
void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
void disableIMUFiltering();
void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
void disableFeatureDetection();
// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
RTABMAP_DEPRECATED void setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
float normalsRadius,
bool forceGroundNormalsUp,
bool deskewing);
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
float normalsRadius = 0.0f,
float groundNormalsUp = 0.0f,
bool deskewing = false);
void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
//getters
bool isPaused() const {return !this->isRunning();}
bool isCapturing() const {return this->isRunning();}
bool odomProvided() const;
Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
private:
virtual void mainLoopBegin();
virtual void mainLoop();
virtual void mainLoopKill();
private:
Camera * _camera;
SensorCapture * _odomSensor;
Lidar * _lidar;
Transform _extrinsicsOdomToCamera;
bool _odomAsGt;
double _poseTimeOffset;
float _poseScaleFactor;
double _poseWaitTime;
bool _mirroring;
bool _stereoExposureCompensation;
bool _colorOnly;
int _imageDecimation;
int _histogramMethod;
bool _stereoToDepth;
bool _scanDeskewing;
bool _scanFromDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
float _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;
float _bilateralSigmaS;
float _bilateralSigmaR;
IMUFilter * _imuFilter;
bool _imuBaseFrameConversion;
Feature2D * _featureDetector;
bool _depthAsMask;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
} // namespace rtabmap
corelib/include/rtabmap/core/SensorEvent.h
+94
View File
@@ -0,0 +1,94 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <rtabmap/core/SensorCaptureInfo.h>
#include <rtabmap/utilite/UEvent.h>
#include "rtabmap/core/SensorData.h"
namespace rtabmap
{
class SensorEvent :
public UEvent
{
public:
enum Code {
kCodeData,
kCodeNoMoreImages
};
public:
SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
UEvent(kCodeData),
data_(image, seq, stamp)
{
sensorCaptureInfo_.cameraName = cameraName;
}
SensorEvent() :
UEvent(kCodeNoMoreImages)
{
}
SensorEvent(const SensorData & data) :
UEvent(kCodeData),
data_(data)
{
}
SensorEvent(const SensorData & data, const std::string & cameraName) :
UEvent(kCodeData),
data_(data)
{
sensorCaptureInfo_.cameraName = cameraName;
}
SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
UEvent(kCodeData),
data_(data),
sensorCaptureInfo_(sensorCaptureInfo)
{
}
// Image or descriptors
const SensorData & data() const {return data_;}
const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
virtual ~SensorEvent() {}
virtual std::string getClassName() const {return std::string("SensorEvent");}
private:
SensorData data_;
SensorCaptureInfo sensorCaptureInfo_;
};
//backward compatibility
RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
} // namespace rtabmap
corelib/include/rtabmap/core/camera/CameraDepthAI.h
+1 -1
View File
@@ -73,7 +73,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_DEPTHAI
corelib/include/rtabmap/core/camera/CameraFreenect.h
+1 -1
View File
@@ -61,7 +61,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT
corelib/include/rtabmap/core/camera/CameraFreenect2.h
+1 -1
View File
@@ -77,7 +77,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FREENECT2
corelib/include/rtabmap/core/camera/CameraImages.h
+1 -1
View File
@@ -118,7 +118,7 @@ public:
}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
bool readPoses(
corelib/include/rtabmap/core/camera/CameraK4A.h
+1 -1
View File
@@ -63,7 +63,7 @@ public:
void setPreferences(int rgb_resolution, int framerate, int depth_resolution);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
void close();
corelib/include/rtabmap/core/camera/CameraK4W2.h
+1 -1
View File
@@ -72,7 +72,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
void close();
corelib/include/rtabmap/core/camera/CameraMyntEye.h
+1 -1
View File
@@ -67,7 +67,7 @@ protected:
/**
* returned rgb and depth images should be already rectified if calibration was loaded
*/
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_MYNTEYE
corelib/include/rtabmap/core/camera/CameraOpenNI2.h
+1 -1
View File
@@ -69,7 +69,7 @@ public:
void setDepthDecimation(int decimation);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_OPENNI2
corelib/include/rtabmap/core/camera/CameraOpenNICV.h
+1 -1
View File
@@ -51,7 +51,7 @@ public:
virtual std::string getSerial() const {return "";} // unknown with OpenCV
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
bool _asus;
corelib/include/rtabmap/core/camera/CameraOpenni.h
+1 -1
View File
@@ -85,7 +85,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
pcl::Grabber* interface_;
corelib/include/rtabmap/core/camera/CameraRGBDImages.h
+1 -1
View File
@@ -53,7 +53,7 @@ public:
virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
CameraImages cameraDepth_;
corelib/include/rtabmap/core/camera/CameraRealSense.h
+1 -1
View File
@@ -72,7 +72,7 @@ public:
virtual bool odomProvided() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE
corelib/include/rtabmap/core/camera/CameraRealSense2.h
+3 -4
View File
@@ -68,7 +68,7 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);
// parameters are set during initialization
// D400 series
@@ -77,7 +77,7 @@ public:
void setResolution(int width, int height, int fps = 30);
void setDepthResolution(int width, int height, int fps = 30);
void setGlobalTimeSync(bool enabled);
void publishInterIMU(bool enabled);
/**
* Dual mode (D400+T265 or L500+T265)
* @param enabled enable dual mode
@@ -105,7 +105,7 @@ private:
#endif
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_REALSENSE2
@@ -142,7 +142,6 @@ private:
int cameraDepthHeight_;
int cameraDepthFps_;
bool globalTimeSync_;
bool publishInterIMU_;
bool dualMode_;
Transform dualExtrinsics_;
std::string jsonConfig_;
corelib/include/rtabmap/core/camera/CameraStereoDC1394.h
+1 -1
View File
@@ -51,7 +51,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_DC1394
corelib/include/rtabmap/core/camera/CameraStereoFlyCapture2.h
+1 -1
View File
@@ -53,7 +53,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_FLYCAPTURE2
corelib/include/rtabmap/core/camera/CameraStereoImages.h
+1 -1
View File
@@ -64,7 +64,7 @@ public:
virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
CameraImages * camera2_;
corelib/include/rtabmap/core/camera/CameraStereoTara.h
+1 -1
View File
@@ -60,7 +60,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
cv::VideoCapture capture_;
corelib/include/rtabmap/core/camera/CameraStereoVideo.h
+1 -1
View File
@@ -71,7 +71,7 @@ public:
void setResolution(int width, int height) {_width=width, _height=height;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
cv::VideoCapture capture_;
corelib/include/rtabmap/core/camera/CameraStereoZed.h
+3 -4
View File
@@ -76,12 +76,12 @@ public:
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
virtual bool odomProvided() const;
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);
void publishInterIMU(bool enabled);
void postInterIMUPublic(const IMU & imu, double stamp);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_ZED
@@ -100,7 +100,6 @@ private:
bool computeOdometry_;
bool lost_;
bool force3DoF_;
bool publishInterIMU_;
ZedIMUThread * imuPublishingThread_;
#endif
};
corelib/include/rtabmap/core/camera/CameraStereoZedOC.h
+1 -1
View File
@@ -63,7 +63,7 @@ public:
virtual std::string getSerial() const;
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
#ifdef RTABMAP_ZEDOC
corelib/include/rtabmap/core/camera/CameraVideo.h
+1 -1
View File
@@ -64,7 +64,7 @@ public:
void setResolution(int width, int height) {_width=width, _height=height;}
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
virtual SensorData captureImage(SensorCaptureInfo * info = 0);
private:
// File type
corelib/include/rtabmap/core/lidar/LidarVLP16.h
+94
View File
@@ -0,0 +1,94 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
// Should be first on windows to avoid "WinSock.h has already been included" error
#include <pcl/io/vlp_grabber.h>
#include <rtabmap/core/Lidar.h>
#include <rtabmap/utilite/USemaphore.h>
namespace rtabmap {
struct PointXYZIT {
float x;
float y;
float z;
float i;
float t;
};
class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber {
public:
LidarVLP16(
const std::string& pcapFile,
bool organized = false,
bool stampLast = true,
float frameRate = 0.0f,
Transform localTransform = Transform::getIdentity());
LidarVLP16(
const boost::asio::ip::address& ipAddress,
const std::uint16_t port = 2368,
bool organized = false,
bool useHostTime = true,
bool stampLast = true,
float frameRate = 0.0f,
Transform localTransform = Transform::getIdentity());
virtual ~LidarVLP16();
SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);}
virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override;
virtual std::string getSerial() const override {return getName();}
void setOrganized(bool enable);
private:
void buildTimings(bool dualMode);
virtual void toPointClouds (HDLDataPacket *dataPacket) override;
protected:
virtual SensorData captureData(SensorCaptureInfo * info = 0) override;
private:
// timing offset lookup table
std::vector< std::vector<float> > timingOffsets_;
bool timingOffsetsDualMode_;
double startSweepTime_;
double startSweepTimeHost_;
bool organized_;
bool useHostTime_;
bool stampLast_;
SensorData lastScan_;
std::vector<std::vector<PointXYZIT> > accumulatedScans_;
USemaphore scanReady_;
UMutex lastScanMutex_;
};
} /* namespace rtabmap */
#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ */
corelib/include/rtabmap/core/util3d.h
+13
View File
@@ -455,6 +455,19 @@ RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadC
int downsampleStep = 1,
float voxelSize = 0.0f);
/**
* @brief Lidar deskewing
* @param input lidar, format should have time channel
* @param input stamp of the lidar
* @param velocity in base frame
* @param velocity stamp at which it has been computed
* @return lidar deskewed
*/
LaserScan RTABMAP_CORE_EXPORT deskew(
const LaserScan & input,
double inputStamp,
const rtabmap::Transform & velocity);
} // namespace util3d
} // namespace rtabmap
corelib/src/CMakeLists.txt
+5 -1
View File
@@ -13,8 +13,10 @@ SET(SRC_FILES
Recovery.cpp
SensorCapture.cpp
SensorCaptureThread.cpp
Camera.cpp
CameraThread.cpp
CameraModel.cpp
camera/CameraFreenect.cpp
@@ -39,6 +41,8 @@ SET(SRC_FILES
camera/CameraMyntEye.cpp
camera/CameraDepthAI.cpp
lidar/LidarVLP16.cpp
EpipolarGeometry.cpp
VisualWord.cpp
VWDictionary.cpp
corelib/src/Camera.cpp
+29 -68
View File
@@ -26,42 +26,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/IMUFilter.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <cmath>
#include <rtabmap/utilite/UEventsManager.h>
namespace rtabmap
{
Camera::Camera(float imageRate, const Transform & localTransform) :
_imageRate(imageRate),
_localTransform(localTransform*CameraModel::opticalRotation()),
_targetImageSize(0,0),
_frameRateTimer(new UTimer()),
_seq(0)
{
}
SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
imuFilter_(0),
publishInterIMU_(false)
{}
Camera::~Camera()
{
UDEBUG("");
delete _frameRateTimer;
UDEBUG("");
}
void Camera::resetTimer()
{
_frameRateTimer->start();
delete imuFilter_;
}
bool Camera::initFromFile(const std::string & calibrationPath)
@@ -69,54 +52,32 @@ bool Camera::initFromFile(const std::string & calibrationPath)
return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
}
SensorData Camera::takeImage(CameraInfo * info)
void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
{
bool warnFrameRateTooHigh = false;
float actualFrameRate = 0;
float imageRate = _imageRate;
if(imageRate>0)
{
int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
else if(sleepTime < 0)
{
warnFrameRateTooHigh = true;
actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
}
publishInterIMU_ = enabled;
delete imuFilter_;
imuFilter_ = filter;
}
// Add precision at the cost of a small overhead
while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
{
//
}
double slept = _frameRateTimer->getElapsedTime();
_frameRateTimer->start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
}
UTimer timer;
SensorData data = this->captureImage(info);
double captureTime = timer.ticks();
if(warnFrameRateTooHigh)
void Camera::postInterIMU(const IMU & imu, double stamp)
{
if(imuFilter_)
{
UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
imageRate, actualFrameRate, captureTime);
imuFilter_->update(
imu.angularVelocity()[0], imu.angularVelocity()[1], imu.angularVelocity()[2],
imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2],
stamp);
cv::Vec4d q;
imuFilter_->getOrientation(q[0],q[1],q[2],q[3]);
UEventsManager::post(new IMUEvent(IMU(
q, cv::Mat(),
imu.angularVelocity(), imu.angularVelocityCovariance(),
imu.linearAcceleration(), imu.linearAccelerationCovariance(),
imu.localTransform()),
stamp));
return;
}
else
{
UDEBUG("Time capturing image = %fs", captureTime);
}
if(info)
{
info->id = data.id();
info->stamp = data.stamp();
info->timeCapture = captureTime;
}
return data;
UEventsManager::post(new IMUEvent(imu, stamp));
}
} // namespace rtabmap
corelib/src/DBReader.cpp
+3 -3
View File
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/SensorEvent.h>
#include "rtabmap/core/DBReader.h"
#include "rtabmap/core/DBDriver.h"
@@ -34,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/util3d.h"
@@ -268,7 +268,7 @@ std::string DBReader::getSerial() const
return "DBReader";
}
SensorData DBReader::captureImage(CameraInfo * info)
SensorData DBReader::captureImage(SensorCaptureInfo * info)
{
SensorData data = this->getNextData(info);
if(data.id()>0 && _stopId>0 && data.id() > _stopId)
@@ -370,7 +370,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
return data;
}
SensorData DBReader::getNextData(CameraInfo * info)
SensorData DBReader::getNextData(SensorCaptureInfo * info)
{
SensorData data;
if(_dbDriver)
corelib/src/LaserScan.cpp
+97 -19
View File
@@ -65,6 +65,9 @@ std::string LaserScan::formatName(const Format & format)
case kXYZRGBNormal:
name = "XYZRGBNormal";
break;
case kXYZIT:
name = "XYZIT";
break;
default:
name = "Unknown";
break;
@@ -88,6 +91,7 @@ int LaserScan::channels(const Format & format)
channels = 4;
break;
case kXYNormal:
case kXYZIT:
channels = 5;
break;
case kXYZNormal:
@@ -119,7 +123,11 @@ bool LaserScan::isScanHasRGB(const Format & format)
}
bool LaserScan::isScanHasIntensity(const Format & format)
{
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
}
bool LaserScan::isScanHasTime(const Format & format)
{
return format==kXYZIT;
}
LaserScan LaserScan::backwardCompatibility(
@@ -213,7 +221,14 @@ LaserScan::LaserScan(
const LaserScan & scan,
int maxPoints,
float maxRange,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
UASSERT(scan.empty() || scan.format() != kUnknown);
init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
@@ -224,7 +239,14 @@ LaserScan::LaserScan(
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
}
@@ -234,7 +256,14 @@ LaserScan::LaserScan(
int maxPoints,
float maxRange,
Format format,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
}
@@ -246,7 +275,14 @@ LaserScan::LaserScan(
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
UASSERT(scan.empty() || scan.format() != kUnknown);
init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
@@ -260,7 +296,14 @@ LaserScan::LaserScan(
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
}
@@ -273,7 +316,14 @@ LaserScan::LaserScan(
float angleMin,
float angleMax,
float angleIncrement,
const Transform & localTransform)
const Transform & localTransform) :
format_(kUnknown),
maxPoints_(0),
rangeMin_(0),
rangeMax_(0),
angleMin_(0),
angleMax_(0),
angleIncrement_(0)
{
init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
}
@@ -289,8 +339,7 @@ void LaserScan::init(
int maxPoints,
const Transform & localTransform)
{
UASSERT(data.empty() || data.rows == 1);
UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6) || data.type() == CV_32FC(7));
UASSERT(!localTransform.isNull());
bool is2D = false;
@@ -307,6 +356,10 @@ void LaserScan::init(
// 3D scan
UASSERT(rangeMax>=rangeMin);
maxPoints_ = maxPoints;
if(maxPoints_ == 0 && data.rows>1)
{
maxPoints_ = data.rows * data.cols;
}
}
data_ = data;
@@ -320,18 +373,18 @@ void LaserScan::init(
if(!data.empty() && !isCompressed())
{
if(is2D && data_.cols > maxPoints_)
if(is2D && (int)data_.total() > maxPoints_)
{
UWARN("The number of points (%d) in the scan is over the maximum "
UWARN("The number of points (%ld) in the scan is over the maximum "
"points (%d) defined by angle settings (min=%f max=%f inc=%f). "
"The scan info may be wrong!",
data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
data_.total(), maxPoints_, angleMin_, angleMax_, angleIncrement_);
}
else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_)
{
UDEBUG("The number of points (%d) in the scan is over the maximum "
UDEBUG("The number of points (%ld) in the scan is over the maximum "
"points (%d) defined by max points setting.",
data_.cols, maxPoints_);
data_.total(), maxPoints_);
}
if(format == kUnknown)
@@ -350,7 +403,7 @@ void LaserScan::init(
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
}
@@ -366,11 +419,36 @@ LaserScan LaserScan::clone() const
return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
}
LaserScan LaserScan::densify() const
{
if(!isOrganized())
{
return *this;
}
cv::Mat output(1, data_.total(), data_.type());
int oi = 0;
for(int i=0; i<data_.rows; ++i)
{
for(int j=0; j<data_.cols; ++j)
{
const float * ptr = data_.ptr<float>(i, j);
float * outputPtr = output.ptr<float>(0, oi);
if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2]))))
{
memcpy(outputPtr, ptr, data_.elemSize());
++oi;
}
}
}
return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone());
}
float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
{
UASSERT(pointIndex < (unsigned int)data_.cols);
UASSERT(pointIndex < (unsigned int)data_.total());
UASSERT(channelOffset < (unsigned int)data_.channels());
return data_.ptr<float>(0, pointIndex)[channelOffset];
unsigned int row = pointIndex / data_.cols;
return data_.ptr<float>(row, pointIndex - row * data_.cols)[channelOffset];
}
LaserScan & LaserScan::operator+=(const LaserScan & scan)
@@ -381,7 +459,7 @@ LaserScan & LaserScan::operator+=(const LaserScan & scan)
LaserScan LaserScan::operator+(const LaserScan & scan)
{
UASSERT(this->empty() || scan.empty() || this->format() == scan.format());
UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized()));
LaserScan dest;
if(!scan.empty())
{
corelib/src/Odometry.cpp
+71
View File
@@ -140,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
_alignWithGround(Parameters::defaultOdomAlignWithGround()),
_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
_deskewing(Parameters::defaultOdomDeskewing()),
_pose(Transform::getIdentity()),
_resetCurrentCount(0),
previousStamp_(0),
@@ -169,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing);
if(_imageDecimation == 0)
{
@@ -620,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
UTimer time;
// Deskewing lidar
if( _deskewing &&
!data.laserScanRaw().empty() &&
data.laserScanRaw().hasTime() &&
dt > 0 &&
!guess.isNull())
{
UDEBUG("Deskewing begin");
// Recompute velocity
float vx,vy,vz, vroll,vpitch,vyaw;
guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// transform to velocity
vx /= dt;
vy /= dt;
vz /= dt;
vroll /= dt;
vpitch /= dt;
vyaw /= dt;
if(!imus_.empty())
{
float scanTime =
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
// replace orientation velocity based on IMU (if available)
Transform imuFirstScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
Transform imuLastScan = Transform::getTransform(imus_,
data.stamp() +
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
if(!imuFirstScan.isNull() && !imuLastScan.isNull())
{
Transform orientation = imuFirstScan.inverse() * imuLastScan;
orientation.getEulerAngles(vroll, vpitch, vyaw);
if(_force3DoF)
{
vroll=0;
vpitch=0;
vyaw /= scanTime;
}
else
{
vroll /= scanTime;
vpitch /= scanTime;
vyaw /= scanTime;
}
}
}
Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
if(!scanDeskewed.isEmpty())
{
data.setLaserScan(scanDeskewed);
}
info->timeDeskewing = time.ticks();
UDEBUG("Deskewing end");
}
if(data.laserScanRaw().isOrganized())
{
// Laser scans should be dense passing this point
data.setLaserScan(data.laserScanRaw().densify());
}
Transform t;
if(_imageDecimation > 1 && !data.imageRaw().empty())
{
corelib/src/OdometryThread.cpp
+5 -5
View File
@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/SensorEvent.h>
#include "rtabmap/core/OdometryThread.h"
#include "rtabmap/core/Odometry.h"
#include "rtabmap/core/odometry/OdometryMono.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/utilite/ULogger.h"
@@ -58,12 +58,12 @@ bool OdometryThread::handleEvent(UEvent * event)
{
if(this->isRunning())
{
if(event->getClassName().compare("CameraEvent") == 0)
if(event->getClassName().compare("SensorEvent") == 0)
{
CameraEvent * cameraEvent = (CameraEvent*)event;
if(cameraEvent->getCode() == CameraEvent::kCodeData)
SensorEvent * sensorEvent = (SensorEvent*)event;
if(sensorEvent->getCode() == SensorEvent::kCodeData)
{
this->addData(cameraEvent->data());
this->addData(sensorEvent->data());
}
}
else if(event->getClassName().compare("IMUEvent") == 0)
corelib/src/Parameters.cpp
+21 -9
View File
@@ -1172,11 +1172,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
return out;
}
void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, ParametersMap & parameters, bool modifiedOnly)
{
CSimpleIniA ini;
ini.LoadFile(configFile.c_str());
const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core");
if(keyValMap)
{
@@ -1191,12 +1188,12 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
{
if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
{
if(configFile.find(".rtabmap") != std::string::npos)
if(configFilePath.find(".rtabmap") != std::string::npos)
{
UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
"current RTAB-Map version used (\"%s\"). The config file will be upgraded "
"to new version.",
configFile.c_str(),
configFilePath.c_str(),
(*iter).second,
RTABMAP_VERSION);
}
@@ -1205,7 +1202,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
UERROR("Version in the config file \"%s\" is more recent (\"%s\") than "
"current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
"be ignored.",
configFile.c_str(),
configFilePath.c_str(),
(*iter).second,
RTABMAP_VERSION);
}
@@ -1255,11 +1252,26 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
else
{
ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
"Ignore this warning if the ini file does not exist yet. "
"The ini file will be automatically created when rtabmap will close.", configFile.c_str());
"Ignore this warning if the ini file does not exist yet. "
"The ini file will be automatically created when rtabmap will close.", configFilePath.c_str());
}
}
void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
{
CSimpleIniA ini;
ini.LoadFile(configFile.c_str());
readINIImpl(ini, configFile, parameters, modifiedOnly);
}
void Parameters::readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly)
{
CSimpleIniA ini;
ini.LoadData(configContent);
readINIImpl(ini, "", parameters, modifiedOnly);
}
void Parameters::writeINI(const std::string & configFile, const ParametersMap & parameters)
{
CSimpleIniA ini;
corelib/src/Recovery.cpp
+1 -1
View File
@@ -160,7 +160,7 @@ bool databaseRecovery(
DBReader dbReader(databasePath, 0, odometryIgnored);
dbReader.init();
CameraInfo info;
SensorCaptureInfo info;
SensorData data = dbReader.takeImage(&info);
int processed = 0;
if (progressState)
corelib/src/RegistrationIcp.cpp
+5 -2
View File
@@ -69,6 +69,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
_epsilon(Parameters::defaultIcpEpsilon()),
_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
_force4DoF(Parameters::defaultIcpForce4DoF()),
_filtersEnabled(Parameters::defaultIcpFiltersEnabled()),
_pointToPlane(Parameters::defaultIcpPointToPlane()),
_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
@@ -115,6 +116,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF);
Parameters::parse(parameters, Parameters::kIcpFiltersEnabled(), _filtersEnabled);
Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio);
Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
@@ -337,6 +339,7 @@ Transform RegistrationIcp::computeTransformationImpl(
UDEBUG("Downsampling step=%d", _downsamplingStep);
UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false");
UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
UDEBUG("Strategy=%d", _strategy);
@@ -360,7 +363,7 @@ Transform RegistrationIcp::computeTransformationImpl(
int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size();
int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size();
if(!dataFrom.laserScanRaw().empty())
if(!dataFrom.laserScanRaw().empty() && (_filtersEnabled & 1))
{
int pointsBeforeFiltering = dataFrom.laserScanRaw().size();
LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(),
@@ -401,7 +404,7 @@ Transform RegistrationIcp::computeTransformationImpl(
float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering);
maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio);
}
if(!dataTo.laserScanRaw().empty())
if(!dataTo.laserScanRaw().empty() && (_filtersEnabled & 2))
{
int pointsBeforeFiltering = dataTo.laserScanRaw().size();
LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(),
corelib/src/RtabmapThread.cpp
+5 -5
View File
@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/SensorEvent.h>
#include "rtabmap/core/Rtabmap.h"
#include "rtabmap/core/RtabmapThread.h"
#include "rtabmap/core/RtabmapEvent.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/ParamEvent.h"
#include "rtabmap/core/OdometryEvent.h"
#include "rtabmap/core/UserDataEvent.h"
@@ -371,11 +371,11 @@ bool RtabmapThread::handleEvent(UEvent* event)
// IMU events are published at high frequency, early exit
return false;
}
else if(event->getClassName().compare("CameraEvent") == 0)
else if(event->getClassName().compare("SensorEvent") == 0)
{
UDEBUG("CameraEvent");
CameraEvent * e = (CameraEvent*)event;
if(e->getCode() == CameraEvent::kCodeData)
UDEBUG("SensorEvent");
SensorEvent * e = (SensorEvent*)event;
if(e->getCode() == SensorEvent::kCodeData)
{
if (_rtabmap->isRGBDMode())
{
corelib/src/SensorCapture.cpp
+114
View File
@@ -0,0 +1,114 @@
/*
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/SensorCapture.h"
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UTimer.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <cmath>
namespace rtabmap
{
SensorCapture::SensorCapture(float frameRate, const Transform & localTransform) :
_frameRate(frameRate),
_localTransform(localTransform),
_frameRateTimer(new UTimer()),
_seq(0)
{
}
SensorCapture::~SensorCapture()
{
delete _frameRateTimer;
}
void SensorCapture::resetTimer()
{
_frameRateTimer->start();
}
SensorData SensorCapture::takeData(SensorCaptureInfo * info)
{
bool warnFrameRateTooHigh = false;
float actualFrameRate = 0;
float frameRate = _frameRate;
if(frameRate>0)
{
int sleepTime = (1000.0f/frameRate - 1000.0f*_frameRateTimer->getElapsedTime());
if(sleepTime > 2)
{
uSleep(sleepTime-2);
}
else if(sleepTime < 0)
{
warnFrameRateTooHigh = true;
actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
}
// Add precision at the cost of a small overhead
while(_frameRateTimer->getElapsedTime() < 1.0/double(frameRate)-0.000001)
{
//
}
double slept = _frameRateTimer->getElapsedTime();
_frameRateTimer->start();
UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
}
UTimer timer;
SensorData data = this->captureData(info);
double captureTime = timer.ticks();
if(warnFrameRateTooHigh)
{
UWARN("Camera: Cannot reach target frame rate %f Hz, current rate is %f Hz and capture time = %f s.",
frameRate, actualFrameRate, captureTime);
}
else
{
UDEBUG("Time capturing data = %fs", captureTime);
}
if(info)
{
info->id = data.id();
info->stamp = data.stamp();
info->timeCapture = captureTime;
}
return data;
}
} // namespace rtabmap
corelib/src/CameraThread.cpp → corelib/src/SensorCaptureThread.cpp
+330 -150
View File
@@ -25,9 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/CameraThread.h"
#include "rtabmap/core/SensorCaptureThread.h"
#include "rtabmap/core/Camera.h"
#include "rtabmap/core/CameraEvent.h"
#include "rtabmap/core/Lidar.h"
#include "rtabmap/core/SensorEvent.h"
#include "rtabmap/core/CameraRGBD.h"
#include "rtabmap/core/util2d.h"
#include "rtabmap/core/util3d.h"
@@ -50,143 +51,149 @@ namespace rtabmap
{
// ownership transferred
CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_camera(camera),
_odomSensor(0),
_odomAsGt(false),
_poseTimeOffset(0.0),
_poseScaleFactor(1.0f),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_histogramMethod(0),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
SensorCaptureThread::SensorCaptureThread(
Camera * camera,
const ParametersMap & parameters) :
SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
{
UASSERT(_camera != 0);
UASSERT(camera != 0);
}
// ownership transferred
CameraThread::CameraThread(
SensorCaptureThread::SensorCaptureThread(
Camera * camera,
Camera * odomSensor,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset,
float poseScaleFactor,
bool odomAsGt,
double poseWaitTime,
const ParametersMap & parameters) :
_camera(camera),
_odomSensor(odomSensor),
_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
_odomAsGt(odomAsGt),
_poseTimeOffset(poseTimeOffset),
_poseScaleFactor(poseScaleFactor),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_histogramMethod(0),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
{
UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
UDEBUG("_poseTimeOffset =%f", _poseTimeOffset);
UDEBUG("_poseScaleFactor =%f", _poseScaleFactor);
UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false");
UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull());
}
// ownership transferred
CameraThread::CameraThread(
SensorCaptureThread::SensorCaptureThread(
Lidar * lidar,
const ParametersMap & parameters) :
SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
{
UASSERT(lidar != 0);
}
SensorCaptureThread::SensorCaptureThread(
Lidar * lidar,
Camera * camera,
bool odomAsGt,
const ParametersMap & parameters) :
_camera(camera),
_odomSensor(0),
_odomAsGt(odomAsGt),
_poseTimeOffset(0.0),
_poseScaleFactor(1.0f),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_histogramMethod(0),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
{
UASSERT(_camera != 0);
UDEBUG("_odomAsGt =%s", _odomAsGt?"true":"false");
UASSERT(lidar != 0 && camera != 0);
}
CameraThread::~CameraThread()
SensorCaptureThread::SensorCaptureThread(
Lidar * lidar,
SensorCapture * odomSensor,
double poseTimeOffset,
float poseScaleFactor,
double poseWaitTime,
const ParametersMap & parameters) :
SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
{
UASSERT(lidar != 0 && odomSensor != 0);
}
SensorCaptureThread::SensorCaptureThread(
Lidar * lidar,
Camera * camera,
SensorCapture * odomSensor,
const Transform & extrinsics,
double poseTimeOffset,
float poseScaleFactor,
double poseWaitTime,
const ParametersMap & parameters) :
_camera(camera),
_odomSensor(odomSensor),
_lidar(lidar),
_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
_odomAsGt(false),
_poseTimeOffset(poseTimeOffset),
_poseScaleFactor(poseScaleFactor),
_poseWaitTime(poseWaitTime),
_mirroring(false),
_stereoExposureCompensation(false),
_colorOnly(false),
_imageDecimation(1),
_histogramMethod(0),
_stereoToDepth(false),
_scanDeskewing(false),
_scanFromDepth(false),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1),
_imuFilter(0),
_imuBaseFrameConversion(false),
_featureDetector(0),
_depthAsMask(Parameters::defaultVisDepthAsMask())
{
UASSERT(_camera != 0 || _lidar != 0);
if(_lidar && _camera)
{
_camera->setFrameRate(0);
}
if(_odomSensor)
{
if(_camera)
{
if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull())
{
_extrinsicsOdomToCamera.setIdentity();
}
UASSERT(!_extrinsicsOdomToCamera.isNull());
UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
}
UDEBUG("_poseTimeOffset =%f", _poseTimeOffset);
UDEBUG("_poseScaleFactor =%f", _poseScaleFactor);
UDEBUG("_poseWaitTime =%f", _poseWaitTime);
}
}
SensorCaptureThread::~SensorCaptureThread()
{
join(true);
if(_odomSensor != _camera && _odomSensor != _lidar)
{
delete _odomSensor;
}
delete _camera;
delete _odomSensor;
delete _lidar;
delete _distortionModel;
delete _stereoDense;
delete _imuFilter;
delete _featureDetector;
}
void CameraThread::setImageRate(float imageRate)
void SensorCaptureThread::setFrameRate(float frameRate)
{
if(_camera)
if(_lidar)
{
_camera->setImageRate(imageRate);
_lidar->setFrameRate(frameRate);
}
else if(_camera)
{
_camera->setFrameRate(frameRate);
}
}
void CameraThread::setDistortionModel(const std::string & path)
void SensorCaptureThread::setDistortionModel(const std::string & path)
{
if(_distortionModel)
{
@@ -206,7 +213,7 @@ void CameraThread::setDistortionModel(const std::string & path)
}
}
void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR)
{
UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
_bilateralFiltering = true;
@@ -214,20 +221,20 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
_bilateralSigmaR = sigmaR;
}
void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
{
delete _imuFilter;
_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
_imuBaseFrameConversion = baseFrameConversion;
}
void CameraThread::disableIMUFiltering()
void SensorCaptureThread::disableIMUFiltering()
{
delete _imuFilter;
_imuFilter = 0;
}
void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters)
{
delete _featureDetector;
ParametersMap params = parameters;
@@ -245,35 +252,38 @@ void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
_featureDetector = Feature2D::create(params);
_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
}
void CameraThread::disableFeatureDetection()
void SensorCaptureThread::disableFeatureDetection()
{
delete _featureDetector;
_featureDetector = 0;
}
void CameraThread::setScanParameters(
void SensorCaptureThread::setScanParameters(
bool fromDepth,
int downsampleStep,
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
bool forceGroundNormalsUp)
float normalsRadius,
bool forceGroundNormalsUp,
bool deskewing)
{
setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing);
}
void CameraThread::setScanParameters(
void SensorCaptureThread::setScanParameters(
bool fromDepth,
int downsampleStep, // decimation of the depth image in case the scan is from depth image
float rangeMin,
float rangeMax,
float voxelSize,
int normalsK,
int normalsRadius,
float groundNormalsUp)
float normalsRadius,
float groundNormalsUp,
bool deskewing)
{
_scanDeskewing = deskewing;
_scanFromDepth = fromDepth;
_scanDownsampleStep=downsampleStep;
_scanRangeMin = rangeMin;
@@ -284,34 +294,178 @@ void CameraThread::setScanParameters(
_scanForceGroundNormalsUp = groundNormalsUp;
}
bool CameraThread::odomProvided() const
bool SensorCaptureThread::odomProvided() const
{
return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided()));
if(_odomAsGt)
{
return false;
}
return _odomSensor != 0;
}
void CameraThread::mainLoopBegin()
void SensorCaptureThread::mainLoopBegin()
{
ULogger::registerCurrentThread("Camera");
if(_lidar)
{
_lidar->resetTimer();
}
else if(_camera)
{
_camera->resetTimer();
}
if(_imuFilter)
{
// In case we paused the camera and moved somewhere else, restart filtering.
_imuFilter->reset();
}
_camera->resetTimer();
}
void CameraThread::mainLoop()
void SensorCaptureThread::mainLoop()
{
UASSERT(_lidar || _camera);
UTimer totalTime;
CameraInfo info;
SensorData data = _camera->takeImage(&info);
if(_odomSensor)
SensorCaptureInfo info;
SensorData data;
SensorData cameraData;
double lidarStamp = 0.0;
double cameraStamp = 0.0;
if(_lidar)
{
data = _lidar->takeData(&info);
if(data.stamp() == 0.0)
{
UERROR("Could not capture scan! Skipping this frame!");
return;
}
else
{
lidarStamp = data.stamp();
if(_camera)
{
cameraData = _camera->takeData();
if(cameraData.stamp() == 0.0)
{
UERROR("Could not capture image! Skipping this frame!");
return;
}
else
{
double stampStart = UTimer::now();
while(cameraData.stamp() < data.stamp() &&
!isKilled() &&
UTimer::now() - stampStart < _poseWaitTime &&
!cameraData.imageRaw().empty())
{
// Make sure the camera frame is newer than lidar frame so
// that if there are imus published by the cameras, we can get
// them all in odometry before deskewing.
cameraData = _camera->takeData();
}
cameraStamp = cameraData.stamp();
if(cameraData.stamp() < data.stamp())
{
UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.",
cameraData.stamp(),
data.stamp(),
_poseWaitTime);
}
if(!cameraData.stereoCameraModels().empty())
{
data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true);
}
else
{
data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true);
}
}
}
}
}
else if(_camera)
{
data = _camera->takeData(&info);
if(data.stamp() == 0.0)
{
UERROR("Could not capture image! Skipping this frame!");
return;
}
else
{
cameraStamp = cameraData.stamp();
}
}
if(_odomSensor && data.stamp() != 0.0)
{
if(lidarStamp!=0.0 && _scanDeskewing)
{
UDEBUG("Deskewing begin");
if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime())
{
float scanTime =
data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
Transform poseFirstScan;
Transform poseLastScan;
cv::Mat cov;
double firstStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
double lastStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()];
if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) &&
_odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0))
{
if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
{
poseFirstScan.x() *= _poseScaleFactor;
poseFirstScan.y() *= _poseScaleFactor;
poseFirstScan.z() *= _poseScaleFactor;
poseLastScan.x() *= _poseScaleFactor;
poseLastScan.y() *= _poseScaleFactor;
poseLastScan.z() *= _poseScaleFactor;
}
UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull());
Transform transform = poseFirstScan.inverse() * poseLastScan;
// convert to velocity
float x,y,z,roll,pitch,yaw;
transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
x/=scanTime;
y/=scanTime;
z/=scanTime;
roll /= scanTime;
pitch /= scanTime;
yaw /= scanTime;
Transform velocity(x,y,z,roll,pitch,yaw);
UTimer timeDeskewing;
LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
info.timeDeskewing = timeDeskewing.ticks();
if(!scanDeskewed.isEmpty())
{
data.setLaserScan(scanDeskewed);
}
}
else
{
UWARN("Failed to get poses for stamps %f and %f! Skipping this frame!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset);
return;
}
}
else if(!data.laserScanRaw().empty())
{
UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str());
}
UDEBUG("Deskewing end");
}
Transform pose;
Transform poseToLeftCam;
cv::Mat covariance;
if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance))
if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
{
info.odomPose = pose;
info.odomCovariance = covariance;
@@ -321,21 +475,47 @@ void CameraThread::mainLoop()
info.odomPose.y() *= _poseScaleFactor;
info.odomPose.z() *= _poseScaleFactor;
}
// Adjust local transform of the camera based on the pose frame
if(!data.cameraModels().empty())
if(cameraStamp != 0.0)
{
UASSERT(data.cameraModels().size()==1);
CameraModel model = data.cameraModels()[0];
model.setLocalTransform(_extrinsicsOdomToCamera);
data.setCameraModel(model);
}
else if(!data.stereoCameraModels().empty())
{
UASSERT(data.stereoCameraModels().size()==1);
StereoCameraModel model = data.stereoCameraModels()[0];
model.setLocalTransform(_extrinsicsOdomToCamera);
data.setStereoCameraModel(model);
Transform cameraCorrection = Transform::getIdentity();
if(lidarStamp > 0.0 && lidarStamp != cameraStamp)
{
if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
{
cameraCorrection = info.odomPose.inverse() * pose;
}
else
{
UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp);
}
}
// Adjust local transform of the camera based on the pose frame
if(!data.cameraModels().empty())
{
UASSERT(data.cameraModels().size()==1);
CameraModel model = data.cameraModels()[0];
model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
data.setCameraModel(model);
}
else if(!data.stereoCameraModels().empty())
{
UASSERT(data.stereoCameraModels().size()==1);
StereoCameraModel model = data.stereoCameraModels()[0];
model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
data.setStereoCameraModel(model);
}
}
// Fake IMU to intialize gravity (assuming pose is aligned with gravity!)
Eigen::Quaterniond q = info.odomPose.getQuaterniond();
data.setIMU(IMU(
cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(),
cv::Vec3d(), cv::Mat(),
cv::Vec3d(), cv::Mat(),
Transform::getIdentity()));
this->disableIMUFiltering();
}
else
{
@@ -352,19 +532,19 @@ void CameraThread::mainLoop()
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
{
postUpdate(&data, &info);
info.cameraName = _camera->getSerial();
info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
info.timeTotal = totalTime.ticks();
this->post(new CameraEvent(data, info));
this->post(new SensorEvent(data, info));
}
else if(!this->isKilled())
{
UWARN("no more images...");
UWARN("no more data...");
this->kill();
this->post(new CameraEvent());
this->post(new SensorEvent());
}
}
void CameraThread::mainLoopKill()
void SensorCaptureThread::mainLoopKill()
{
if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
{
@@ -389,7 +569,7 @@ void CameraThread::mainLoopKill()
}
}
void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const
{
UASSERT(dataPtr!=0);
SensorData & data = *dataPtr;
corelib/src/camera/CameraDepthAI.cpp
+1 -1
View File
@@ -638,7 +638,7 @@ std::string CameraDepthAI::getSerial() const
return "";
}
SensorData CameraDepthAI::captureImage(CameraInfo * info)
SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_DEPTHAI
corelib/src/camera/CameraFreenect.cpp
+1 -1
View File
@@ -418,7 +418,7 @@ std::string CameraFreenect::getSerial() const
return "";
}
SensorData CameraFreenect::captureImage(CameraInfo * info)
SensorData CameraFreenect::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT
corelib/src/camera/CameraFreenect2.cpp
+1 -1
View File
@@ -334,7 +334,7 @@ std::string CameraFreenect2::getSerial() const
return "";
}
SensorData CameraFreenect2::captureImage(CameraInfo * info)
SensorData CameraFreenect2::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_FREENECT2
corelib/src/camera/CameraImages.cpp
+1 -1
View File
@@ -670,7 +670,7 @@ std::vector<std::string> CameraImages::filenames() const
return std::vector<std::string>();
}
SensorData CameraImages::captureImage(CameraInfo * info)
SensorData CameraImages::captureImage(SensorCaptureInfo * info)
{
if(_syncImageRateWithStamps && _captureDelay>0.0)
{
corelib/src/camera/CameraK4A.cpp
+1 -1
View File
@@ -424,7 +424,7 @@ std::string CameraK4A::getSerial() const
#endif
}
SensorData CameraK4A::captureImage(CameraInfo * info)
SensorData CameraK4A::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraK4W2.cpp
+1 -1
View File
@@ -278,7 +278,7 @@ std::string CameraK4W2::getSerial() const
return "";
}
SensorData CameraK4W2::captureImage(CameraInfo * info)
SensorData CameraK4W2::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraMyntEye.cpp
+1 -1
View File
@@ -598,7 +598,7 @@ void CameraMyntEye::getPoseAndIMU(
}
#endif
SensorData CameraMyntEye::captureImage(CameraInfo * info)
SensorData CameraMyntEye::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_MYNTEYE
corelib/src/camera/CameraOpenNI2.cpp
+1 -1
View File
@@ -473,7 +473,7 @@ std::string CameraOpenNI2::getSerial() const
return "";
}
SensorData CameraOpenNI2::captureImage(CameraInfo * info)
SensorData CameraOpenNI2::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_OPENNI2
corelib/src/camera/CameraOpenNICV.cpp
+1 -1
View File
@@ -105,7 +105,7 @@ bool CameraOpenNICV::isCalibrated() const
return true;
}
SensorData CameraOpenNICV::captureImage(CameraInfo * info)
SensorData CameraOpenNICV::captureImage(SensorCaptureInfo * info)
{
SensorData data;
if(_capture.isOpened())
corelib/src/camera/CameraOpenni.cpp
+1 -1
View File
@@ -182,7 +182,7 @@ std::string CameraOpenni::getSerial() const
return "";
}
SensorData CameraOpenni::captureImage(CameraInfo * info)
SensorData CameraOpenni::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_OPENNI
corelib/src/camera/CameraRGBDImages.cpp
+1 -1
View File
@@ -70,7 +70,7 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
return success;
}
SensorData CameraRGBDImages::captureImage(CameraInfo * info)
SensorData CameraRGBDImages::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraRealSense.cpp
+1 -1
View File
@@ -856,7 +856,7 @@ Transform rsPoseToTransform(const rs::slam::PoseMatrix4f & pose)
}
#endif
SensorData CameraRealSense::captureImage(CameraInfo * info)
SensorData CameraRealSense::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_REALSENSE
corelib/src/camera/CameraRealSense2.cpp
+13 -22
View File
@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UStl.h>
#include <opencv2/imgproc/types_c.h>
@@ -78,7 +77,6 @@ CameraRealSense2::CameraRealSense2(
cameraDepthHeight_(480),
cameraDepthFps_(30),
globalTimeSync_(true),
publishInterIMU_(false),
dualMode_(false),
closing_(false)
#endif
@@ -138,12 +136,12 @@ void CameraRealSense2::imu_callback(rs2::frame frame)
{
auto stream = frame.get_profile().stream_type();
cv::Vec3f crnt_reading = *reinterpret_cast<const cv::Vec3f*>(frame.get_data());
UDEBUG("%s callback! %f (%f %f %f)",
stream == RS2_STREAM_GYRO?"GYRO":"ACC",
frame.get_timestamp(),
crnt_reading[0],
crnt_reading[1],
crnt_reading[2]);
//UDEBUG("%s callback! %f (%f %f %f)",
// stream == RS2_STREAM_GYRO?"GYRO":"ACC",
// frame.get_timestamp(),
// crnt_reading[0],
// crnt_reading[1],
// crnt_reading[2]);
UScopeMutex sm(imuMutex_);
if(stream == RS2_STREAM_GYRO)
{
@@ -194,7 +192,7 @@ void CameraRealSense2::pose_callback(rs2::frame frame)
void CameraRealSense2::frame_callback(rs2::frame frame)
{
UDEBUG("Frame callback! %f", frame.get_timestamp());
//UDEBUG("Frame callback! %f", frame.get_timestamp());
syncer_(frame);
}
void CameraRealSense2::multiple_message_callback(rs2::frame frame)
@@ -1139,14 +1137,14 @@ bool CameraRealSense2::odomProvided() const
#endif
}
bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance)
bool CameraRealSense2::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
{
#ifdef RTABMAP_REALSENSE2
IMU imu;
unsigned int confidence = 0;
double rsStamp = stamp*1000.0;
Transform p;
getPoseAndIMU(rsStamp, p, confidence, imu);
getPoseAndIMU(rsStamp, p, confidence, imu, maxWaitTime*1000);
if(!p.isNull())
{
@@ -1202,13 +1200,6 @@ void CameraRealSense2::setGlobalTimeSync(bool enabled)
#endif
}
void CameraRealSense2::publishInterIMU(bool enabled)
{
#ifdef RTABMAP_REALSENSE2
publishInterIMU_ = enabled;
#endif
}
void CameraRealSense2::setDualMode(bool enabled, const Transform & extrinsics)
{
#ifdef RTABMAP_REALSENSE2
@@ -1252,7 +1243,7 @@ void CameraRealSense2::setOdomProvided(bool enabled, bool imageStreamsDisabled,
#endif
}
SensorData CameraRealSense2::captureImage(CameraInfo * info)
SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_REALSENSE2
@@ -1466,11 +1457,11 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);
info->odomCovariance.rowRange(3,6) *= pow(10, 1-(int)confidence);
}
if(!imu.empty() && !publishInterIMU_)
if(!imu.empty() && !isInterIMUPublishing())
{
data.setIMU(imu);
}
else if(publishInterIMU_ && !gyroBuffer_.empty())
else if(isInterIMUPublishing() && !gyroBuffer_.empty())
{
if(lastImuStamp_ > 0.0)
{
@@ -1501,7 +1492,7 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
getPoseAndIMU(stamps[i], tmp, confidence, imuTmp);
if(!imuTmp.empty())
{
UEventsManager::post(new IMUEvent(imuTmp, stamps[i]/1000.0));
this->postInterIMU(imuTmp, stamps[i]/1000.0);
pub++;
}
else
corelib/src/camera/CameraStereoDC1394.cpp
+1 -1
View File
@@ -396,7 +396,7 @@ std::string CameraStereoDC1394::getSerial() const
return "";
}
SensorData CameraStereoDC1394::captureImage(CameraInfo * info)
SensorData CameraStereoDC1394::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_DC1394
corelib/src/camera/CameraStereoFlyCapture2.cpp
+1 -1
View File
@@ -260,7 +260,7 @@ std::string CameraStereoFlyCapture2::getSerial() const
return "";
}
SensorData CameraStereoFlyCapture2::captureImage(CameraInfo * info)
SensorData CameraStereoFlyCapture2::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_FLYCAPTURE2
corelib/src/camera/CameraStereoImages.cpp
+1 -1
View File
@@ -156,7 +156,7 @@ std::string CameraStereoImages::getSerial() const
return stereoModel_.name();
}
SensorData CameraStereoImages::captureImage(CameraInfo * info)
SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraStereoTara.cpp
+1 -1
View File
@@ -136,7 +136,7 @@ std::string CameraStereoTara::getSerial() const
return cameraName_;
}
SensorData CameraStereoTara::captureImage(CameraInfo * info)
SensorData CameraStereoTara::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraStereoVideo.cpp
+1 -1
View File
@@ -243,7 +243,7 @@ std::string CameraStereoVideo::getSerial() const
return cameraName_;
}
SensorData CameraStereoVideo::captureImage(CameraInfo * info)
SensorData CameraStereoVideo::captureImage(SensorCaptureInfo * info)
{
SensorData data;
corelib/src/camera/CameraStereoZed.cpp
+33 -21
View File
@@ -28,7 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/camera/CameraStereoZed.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <rtabmap/utilite/UConversion.h>
#ifdef RTABMAP_ZED
@@ -167,12 +166,13 @@ IMU zedIMUtoIMU(const sl::SensorsData & sensorData, const Transform & imuLocalTr
class ZedIMUThread: public UThread
{
public:
ZedIMUThread(float rate, sl::Camera * zed, const Transform & imuLocalTransform, bool accurate)
ZedIMUThread(float rate, sl::Camera * zed, CameraStereoZed * camera, const Transform & imuLocalTransform, bool accurate)
{
UASSERT(rate > 0.0f);
UASSERT(zed != 0);
UASSERT(zed != 0 && camera != 0);
rate_ = rate;
zed_= zed;
camera_ = camera;
accurate_ = accurate;
imuLocalTransform_ = imuLocalTransform;
}
@@ -212,19 +212,20 @@ private:
bool res = zed_->getIMUData(imudata, sl::TIME_REFERENCE_IMAGE);
if(res == sl::SUCCESS && imudata.valid)
{
UEventsManager::post(new IMUEvent(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now()));
this->postInterIMU(zedIMUtoIMU(imudata, imuLocalTransform_), UTimer::now());
}
#else
sl::SensorsData sensordata;
sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::IMAGE);
sl::ERROR_CODE res = zed_->getSensorsData(sensordata, sl::TIME_REFERENCE::CURRENT);
if(res == sl::ERROR_CODE::SUCCESS && sensordata.imu.is_available)
{
UEventsManager::post(new IMUEvent(zedIMUtoIMU(sensordata, imuLocalTransform_), UTimer::now()));
camera_->postInterIMUPublic(zedIMUtoIMU(sensordata, imuLocalTransform_), double(sensordata.imu.timestamp)/10e9);
}
#endif
}
float rate_;
sl::Camera * zed_;
CameraStereoZed * camera_;
bool accurate_;
Transform imuLocalTransform_;
UTimer frameRateTimer_;
@@ -279,7 +280,6 @@ CameraStereoZed::CameraStereoZed(
computeOdometry_(computeOdometry),
lost_(true),
force3DoF_(odomForce3DoF),
publishInterIMU_(false),
imuPublishingThread_(0)
#endif
{
@@ -345,7 +345,6 @@ CameraStereoZed::CameraStereoZed(
computeOdometry_(computeOdometry),
lost_(true),
force3DoF_(odomForce3DoF),
publishInterIMU_(false),
imuPublishingThread_(0)
#endif
{
@@ -386,13 +385,6 @@ CameraStereoZed::~CameraStereoZed()
#endif
}
void CameraStereoZed::publishInterIMU(bool enabled)
{
#ifdef RTABMAP_ZED
publishInterIMU_ = enabled;
#endif
}
bool CameraStereoZed::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
@@ -564,9 +556,9 @@ bool CameraStereoZed::init(const std::string & calibrationFolder, const std::str
imuLocalTransform_.prettyPrint().c_str(),
zedPoseToTransform(infos.sensors_configuration.camera_imu_transform).prettyPrint().c_str());
#endif
if(publishInterIMU_)
if(isInterIMUPublishing())
{
imuPublishingThread_ = new ZedIMUThread(200, zed_, imuLocalTransform_, true);
imuPublishingThread_ = new ZedIMUThread(200, zed_, this, imuLocalTransform_, true);
imuPublishingThread_->start();
}
}
@@ -607,7 +599,7 @@ bool CameraStereoZed::odomProvided() const
#endif
}
bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance)
bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
{
#ifdef RTABMAP_ZED
@@ -683,7 +675,7 @@ bool CameraStereoZed::getPose(double stamp, Transform & pose, cv::Mat & covarian
return false;
}
SensorData CameraStereoZed::captureImage(CameraInfo * info)
SensorData CameraStereoZed::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZED
@@ -711,10 +703,12 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
#else
sl::ERROR_CODE res;
sl::Timestamp timestamp;
bool imuReceived = true;
do
{
res = zed_->grab(rparam);
timestamp = zed_->getTimestamp(sl::TIME_REFERENCE::IMAGE);
// If the sensor supports IMU, wait IMU to be available before sending data.
if(imuPublishingThread_ == 0 && !imuLocalTransform_.isNull())
@@ -752,8 +746,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
zed_->retrieveMeasure(tmp,sl::MEASURE::DEPTH);
#endif
slMat2cvMat(tmp).copyTo(depth);
#if ZED_SDK_MAJOR_VERSION < 3
data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), UTimer::now());
#else
data = SensorData(left, depth, stereoModel_.left(), this->getNextSeqID(), double(timestamp)/10e9);
#endif
}
else
{
@@ -766,8 +763,11 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
cv::Mat rgbaRight = slMat2cvMat(tmp);
cv::Mat right;
cv::cvtColor(rgbaRight, right, cv::COLOR_BGRA2GRAY);
#if ZED_SDK_MAJOR_VERSION < 3
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), UTimer::now());
#else
data = SensorData(left, right, stereoModel_, this->getNextSeqID(), double(timestamp)/10e9);
#endif
}
if(imuPublishingThread_ == 0)
@@ -803,6 +803,13 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
info->odomPose = zedPoseToTransform(pose);
if (!info->odomPose.isNull())
{
#if ZED_SDK_MAJOR_VERSION >=3
if(pose.timestamp != timestamp)
{
UWARN("Pose retrieve doesn't have same stamp (%ld) than grabbed image (%ld)", pose.timestamp, timestamp);
}
#endif
//transform from:
// x->right, y->down, z->forward
//to:
@@ -858,4 +865,9 @@ SensorData CameraStereoZed::captureImage(CameraInfo * info)
return data;
}
void CameraStereoZed::postInterIMUPublic(const IMU & imu, double stamp)
{
postInterIMU(imu, stamp);
}
} // namespace rtabmap
corelib/src/camera/CameraStereoZedOC.cpp
+1 -1
View File
@@ -710,7 +710,7 @@ std::string CameraStereoZedOC::getSerial() const
return "";
}
SensorData CameraStereoZedOC::captureImage(CameraInfo * info)
SensorData CameraStereoZedOC::captureImage(SensorCaptureInfo * info)
{
SensorData data;
#ifdef RTABMAP_ZEDOC
corelib/src/camera/CameraVideo.cpp
+1 -1
View File
@@ -162,7 +162,7 @@ std::string CameraVideo::getSerial() const
return _guid;
}
SensorData CameraVideo::captureImage(CameraInfo * info)
SensorData CameraVideo::captureImage(SensorCaptureInfo * info)
{
cv::Mat img;
if(_capture.isOpened())
corelib/src/lidar/LidarVLP16.cpp
+378
View File
@@ -0,0 +1,378 @@
/*
Copyright (c) 2010-2022, Mathieu Labbe
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/core/lidar/LidarVLP16.h>
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UThread.h>
#include <pcl/pcl_config.h>
#if PCL_VERSION_COMPARE(<, 1, 9, 0)
#define VLP_MAX_NUM_LASERS 16
#define VLP_DUAL_MODE 0x39
#endif
namespace rtabmap {
/** @brief Function used to check that hour assigned to timestamp in conversion is
* correct. Velodyne only returns time since the top of the hour, so if the computer clock
* and the velodyne clock (gps-synchronized) are a little off, there is a chance the wrong
* hour may be associated with the timestamp
*
* Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
* Original license: BSD License 2.0
* Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
*
* @param stamp timestamp recovered from velodyne
* @param nominal_stamp time coming from computer's clock
* @return timestamp from velodyne, possibly shifted by 1 hour if the function arguments
* disagree by more than a half-hour.
*/
double resolveHourAmbiguity(const double &stamp, const double &nominal_stamp) {
const int HALFHOUR_TO_SEC = 1800;
double retval = stamp;
if (nominal_stamp > stamp) {
if (nominal_stamp - stamp > HALFHOUR_TO_SEC) {
retval = retval + 2*HALFHOUR_TO_SEC;
}
} else if (stamp - nominal_stamp > HALFHOUR_TO_SEC) {
retval = retval - 2*HALFHOUR_TO_SEC;
}
return retval;
}
/*
* Original author: Copyright (C) 2019 Matthew Pitropov, Joshua Whitley
* Original license: BSD License 2.0
* Original code: https://github.com/ros-drivers/velodyne/blob/master/velodyne_driver/include/velodyne_driver/time_conversion.hpp
*/
double rosTimeFromGpsTimestamp(const uint32_t data) {
const int HOUR_TO_SEC = 3600;
// time for each packet is a 4 byte uint
// It is the number of microseconds from the top of the hour
double time_nom = UTimer::now();
uint32_t cur_hour = time_nom / HOUR_TO_SEC;
double stamp = double(cur_hour * HOUR_TO_SEC) + double(data) / 1000000;
stamp = resolveHourAmbiguity(stamp, time_nom);
return stamp;
}
LidarVLP16::LidarVLP16(
const std::string& pcapFile,
bool organized,
bool stampLast,
float frameRate,
Transform localTransform) :
Lidar(frameRate, localTransform),
pcl::VLPGrabber(pcapFile),
timingOffsetsDualMode_(false),
startSweepTime_(0),
startSweepTimeHost_(0),
organized_(organized),
useHostTime_(false),
stampLast_(stampLast)
{
UDEBUG("Using PCAP file \"%s\"", pcapFile.c_str());
}
LidarVLP16::LidarVLP16(
const boost::asio::ip::address& ipAddress,
const std::uint16_t port,
bool organized,
bool useHostTime,
bool stampLast,
float frameRate,
Transform localTransform) :
Lidar(frameRate, localTransform),
pcl::VLPGrabber(ipAddress, port),
timingOffsetsDualMode_(false),
startSweepTime_(0),
startSweepTimeHost_(0),
organized_(organized),
useHostTime_(useHostTime),
stampLast_(stampLast)
{
UDEBUG("Using network lidar with IP=%s port=%d", ipAddress.to_string().c_str(), port);
}
LidarVLP16::~LidarVLP16()
{
UDEBUG("Stopping lidar...");
stop();
scanReady_.release();
UDEBUG("Stopped lidar!");
}
void LidarVLP16::setOrganized(bool enable)
{
organized_ = true;
}
bool LidarVLP16::init(const std::string &, const std::string &)
{
UDEBUG("Init lidar");
if(isRunning())
{
UDEBUG("Stopping lidar...");
stop();
uSleep(2000); // make sure all callbacks are finished
UDEBUG("Stopped lidar!");
}
startSweepTime_ = 0.0;
startSweepTimeHost_ = 0.0;
accumulatedScans_.clear();
if(organized_)
{
accumulatedScans_.resize(16);
}
else
{
accumulatedScans_.resize(1);
}
buildTimings(false);
start();
UDEBUG("Lidar capture started");
return true;
}
/**
* Build a timing table for each block/firing. Stores in timing_offsets vector
*/
void LidarVLP16::buildTimings(bool dualMode)
{
// vlp16
// timing table calculation, from velodyne user manual
timingOffsets_.resize(12);
for (size_t i=0; i < timingOffsets_.size(); ++i){
timingOffsets_[i].resize(32);
}
// constants
double full_firing_cycle = 55.296 * 1e-6; // seconds
double single_firing = 2.304 * 1e-6; // seconds
double dataBlockIndex, dataPointIndex;
// compute timing offsets
for (size_t x = 0; x < timingOffsets_.size(); ++x){
for (size_t y = 0; y < timingOffsets_[x].size(); ++y){
if (dualMode){
dataBlockIndex = (x - (x % 2)) + (y / 16);
}
else{
dataBlockIndex = (x * 2) + (y / 16);
}
dataPointIndex = y % 16;
//timing_offsets[block][firing]
timingOffsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
}
}
timingOffsetsDualMode_ = dualMode;
}
void LidarVLP16::toPointClouds (HDLDataPacket *dataPacket)
{
if (sizeof(HDLLaserReturn) != 3)
return;
double receivedHostTime = UTimer::now();
double packetStamp = rosTimeFromGpsTimestamp(dataPacket->gpsTimestamp);
if(startSweepTime_==0)
{
startSweepTime_ = packetStamp;
startSweepTimeHost_ = receivedHostTime;
}
bool dualMode = dataPacket->mode == VLP_DUAL_MODE;
if(timingOffsets_.empty() || timingOffsetsDualMode_ != dualMode)
{
// reset everything
timingOffsets_.clear();
buildTimings(dualMode);
startSweepTime_ = packetStamp;
startSweepTimeHost_ = receivedHostTime;
for(size_t i=0; i<accumulatedScans_.size(); ++i)
{
accumulatedScans_[i].clear();
}
}
double interpolated_azimuth_delta;
std::uint8_t index = 1;
if (dualMode)
{
index = 2;
}
if (dataPacket->firingData[index].rotationalPosition < dataPacket->firingData[0].rotationalPosition)
{
interpolated_azimuth_delta = ((dataPacket->firingData[index].rotationalPosition + 36000) - dataPacket->firingData[0].rotationalPosition) / 2.0;
}
else
{
interpolated_azimuth_delta = (dataPacket->firingData[index].rotationalPosition - dataPacket->firingData[0].rotationalPosition) / 2.0;
}
for (std::uint8_t i = 0; i < HDL_FIRING_PER_PKT; ++i)
{
HDLFiringData firing_data = dataPacket->firingData[i];
for (std::uint8_t j = 0; j < HDL_LASER_PER_FIRING; j++)
{
double current_azimuth = firing_data.rotationalPosition;
if (j >= VLP_MAX_NUM_LASERS)
{
current_azimuth += interpolated_azimuth_delta;
}
if (current_azimuth > 36000)
{
current_azimuth -= 36000;
}
double t = 0;
if (timingOffsets_.size())
t = timingOffsets_[i][j];
if (current_azimuth < HDLGrabber::last_azimuth_)
{
if (!accumulatedScans_[0].empty())
{
UScopeMutex lock(lastScanMutex_);
bool notify = lastScan_.laserScanRaw().empty();
if(stampLast_)
{
double lastStamp = startSweepTime_ + accumulatedScans_[accumulatedScans_.size()-1].back().t;
double diff = lastStamp - startSweepTime_;
lastScan_.setStamp(useHostTime_?startSweepTimeHost_+diff:lastStamp);
for(size_t r=0; r<accumulatedScans_.size(); ++r)
{
for(size_t k=0; k<accumulatedScans_[r].size(); ++k)
{
accumulatedScans_[r][k].t -= diff;
}
}
}
else
{
lastScan_.setStamp(useHostTime_?startSweepTimeHost_:startSweepTime_);
}
if(accumulatedScans_.size() > 1)
{
cv::Mat organizedScan = cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone();
for(size_t k=1; k<accumulatedScans_.size(); ++k)
{
UASSERT((int)accumulatedScans_[k].size() == organizedScan.cols);
organizedScan.push_back(cv::Mat(1, accumulatedScans_[k].size(), CV_32FC(5), accumulatedScans_[k].data()).clone());
}
lastScan_.setLaserScan(LaserScan(organizedScan, 0, 0, LaserScan::kXYZIT, getLocalTransform()));
}
else
{
lastScan_.setLaserScan(LaserScan(cv::Mat(1, accumulatedScans_[0].size(), CV_32FC(5), accumulatedScans_[0].data()).clone(), 0, 0, LaserScan::kXYZIT, getLocalTransform()));
}
if(notify)
{
scanReady_.release();
}
startSweepTime_ = packetStamp + t;
startSweepTimeHost_ = receivedHostTime + t;
}
for(size_t k=0; k<accumulatedScans_.size(); ++k)
{
accumulatedScans_[k].clear();
}
}
double timeSinceStartOfThisScan = packetStamp + t - startSweepTime_;
pcl::PointXYZI xyzi;
HDLGrabber::computeXYZI (xyzi, current_azimuth, firing_data.laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
PointXYZIT xyzit;
xyzit.x = xyzi.y;
xyzit.y = -xyzi.x;
xyzit.z = xyzi.z;
xyzit.i = xyzi.intensity;
xyzit.t = timeSinceStartOfThisScan;
if(accumulatedScans_.size()>1)
{
accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back(xyzit);
}
else if (! (std::isnan (xyzit.x) || std::isnan (xyzit.y) || std::isnan (xyzit.z)))
{
accumulatedScans_[0].push_back (xyzit);
}
last_azimuth_ = current_azimuth;
if (dualMode)
{
pcl::PointXYZI dual_xyzi;
HDLGrabber::computeXYZI (dual_xyzi, current_azimuth, dataPacket->firingData[i + 1].laserReturns[j], laser_corrections_[j % VLP_MAX_NUM_LASERS]);
if(accumulatedScans_.size()>1)
{
xyzit.x = dual_xyzi.y;
xyzit.y = -dual_xyzi.x;
xyzit.z = dual_xyzi.z;
xyzit.i = dual_xyzi.intensity;
xyzit.t = timeSinceStartOfThisScan;
accumulatedScans_[j % VLP_MAX_NUM_LASERS].push_back (xyzit);
}
else if ((dual_xyzi.x != xyzi.x || dual_xyzi.y != xyzi.y || dual_xyzi.z != xyzi.z)
&& ! (std::isnan (dual_xyzi.x) || std::isnan (dual_xyzi.y) || std::isnan (dual_xyzi.z)))
{
xyzit.x = dual_xyzi.y;
xyzit.y = -dual_xyzi.x;
xyzit.z = dual_xyzi.z;
xyzit.i = dual_xyzi.intensity;
xyzit.t = timeSinceStartOfThisScan;
accumulatedScans_[0].push_back (xyzit);
}
}
}
if (dualMode)
{
i++;
}
}
}
SensorData LidarVLP16::captureData(SensorCaptureInfo * info)
{
SensorData data;
if(scanReady_.acquire(1, 5000))
{
UScopeMutex lock(lastScanMutex_);
if(!lastScan_.laserScanRaw().empty())
{
data = lastScan_;
lastScan_ = SensorData();
}
}
else
{
UWARN("Did not receive any scans for the past 5 seconds.");
}
return data;
}
} /* namespace rtabmap */
corelib/src/odometry/OdometryF2M.cpp
+8 -5
View File
@@ -156,11 +156,14 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
regPipeline_ = Registration::create(bundleParameters);
if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
{
UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
Parameters::kOdomF2MBundleAdjustment().c_str(),
bundleAdjustment_,
Parameters::kRegStrategy().c_str(),
uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
if(regPipeline_->isImageRequired())
{
UWARN("%s=%d cannot be used with registration not done only with images (%s=%s), disabling bundle adjustment.",
Parameters::kOdomF2MBundleAdjustment().c_str(),
bundleAdjustment_,
Parameters::kRegStrategy().c_str(),
uValue(bundleParameters, Parameters::kRegStrategy(), uNumber2Str(Parameters::defaultRegStrategy())).c_str());
}
bundleAdjustment_ = 0;
}
corelib/src/pcl18/surface/impl/texture_mapping.hpp
-4
View File
@@ -975,10 +975,6 @@ pcl::TextureMapping<PointInT>::textureMeshwithMultipleCameras (pcl::TextureMesh
visible_faces.resize (cpt_visible_faces);
mesh.tex_polygons[current_cam].clear ();
mesh.tex_polygons[current_cam] = visible_faces;
int nb_faces = 0;
for (int i = 0; i < static_cast<int> (mesh.tex_polygons.size ()); i++)
nb_faces += static_cast<int> (mesh.tex_polygons[i].size ());
}
// we have been through all the cameras.
corelib/src/util3d.cpp
+230 -13
View File
@@ -35,6 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UTimer.h>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
@@ -2238,7 +2239,7 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
{
pcl::toPCLPointCloud2(*laserScanToPointCloud(laserScan, transform), *cloud);
}
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI)
else if(laserScan.format() == LaserScan::kXYI || laserScan.format() == LaserScan::kXYZI || laserScan.format() == LaserScan::kXYZIT)
{
pcl::toPCLPointCloud2(*laserScanToPointCloudI(laserScan, transform), *cloud);
}
@@ -2268,8 +2269,17 @@ pcl::PCLPointCloud2::Ptr laserScanToPointCloud2(const LaserScan & laserScan, con
pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & laserScan, const Transform & transform)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr output(new pcl::PointCloud<pcl::PointXYZ>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2286,8 +2296,17 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr laserScanToPointCloud(const LaserScan & lase
pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserScan & laserScan, const Transform & transform)
{
pcl::PointCloud<pcl::PointNormal>::Ptr output(new pcl::PointCloud<pcl::PointNormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2303,8 +2322,17 @@ pcl::PointCloud<pcl::PointNormal>::Ptr laserScanToPointCloudNormal(const LaserSc
pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2321,8 +2349,17 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr laserScanToPointCloudRGB(const LaserScan
pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & laserScan, const Transform & transform, float intensity)
{
pcl::PointCloud<pcl::PointXYZI>::Ptr output(new pcl::PointCloud<pcl::PointXYZI>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
Eigen::Affine3f transform3f = transform.toEigen3f();
for(int i=0; i<laserScan.size(); ++i)
@@ -2339,8 +2376,17 @@ pcl::PointCloud<pcl::PointXYZI>::Ptr laserScanToPointCloudI(const LaserScan & la
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(const LaserScan & laserScan, const Transform & transform, unsigned char r, unsigned char g, unsigned char b)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2356,8 +2402,17 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr laserScanToPointCloudRGBNormal(cons
pcl::PointCloud<pcl::PointXYZINormal>::Ptr laserScanToPointCloudINormal(const LaserScan & laserScan, const Transform & transform, float intensity)
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr output(new pcl::PointCloud<pcl::PointXYZINormal>);
if(laserScan.isOrganized())
{
output->width = laserScan.data().cols;
output->height = laserScan.data().rows;
output->is_dense = false;
}
else
{
output->is_dense = true;
}
output->resize(laserScan.size());
output->is_dense = true;
bool nullTransform = transform.isNull() || transform.isIdentity();
for(int i=0; i<laserScan.size(); ++i)
{
@@ -2374,7 +2429,8 @@ pcl::PointXYZ laserScanToPoint(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZ output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2388,7 +2444,8 @@ pcl::PointNormal laserScanToPointNormal(const LaserScan & laserScan, int index)
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointNormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2409,7 +2466,8 @@ pcl::PointXYZRGB laserScanToPointRGB(const LaserScan & laserScan, int index, uns
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGB output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2448,7 +2506,8 @@ pcl::PointXYZI laserScanToPointI(const LaserScan & laserScan, int index, float i
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZI output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2473,7 +2532,8 @@ pcl::PointXYZRGBNormal laserScanToPointRGBNormal(const LaserScan & laserScan, in
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZRGBNormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -2520,7 +2580,8 @@ pcl::PointXYZINormal laserScanToPointINormal(const LaserScan & laserScan, int in
{
UASSERT(!laserScan.isEmpty() && !laserScan.isCompressed() && index < laserScan.size());
pcl::PointXYZINormal output;
const float * ptr = laserScan.data().ptr<float>(0, index);
int row = index / laserScan.data().cols;
const float * ptr = laserScan.data().ptr<float>(row, index - row*laserScan.data().cols);
output.x = ptr[0];
output.y = ptr[1];
if(!laserScan.is2d())
@@ -3489,6 +3550,162 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr loadCloud(
return util3d::transformPointCloud(cloud, transform);
}
LaserScan deskew(
const LaserScan & input,
double inputStamp,
const rtabmap::Transform & velocity)
{
if(velocity.isNull())
{
UERROR("velocity should be valid!");
return LaserScan();
}
if(input.format() != LaserScan::kXYZIT)
{
UERROR("input scan doesn't have \"time\" channel! Only format \"%s\" supported yet.", LaserScan::formatName(LaserScan::kXYZIT).c_str());
return LaserScan();
}
if(input.empty())
{
UERROR("input scan is empty!");
return LaserScan();
}
int offsetTime = input.getTimeOffset();
// Get latest timestamp
double firstStamp;
double lastStamp;
firstStamp = inputStamp + input.data().ptr<float>(0, 0)[offsetTime];
lastStamp = inputStamp + input.data().ptr<float>(0, input.size()-1)[offsetTime];
if(lastStamp <= firstStamp)
{
UERROR("First and last stamps in the scan are the same!");
return LaserScan();
}
rtabmap::Transform firstPose;
rtabmap::Transform lastPose;
float vx,vy,vz, vroll,vpitch,vyaw;
velocity.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
// 1- The pose of base frame in odom frame at first stamp
// 2- The pose of base frame in odom frame at last stamp
double dt1 = firstStamp - inputStamp;
double dt2 = lastStamp - inputStamp;
firstPose = rtabmap::Transform(vx*dt1, vy*dt1, vz*dt1, vroll*dt1, vpitch*dt1, vyaw*dt1);
lastPose = rtabmap::Transform(vx*dt2, vy*dt2, vz*dt2, vroll*dt2, vpitch*dt2, vyaw*dt2);
if(firstPose.isNull())
{
UERROR("Could not get transform between stamps %f and %f!",
firstStamp,
inputStamp);
return LaserScan();
}
if(lastPose.isNull())
{
UERROR("Could not get transform between stamps %f and %f!",
lastStamp,
inputStamp);
return LaserScan();
}
double stamp;
UTimer processingTime;
double scanTime = lastStamp - firstStamp;
cv::Mat output(1, input.size(), CV_32FC4); // XYZI - Dense
int offsetIntensity = input.getIntensityOffset();
bool isLocalTransformIdentity = input.localTransform().isIdentity();
Transform localTransformInv = input.localTransform().inverse();
bool timeOnColumns = input.data().cols > input.data().rows;
int oi = 0;
if(timeOnColumns)
{
// t1 t2 ...
// ring1 ring1 ...
// ring2 ring2 ...
// ring3 ring4 ...
// ring4 ring3 ...
for(int u=0; u<input.data().cols; ++u)
{
const float * inputPtr = input.data().ptr<float>(0, u);
stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
for(int v=0; v<input.data().rows; ++v)
{
inputPtr = input.data().ptr<float>(v, u);
pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
if(pcl::isFinite(pt))
{
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
}
pt = rtabmap::util3d::transformPoint(pt, transform);
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
}
float * dataPtr = output.ptr<float>(0, oi++);
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
}
}
}
}
else // time on rows
{
// t1 ring1 ring2 ring3 ring4
// t2 ring1 ring2 ring3 ring4
// t3 ring1 ring2 ring3 ring4
// t4 ring1 ring2 ring3 ring4
// ... ... ... ... ...
for(int v=0; v<input.data().rows; ++v)
{
const float * inputPtr = input.data().ptr<float>(v, 0);
stamp = inputStamp + inputPtr[offsetTime];
rtabmap::Transform transform = firstPose.interpolate((stamp-firstStamp) / scanTime, lastPose);
for(int u=0; u<input.data().cols; ++u)
{
inputPtr = input.data().ptr<float>(v, u);
pcl::PointXYZ pt(inputPtr[0],inputPtr[1],inputPtr[2]);
if(pcl::isFinite(pt))
{
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, input.localTransform());
}
pt = rtabmap::util3d::transformPoint(pt, transform);
if(!isLocalTransformIdentity)
{
pt = rtabmap::util3d::transformPoint(pt, localTransformInv);
}
float * dataPtr = output.ptr<float>(0, oi++);
dataPtr[0] = pt.x;
dataPtr[1] = pt.y;
dataPtr[2] = pt.z;
dataPtr[3] = input.data().ptr<float>(v, u)[offsetIntensity];
}
}
}
}
output = cv::Mat(output, cv::Range::all(), cv::Range(0, oi));
UDEBUG("Lidar deskewing time=%fs", processingTime.elapsed());
return LaserScan(output, input.maxPoints(), input.rangeMax(), LaserScan::kXYZI, input.localTransform());
}
}
}
corelib/src/util3d_filtering.cpp
+34 -24
View File
@@ -82,8 +82,8 @@ LaserScan commonFiltering(
float groundNormalsUp)
{
LaserScan scan = scanIn;
UDEBUG("scan size=%d format=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
scan.size(), (int)scan.format(), downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
UDEBUG("scan size=%d format=%d, organized=%d, step=%d, rangeMin=%f, rangeMax=%f, voxel=%f, normalK=%d, normalRadius=%f, groundNormalsUp=%f",
scan.size(), (int)scan.format(), scan.isOrganized()?1:0, downsamplingStep, rangeMin, rangeMax, voxelSize, normalK, normalRadius, groundNormalsUp);
if(!scan.isEmpty())
{
// combined downsampling and range filtering step
@@ -99,34 +99,44 @@ LaserScan commonFiltering(
int oi = 0;
float rangeMinSqrd = rangeMin * rangeMin;
float rangeMaxSqrd = rangeMax * rangeMax;
for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
int downsamplingRows = scan.data().rows > scan.data().cols?downsamplingStep:1;
int downsamplingCols = scan.data().cols > scan.data().rows?downsamplingStep:1;
for(int j=0; j<scan.data().rows-downsamplingRows+1; j+=downsamplingRows)
{
const float * ptr = scan.data().ptr<float>(0, i);
if(rangeMin>0.0f || rangeMax>0.0f)
for(int i=0; i<scan.data().cols-downsamplingCols+1; i+=downsamplingCols)
{
float r;
if(is2d)
const float * ptr = scan.data().ptr<float>(j, i);
if(rangeMin>0.0f || rangeMax>0.0f)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
float r;
if(is2d)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
}
if(!uIsFinite(r))
{
continue;
}
if(rangeMin > 0.0f && r < rangeMinSqrd)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
}
}
if(rangeMin > 0.0f && r < rangeMinSqrd)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
}
cv::Mat(scan.data(), cv::Range(j,j+1), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
++oi;
}
int previousSize = scan.size();
int scanMaxPtsTmp = scan.maxPoints();
corelib/src/util3d_mapping.cpp
-4
View File
@@ -655,7 +655,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
map = cv::Mat::ones((yMax - yMin) / cellSize, (xMax - xMin) / cellSize, CV_8S)*-1;
UDEBUG("map size = %dx%d", map.cols, map.rows);
int j=0;
float scanMaxRangeSqr = scanMaxRange * scanMaxRange;
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
@@ -741,14 +740,12 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
}
}
}
++j;
}
UDEBUG("Ray trace known space=%fs", timer.ticks());
// now fill unknown spaces
if(unknownSpaceFilled && scanMaxRange > 0)
{
j=0;
float angleIncrement = CV_PI/90.0f; // angle increment
for(std::map<int, std::pair<cv::Mat, cv::Mat> >::iterator iter = localScans.begin(); iter!=localScans.end(); ++iter)
{
@@ -813,7 +810,6 @@ cv::Mat create2DMap(const std::map<int, Transform> & poses,
}
}
}
++j;
}
UDEBUG("Fill empty space=%fs", timer.ticks());
//cv::imwrite("map.png", util3d::convertMap2Image8U(map));
corelib/src/util3d_surface.cpp
+16 -13
View File
@@ -3518,22 +3518,25 @@ LaserScan adjustNormalsToViewPoint(
int nz = ny+1;
cv::Mat output = scan.data().clone();
#pragma omp parallel for
for(int i=0; i<scan.size(); ++i)
for(int j=0; j<scan.data().rows; ++j)
{
float * ptr = output.ptr<float>(0, i);
if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
for(int i=0; i<scan.data().cols; ++i)
{
Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
float result = v.dot(n);
if(result < 0
|| (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
float * ptr = output.ptr<float>(j, i);
if(uIsFinite(ptr[nx]) && uIsFinite(ptr[ny]) && uIsFinite(ptr[nz]))
{
//reverse normal
ptr[nx] *= -1.0f;
ptr[ny] *= -1.0f;
ptr[nz] *= -1.0f;
Eigen::Vector3f v = viewpoint - Eigen::Vector3f(ptr[0], ptr[1], ptr[2]);
Eigen::Vector3f n(ptr[nx], ptr[ny], ptr[nz]);
float result = v.dot(n);
if(result < 0
|| (groundNormalsUp>0.0f && ptr[nz] < -groundNormalsUp && ptr[2] < viewpoint[3])) // some far velodyne rays on road can have normals toward ground
{
//reverse normal
ptr[nx] *= -1.0f;
ptr[ny] *= -1.0f;
ptr[nz] *= -1.0f;
}
}
}
}
data/presets/camera_tof_icp.ini
+31
View File
@@ -0,0 +1,31 @@
# Could be used with TOF cameras like Kinect v2, Kinect For Azure or L515
[Camera]
Scan\downsampleStep = 4
Scan\fromDepth = true
Scan\normalsK = 20
Scan\normalsRadius = 0
Scan\normalsUp = false
Scan\rangeMax = 0
Scan\rangeMin = 0
Scan\voxelSize = 0.05
[Gui]
General\showClouds0 = false
General\showClouds1 = false
[Core]
Icp\CorrespondenceRatio = 0.2
Icp\Epsilon = 0.005
Icp\OutlierRatio = 0.65
Icp\PointToPlaneMinComplexity = 0
Icp\Strategy = 0
Icp\VoxelSize = 0
Odom\Deskewing = false
Odom\ScanKeyFrameThr = 0.7
OdomF2M\ScanMaxSize = 15000
# match the input voxel size:
OdomF2M\ScanSubtractRadius = 0.05
RGBD\ProximityPathMaxNeighbors = 1
# Make sure PM is used:
Reg\Strategy = 1
data/presets/lidar3d_icp.ini
+46
View File
@@ -0,0 +1,46 @@
# Could be used with LiDARs like Velodyne, RoboSense, Ouster
[Camera]
Scan\downsampleStep = 1
Scan\fromDepth = false
Scan\normalsK = 0
Scan\normalsRadius = 0
Scan\normalsUp = false
Scan\rangeMax = 0
Scan\rangeMin = 0
Scan\voxelSize = 0
[Gui]
General\showClouds0 = false
General\showClouds1 = false
[Core]
# Would be 0.01 for odom and 0.2 for mapping:
Icp\CorrespondenceRatio = 0.1
Icp\Epsilon = 0.001
Icp\FiltersEnabled = 2
Icp\Iterations = 10
Icp\OutlierRatio = 0.7
# ~10x the voxel size:
Icp\MaxCorrespondenceDistance = 0.5
Icp\MaxTranslation = 2
# Uncomment if lidar can see ground most of the time (on a car or wheeled robot):
#Icp\PointToPlaneGroundNormalsUp = 0.8
Icp\PointToPlaneK = 20
Icp\PointToPlaneMinComplexity = 0
# Make sure PM is used:
Icp\Strategy = 1
Icp\VoxelSize = 0.05
Mem\NotLinkedNodesKept = false
Mem\STMSize = 30
Odom\Deskewing = true
Odom\GuessSmoothingDelay = 0.3
Odom\ScanKeyFrameThr = 0.6
OdomF2M\ScanMaxSize = 15000
# Match voxel size:
OdomF2M\ScanSubtractRadius = 0.05
RGBD\AngularUpdate = 0.05
RGBD\LinearUpdate = 0.05
RGBD\ProximityMaxGraphDepth = 0
RGBD\ProximityPathMaxNeighbors = 1
Reg\Strategy = 1
examples/CMakeLists.txt
+1
View File
@@ -5,6 +5,7 @@ IF(TARGET rtabmap_gui)
ADD_SUBDIRECTORY( RGBDMapping )
ADD_SUBDIRECTORY( WifiMapping )
ADD_SUBDIRECTORY( NoEventsExample )
ADD_SUBDIRECTORY( LidarMapping )
ELSE()
MESSAGE(STATUS "RTAB-Map GUI lib is not built, the RGBDMapping and WifiMapping examples will not be built...")
ENDIF()

Some files were not shown because too many files have changed in this diff Show More