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

CameraFreenect: set accelerometer values in IMU member of SensorData. OdomF2M and OdomF2F: intialize orientation with gravity for the first frame if accelerometer value is valid in SensorData.

Browse Source
This commit is contained in:
matlabbe
2018-12-01 00:25:48 -05:00
parent 8a8f46c325
commit abbcc4f8a9
3 changed files with 67 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
corelib/src/camera/CameraFreenect.cpp
View File

@@ -166,6 +166,13 @@ class FreenectDevice : public UThread {
} }
} }
void getAccelerometerValues(double & x, double & y, double & z)
{
freenect_update_tilt_state(device_);
freenect_raw_tilt_state* state = freenect_get_tilt_state(device_);
freenect_get_mks_accel(state, &x,&y,&z);
}
private: private:
// Do not call directly even in child // Do not call directly even in child
void VideoCallback(void* rgb) void VideoCallback(void* rgb)
@@ -216,6 +223,7 @@ private:
virtual void mainLoopBegin() virtual void mainLoopBegin()
{ {
if(device_) freenect_set_led(device_, LED_RED);
this->startDepth(); this->startDepth();
this->startVideo(); this->startVideo();
} }
@@ -234,6 +242,7 @@ private:
virtual void mainLoopEnd() virtual void mainLoopEnd()
{ {
if(device_) freenect_set_led(device_, LED_GREEN);
this->stopDepth(); this->stopDepth();
this->stopVideo(); this->stopVideo();
dataReady_.release(); dataReady_.release();
@@ -294,7 +303,7 @@ CameraFreenect::CameraFreenect(int deviceId, Type type, float imageRate, const T
#ifdef RTABMAP_FREENECT #ifdef RTABMAP_FREENECT
if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library"); if(freenect_init(&ctx_, NULL) < 0) UERROR("Cannot initialize freenect library");
// claim camera // claim camera
freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_CAMERA)); freenect_select_subdevices(ctx_, static_cast<freenect_device_flags>(FREENECT_DEVICE_MOTOR | FREENECT_DEVICE_CAMERA));
#endif #endif
} }
@@ -459,6 +468,14 @@ SensorData CameraFreenect::captureImage(CameraInfo * info)
model.setLocalTransform(this->getLocalTransform()); model.setLocalTransform(this->getLocalTransform());
data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now()); data = SensorData(rgb, depth, model, this->getNextSeqID(), UTimer::now());
double x=0,y=0,z=0;
freenectDevice_->getAccelerometerValues(x,y,z);
if(x != 0.0 && y != 0.0 && z != 0.0)
{
// frame of imu on kinect is x->right, y->down, z->backward
data.setIMU(IMU(cv::Vec3d(0,0,0), cv::Mat(), cv::Vec3d(x, y, z), cv::Mat(), Transform(0,0,-1,0, -1,0,0,0, 0,-1,0,0)));
}
} }
} }
else else

24
corelib/src/odometry/OdometryF2F.cpp
View File

@@ -91,6 +91,30 @@ Transform OdometryF2F::computeTransform(
UASSERT(!this->getPose().isNull()); UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull()) if(lastKeyFramePose_.isNull())
{ {
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
lastKeyFramePose_ = this->getPose(); // reset to current pose lastKeyFramePose_ = this->getPose(); // reset to current pose
} }
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose(); Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();

25
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -938,6 +938,31 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0; regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false; bool frameValid = false;
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
Transform newFramePose = this->getPose(); // initial pose may be not identity... Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired()) if(regPipeline_->isImageRequired())
{ {