i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-10 20:09:51 +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
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