Alignment to nullify X

src/aliceVision/sfm/utils/alignment.cpp

@@ -938,5 +938,22 @@ bool computeNewCoordinateSystemFromGpsData(const sfmData::SfMData& sfmData, std:
     return aliceVision::geometry::ACRansac_FindRTS(x1, x2, randomNumberGenerator, out_S, out_t, out_R, inliers, refine);
 }
 
+void getRotationNullifyX(Eigen::Matrix3d & out_R, const Eigen::Vector3d & pt)
+{
+    /*
+    0 =  [cos(x) 0 -sin(x)][X]
+    Y' = [0      1 0      ][Y]
+    Z' = [sin(x) 0  cos(x)][Z]
+
+    cos(x)X - sin(x)Z = 0
+    sin(x)/cos(x) = X/Z
+    tan(x) = X/Z
+    x = atan2(X, Z)
+    */
+
+    double angle = std::atan2(pt(0), pt(2));
+    out_R = Eigen::AngleAxisd(angle, Vec3(0,-1,0)).toRotationMatrix();
+}
+
 } // namespace sfm
 } // namespace aliceVision

src/aliceVision/sfm/utils/alignment.hpp

@@ -306,5 +306,14 @@ bool computeNewCoordinateSystemFromSpecificMarkers(const sfmData::SfMData& sfmDa
     Mat3& out_R,
     Vec3& out_t);
 
+/**
+ * @brief Compute the 3D rotation matrix such that "pt"
+ * once rotated will have its x component equal to 0.
+ * This rotation will only affect rotate around the Y axis
+ * @param[out] out_R the result rotation matrix
+ * @param[in] pt the input point to nullify on X
+ */
+void getRotationNullifyX(Eigen::Matrix3d & out_R, const Eigen::Vector3d & pt);
+
 } // namespace sfm
 } // namespace aliceVision

src/software/utils/main_sfmTransform.cpp

@@ -269,6 +269,20 @@ int aliceVision_main(int argc, char **argv)
     return EXIT_FAILURE;
   }
 
+
+  // Sort views per timestamps
+  std::vector<std::pair<int64_t, IndexT>> sorted_views;
+  for (auto v : sfmData.getViews()) {
+      int64_t t = v.second->getMetadataDateTimestamp();
+      sorted_views.push_back(std::make_pair(t, v.second->getPoseId()));
+  }
+  std::sort(sorted_views.begin(), sorted_views.end());
+
+  // Get the view which was taken at the middle of the sequence 
+  int median = sorted_views.size() / 2;
+  IndexT refPoseId = sorted_views[sorted_views.size() - 1].second;
+  Eigen::Matrix3d ref_R_world = sfmData.getAbsolutePose(refPoseId).getTransform().rotation();
+
   double S = 1.0;
   Mat3 R = Mat3::Identity();
   Vec3 t = Vec3::Zero();
@@ -306,8 +320,22 @@ int aliceVision_main(int argc, char **argv)
     break;
 
     case EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS:
+    {
         sfm::computeNewCoordinateSystemFromCamerasXAxis(sfmData, S, R, t);
-        break;
+
+        Eigen::Matrix3d refcam_R_updatedWorld = ref_R_world * R.transpose();
+        Eigen::Matrix3d updatedWorld_R_refcam = refcam_R_updatedWorld.transpose();
+        Eigen::Vector3d alignmentVector = updatedWorld_R_refcam * Eigen::Vector3d::UnitZ();
+
+        Eigen::Matrix3d zeroX_R_world;
+        sfm::getRotationNullifyX(zeroX_R_world, alignmentVector);
+
+        //(zeroX_R_world * updatedWorld_R_refcam).transpose()
+        //(zeroX_R_world * (ref_R_world * R.transpose()).transpose()).transpose()
+        // ref_R_world * R.transpose() * zeroX_R_world.transpose()
+        R = zeroX_R_world * R;
+    }
+    break;
 
     case EAlignmentMethod::AUTO_FROM_LANDMARKS:
       sfm::computeNewCoordinateSystemFromLandmarks(sfmData, feature::EImageDescriberType_stringToEnums(landmarksDescriberTypesName), S, R, t);