diff --git a/applications/calibration-ceres/src/calibration.cpp b/applications/calibration-ceres/src/calibration.cpp
index 1f468bb0f677f059ca6d20583654a14e502bbfbd..2c09538e3672742b9874b287994cb3dde8d92e37 100644
--- a/applications/calibration-ceres/src/calibration.cpp
+++ b/applications/calibration-ceres/src/calibration.cpp
@@ -142,7 +142,9 @@ double ftl::calibration::computeExtrinsicParameters(const Mat &K1, const Mat &D1
 	double error_after = ba.reprojectionError();
 
 	// bundle adjustment didn't work correctly if these checks fail
-	CHECK(error_before > error_after);
+	if (error_before < error_after) {
+		LOG(WARNING) << "error before < error_after (" << error_before << "  <" << error_after << ")";
+	}
 	CHECK((cv::countNonZero(params1.rvec()) == 0) && (cv::countNonZero(params1.tvec()) == 0));
 
 	R = params2.rmat();
diff --git a/applications/calibration-ceres/src/calibration_data.cpp b/applications/calibration-ceres/src/calibration_data.cpp
index 02ede5ce6b1793592d14a3af840d7cc773a89bfc..28b77fa8b6b21a07abb8b7e32f46dab0f4363505 100644
--- a/applications/calibration-ceres/src/calibration_data.cpp
+++ b/applications/calibration-ceres/src/calibration_data.cpp
@@ -180,7 +180,7 @@ int CalibrationData::addObservations(const vector<bool>& visible, const vector<v
 	return retval;
 }
 
-pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::getVisible(const vector<int> &cameras) {
+pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationData::_getVisible(const vector<int> &cameras) {
 
 	int n_points = npoints();
 	vector<vector<Point2d>> observations(cameras.size());
@@ -204,9 +204,9 @@ pair<vector<vector<Point2d>>, vector<reference_wrapper<Point3d>>> CalibrationDat
 }
 
 vector<vector<Point2d>> CalibrationData::getObservations(const vector<int> &cameras) {
-	return getVisible(cameras).first;
+	return _getVisible(cameras).first;
 }
 
 vector<reference_wrapper<Point3d>> CalibrationData::getPoints(const vector<int> &cameras) {
-	return getVisible(cameras).second;
+	return _getVisible(cameras).second;
 }
diff --git a/applications/calibration-ceres/src/calibration_data.hpp b/applications/calibration-ceres/src/calibration_data.hpp
index da4c00eaf210433990222ddd26a3f4b843017058..e98ed3d3c0f86a97e78961eeeb2e102581da9e14 100644
--- a/applications/calibration-ceres/src/calibration_data.hpp
+++ b/applications/calibration-ceres/src/calibration_data.hpp
@@ -77,11 +77,14 @@ public:
 
 	void reserve(int n_points) {
 		points_.reserve(n_points);
+		points_camera_.reserve(n_points*n_cameras_);
 		observations_.reserve(n_points*n_cameras_);
 		visible_.reserve(n_points*n_cameras_);
 	}
 
-	std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> getVisible(const std::vector<int> &cameras);
+	// TODO: method for save poinst_camera_, could return (for example)
+	// vector<pair<Point3d*>, vector<Point3d*>>
+	
 	std::vector<std::vector<cv::Point2d>> getObservations(const std::vector<int> &cameras);
 	/* Get points corresponding to observations returned by getObservations() or getObservationsPtr() */
 	std::vector<std::reference_wrapper<cv::Point3d>> getPoints(const std::vector<int> &cameras);
@@ -101,11 +104,19 @@ public:
 	int ncameras() const { return n_cameras_; }
 
 private:
+	std::pair<std::vector<std::vector<cv::Point2d>>, std::vector<std::reference_wrapper<cv::Point3d>>> _getVisible(const std::vector<int> &cameras);
+	
 	int n_cameras_;
 
+	// cameras
 	std::vector<Camera> cameras_;
+	// points for each observation
 	std::vector<cv::Point3d> points_;
+	// points estimated from cameras' observations
+	std::vector<cv::Point3d> points_camera_;
+	// observations
 	std::vector<cv::Point2d> observations_;
+	// visibility
 	std::vector<bool> visible_;
 };
 
diff --git a/applications/calibration-ceres/src/main.cpp b/applications/calibration-ceres/src/main.cpp
index f73f97dcbb8620bf588a01a4692829233ad1c868..087c3283ba303eceab82ac565c626ec0051e403e 100644
--- a/applications/calibration-ceres/src/main.cpp
+++ b/applications/calibration-ceres/src/main.cpp
@@ -104,7 +104,8 @@ void calibrate(const string &fname) {
 	// Needs better solution which allows simple access to all estimations.
 	// Required for calculating average coordinates and to know which points
 	// are missing.
-	vector<tuple<int, vector<Point3d>, vector<reference_wrapper<Point3d>>>> points_all;
+
+	vector<tuple<int, vector<Point3d>>> points_all;
 
 	transformations[make_pair(refcamera, refcamera)] =
 		make_pair(Mat::eye(3, 3, CV_64FC1), Mat::zeros(3, 1, CV_64FC1));
@@ -129,7 +130,7 @@ void calibrate(const string &fname) {
 				Mat t_;
 				vector<Point3d> points;
 
-				auto [observations, points_ref] = data.getVisible({current, to});
+				auto observations = data.getObservations({current, to});
 				double err = computeExtrinsicParameters(
 					data.getCamera(current).intrinsicMatrix(),
 					data.getCamera(current).distortionCoefficients(),
@@ -141,7 +142,7 @@ void calibrate(const string &fname) {
 				LOG(INFO) << current << " -> " << to << " (RMS error: " << err << ")";
 
 				transformations[edge] = make_pair(R_, t_);
-				points_all.push_back(make_tuple(current, points, points_ref));
+				points_all.push_back(make_tuple(current, points));
 			}
 
 			const auto [R_update, t_update] = transformations[edge];
@@ -158,7 +159,8 @@ void calibrate(const string &fname) {
 	}
 
 	// TODO: see points_all comment
-	for (auto& [i, points, points_ref] : points_all) {
+	/*
+	for (auto& [i, points] : points_all) {
 
 		Mat R = data.getCamera(i).rmat();
 		Mat t = data.getCamera(i).tvec();
@@ -177,7 +179,7 @@ void calibrate(const string &fname) {
 			}
 		}
 	}
-
+	*/
 	vector<int> idx;
 	BundleAdjustment ba;
 
@@ -213,11 +215,9 @@ void calibrate(const string &fname) {
 
 	LOG(INFO) << "Finale reprojection error: " << ba.reprojectionError();
 
-	// calibration done
+	// calibration done, updated values in data
 }
 
-
 int main(int argc, char* argv[]) {
-	calibrate("");
 	return 0;
 }
diff --git a/applications/calibration-ceres/src/optimization.cpp b/applications/calibration-ceres/src/optimization.cpp
index 493bdc6abeef0510b25512becf4616274b7c04c3..aafe2a38a960c3349521a0d2f88369cafb0f8597 100644
--- a/applications/calibration-ceres/src/optimization.cpp
+++ b/applications/calibration-ceres/src/optimization.cpp
@@ -393,7 +393,14 @@ void BundleAdjustment::run(const BundleAdjustment::Options &bundle_adjustment_op
 	ceres::Solver::Options options;
 	options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
 	options.minimizer_progress_to_stdout = bundle_adjustment_options.verbose;
-	options.max_num_iterations = bundle_adjustment_options.max_iter;
+	
+	if (bundle_adjustment_options.max_iter > 0) {
+		options.max_num_iterations = bundle_adjustment_options.max_iter;
+	}
+	
+	if (bundle_adjustment_options.num_threads > 0) {
+		options.num_threads = bundle_adjustment_options.num_threads;
+	}
 
 	ceres::Solver::Summary summary;
 	ceres::Solve(options, &problem, &summary);
diff --git a/applications/calibration-ceres/src/optimization.hpp b/applications/calibration-ceres/src/optimization.hpp
index 9fbd16129581fbb9e360919f07a7dcd78fc41220..3b0716c7b6b73ab6cc25bdcc2cedf65b33a5da22 100644
--- a/applications/calibration-ceres/src/optimization.hpp
+++ b/applications/calibration-ceres/src/optimization.hpp
@@ -83,7 +83,8 @@ public:
 		bool optimize_motion = true;
 		bool optmize_structure = true;
 
-		int max_iter = 50;
+		int num_threads = -1;
+		int max_iter = -1;
 		bool verbose = false;
 	};
 
diff --git a/applications/calibration-multi/src/main.cpp b/applications/calibration-multi/src/main.cpp
index b6622f70e62dc54453d7a5eb058019d889d1b621..4cbd5f28466d52e6cbdf55f774e85f9cbad94925 100644
--- a/applications/calibration-multi/src/main.cpp
+++ b/applications/calibration-multi/src/main.cpp
@@ -232,7 +232,8 @@ void calibrateRPC(	ftl::net::Universe* net,
 		while(true) {
 			try {
 				if (params.optimize_intrinsic) {
-					setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {D1, D2});
+					// never update distortion during extrinsic calibration
+					setIntrinsicsRPC(net, nstream, params.size, {K1, K2}, {Mat(0, 0, CV_64FC1), Mat(0, 0, CV_64FC1)});
 				}
 				setExtrinsicsRPC(net, nstream, R_c1c2, T_c1c2);
 				setPoseRPC(net, nstream, getMat4x4(R[c], t[c]));
@@ -667,6 +668,8 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 				cv::cvtColor(left, rgb_new[2*idx], cv::COLOR_BGRA2BGR);
 				cv::cvtColor(right, rgb_new[2*idx+1], cv::COLOR_BGRA2BGR);
 				
+				camera_parameters[2*idx] = createCameraMatrix(fs.frames[i].getLeftCamera());
+				camera_parameters[2*idx+1] = createCameraMatrix(fs.frames[i].getRightCamera());
 				if (res.empty()) res = rgb_new[2*idx].size();
 			}
 		}
@@ -721,6 +724,27 @@ void runCameraCalibrationPath(	ftl::Configurable* root,
 	std::iota(params.idx_cameras.begin(), params.idx_cameras.end(), 0);
 	calib.loadInput(path + filename, params.idx_cameras);
 
+	for (size_t i = 0; i < nstreams.size(); i++) {
+		while(true) {
+			try {
+				if (camera_parameters[2*i].empty() || camera_parameters[2*i+1].empty()) {
+					std::this_thread::sleep_for(std::chrono::seconds(1));
+					continue;
+				}
+				vector<Mat> D = getDistortionParametersRPC(net, nstreams[i]);
+				LOG(INFO) << "K[" << 2*i << "] = \n" << camera_parameters[2*i];
+				LOG(INFO) << "D[" << 2*i << "] = " << D[0];
+				LOG(INFO) << "K[" << 2*i+1 << "] = \n" << camera_parameters[2*i+1];
+				LOG(INFO) << "D[" << 2*i+1 << "] = " << D[1];
+				calib.setCameraParameters(2*i, camera_parameters[2*i], D[0]);
+				calib.setCameraParameters(2*i+1, camera_parameters[2*i+1], D[1]);
+				break;
+			}
+			catch (...) {}
+		}
+	}
+
+
 	Mat out;
 	vector<Mat> map1, map2;
 	vector<cv::Rect> roi;
diff --git a/applications/calibration-multi/src/multicalibrate.cpp b/applications/calibration-multi/src/multicalibrate.cpp
index 60f6209821f5dce2ea31ba0e842e4b862ab152ce..b9627ce252211ef9075e165270d6d88376038814 100644
--- a/applications/calibration-multi/src/multicalibrate.cpp
+++ b/applications/calibration-multi/src/multicalibrate.cpp
@@ -146,9 +146,9 @@ Mat MultiCameraCalibrationNew::getDistCoeffs(size_t idx) {
 }
 
 void MultiCameraCalibrationNew::setCameraParameters(size_t idx, const Mat &K, const Mat &distCoeffs) {
-	DCHECK(idx < n_cameras_);
-	DCHECK(K.size() == Size(3, 3));
-	DCHECK(distCoeffs.size() == Size(5, 1));
+	CHECK(idx < n_cameras_);
+	CHECK(K.size() == Size(3, 3));
+	CHECK(distCoeffs.total() == 5);
 	K.convertTo(K_[idx], CV_64FC1);
 	distCoeffs.convertTo(dist_coeffs_[idx], CV_64FC1);
 }
@@ -529,6 +529,7 @@ double MultiCameraCalibrationNew::calibratePair(size_t camera_from, size_t camer
 	t1 = Mat(Size(1, 3), CV_64FC1, Scalar(0.0));
 	
 	vector<Point3d> points3d;
+	
 	double err = ftl::calibration::computeExtrinsicParameters(K1, dist_coeffs_[camera_from], K2, dist_coeffs_[camera_to], points1, points2, object_points_, R2, t2, points3d);
 	calculateTransform(R2, t2, R1, t1, rmat, tvec);
 	
diff --git a/components/common/cpp/include/ftl/calibration.hpp b/components/common/cpp/include/ftl/calibration.hpp
index 719da54d8c591f0077c43a47387617d9a786f24d..00662685237cdabf5e9d8a855ced5d1493f4a4e5 100644
--- a/components/common/cpp/include/ftl/calibration.hpp
+++ b/components/common/cpp/include/ftl/calibration.hpp
@@ -9,6 +9,9 @@ namespace calibration {
 
 namespace validate {
 
+/**
+ * @brief Valid translation for stereo camera.
+ */
 bool translationStereo(const cv::Mat &t);
 
 bool rotationMatrix(const cv::Mat &M);
@@ -34,6 +37,11 @@ bool distortionCoefficients(const cv::Mat &D, cv::Size size);
 
 }
 
+/**
+ * @brief Scale camera intrinsic matrix
+ * @param size_new	New resolution
+ * @param size_old	Original (camera matrix) resolution
+ */
 cv::Mat scaleCameraMatrix(const cv::Mat &K, const cv::Size &size_new, const cv::Size &size_old);
 
 }
diff --git a/components/common/cpp/src/calibration.cpp b/components/common/cpp/src/calibration.cpp
index 4ee3425a7806ad876ed62c8c485f9f9aa2224acb..6a6bd8fa31197b72bec301a510a603c8b45213f1 100644
--- a/components/common/cpp/src/calibration.cpp
+++ b/components/common/cpp/src/calibration.cpp
@@ -106,8 +106,8 @@ bool ftl::calibration::validate::distortionCoefficients(const Mat &D, Size size)
 	double dist_prev_n = 0;
 	double dist_prev_p = 0;
 
-	for (int r2_ = 0; r2_ < diagonal; r2_++) {
-		double r2 = r2_;
+	for (int r = 0; r < diagonal; r++) {
+		double r2 = r*r;
 		double r4 = r2*r2;
 		double r6 = r4*r2;