void LiveSLAMWrapper::popAndSetGravity()
{
unsigned int image0BufSize ;
unsigned int image1BufSize ;
std::list<ImageMeasurement>::reverse_iterator reverse_iterImage ;
ros::Time tImage ;
ros::Rate r(100) ;
gravity_b0.setZero() ;
while ( nh.ok() )
{
ros::spinOnce() ;
image0_queue_mtx.lock();
image1_queue_mtx.lock();
imu_queue_mtx.lock();
image0BufSize = image0Buf.size();
image1BufSize = image1Buf.size();
if ( image0BufSize < 15 || image1BufSize < 15 || imuQueue.size() < 120 ){
image0_queue_mtx.unlock();
image1_queue_mtx.unlock();
imu_queue_mtx.unlock();
r.sleep() ;
continue ;
}
reverse_iterImage = image0Buf.rbegin() ;
tImage = reverse_iterImage->t ;
reverse_iterImage = image1Buf.rbegin() ;
if ( reverse_iterImage->t < tImage ){
tImage = reverse_iterImage->t ;
}
pImage0Iter = image0Buf.begin();
pImage1Iter = image1Buf.begin();
while ( pImage0Iter->t < tImage ){
pImage0Iter = image0Buf.erase( pImage0Iter ) ;
}
while ( pImage1Iter->t < tImage ){
pImage1Iter = image1Buf.erase( pImage1Iter ) ;
}
image0_queue_mtx.unlock();
image1_queue_mtx.unlock();
//imu_queue_mtx.lock();
int imuNum = 0;
currentIMU_iter = imuQueue.begin() ;
while( currentIMU_iter->header.stamp < tImage )
{
imuNum++;
gravity_b0(0) += currentIMU_iter->linear_acceleration.x;
gravity_b0(1) += currentIMU_iter->linear_acceleration.y;
gravity_b0(2) += currentIMU_iter->linear_acceleration.z;
currentIMU_iter = imuQueue.erase(currentIMU_iter);
}
imu_queue_mtx.unlock();
gravity_b0 /= imuNum ;
//gravity_b0 = -gravity_b0 ;
break ;
}
initialTime = tImage ;
std::cout << "gravity_b0 =\n" ;
std::cout << gravity_b0.transpose() << "\n" ;
monoOdometry->gravity_b0 = gravity_b0 ;
printf("gravity_b0.norm() = %lf\n", gravity_b0.norm() );
//imu_queue_mtx.unlock();
cv::Mat image0 = pImage0Iter->image.clone();
cv::Mat image1 = pImage1Iter->image.clone();
//image1_queue_mtx.unlock();
//image0_queue_mtx.unlock();
monoOdometry->insertFrame(imageSeqNumber, image0, tImage,
Eigen::Matrix3d::Identity(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero() );
cv::Mat disparity, depth ;
monoOdometry->bm_(image0, image1, disparity, CV_32F);
calculateDepthImage(disparity, depth, 0.11, fx );
monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[0] ;
monoOdometry->currentKeyFrame->setDepthFromGroundTruth( (float*)depth.data ) ;
monoOdometry->currentKeyFrame->keyFrameFlag = true ;
monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
monoOdometry->RefToFrame = Sophus::SE3() ;
monoOdometry->margin.initPrior();
monoOdometry->updateTrackingReference();
pubOdometry(Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(),
Eigen::Matrix3d::Identity(),
monoOdometry->pub_odometry, monoOdometry->pub_pose,
0, R_vi_2_odometry,
true, tImage );
lastLoopClorsureTime = tImage ;
}
void LiveSLAMWrapper::BALoop()
{
ros::Rate BARate(2000) ;
list<ImageMeasurement>::iterator iterImage ;
std::list<visensor_node::visensor_imu>::iterator iterIMU ;
cv::Mat image0 ;
cv::Mat image1 ;
cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
sensor_msgs::Image msg;
double t ;
while ( nh.ok() )
{
monoOdometry->frameInfoList_mtx.lock();
int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
if ( ttt < 0 ){
ttt += frameInfoListSize ;
}
//printf("[BA thread] sz=%d\n", ttt ) ;
if ( ttt < 1 ){
monoOdometry->frameInfoList_mtx.unlock();
BARate.sleep() ;
continue ;
}
for ( int sz ; ; )
{
monoOdometry->frameInfoListHead++ ;
if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
monoOdometry->frameInfoListHead -= frameInfoListSize ;
}
sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
if ( sz == 0 ){
break ;
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
break ;
}
}
ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
monoOdometry->frameInfoList_mtx.unlock();
//Pop out the image list
image1_queue_mtx.lock();
iterImage = image1Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image1Buf.erase( iterImage ) ;
}
image1 = iterImage->image.clone();
image1_queue_mtx.unlock();
image0_queue_mtx.lock();
iterImage = image0Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image0Buf.erase( iterImage ) ;
}
image0 = iterImage->image.clone();
image0_queue_mtx.unlock();
imu_queue_mtx.lock();
iterIMU = imuQueue.begin() ;
Vector3d linear_acceleration;
Vector3d angular_velocity;
//std::cout << "imageTime=" << imageTimeStamp << std::endl;
while ( iterIMU->header.stamp < imageTimeStamp )
{
linear_acceleration(0) = iterIMU->linear_acceleration.x;
linear_acceleration(1) = iterIMU->linear_acceleration.y;
linear_acceleration(2) = iterIMU->linear_acceleration.z;
angular_velocity(0) = iterIMU->angular_velocity.x;
angular_velocity(1) = iterIMU->angular_velocity.y;
angular_velocity(2) = iterIMU->angular_velocity.z;
//linear_acceleration = -linear_acceleration;
//angular_velocity = -angular_velocity ;
// double pre_t = iterIMU->header.stamp.toSec();
// iterIMU = imuQueue.erase(iterIMU);
// //std::cout << imuQueue.size() <<" "<< iterIMU->header.stamp << std::endl;
// double next_t = iterIMU->header.stamp.toSec();
// double dt = next_t - pre_t ;
double pre_t = iterIMU->header.stamp.toSec();
iterIMU = imuQueue.erase(iterIMU);
double next_t = iterIMU->header.stamp.toSec();
double dt = next_t - pre_t ;
// std::cout << linear_acceleration.transpose() << std::endl ;
// std::cout << angular_velocity.transpose() << std::endl ;
monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
}
imu_queue_mtx.unlock();
//propagate the last frame info to the current frame
Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
float dt = lastFrame->timeIntegral;
Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
+ lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt + lastFrame->alpha_c_k);
Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
(lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
+ lastFrame->beta_c_k);
Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;
monoOdometry->insertFrame(imageSeqNumber, image1, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
Frame* keyFrame = monoOdometry->currentKeyFrame.get();
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
{
//prepare key frame
cv::Mat disparity, depth ;
monoOdometry->bm_(image1, image0, disparity, CV_32F);
calculateDepthImage(disparity, depth, 0.11, fx );
currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;
//pub debugMap
cv::cvtColor(image1, gradientMapForDebug, CV_GRAY2BGR);
monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
msg.header.stamp = imageTimeStamp;
sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
width, width*3, gradientMapForDebug.data );
monoOdometry->pub_gradientMapForDebug.publish(msg) ;
//set key frame
monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
monoOdometry->currentKeyFrame->keyFrameFlag = true ;
monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
//reset the initial guess
monoOdometry->RefToFrame = Sophus::SE3() ;
//unlock dense tracking
monoOdometry->tracking_mtx.lock();
monoOdometry->lock_densetracking = false;
monoOdometry->tracking_mtx.unlock();
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
{
// cout << "insert camera link" << endl ;
// cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;
monoOdometry->insertCameraLink(keyFrame, currentFrame,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
}
cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//BA
t = (double)cvGetTickCount() ;
monoOdometry->BA();
printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//marginalziation
monoOdometry->twoWayMarginalize();
monoOdometry->setNewMarginalzationFlag();
// pubOdometry(-T_bk1_2_b0, R_bk1_2_b0, pub_odometry, pub_pose );
// pubPath(-T_bk1_2_b0, path_line, pub_path );
pubOdometry(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
monoOdometry->pub_odometry, monoOdometry->pub_pose );
pubPath(-monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag,
monoOdometry->path_line, monoOdometry->pub_path);
}
}
void LiveSLAMWrapper::BALoop()
{
ros::Rate BARate(2000) ;
list<ImageMeasurement>::iterator iterImage ;
std::list<visensor_node::visensor_imu>::iterator iterIMU ;
cv::Mat image0 ;
cv::Mat image1 ;
cv::Mat gradientMapForDebug(height, width, CV_8UC3) ;
sensor_msgs::Image msg;
double t ;
while ( nh.ok() )
{
monoOdometry->frameInfoList_mtx.lock();
int ttt = (monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead);
if ( ttt < 0 ){
ttt += frameInfoListSize ;
}
//printf("[BA thread] sz=%d\n", ttt ) ;
if ( ttt < 1 ){
monoOdometry->frameInfoList_mtx.unlock();
BARate.sleep() ;
continue ;
}
for ( int sz ; ; )
{
monoOdometry->frameInfoListHead++ ;
if ( monoOdometry->frameInfoListHead >= frameInfoListSize ){
monoOdometry->frameInfoListHead -= frameInfoListSize ;
}
sz = monoOdometry->frameInfoListTail - monoOdometry->frameInfoListHead ;
if ( sz == 0 ){
break ;
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ){
break ;
}
}
ros::Time imageTimeStamp = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].t ;
monoOdometry->frameInfoList_mtx.unlock();
//Pop out the image list
image1_queue_mtx.lock();
iterImage = image1Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image1Buf.erase( iterImage ) ;
}
image1 = iterImage->image.clone();
image1_queue_mtx.unlock();
image0_queue_mtx.lock();
iterImage = image0Buf.begin() ;
while ( iterImage->t < imageTimeStamp ){
iterImage = image0Buf.erase( iterImage ) ;
}
image0 = iterImage->image.clone();
image0_queue_mtx.unlock();
imu_queue_mtx.lock();
iterIMU = imuQueue.begin() ;
Vector3d linear_acceleration;
Vector3d angular_velocity;
//std::cout << "imageTime=" << imageTimeStamp << std::endl;
while ( iterIMU->header.stamp < imageTimeStamp )
{
linear_acceleration(0) = iterIMU->linear_acceleration.x;
linear_acceleration(1) = iterIMU->linear_acceleration.y;
linear_acceleration(2) = iterIMU->linear_acceleration.z;
angular_velocity(0) = iterIMU->angular_velocity.x;
angular_velocity(1) = iterIMU->angular_velocity.y;
angular_velocity(2) = iterIMU->angular_velocity.z;
// to_pub_info.x = angular_velocity(0)*180/PI ;
// to_pub_info.y = angular_velocity(1)*180/PI ;
// to_pub_info.z = angular_velocity(2)*180/PI ;
// monoOdometry->pub_angular_velocity.publish( to_pub_info ) ;
// outFile << to_pub_info.x << " "
// << to_pub_info.y << " "
// << to_pub_info.z << "\n";
double pre_t = iterIMU->header.stamp.toSec();
iterIMU = imuQueue.erase(iterIMU);
double next_t = iterIMU->header.stamp.toSec();
double dt = next_t - pre_t ;
// std::cout << linear_acceleration.transpose() << std::endl ;
// std::cout << angular_velocity.transpose() << std::endl ;
monoOdometry->processIMU( dt, linear_acceleration, angular_velocity );
}
imu_queue_mtx.unlock();
//propagate the last frame info to the current frame
Frame* lastFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
float dt = lastFrame->timeIntegral;
Vector3d T_bk1_2_b0 = lastFrame->T_bk_2_b0 - 0.5 * gravity_b0 * dt *dt
+ lastFrame->R_bk_2_b0*(lastFrame->v_bk * dt + lastFrame->alpha_c_k);
Vector3d v_bk1 = lastFrame->R_k1_k.transpose() *
(lastFrame->v_bk - lastFrame->R_bk_2_b0.transpose() * gravity_b0 * dt
+ lastFrame->beta_c_k);
Matrix3d R_bk1_2_b0 = lastFrame->R_bk_2_b0 * lastFrame->R_k1_k;
monoOdometry->insertFrame(imageSeqNumber, image0, imageTimeStamp, R_bk1_2_b0, T_bk1_2_b0, v_bk1);
Frame* currentFrame = monoOdometry->slidingWindow[monoOdometry->tail].get();
Frame* keyFrame = monoOdometry->currentKeyFrame.get();
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag )
{
//prepare key frame
cv::Mat disparity, depth ;
monoOdometry->bm_(image0, image1, disparity, CV_32F);
calculateDepthImage(disparity, depth, 0.11, fx );
currentFrame->setDepthFromGroundTruth( (float*)depth.data ) ;
#ifdef PRINT_DEBUG_INFO
//pub debugMap
cv::cvtColor(image0, gradientMapForDebug, CV_GRAY2BGR);
monoOdometry->generateDubugMap(currentFrame, gradientMapForDebug);
msg.header.stamp = imageTimeStamp;
sensor_msgs::fillImage(msg, sensor_msgs::image_encodings::BGR8, height,
width, width*3, gradientMapForDebug.data );
monoOdometry->pub_gradientMapForDebug.publish(msg) ;
#endif
int preKeyFrameID = monoOdometry->currentKeyFrame->id() ;
//set key frame
monoOdometry->currentKeyFrame = monoOdometry->slidingWindow[monoOdometry->tail] ;
monoOdometry->currentKeyFrame->keyFrameFlag = true ;
monoOdometry->currentKeyFrame->cameraLinkList.clear() ;
//reset the initial guess
monoOdometry->RefToFrame = Sophus::SE3() ;
//update tracking reference
monoOdometry->updateTrackingReference();
#ifdef PUB_POINT_CLOUD
pubPointCloud(valid_num, imageTimeStamp, R_vi_2_odometry);
#endif
//unlock dense tracking
monoOdometry->tracking_mtx.lock();
monoOdometry->lock_densetracking = false;
monoOdometry->tracking_mtx.unlock();
if ( (imageTimeStamp - lastLoopClorsureTime).toSec() > 0.18 )
{
//add possible loop closure link
t = (double)cvGetTickCount() ;
monoOdometry->setReprojectionListRelateToLastestKeyFrame( monoOdometry->head, preKeyFrameID,
monoOdometry->slidingWindow[monoOdometry->tail].get() ) ;
ROS_WARN("loop closure link cost time: %f", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
lastLoopClorsureTime = imageTimeStamp ;
}
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust )
{
// cout << "insert camera link" << endl ;
// cout << monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c << endl ;
monoOdometry->insertCameraLink(keyFrame, currentFrame,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].R_k_2_c,
-monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].T_k_2_c,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].lastestATA );
}
int control_flag = 0 ;
Vector3d preBAt = currentFrame->T_bk_2_b0 ;
// cout << "[-BA]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
// cout << "[-BA]current Velocity: " << currentFrame->v_bk.transpose() << endl;
//BA
t = (double)cvGetTickCount() ;
monoOdometry->BA();
printf("BA cost time: %f\n", ((double)cvGetTickCount() - t) / (cvGetTickFrequency() * 1000) );
t = (double)cvGetTickCount() ;
//cout << "[BA-]current Position: " << currentFrame->T_bk_2_b0.transpose() << endl;
//cout << "[BA-]current Velocity: " << currentFrame->v_bk.transpose() << endl;
if ( (currentFrame->T_bk_2_b0 - preBAt ).norm() > 0.1 ){
control_flag = 1 ; //loop_closure or other sudden position change case
}
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
control_flag = 2 ; //only IMU link, dense tracking fails
}
#ifdef PUB_GRAPH
pubCameraLink();
#endif
//marginalziation
monoOdometry->twoWayMarginalize();
monoOdometry->setNewMarginalzationFlag();
pubOdometry(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
monoOdometry->slidingWindow[monoOdometry->tail]->v_bk,
monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0,
monoOdometry->pub_odometry, monoOdometry->pub_pose,
control_flag, R_vi_2_odometry,
monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag, imageTimeStamp );
#ifdef PRINT_DEBUG_INFO
int colorFlag = 0 ;
colorFlag = monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].keyFrameFlag ;
if ( monoOdometry->frameInfoList[monoOdometry->frameInfoListHead].trust == false ){
colorFlag = 2 ;
}
pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0,
colorFlag,
monoOdometry->path_line, monoOdometry->pub_path, R_vi_2_odometry);
#endif
#ifdef PUB_TF
static tf::TransformBroadcaster br;
tf::Transform transform;
Vector3d t_translation = R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 ;
Quaterniond t_q(monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0) ;
transform.setOrigin(tf::Vector3(t_translation(0),
t_translation(1),
t_translation(2)) );
tf::Quaternion q;
Quaterniond tt_q(R_vi_2_odometry * monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0 * R_vi_2_odometry.transpose());
q.setW(tt_q.w());
q.setX(tt_q.x());
q.setY(tt_q.y());
q.setZ(tt_q.z());
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "densebody"));
#endif
// int preIndex = monoOdometry->tail - 1 ;
// if ( preIndex < 0 ){
// preIndex += slidingWindowSize ;
// }
// Vector3d tt_dist = (monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0 -
// monoOdometry->slidingWindow[preIndex]->T_bk_2_b0) ;
// Matrix3d tt_rotate = monoOdometry->slidingWindow[monoOdometry->tail]->R_bk_2_b0.transpose() *
// monoOdometry->slidingWindow[preIndex]->R_bk_2_b0 ;
// Quaterniond tt_q(tt_rotate) ;
// to_pub_info.x = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(0) ;
// to_pub_info.y = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(1) ;
// to_pub_info.z = monoOdometry->slidingWindow[monoOdometry->tail]->v_bk(2) ;
// monoOdometry->pub_linear_velocity.publish(to_pub_info) ;
}
}
