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();
}