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(), R_i_2_c, T_i_2_c ) ; 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(); // R_vi_2_odometry.setIdentity() ; // if ( onUAV ) // { // 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 ); // } // else // { // 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, Eigen::Matrix3d::Identity(), // 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 ; } if ( onUAV ){ pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0, colorFlag, monoOdometry->path_line, monoOdometry->pub_path, R_vi_2_odometry); } else{ pubPath(monoOdometry->slidingWindow[monoOdometry->tail]->T_bk_2_b0, colorFlag, monoOdometry->path_line, monoOdometry->pub_path, Eigen::Matrix3d::Identity() ); } #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 logCameraPose() ; // 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) ; } }
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); } }
//////////////////////////////////////////////////////////////////////////////// // Update the controller void GazeboRosIMU::UpdateChild() { common::Time cur_time = this->world_->GetSimTime(); // rate control if (this->update_rate_ > 0 && (cur_time - this->last_time_).Double() < (1.0 / this->update_rate_)) return; if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) { ignition::math::Pose3d pose; ignition::math::Quaterniond rot; ignition::math::Vector3d pos; // Get Pose/Orientation ///@todo: verify correctness #if GAZEBO_MAJOR_VERSION >= 8 pose = this->link->WorldPose(); #else pose = this->link->GetWorldPose().Ign(); #endif // apply xyz offsets and get position and rotation components pos = pose.Pos() + this->offset_.Pos(); rot = pose.Rot(); // apply rpy offsets rot = this->offset_.Rot()*rot; rot.Normalize(); // get Rates ignition::math::Vector3d vpos = this->link->GetWorldLinearVel().Ign(); ignition::math::Vector3d veul = this->link->GetWorldAngularVel().Ign(); // differentiate to get accelerations double tmp_dt = this->last_time_.Double() - cur_time.Double(); if (tmp_dt != 0) { this->apos_ = (this->last_vpos_ - vpos) / tmp_dt; this->aeul_ = (this->last_veul_ - veul) / tmp_dt; this->last_vpos_ = vpos; this->last_veul_ = veul; } // copy data into pose message this->imu_msg_.header.frame_id = this->frame_name_; this->imu_msg_.header.stamp.sec = cur_time.sec; this->imu_msg_.header.stamp.nsec = cur_time.nsec; // orientation quaternion // uncomment this if we are reporting orientation in the local frame // not the case for our imu definition // // apply fixed orientation offsets of initial pose // rot = this->initial_pose_.Rot()*rot; // rot.Normalize(); this->imu_msg_.orientation.x = rot.X(); this->imu_msg_.orientation.y = rot.Y(); this->imu_msg_.orientation.z = rot.Z(); this->imu_msg_.orientation.w = rot.W(); // pass euler angular rates ignition::math::Vector3d linear_velocity( veul.X() + this->GaussianKernel(0, this->gaussian_noise_), veul.Y() + this->GaussianKernel(0, this->gaussian_noise_), veul.Z() + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_velocity = rot.RotateVector(linear_velocity); this->imu_msg_.angular_velocity.x = linear_velocity.X(); this->imu_msg_.angular_velocity.y = linear_velocity.Y(); this->imu_msg_.angular_velocity.z = linear_velocity.Z(); // pass accelerations ignition::math::Vector3d linear_acceleration( apos_.X() + this->GaussianKernel(0, this->gaussian_noise_), apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_), apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_acceleration = rot.RotateVector(linear_acceleration); this->imu_msg_.linear_acceleration.x = linear_acceleration.X(); this->imu_msg_.linear_acceleration.y = linear_acceleration.Y(); this->imu_msg_.linear_acceleration.z = linear_acceleration.Z(); // fill in covariance matrix /// @todo: let user set separate linear and angular covariance values. /// @todo: apply appropriate rotations from frame_pose double gn2 = this->gaussian_noise_*this->gaussian_noise_; this->imu_msg_.orientation_covariance[0] = gn2; this->imu_msg_.orientation_covariance[4] = gn2; this->imu_msg_.orientation_covariance[8] = gn2; this->imu_msg_.angular_velocity_covariance[0] = gn2; this->imu_msg_.angular_velocity_covariance[4] = gn2; this->imu_msg_.angular_velocity_covariance[8] = gn2; this->imu_msg_.linear_acceleration_covariance[0] = gn2; this->imu_msg_.linear_acceleration_covariance[4] = gn2; this->imu_msg_.linear_acceleration_covariance[8] = gn2; { boost::mutex::scoped_lock lock(this->lock_); // publish to ros if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "") this->pub_Queue->push(this->imu_msg_, this->pub_); } // save last time stamp this->last_time_ = cur_time; } }
//////////////////////////////////////////////////////////////////////////////// // Update the controller void GazeboRosIMU::UpdateChild() { if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")) { math::Pose pose; math::Quaternion rot; math::Vector3 pos; // Get Pose/Orientation ///@todo: verify correctness pose = this->link->GetWorldPose(); // apply xyz offsets and get position and rotation components pos = pose.pos + this->offset_.pos; rot = pose.rot; // apply rpy offsets rot = this->offset_.rot*rot; rot.Normalize(); common::Time cur_time = this->world_->GetSimTime(); // get Rates math::Vector3 vpos = this->link->GetWorldLinearVel(); math::Vector3 veul = this->link->GetWorldAngularVel(); // differentiate to get accelerations double tmp_dt = this->last_time_.Double() - cur_time.Double(); if (tmp_dt != 0) { this->apos_ = (this->last_vpos_ - vpos) / tmp_dt; this->aeul_ = (this->last_veul_ - veul) / tmp_dt; this->last_vpos_ = vpos; this->last_veul_ = veul; } // copy data into pose message this->imu_msg_.header.frame_id = this->frame_name_; this->imu_msg_.header.stamp.sec = cur_time.sec; this->imu_msg_.header.stamp.nsec = cur_time.nsec; // orientation quaternion // uncomment this if we are reporting orientation in the local frame // not the case for our imu definition // // apply fixed orientation offsets of initial pose // rot = this->initial_pose_.rot*rot; // rot.Normalize(); this->imu_msg_.orientation.x = rot.x; this->imu_msg_.orientation.y = rot.y; this->imu_msg_.orientation.z = rot.z; this->imu_msg_.orientation.w = rot.w; // pass euler angular rates math::Vector3 linear_velocity( veul.x + this->GaussianKernel(0, this->gaussian_noise_), veul.y + this->GaussianKernel(0, this->gaussian_noise_), veul.z + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_velocity = rot.RotateVector(linear_velocity); this->imu_msg_.angular_velocity.x = linear_velocity.x; this->imu_msg_.angular_velocity.y = linear_velocity.y; this->imu_msg_.angular_velocity.z = linear_velocity.z; // pass accelerations math::Vector3 linear_acceleration( apos_.x + this->GaussianKernel(0, this->gaussian_noise_), apos_.y + this->GaussianKernel(0, this->gaussian_noise_), apos_.z + this->GaussianKernel(0, this->gaussian_noise_)); // rotate into local frame // @todo: deal with offsets! linear_acceleration = rot.RotateVector(linear_acceleration); this->imu_msg_.linear_acceleration.x = linear_acceleration.x; this->imu_msg_.linear_acceleration.y = linear_acceleration.y; this->imu_msg_.linear_acceleration.z = linear_acceleration.z; // fill in covariance matrix /// @todo: let user set separate linear and angular covariance values. /// @todo: apply appropriate rotations from frame_pose double gn2 = this->gaussian_noise_*this->gaussian_noise_; this->imu_msg_.orientation_covariance[0] = gn2; this->imu_msg_.orientation_covariance[4] = gn2; this->imu_msg_.orientation_covariance[8] = gn2; this->imu_msg_.angular_velocity_covariance[0] = gn2; this->imu_msg_.angular_velocity_covariance[4] = gn2; this->imu_msg_.angular_velocity_covariance[8] = gn2; this->imu_msg_.linear_acceleration_covariance[0] = gn2; this->imu_msg_.linear_acceleration_covariance[4] = gn2; this->imu_msg_.linear_acceleration_covariance[8] = gn2; { boost::mutex::scoped_lock lock(this->lock_); // publish to ros if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "") this->pub_.publish(this->imu_msg_); } // save last time stamp this->last_time_ = cur_time; } }