// Finds the 3D position of a point (in reference frame B) given the inputs
TooN::Vector<3> MapMakerServerBase::ReprojectPoint(TooN::SE3<> se3AfromB, const TooN::Vector<3>& v3A,
const TooN::Vector<3>& v3B)
{
// Algorithm from Hartley & Zisserman's Multiple View Geometry book section 12.2
TooN::Matrix<3, 4> PDash;
PDash.slice<0, 0, 3, 3>() = se3AfromB.get_rotation().get_matrix();
PDash.slice<0, 3, 3, 1>() = se3AfromB.get_translation().as_col();
TooN::Matrix<4> A;
A[0][0] = -v3B[2];
A[0][1] = 0.0;
A[0][2] = v3B[0];
A[0][3] = 0.0;
A[1][0] = 0.0;
A[1][1] = -v3B[2];
A[1][2] = v3B[1];
A[1][3] = 0.0;
A[2] = v3A[0] * PDash[2] - v3A[2] * PDash[0];
A[3] = v3A[1] * PDash[2] - v3A[2] * PDash[1];
TooN::SVD<4, 4> svd(A);
TooN::Vector<4> v4Smallest = svd.get_VT()[3];
if (v4Smallest[3] == 0.0)
v4Smallest[3] = 0.00001;
return project(v4Smallest);
}
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));
}
}
// Publish the current tracker pose
void SystemFrontendBase::PublishPose()
{
ROS_ASSERT(mpTracker);
if (mpTracker->GetTrackingQuality() == Tracker::NONE || mpTracker->IsLost())
return;
geometry_msgs::PoseWithCovarianceStamped pose_cov_msg;
pose_cov_msg.header.stamp = mpTracker->GetCurrentTimestamp();
pose_cov_msg.header.frame_id = "vision_world";
TooN::SE3<> pose = mpTracker->GetCurrentPose().inverse();
TooN::Matrix<3> rot = pose.get_rotation().get_matrix();
TooN::Matrix<6> cov = mpTracker->GetCurrentCovariance();
// clear cross correlation
cov.slice<0, 3, 3, 3>() = TooN::Zeros;
cov.slice<3, 0, 3, 3>() = TooN::Zeros;
// Change covariance matrix frame from camera to world
cov.slice<0, 0, 3, 3>() = rot * cov.slice<0, 0, 3, 3>() * rot.T();
cov.slice<3, 3, 3, 3>() = rot * cov.slice<3, 3, 3, 3>() * rot.T();
// Some hacky heuristics here
if (mpTracker->GetTrackingQuality() == Tracker::GOOD)
{
cov = cov * 1e2;
}
else if (mpTracker->GetTrackingQuality() == Tracker::DODGY)
{
cov = cov * 1e5;
}
else if (mpTracker->GetTrackingQuality() == Tracker::BAD)
{
cov = cov * 1e8;
}
pose_cov_msg.pose.pose = util::SE3ToPoseMsg(pose);
int numElems = 6;
for (int i = 0; i < numElems; ++i)
{
for (int j = 0; j < numElems; ++j)
{
pose_cov_msg.pose.covariance[i * numElems + j] = cov(i, j);
}
}
mPoseWithCovPub.publish(pose_cov_msg);
static tf::TransformBroadcaster br;
tf::Transform transform;
tf::poseMsgToTF(pose_cov_msg.pose.pose, transform);
br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", "vision_pose"));
SE3Map mPoses = mpTracker->GetCurrentCameraPoses();
geometry_msgs::PoseArray pose_array_msg;
pose_array_msg.header.stamp = pose_cov_msg.header.stamp;
pose_array_msg.header.frame_id = pose_cov_msg.header.frame_id;
pose_array_msg.poses.resize(1 + mPoses.size());
pose_array_msg.poses[0] = pose_cov_msg.pose.pose;
int i = 1;
for (SE3Map::iterator it = mPoses.begin(); it != mPoses.end(); ++it, ++i)
{
pose_array_msg.poses[i] = util::SE3ToPoseMsg(it->second.inverse());
tf::poseMsgToTF(pose_array_msg.poses[i], transform);
br.sendTransform(tf::StampedTransform(transform, mpTracker->GetCurrentTimestamp(), "vision_world", it->first));
}
mPoseArrayPub.publish(pose_array_msg);
}