diff --git a/applications/gui/CMakeLists.txt b/applications/gui/CMakeLists.txt
index cac36513e264f7176daf75eb75d5c1a8e9fb5839..8cd06d328bef06845dd5626feeaff340d6a26529 100644
--- a/applications/gui/CMakeLists.txt
+++ b/applications/gui/CMakeLists.txt
@@ -42,4 +42,6 @@ target_include_directories(ftl-gui PUBLIC
 #target_include_directories(cv-node PUBLIC ${PROJECT_SOURCE_DIR}/include)
 target_link_libraries(ftl-gui ftlcommon ftlctrl ftlrgbd ftlstreams ftlrender Threads::Threads ${OpenCV_LIBS} openvr ftlnet nanogui ${NANOGUI_EXTRA_LIBS})
 
-
+if (BUILD_TESTS)
+	add_subdirectory(test)
+endif()
diff --git a/applications/gui/src/camera.cpp b/applications/gui/src/camera.cpp
index f21cacd122eac741af38cba4809594d2168a9a81..97d217d0df31e2b6d2c885a9d518e2be8b268481 100644
--- a/applications/gui/src/camera.cpp
+++ b/applications/gui/src/camera.cpp
@@ -376,6 +376,13 @@ void ftl::gui::Camera::_draw(std::vector<ftl::rgbd::FrameSet*> &fss) {
 
 	channels_ = frame_.getChannels();
 
+	frame_.get<cv::cuda::GpuMat>(Channel::Depth).download(im_depth_);
+	cv::flip(im_depth_, im_depth_, 0);
+
+	//frame_.get<cv::cuda::GpuMat>(Channel::Normals).download(im_normals_);
+	//im_normals_.createMatHeader().convertTo(im_normals_f_, CV_32FC4);
+	//cv::flip(im_normals_f_, im_normals_f_, 0);
+
 	// Normalize depth map
 	frame_.get<cv::cuda::GpuMat>(Channel::Depth).convertTo(frame_.get<cv::cuda::GpuMat>(Channel::Depth), CV_32F, 1.0/8.0);
 
@@ -419,6 +426,18 @@ void ftl::gui::Camera::update(std::vector<ftl::rgbd::FrameSet*> &fss) {
 	framesets_ = &fss;
 	stale_frame_.clear();
 
+	if (screen_->activeCamera() == this) {
+		for (auto *fs : fss) {
+			if (!usesFrameset(fs->id)) continue;
+
+			for (auto &f : fs->frames) {
+				//if (f.hasChanged(Channel::Pose)) {
+					f.patchPose(T_);
+				//}
+			}
+		}
+	}
+
 	//if (fss.size() <= fsid_) return;
 	if (fid_ == 255) {
 		name_ = "Virtual Camera";
@@ -749,3 +768,42 @@ void ftl::gui::Camera::stopVideoRecording() {
 	if (record_stream_ && record_stream_->active()) record_stream_->end();
 }
 
+float ftl::gui::Camera::getDepth(int x, int y) {
+	if (x < 0 || y < 0) { return NAN; }
+	UNIQUE_LOCK(mutex_, lk);
+	if (x >= im_depth_.cols || y >= im_depth_.rows) { return NAN; }
+	LOG(INFO) << y << ", " << x;
+	return im_depth_.createMatHeader().at<float>(y, x);
+}
+
+cv::Point3f ftl::gui::Camera::getPoint(int x, int y) {
+	if (x < 0 || y < 0) { return cv::Point3f(); }
+	UNIQUE_LOCK(mutex_, lk);
+		LOG(INFO) << y << ", " << x;
+	if (x >= im_depth_.cols || y >= im_depth_.rows) { return cv::Point3f(); }
+	float d = im_depth_.createMatHeader().at<float>(y, x);
+
+	auto point = frame_.getLeftCamera().screenToCam(x, y, d);
+	Eigen::Vector4d p(point.x, point.y, point.z, 1.0f);
+	Eigen::Matrix4d pose = frame_.getPose();
+	Eigen::Vector4d point_eigen = pose * p;
+	return cv::Point3f(point_eigen(0), point_eigen(1), point_eigen(2));
+}
+
+/*
+cv::Point3f ftl::gui::Camera::getNormal(int x, int y) {
+	UNIQUE_LOCK(mutex_, lk);
+		LOG(INFO) << y << ", " << x;
+	if (x >= im_normals_.cols || y >= im_normals_.rows) { return cv::Point3f(); }
+	auto n = im_normals_f_.at<cv::Vec4f>(y, x);
+	return cv::Point3f(n[0], n[1], n[2]);
+}
+*/
+
+void ftl::gui::Camera::setTransform(const Eigen::Matrix4d &T) {
+	T_ = T * T_;
+}
+
+Eigen::Matrix4d ftl::gui::Camera::getTransform() const {
+	return T_;
+}
diff --git a/applications/gui/src/camera.hpp b/applications/gui/src/camera.hpp
index 99affdae20682ed4345796ede12ccfd5c8aab986..d8c2dda708e9858b4929e4613fb779dc2510a271 100644
--- a/applications/gui/src/camera.hpp
+++ b/applications/gui/src/camera.hpp
@@ -106,6 +106,14 @@ class Camera {
 
 	StatisticsImage *stats_ = nullptr;
 
+	float getDepth(int x, int y);
+	float getDepth(float x, float y) { return getDepth((int) round(x), (int) round(y)); }
+	cv::Point3f getPoint(int x, int y);
+	cv::Point3f getPoint(float x, float y) { return getPoint((int) round(x), (int) round(y)); }
+	//cv::Point3f getNormal(int x, int y);
+	//cv::Point3f getNormal(float x, float y) { return getNormal((int) round(x), (int) round(y)); }
+	void setTransform(const Eigen::Matrix4d &T);
+	Eigen::Matrix4d getTransform() const;
 
 #ifdef HAVE_OPENVR
 	bool isVR() { return vr_mode_; }
@@ -140,8 +148,12 @@ class Camera {
 	bool pause_;
 	ftl::codecs::Channel channel_;
 	ftl::codecs::Channels<0> channels_;
+	
+	cv::cuda::HostMem im_depth_;
+	//cv::cuda::HostMem im_normals_;
+	cv::Mat im_normals_f_;
+
 	cv::Mat overlay_; // first channel (left)
-	//cv::Mat im2_; // second channel ("right")
 	bool stereo_;
 	std::atomic_flag stale_frame_;
 	int rx_;
@@ -164,6 +176,7 @@ class Camera {
 
 	int transform_ix_;
 	std::array<Eigen::Matrix4d,ftl::stream::kMaxStreams> transforms_;  // Frameset transforms for virtual cam
+	Eigen::Matrix4d T_ = Eigen::Matrix4d::Identity();
 
 	MUTEX mutex_;
 
@@ -173,6 +186,9 @@ class Camera {
 	float baseline_;
 	#endif
 
+	void _downloadFrames(ftl::cuda::TextureObject<uchar4> &, ftl::cuda::TextureObject<uchar4> &);
+	void _downloadFrames(ftl::cuda::TextureObject<uchar4> &);
+	void _downloadFrames();
 	void _draw(std::vector<ftl::rgbd::FrameSet*> &fss);
 	void _applyPoseEffects(std::vector<ftl::rgbd::FrameSet*> &fss);
 	std::pair<const ftl::rgbd::Frame *, const ftl::codecs::Face *> _selectFace(std::vector<ftl::rgbd::FrameSet*> &fss);
diff --git a/applications/gui/src/screen.cpp b/applications/gui/src/screen.cpp
index 4a65046aa5262c0cccc5946779d264b72588b6f6..ea32b222b76510361ce946e25ba1af9af8d4ba22 100644
--- a/applications/gui/src/screen.cpp
+++ b/applications/gui/src/screen.cpp
@@ -19,6 +19,8 @@
 
 #include <loguru.hpp>
 
+#include <opencv2/core/eigen.hpp>
+
 #include "ctrl_window.hpp"
 #include "src_window.hpp"
 #include "config_window.hpp"
@@ -423,33 +425,47 @@ bool ftl::gui::Screen::mouseButtonEvent(const nanogui::Vector2i &p, int button,
 	} else {
 		if (!camera_) return false;
 		
-		//ol movable = camera_->source()->hasCapabilities(ftl::rgbd::kCapMovable);
-		if (button == 0 && down) {
-			Eigen::Vector2f screenSize = size().cast<float>();
-			auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff());
-			Eigen::Vector2f scaleFactor = mScale * imageSize.cwiseQuotient(screenSize);
-			Eigen::Vector2f positionInScreen(0.0f, 0.0f);
-			auto mOffset = (screenSize - (screenSize.cwiseProduct(scaleFactor))) / 2;
-			Eigen::Vector2f positionAfterOffset = positionInScreen + mOffset;
+		Eigen::Vector2f screenSize = size().cast<float>();
+		auto mScale = (screenSize.cwiseQuotient(imageSize).minCoeff());
+		Eigen::Vector2f scaleFactor = mScale * imageSize.cwiseQuotient(screenSize);
+		Eigen::Vector2f positionInScreen(0.0f, 0.0f);
+		auto mOffset = (screenSize - (screenSize.cwiseProduct(scaleFactor))) / 2;
+		Eigen::Vector2f positionAfterOffset = positionInScreen + mOffset;
 
-			float sx = ((float)p[0] - positionAfterOffset[0]) / mScale;
-			float sy = ((float)p[1] - positionAfterOffset[1]) / mScale;
+		float sx = ((float)p[0] - positionAfterOffset[0]) / mScale;
+		float sy = ((float)p[1] - positionAfterOffset[1]) / mScale;
 
-			//Eigen::Vector4f camPos;
+		if (button == 1 && down) {
+			
+			auto p = camera_->getPoint(sx, sy);
+			points_.push_back(p);
 
-			//try {
-				//camPos = camera_->source()->point(sx,sy).cast<float>();
-			//} catch(...) {
-			//	return true;
-			//}
+			//auto n = camera_->getNormal(sx, sy);
+
+			LOG(INFO) << "point: (" << p.x << ", " << p.y << ", " << p.z << ") added";
+			if (points_.size() < 2) { return true; }
 			
-			//camPos *= -1.0f;
-			//Eigen::Vector4f worldPos =  camera_->source()->getPose().cast<float>() * camPos;
-			//lookPoint_ = Eigen::Vector3f(worldPos[0],worldPos[1],worldPos[2]);
-			//LOG(INFO) << "Depth at click = " << -camPos[2];
-			return true;
-		} else {
+			auto p1 = Eigen::Vector3f(points_[0].x, points_[0].y, points_[0].z);
+			auto p2 = Eigen::Vector3f(points_[1].x, points_[1].y, points_[1].z);
 			
+			points_.clear();
+			// TODO: check p1 and p2 valid
+			if (p1 == p2) { return true; }
+			auto T = nanogui::lookAt(p1, p2, Eigen::Vector3f(0.0,1.0,0.0));
+			cv::Mat T_cv;
+			cv::eigen2cv(T, T_cv);
+			T_cv.convertTo(T_cv, CV_64FC1);
+			net_->broadcast("set_pose_adjustment", T_cv);
+			
+			return true;
+
+		}
+		else if (button == 0 && down) {
+			auto p = camera_->getPoint(sx, sy);
+			LOG(INFO) << "point: " << (Eigen::Vector4d(p.x, p.y, p.z, -1.0f)).transpose();
+
+			//auto q = camera_->getNormal(sx, sy);
+			//LOG(INFO) << "normal: " << (Eigen::Vector4d(q.x, q.y, q.z, 1.0)).transpose();
 		}
 		return false;
 	}
diff --git a/applications/gui/src/screen.hpp b/applications/gui/src/screen.hpp
index 4f683bef4173d266838dff82ecca6b44bcab282f..7af733bd78a4ff2a237074878c6e56fe163167e6 100644
--- a/applications/gui/src/screen.hpp
+++ b/applications/gui/src/screen.hpp
@@ -109,6 +109,8 @@ class Screen : public nanogui::Screen {
 	GLuint leftEye_;
 	GLuint rightEye_;
 
+	std::vector<cv::Point3d> points_;
+
 	bool show_two_images_ = false;
 
 	ftl::Configurable *shortcuts_;
diff --git a/applications/gui/test/CMakeLists.txt b/applications/gui/test/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/applications/gui/test/tests.cpp b/applications/gui/test/tests.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0c7c351f437f5f43f3bb62beb254a9f1ecbec5a0
--- /dev/null
+++ b/applications/gui/test/tests.cpp
@@ -0,0 +1,2 @@
+#define CATCH_CONFIG_MAIN
+#include "catch.hpp"
diff --git a/components/rgbd-sources/include/ftl/rgbd/camera.hpp b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
index f6ae4e1b603a3217cf78083f100b18a66926b3cf..2abcf8f8755d096d4e099e4911204e3600158367 100644
--- a/components/rgbd-sources/include/ftl/rgbd/camera.hpp
+++ b/components/rgbd-sources/include/ftl/rgbd/camera.hpp
@@ -35,7 +35,7 @@ struct __align__(16) Camera {
 	/**
 	 * Convert camera coordinates into screen coordinates.
 	 */
-	template <typename T> __device__ T camToScreen(const float3 &pos) const;
+	template <typename T> __device__ __host__ T camToScreen(const float3 &pos) const;
 
 	/**
 	 * Convert screen plus depth into camera coordinates.
@@ -70,7 +70,7 @@ struct __align__(16) Camera {
 
 // ---- IMPLEMENTATIONS --------------------------------------------------------
 
-template <> __device__
+template <> __device__ __host__
 inline float2 ftl::rgbd::Camera::camToScreen<float2>(const float3 &pos) const {
 	return make_float2(
 		static_cast<float>(pos.x*fx/pos.z - cx),			
@@ -78,19 +78,19 @@ inline float2 ftl::rgbd::Camera::camToScreen<float2>(const float3 &pos) const {
 	);
 }
 
-template <> __device__
+template <> __device__ __host__
 inline int2 ftl::rgbd::Camera::camToScreen<int2>(const float3 &pos) const {
 	float2 pImage = camToScreen<float2>(pos);
 	return make_int2(pImage + make_float2(0.5f, 0.5f));
 }
 
-template <> __device__
+template <> __device__ __host__
 inline uint2 ftl::rgbd::Camera::camToScreen<uint2>(const float3 &pos) const {
 	int2 p = camToScreen<int2>(pos);
 	return make_uint2(p.x, p.y);
 }
 
-__device__
+__device__ __host__
 inline float3 ftl::rgbd::Camera::screenToCam(uint ux, uint uy, float depth) const {
 	const float x = static_cast<float>(((float)ux+cx) / fx);
 	const float y = static_cast<float>(((float)uy+cy) / fy);
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
index c4200e0c3b6b50465b426f4caaa38b76552b5305..07115839ce2166d152575ae32b6ae92c5613c8d0 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.cpp
@@ -11,7 +11,6 @@
 #include "calibrate.hpp"
 #include "ftl/exception.hpp"
 
-
 #include <opencv2/core.hpp>
 #include <opencv2/core/utility.hpp>
 #include <opencv2/imgproc.hpp>
@@ -54,6 +53,7 @@ Calibrate::Calibrate(nlohmann::json &config, Size image_size, cv::cuda::Stream &
 	D_[0] = Mat::zeros(Size(5, 1), CV_64FC1);
 	D_[1] = Mat::zeros(Size(5, 1), CV_64FC1);
 	pose_ = Mat::eye(Size(4, 4), CV_64FC1);
+	pose_adjustment_ = Mat::eye(Size(4, 4), CV_64FC1);
 	Q_ = Mat::eye(Size(4, 4), CV_64FC1);
 	Q_.at<double>(3, 2) = -1;
 	Q_.at<double>(2, 3) = 1;
@@ -120,7 +120,7 @@ Mat Calibrate::getPose() const {
 	else {
 		pose_.copyTo(T);
 	}
-	return T;
+	return pose_adjustment_ * T;
 }
 
 bool Calibrate::setRectify(bool enabled) {
@@ -172,6 +172,12 @@ bool Calibrate::setPose(const Mat &P) {
 	return true;
 }
 
+bool Calibrate::setPoseAdjustment(const Mat &T) {
+	if (!ftl::calibration::validate::pose(T)) { return false; }
+	pose_adjustment_ = T * pose_adjustment_;
+	return true;
+}
+
 bool Calibrate::loadCalibration(const string &fname) {
 	FileStorage fs;
 
@@ -187,6 +193,7 @@ bool Calibrate::loadCalibration(const string &fname) {
 	Mat R;
 	Mat t;
 	Mat pose;
+	Mat pose_adjustment;
 
 	fs["resolution"] >> calib_size;
 	fs["K"] >> K;
@@ -194,6 +201,7 @@ bool Calibrate::loadCalibration(const string &fname) {
 	fs["R"] >> R;
 	fs["t"] >> t;
 	fs["P"] >> pose;
+	fs["adjustment"] >> pose_adjustment;
 	fs.release();
 
 	bool retval = true;
@@ -217,6 +225,9 @@ bool Calibrate::loadCalibration(const string &fname) {
 		LOG(ERROR) << "invalid pose in calibration file";
 		retval = false;
 	}
+	if (!setPoseAdjustment(pose_adjustment)) {
+		LOG(WARNING) << "invalid pose adjustment in calibration file (using identity)";
+	}
 
 	LOG(INFO) << "calibration loaded from: " << fname;
 	return retval;
@@ -224,7 +235,8 @@ bool Calibrate::loadCalibration(const string &fname) {
 
 bool Calibrate::writeCalibration(	const string &fname, const Size &size,
 									const vector<Mat> &K, const vector<Mat> &D, 
-									const Mat &R, const Mat &t, const Mat &pose) {
+									const Mat &R, const Mat &t, const Mat &pose,
+									const Mat &pose_adjustment) {
 	
 	cv::FileStorage fs(fname, cv::FileStorage::WRITE);
 	if (!fs.isOpened()) { return false; }
@@ -235,6 +247,7 @@ bool Calibrate::writeCalibration(	const string &fname, const Size &size,
 		<< "R" << R
 		<< "t" << t
 		<< "P" << pose
+		<< "adjustment" << pose_adjustment;
 	;
 	
 	fs.release();
@@ -249,7 +262,7 @@ bool Calibrate::saveCalibration(const string &fname) {
 	//	// copy to fname + ".bak"
 	//}
 
-	return writeCalibration(fname, calib_size_, K_, D_, R_, t_, pose_);
+	return writeCalibration(fname, calib_size_, K_, D_, R_, t_, pose_, pose_adjustment_);
 }
 
 bool Calibrate::calculateRectificationParameters() {
diff --git a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
index 967d846767da8cfd0e24dc2caf125947fa69e17c..523608a044b2712f797a653da45a09aea3368caa 100644
--- a/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
+++ b/components/rgbd-sources/src/sources/stereovideo/calibrate.hpp
@@ -143,6 +143,11 @@ class Calibrate : public ftl::Configurable {
 	 */
 	bool setPose(const cv::Mat &P);
 
+	/**
+	 * @brief	Set adjustment, which is applied to pose: T_update*T_pose 
+	 */
+	bool setPoseAdjustment(const cv::Mat &T);
+
 	/**
 	 * @brief	Calculate rectification parameters and maps. Can fail if
 	 * 			calibration parameters are invalid.
@@ -177,7 +182,8 @@ class Calibrate : public ftl::Configurable {
 								const std::vector<cv::Mat> &K,
 								const std::vector<cv::Mat> &D,
 								const cv::Mat &R, const cv::Mat &t,
-								const cv::Mat &pose);
+								const cv::Mat &pose,
+								const cv::Mat &pose_adjustment);
 
 	/*	@brief	Save current calibration to file
 	 *	@param	File name
@@ -225,6 +231,7 @@ private:
 	cv::Mat t_;
 	// pose for left camera
 	cv::Mat pose_;
+	cv::Mat pose_adjustment_;
 };
 
 }
diff --git a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
index 5c4b6777286267e745e267ea7eb5416d3d24315e..4ddc992b63f6ac30e1aeb34bbfdb7f04d85385ab 100644
--- a/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
+++ b/components/rgbd-sources/src/sources/stereovideo/stereovideo.cpp
@@ -125,10 +125,23 @@ void StereoVideoSource::init(const string &file) {
 				LOG(ERROR) << "invalid pose received (bad value)";
 				return false;
 			}
+			updateParameters();
+			LOG(INFO) << "new pose";
+			return true;
+	});
 
+	host_->getNet()->bind("set_pose_adjustment",
+		[this](cv::Mat T){
+			if (!calib_->setPoseAdjustment(T)) {
+				LOG(ERROR) << "invalid pose received (bad value)";
+				return false;
+			}
+			updateParameters();
+			LOG(INFO) << "new pose adjustment";
 			return true;
 	});
 
+
 	host_->getNet()->bind("set_intrinsics",
 		[this](cv::Size size, cv::Mat K_l, cv::Mat D_l, cv::Mat K_r, cv::Mat D_r) {
 
@@ -143,7 +156,8 @@ void StereoVideoSource::init(const string &file) {
 					return false;
 				}
 			}
-
+			updateParameters();
+			LOG(INFO) << "new intrinsic parameters";
 			return true;
 	});
 
@@ -153,11 +167,14 @@ void StereoVideoSource::init(const string &file) {
 				LOG(ERROR) << "invalid extrinsic parameters (bad values)";
 				return false;
 			}
+			updateParameters();
+			LOG(INFO) << "new extrinsic (stereo) parameters";
 			return true;
 	});
 
 	host_->getNet()->bind("save_calibration",
 		[this, fname](){
+			LOG(INFO) << "saving calibration to " << fname;
 			return calib_->saveCalibration(fname);
 	});
 
@@ -165,6 +182,7 @@ void StereoVideoSource::init(const string &file) {
 		[this](bool enable){
 			bool retval = calib_->setRectify(enable);
 			updateParameters();
+			LOG(INFO) << "rectification " << (retval ? "on" : "off");
 			return retval;
 	});
 
@@ -215,6 +233,7 @@ void StereoVideoSource::updateParameters() {
 	
 	if (d_resolution > 0.0) {
 		// Learning OpenCV p. 442
+		// TODO: remove, should not be used here
 		float max_depth_new = sqrt(d_resolution * fx * baseline);
 		max_depth = (max_depth_new > max_depth) ? max_depth : max_depth_new;
 	}
diff --git a/components/streams/src/receiver.cpp b/components/streams/src/receiver.cpp
index 7e6cacbf2bbd053c8b1005b73a5d26a4640f81aa..b98b65534a7c0e8cd391e26da98d897adbc230ae 100644
--- a/components/streams/src/receiver.cpp
+++ b/components/streams/src/receiver.cpp
@@ -125,7 +125,8 @@ void Receiver::_processState(const StreamPacket &spkt, const Packet &pkt) {
 		case Channel::Configuration		: ftl::config::parseJSON(frame.state.getConfig(), parseConfig(pkt)); break;
 		case Channel::Calibration		: frame.state.getLeft() = parseCalibration(pkt); break;
 		case Channel::Calibration2		: frame.state.getRight() = parseCalibration(pkt); break;
-		case Channel::Pose				: frame.state.getPose() = parsePose(pkt); break;
+		//case Channel::Pose				: frame.state.getPose() = parsePose(pkt); break;
+		case Channel::Pose				: frame.state.setPose(parsePose(pkt)); break;
 		default: break;
 		}
 	}
@@ -331,9 +332,9 @@ void Receiver::_processVideo(const StreamPacket &spkt, const Packet &pkt) {
 				builder_[spkt.streamID].completed(spkt.timestamp, spkt.frame_number+i);
 
 				// Check for any state changes and send them back
-				if (vidstate.state.hasChanged(Channel::Pose)) injectPose(stream_, frame, spkt.timestamp, spkt.frameNumber()+i);
-				if (vidstate.state.hasChanged(Channel::Calibration)) injectCalibration(stream_, frame, spkt.timestamp, spkt.streamID, spkt.frameNumber()+i);
-				if (vidstate.state.hasChanged(Channel::Calibration2)) injectCalibration(stream_, frame, spkt.timestamp, spkt.streamID, spkt.frameNumber()+i, true);
+				//if (vidstate.state.hasChanged(Channel::Pose)) injectPose(stream_, frame, spkt.timestamp, spkt.frameNumber()+i);
+				//if (vidstate.state.hasChanged(Channel::Calibration)) injectCalibration(stream_, frame, spkt.timestamp, spkt.streamID, spkt.frameNumber()+i);
+				//if (vidstate.state.hasChanged(Channel::Calibration2)) injectCalibration(stream_, frame, spkt.timestamp, spkt.streamID, spkt.frameNumber()+i, true);
 
 				//frame.reset();
 				//frame.completed.clear();
diff --git a/components/structures/include/ftl/data/frame.hpp b/components/structures/include/ftl/data/frame.hpp
index 74c52d20731b25923ea87fbdd15d36d89a3a48fb..88f31c4b8dcd22f57ea953e9978fdc5679e57bd5 100644
--- a/components/structures/include/ftl/data/frame.hpp
+++ b/components/structures/include/ftl/data/frame.hpp
@@ -201,7 +201,12 @@ public:
 	/**
 	 * Change the pose of the origin state and mark as changed.
 	 */
-	void setPose(const Eigen::Matrix4d &pose);
+	void setPose(const Eigen::Matrix4d &pose, bool mark=true);
+
+	/**
+	 * Change the pose of the origin state and mark as changed.
+	 */
+	void patchPose(const Eigen::Matrix4d &pose);
 
 	/**
 	 * Wrapper to access left settings channel.
@@ -505,8 +510,16 @@ const typename STATE::Settings &ftl::data::Frame<BASE,N,STATE,DATA>::getRight()
 }
 
 template <int BASE, int N, typename STATE, typename DATA>
-void ftl::data::Frame<BASE,N,STATE,DATA>::setPose(const Eigen::Matrix4d &pose) {
-	if (origin_) origin_->setPose(pose);
+void ftl::data::Frame<BASE,N,STATE,DATA>::setPose(const Eigen::Matrix4d &pose, bool mark) {
+	if (origin_) {
+		if (mark) origin_->setPose(pose);
+		else origin_->getPose() = pose;
+	}
+}
+
+template <int BASE, int N, typename STATE, typename DATA>
+void ftl::data::Frame<BASE,N,STATE,DATA>::patchPose(const Eigen::Matrix4d &pose) {
+	state_.getPose() = pose * state_.getPose();
 }
 
 template <int BASE, int N, typename STATE, typename DATA>