Merge pull request #1334 from alicevision/dev/panoramaAlign

Panorama alignment with a reference camera

src/aliceVision/sfm/utils/alignment.cpp

@@ -695,8 +695,33 @@ IndexT getViewIdFromExpression(const sfmData::SfMData& sfmData, const std::strin
   try
   {
     viewId = boost::lexical_cast<IndexT>(camName);
-    if(!sfmData.getViews().count(viewId))
-      viewId = -1;
+    if (!sfmData.getViews().count(viewId))
+    {   
+        bool found = false;
+        //check if this view is an ancestor of a view
+        for (auto pv : sfmData.getViews())
+        {
+            for (auto ancestor : pv.second->getAncestors())
+            {
+                if (ancestor == viewId)
+                {
+                    viewId = pv.first;
+                    found = true;
+                    break;
+                }
+            }
+
+            if (found)
+            {
+                break;
+            }
+        }
+
+        if (!found)
+        {
+            viewId = -1;
+        }
+    }
   }
   catch(const boost::bad_lexical_cast &)
   {
@@ -938,5 +963,28 @@ 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::Matrix3d & R)
+{
+    Eigen::Vector3d alignmentVector = R.transpose() * Eigen::Vector3d::UnitZ();
+    getRotationNullifyX(out_R, alignmentVector);
+}
+
+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,23 @@ bool computeNewCoordinateSystemFromSpecificMarkers(const sfmData::SfMData& sfmDa
     Mat3& out_R,
     Vec3& out_t);
 
+/**
+ * @brief Compute the 3D rotation matrix such that "R.t() * unit_z"
+ * 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] R the input rotation matrix
+ */
+void getRotationNullifyX(Eigen::Matrix3d & out_R, const Eigen::Matrix3d & R);
+
+/**
+ * @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

@@ -184,6 +184,36 @@ static void parseManualTransform(const std::string& manualTransform, double& S,
 
 } // namespace
 
+IndexT getReferenceViewId(const sfmData::SfMData & sfmData, const std::string & transform)
+{
+    IndexT refViewId;
+    try
+    {
+        refViewId = sfm::getViewIdFromExpression(sfmData, transform);
+    }
+    catch (...)
+    {
+        refViewId = UndefinedIndexT;
+    }
+
+    if (refViewId == UndefinedIndexT)
+    {
+        // 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.first));
+        }
+        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;
+        refViewId = sorted_views[sorted_views.size() - 1].second;
+    }
+
+    return refViewId;
+}
+
 int aliceVision_main(int argc, char **argv)
 {
   // command-line parameters
@@ -306,8 +336,22 @@ int aliceVision_main(int argc, char **argv)
     break;
 
     case EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS:
+    {
+        // Align with x axis
         sfm::computeNewCoordinateSystemFromCamerasXAxis(sfmData, S, R, t);
-        break;
+
+        const IndexT refViewId = getReferenceViewId(sfmData, transform);
+
+        const Eigen::Matrix3d ref_R_world = sfmData.getPose(sfmData.getView(refViewId)).getTransform().rotation();
+
+        // Apply x axis alignment before doing the y alignment
+        const Eigen::Matrix3d refcam_R_updatedWorld = ref_R_world * R.transpose();
+
+        Eigen::Matrix3d zeroX_R_world;
+        sfm::getRotationNullifyX(zeroX_R_world, refcam_R_updatedWorld);
+        R = zeroX_R_world * R;
+    }
+    break;
 
     case EAlignmentMethod::AUTO_FROM_LANDMARKS:
       sfm::computeNewCoordinateSystemFromLandmarks(sfmData, feature::EImageDescriberType_stringToEnums(landmarksDescriberTypesName), S, R, t);