void EstimationNode::publishTf(TooN::SE3<> trans, ros::Time stamp, int seq, std::string system) { trans = trans.inverse(); tf::Matrix3x3 m; m[0][0] = trans.get_rotation().get_matrix()(0,0); m[0][1] = trans.get_rotation().get_matrix()(0,1); m[0][2] = trans.get_rotation().get_matrix()(0,2); m[1][0] = trans.get_rotation().get_matrix()(1,0); m[1][1] = trans.get_rotation().get_matrix()(1,1); m[1][2] = trans.get_rotation().get_matrix()(1,2); m[2][0] = trans.get_rotation().get_matrix()(2,0); m[2][1] = trans.get_rotation().get_matrix()(2,1); m[2][2] = trans.get_rotation().get_matrix()(2,2); tf::Vector3 v; v[0] = trans.get_translation()[0]; v[1] = trans.get_translation()[1]; v[2] = trans.get_translation()[2]; tf::Transform tr = tf::Transform(m,v); tf::StampedTransform t = tf::StampedTransform(tr,stamp,"map",system); tf_broadcaster.sendTransform(t); if(logfilePTAMRaw != NULL) { pthread_mutex_lock(&(logPTAMRaw_CS)); // log: // - filterPosePrePTAM estimated for videoFrameTimestamp-delayVideo. // - PTAMResulttransformed estimated for videoFrameTimestamp-delayVideo. (using imu only for last step) // - predictedPoseSpeed estimated for lastNfoTimestamp+filter->delayControl (actually predicting) // - predictedPoseSpeedATLASTNFO estimated for lastNfoTimestamp (using imu only) if(logfilePTAMRaw != NULL) (*(logfilePTAMRaw)) << seq << " " << stamp << " " << tr.getOrigin().x() << " " << tr.getOrigin().y() << " " << tr.getOrigin().z() << " " << tr.getRotation().x() << " " << tr.getRotation().y() << " " << tr.getRotation().z() << " " << tr.getRotation().w() << std::endl; pthread_mutex_unlock(&(logPTAMRaw_CS)); } }
// Rotates/translates the whole map and all keyFrames void MapMakerServerBase::ApplyGlobalTransformationToMap(TooN::SE3<> se3NewFromOld) { for (MultiKeyFramePtrList::iterator it = mMap.mlpMultiKeyFrames.begin(); it != mMap.mlpMultiKeyFrames.end(); ++it) { MultiKeyFrame& mkf = *(*it); mkf.mse3BaseFromWorld = mkf.mse3BaseFromWorld * se3NewFromOld.inverse(); for (KeyFramePtrMap::iterator jiter = mkf.mmpKeyFrames.begin(); jiter != mkf.mmpKeyFrames.end(); ++jiter) { KeyFrame& kf = *(jiter->second); kf.mse3CamFromWorld = kf.mse3CamFromBase * mkf.mse3BaseFromWorld; } } for (MapPointPtrList::iterator it = mMap.mlpPoints.begin(); it != mMap.mlpPoints.end(); ++it) { MapPoint& point = *(*it); point.mv3WorldPos = se3NewFromOld * point.mv3WorldPos; point.RefreshPixelVectors(); } }
void AbstractView::drawPose3D(const TooN::SE3<double> & pose) { glPushMatrix(); Vector<3> center = RobotVision::trans2center(pose); glTranslate(center); Vector<3> aa = pose.inverse().get_rotation().ln(); double angle = norm(aa); if ( angle != 0.0 ) { glRotatef( angle * 180.0 / M_PI, aa[0], aa[1], aa[2]); } drawLine3D(TooN::makeVector(0,0,0), TooN::makeVector(0.05, 0, 0)); drawLine3D(TooN::makeVector(0,0,0), TooN::makeVector(0, 0.05, 0)); drawLine3D(TooN::makeVector(0,0,0), TooN::makeVector(0, 0, 0.05)); drawLine3D(TooN::makeVector(0.025,0.025,0), TooN::makeVector(-0.025, 0.025, 0)); drawLine3D(TooN::makeVector(-0.025,-0.025,0), TooN::makeVector(-0.025, 0.025, 0)); drawLine3D(TooN::makeVector(-0.025,-0.025,0), TooN::makeVector(0.025, -0.025, 0)); drawLine3D(TooN::makeVector(0.025,0.025,0), TooN::makeVector(0.025, -0.025, 0)); glPopMatrix(); }
void PosedCamera::SetPose(const toon::SE3<>& pose) { pose_ = pose; invpose_ = pose.inverse(); }
void AbstractView::setPose(const TooN::SE3<>& T_wc ) { T_cw_ = T_wc.inverse(); }