diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index 0e25e69413..5b1a58c07c 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/src/aliceVision/sfm/utils/alignment.cpp @@ -695,8 +695,33 @@ IndexT getViewIdFromExpression(const sfmData::SfMData& sfmData, const std::strin try { viewId = boost::lexical_cast(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 diff --git a/src/aliceVision/sfm/utils/alignment.hpp b/src/aliceVision/sfm/utils/alignment.hpp index 70031db405..fbf2418e8c 100644 --- a/src/aliceVision/sfm/utils/alignment.hpp +++ b/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 diff --git a/src/software/utils/main_sfmTransform.cpp b/src/software/utils/main_sfmTransform.cpp index 069c3631bc..e24af4b5af 100644 --- a/src/software/utils/main_sfmTransform.cpp +++ b/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> 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);