| 
					
				 | 
			
			
				@@ -87,16 +87,17 @@ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // There're existing problem files dumped from blender stored in folder 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // ../data/libmv-ba-problems. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-#include <cstdio> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #include <fcntl.h> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+#include <cstdio> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #include <sstream> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #include <string> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #include <vector> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #ifdef _MSC_VER 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-#  include <io.h> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-#  define open _open 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-#  define close _close 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+#include <io.h> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+#define open _open 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+#define close _close 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 typedef unsigned __int32 uint32_t; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #else 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #include <stdint.h> 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -121,8 +122,9 @@ typedef Eigen::Vector4d Vec4; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 using std::vector; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 DEFINE_string(input, "", "Input File name"); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-DEFINE_string(refine_intrinsics, "", "Camera intrinsics to be refined. " 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-              "Options are: none, radial."); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+DEFINE_string(refine_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+              "", 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+              "Camera intrinsics to be refined. Options are: none, radial."); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 namespace { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -134,7 +136,7 @@ namespace { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // t is a translation vector representing its positions. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 struct EuclideanCamera { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   EuclideanCamera() : image(-1) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  EuclideanCamera(const EuclideanCamera &c) : image(c.image), R(c.R), t(c.t) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  EuclideanCamera(const EuclideanCamera& c) : image(c.image), R(c.R), t(c.t) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   int image; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   Mat3 R; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -147,7 +149,7 @@ struct EuclideanCamera { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // X represents the 3D position of the track. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 struct EuclideanPoint { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   EuclideanPoint() : track(-1) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  EuclideanPoint(const EuclideanPoint &p) : track(p.track), X(p.X) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  EuclideanPoint(const EuclideanPoint& p) : track(p.track), X(p.X) {} 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   int track; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   Vec3 X; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 }; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -201,25 +203,24 @@ enum { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 }; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Returns a pointer to the camera corresponding to a image. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-EuclideanCamera *CameraForImage(vector<EuclideanCamera> *all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+EuclideanCamera* CameraForImage(vector<EuclideanCamera>* all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                 const int image) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (image < 0 || image >= all_cameras->size()) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  EuclideanCamera *camera = &(*all_cameras)[image]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  EuclideanCamera* camera = &(*all_cameras)[image]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (camera->image == -1) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   return camera; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-const EuclideanCamera *CameraForImage( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const vector<EuclideanCamera> &all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const int image) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+const EuclideanCamera* CameraForImage( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const vector<EuclideanCamera>& all_cameras, const int image) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (image < 0 || image >= all_cameras.size()) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  const EuclideanCamera *camera = &all_cameras[image]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  const EuclideanCamera* camera = &all_cameras[image]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (camera->image == -1) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -227,7 +228,7 @@ const EuclideanCamera *CameraForImage( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Returns maximal image number at which marker exists. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-int MaxImage(const vector<Marker> &all_markers) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+int MaxImage(const vector<Marker>& all_markers) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (all_markers.size() == 0) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return -1; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -240,12 +241,12 @@ int MaxImage(const vector<Marker> &all_markers) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Returns a pointer to the point corresponding to a track. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-EuclideanPoint *PointForTrack(vector<EuclideanPoint> *all_points, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+EuclideanPoint* PointForTrack(vector<EuclideanPoint>* all_points, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                               const int track) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (track < 0 || track >= all_points->size()) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  EuclideanPoint *point = &(*all_points)[track]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  EuclideanPoint* point = &(*all_points)[track]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (point->track == -1) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -266,7 +267,7 @@ class EndianAwareFileReader { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     union { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       unsigned char bytes[4]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       uint32_t value; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    } endian_test = { { 0, 1, 2, 3 } }; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    } endian_test = {{0, 1, 2, 3}}; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     host_endian_type_ = endian_test.value; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     file_endian_type_ = host_endian_type_; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -277,7 +278,7 @@ class EndianAwareFileReader { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  bool OpenFile(const std::string &file_name) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  bool OpenFile(const std::string& file_name) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     file_descriptor_ = open(file_name.c_str(), O_RDONLY | O_BINARY); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (file_descriptor_ < 0) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       return false; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -306,6 +307,7 @@ class EndianAwareFileReader { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return value; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  private: 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   static constexpr long int kLittleEndian = 0x03020100ul; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   static constexpr long int kBigEndian = 0x00010203ul; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -315,14 +317,17 @@ class EndianAwareFileReader { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T SwitchEndian(const T value) const { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (sizeof(T) == 4) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       unsigned int temp_value = static_cast<unsigned int>(value); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      // clang-format off 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       return ((temp_value >> 24)) | 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				              ((temp_value << 8) & 0x00ff0000) | 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				              ((temp_value >> 8) & 0x0000ff00) | 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				              ((temp_value << 24)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      // clang-format on 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } else if (sizeof(T) == 1) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       return value; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } else { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      LOG(FATAL) << "Entered non-implemented part of endian switching function."; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      LOG(FATAL) << "Entered non-implemented part of endian " 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                    "switching function."; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -332,16 +337,14 @@ class EndianAwareFileReader { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 }; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Read 3x3 column-major matrix from the file 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-void ReadMatrix3x3(const EndianAwareFileReader &file_reader, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                   Mat3 *matrix) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+void ReadMatrix3x3(const EndianAwareFileReader& file_reader, Mat3* matrix) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   for (int i = 0; i < 9; i++) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     (*matrix)(i % 3, i / 3) = file_reader.Read<float>(); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Read 3-vector from file 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-void ReadVector3(const EndianAwareFileReader &file_reader, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                 Vec3 *vector) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+void ReadVector3(const EndianAwareFileReader& file_reader, Vec3* vector) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   for (int i = 0; i < 3; i++) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     (*vector)(i) = file_reader.Read<float>(); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -364,12 +367,12 @@ void ReadVector3(const EndianAwareFileReader &file_reader, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Returns false if any kind of error happened during 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // reading. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-bool ReadProblemFromFile(const std::string &file_name, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+bool ReadProblemFromFile(const std::string& file_name, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                          double camera_intrinsics[8], 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                         vector<EuclideanCamera> *all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                         vector<EuclideanPoint> *all_points, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                         bool *is_image_space, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                         vector<Marker> *all_markers) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                         vector<EuclideanCamera>* all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                         vector<EuclideanPoint>* all_points, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                         bool* is_image_space, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                         vector<Marker>* all_markers) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   EndianAwareFileReader file_reader; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (!file_reader.OpenFile(file_name)) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     return false; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -451,24 +454,24 @@ bool ReadProblemFromFile(const std::string &file_name, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // camera coordinates (i.e. the principal point is at (0, 0)) to get image 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // coordinates in pixels. Templated for use with autodifferentiation. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 template <typename T> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-inline void ApplyRadialDistortionCameraIntrinsics(const T &focal_length_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &focal_length_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &principal_point_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &principal_point_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &k1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &k2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &k3, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &p1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &p2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &normalized_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  const T &normalized_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  T *image_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                                  T *image_y) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+inline void ApplyRadialDistortionCameraIntrinsics(const T& focal_length_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& focal_length_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& principal_point_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& principal_point_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& k1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& k2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& k3, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& p1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& p2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& normalized_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  const T& normalized_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  T* image_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                                  T* image_y) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T x = normalized_x; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T y = normalized_y; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   // Apply distortion to the normalized points to get (xd, yd). 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  T r2 = x*x + y*y; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  T r2 = x * x + y * y; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T r4 = r2 * r2; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T r6 = r4 * r2; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   T r_coeff = 1.0 + k1 * r2 + k2 * r4 + k3 * r6; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -496,14 +499,14 @@ struct OpenCVReprojectionError { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                   const T* const X,    // Point coordinates 3x1. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                   T* residuals) const { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // Unpack the intrinsics. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& focal_length      = intrinsics[OFFSET_FOCAL_LENGTH]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& focal_length = intrinsics[OFFSET_FOCAL_LENGTH]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     const T& principal_point_x = intrinsics[OFFSET_PRINCIPAL_POINT_X]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     const T& principal_point_y = intrinsics[OFFSET_PRINCIPAL_POINT_Y]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& k1                = intrinsics[OFFSET_K1]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& k2                = intrinsics[OFFSET_K2]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& k3                = intrinsics[OFFSET_K3]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& p1                = intrinsics[OFFSET_P1]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const T& p2                = intrinsics[OFFSET_P2]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& k1 = intrinsics[OFFSET_K1]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& k2 = intrinsics[OFFSET_K2]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& k3 = intrinsics[OFFSET_K3]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& p1 = intrinsics[OFFSET_P1]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const T& p2 = intrinsics[OFFSET_P2]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // Compute projective coordinates: x = RX + t. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     T x[3]; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -526,9 +529,13 @@ struct OpenCVReprojectionError { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                           focal_length, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                           principal_point_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                           principal_point_y, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                          k1, k2, k3, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                          p1, p2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                          xn, yn, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          k1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          k2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          k3, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          p1, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          p2, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          xn, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                          yn, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                           &predicted_x, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                           &predicted_y); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -551,40 +558,41 @@ void BundleIntrinsicsLogMessage(const int bundle_intrinsics) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     std::string bundling_message = ""; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #define APPEND_BUNDLING_INTRINSICS(name, flag) \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    if (bundle_intrinsics & flag) { \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      if (!bundling_message.empty()) { \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-        bundling_message += ", "; \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      } \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      bundling_message += name; \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    } (void)0 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				- 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    APPEND_BUNDLING_INTRINSICS("f",      BUNDLE_FOCAL_LENGTH); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  if (bundle_intrinsics & flag) {              \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    if (!bundling_message.empty()) {           \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      bundling_message += ", ";                \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    }                                          \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    bundling_message += name;                  \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  }                                            \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  (void)0 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    APPEND_BUNDLING_INTRINSICS("f", BUNDLE_FOCAL_LENGTH); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     APPEND_BUNDLING_INTRINSICS("px, py", BUNDLE_PRINCIPAL_POINT); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    APPEND_BUNDLING_INTRINSICS("k1",     BUNDLE_RADIAL_K1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    APPEND_BUNDLING_INTRINSICS("k2",     BUNDLE_RADIAL_K2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    APPEND_BUNDLING_INTRINSICS("p1",     BUNDLE_TANGENTIAL_P1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    APPEND_BUNDLING_INTRINSICS("p2",     BUNDLE_TANGENTIAL_P2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    APPEND_BUNDLING_INTRINSICS("k1", BUNDLE_RADIAL_K1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    APPEND_BUNDLING_INTRINSICS("k2", BUNDLE_RADIAL_K2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    APPEND_BUNDLING_INTRINSICS("p1", BUNDLE_TANGENTIAL_P1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    APPEND_BUNDLING_INTRINSICS("p2", BUNDLE_TANGENTIAL_P2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     LOG(INFO) << "Bundling " << bundling_message << "."; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Print a message to the log containing all the camera intriniscs values. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+void PrintCameraIntrinsics(const char* text, const double* camera_intrinsics) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   std::ostringstream intrinsics_output; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   intrinsics_output << "f=" << camera_intrinsics[OFFSET_FOCAL_LENGTH]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  intrinsics_output << 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    " cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] << 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    " cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  intrinsics_output << " cx=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_X] 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                    << " cy=" << camera_intrinsics[OFFSET_PRINCIPAL_POINT_Y]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-#define APPEND_DISTORTION_COEFFICIENT(name, offset) \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  { \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    if (camera_intrinsics[offset] != 0.0) { \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      intrinsics_output << " " name "=" << camera_intrinsics[offset];  \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    } \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  } (void)0 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+#define APPEND_DISTORTION_COEFFICIENT(name, offset)                   \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  {                                                                   \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    if (camera_intrinsics[offset] != 0.0) {                           \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      intrinsics_output << " " name "=" << camera_intrinsics[offset]; \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    }                                                                 \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  }                                                                   \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  (void)0 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   APPEND_DISTORTION_COEFFICIENT("k1", OFFSET_K1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   APPEND_DISTORTION_COEFFICIENT("k2", OFFSET_K2); 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -603,22 +611,21 @@ void PrintCameraIntrinsics(const char *text, const double *camera_intrinsics) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Element with index i matches to a rotation+translation for 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // camera at image i. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 vector<Vec6> PackCamerasRotationAndTranslation( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const vector<EuclideanCamera> &all_cameras) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const vector<Marker>& all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const vector<EuclideanCamera>& all_cameras) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   vector<Vec6> all_cameras_R_t; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   int max_image = MaxImage(all_markers); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   all_cameras_R_t.resize(max_image + 1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   for (int i = 0; i <= max_image; i++) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const EuclideanCamera *camera = CameraForImage(all_cameras, i); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const EuclideanCamera* camera = CameraForImage(all_cameras, i); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (!camera) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       continue; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    ceres::RotationMatrixToAngleAxis(&camera->R(0, 0), 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                     &all_cameras_R_t[i](0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    ceres::RotationMatrixToAngleAxis(&camera->R(0, 0), &all_cameras_R_t[i](0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     all_cameras_R_t[i].tail<3>() = camera->t; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -626,31 +633,29 @@ vector<Vec6> PackCamerasRotationAndTranslation( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 // Convert cameras rotations fro mangle axis back to rotation matrix. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-void UnpackCamerasRotationAndTranslation( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const vector<Vec6> &all_cameras_R_t, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    vector<EuclideanCamera> *all_cameras) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+void UnpackCamerasRotationAndTranslation(const vector<Marker>& all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                         const vector<Vec6>& all_cameras_R_t, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                         vector<EuclideanCamera>* all_cameras) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   int max_image = MaxImage(all_markers); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   for (int i = 0; i <= max_image; i++) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    EuclideanCamera *camera = CameraForImage(all_cameras, i); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    EuclideanCamera* camera = CameraForImage(all_cameras, i); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (!camera) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       continue; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0), 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                     &camera->R(0, 0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    ceres::AngleAxisToRotationMatrix(&all_cameras_R_t[i](0), &camera->R(0, 0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     camera->t = all_cameras_R_t[i].tail<3>(); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+void EuclideanBundleCommonIntrinsics(const vector<Marker>& all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                      const int bundle_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				                                      const int bundle_constraints, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                     double *camera_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                     vector<EuclideanCamera> *all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                     vector<EuclideanPoint> *all_points) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                     double* camera_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                     vector<EuclideanCamera>* all_cameras, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                                     vector<EuclideanPoint>* all_points) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   PrintCameraIntrinsics("Original intrinsics: ", camera_intrinsics); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   ceres::Problem::Options problem_options; 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -663,49 +668,50 @@ void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   // Block for minimization has got the following structure: 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   //   <3 elements for angle-axis> <3 elements for translation> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   vector<Vec6> all_cameras_R_t = 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    PackCamerasRotationAndTranslation(all_markers, *all_cameras); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      PackCamerasRotationAndTranslation(all_markers, *all_cameras); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   // Parameterization used to restrict camera motion for modal solvers. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  ceres::SubsetParameterization *constant_transform_parameterization = NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  ceres::SubsetParameterization* constant_transform_parameterization = NULL; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   if (bundle_constraints & BUNDLE_NO_TRANSLATION) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      std::vector<int> constant_translation; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    std::vector<int> constant_translation; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      // First three elements are rotation, last three are translation. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      constant_translation.push_back(3); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      constant_translation.push_back(4); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      constant_translation.push_back(5); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    // First three elements are rotation, last three are translation. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    constant_translation.push_back(3); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    constant_translation.push_back(4); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    constant_translation.push_back(5); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      constant_transform_parameterization = 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    constant_transform_parameterization = 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				         new ceres::SubsetParameterization(6, constant_translation); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   std::vector<OpenCVReprojectionError> errors; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  std::vector<ceres::AutoDiffCostFunction<OpenCVReprojectionError, 2, 8, 6, 3>> costFunctions; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  std::vector<ceres::AutoDiffCostFunction<OpenCVReprojectionError, 2, 8, 6, 3>> 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      costFunctions; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   errors.reserve(all_markers.size()); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   costFunctions.reserve(all_markers.size()); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   int num_residuals = 0; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   bool have_locked_camera = false; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   for (int i = 0; i < all_markers.size(); ++i) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    const Marker &marker = all_markers[i]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    EuclideanCamera *camera = CameraForImage(all_cameras, marker.image); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    EuclideanPoint *point = PointForTrack(all_points, marker.track); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    const Marker& marker = all_markers[i]; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    EuclideanCamera* camera = CameraForImage(all_cameras, marker.image); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    EuclideanPoint* point = PointForTrack(all_points, marker.track); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (camera == NULL || point == NULL) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				       continue; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // Rotation of camera denoted in angle axis followed with 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // camera translaiton. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    double *current_camera_R_t = &all_cameras_R_t[camera->image](0); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    double* current_camera_R_t = &all_cameras_R_t[camera->image](0); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     errors.emplace_back(marker.x, marker.y); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     costFunctions.emplace_back(&errors.back(), ceres::DO_NOT_TAKE_OWNERSHIP); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     problem.AddResidualBlock(&costFunctions.back(), 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-        NULL, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-        camera_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-        current_camera_R_t, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-        &point->X(0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                             NULL, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                             camera_intrinsics, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                             current_camera_R_t, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+                             &point->X(0)); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // We lock the first camera to better deal with scene orientation ambiguity. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     if (!have_locked_camera) { 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -739,23 +745,23 @@ void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     std::vector<int> constant_intrinsics; 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #define MAYBE_SET_CONSTANT(bundle_enum, offset) \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    if (!(bundle_intrinsics & bundle_enum)) { \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      constant_intrinsics.push_back(offset); \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH,    OFFSET_FOCAL_LENGTH); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  if (!(bundle_intrinsics & bundle_enum)) {     \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    constant_intrinsics.push_back(offset);      \ 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    MAYBE_SET_CONSTANT(BUNDLE_FOCAL_LENGTH, OFFSET_FOCAL_LENGTH); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_X); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     MAYBE_SET_CONSTANT(BUNDLE_PRINCIPAL_POINT, OFFSET_PRINCIPAL_POINT_Y); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1,       OFFSET_K1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2,       OFFSET_K2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1,   OFFSET_P1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2,   OFFSET_P2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K1, OFFSET_K1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    MAYBE_SET_CONSTANT(BUNDLE_RADIAL_K2, OFFSET_K2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P1, OFFSET_P1); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    MAYBE_SET_CONSTANT(BUNDLE_TANGENTIAL_P2, OFFSET_P2); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 #undef MAYBE_SET_CONSTANT 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     // Always set K3 constant, it's not used at the moment. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     constant_intrinsics.push_back(OFFSET_K3); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-    ceres::SubsetParameterization *subset_parameterization = 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-      new ceres::SubsetParameterization(8, constant_intrinsics); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+    ceres::SubsetParameterization* subset_parameterization = 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+        new ceres::SubsetParameterization(8, constant_intrinsics); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				     problem.SetParameterization(camera_intrinsics, subset_parameterization); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   } 
			 | 
		
	
	
		
			
				| 
					
				 | 
			
			
				@@ -776,15 +782,14 @@ void EuclideanBundleCommonIntrinsics(const vector<Marker> &all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   std::cout << "Final report:\n" << summary.FullReport(); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   // Copy rotations and translations back. 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-  UnpackCamerasRotationAndTranslation(all_markers, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                      all_cameras_R_t, 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-                                      all_cameras); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+  UnpackCamerasRotationAndTranslation( 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+      all_markers, all_cameras_R_t, all_cameras); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   PrintCameraIntrinsics("Final intrinsics: ", camera_intrinsics); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 } 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				 }  // namespace 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 | 
		
	
		
			
				 | 
				 | 
			
			
				-int main(int argc, char **argv) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				+int main(int argc, char** argv) { 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				   google::InitGoogleLogging(argv[0]); 
			 | 
		
	
		
			
				 | 
				 | 
			
			
				  
			 |