diff --git a/src/aliceVision/sfm/utils/alignment.cpp b/src/aliceVision/sfm/utils/alignment.cpp index 4f99e77c60..6ff7c65144 100644 --- a/src/aliceVision/sfm/utils/alignment.cpp +++ b/src/aliceVision/sfm/utils/alignment.cpp @@ -1026,6 +1026,62 @@ void computeCentersVarCov(const sfmData::SfMData& sfmData, const Vec3 & mean, Ei } } +void computeNewCoordinateSystemGroundAuto(const sfmData::SfMData& sfmData, Vec3& out_t) +{ + out_t.fill(0.0); + + // Collect landmark positions for ground detection + // Note: the Y axis is pointing down, therefore +Y is the direction to the ground + std::vector points; + for (auto & plandmark: sfmData.getLandmarks()) + { + // Filter out landmarks with not enough observations + if (plandmark.second.observations.size() < 3) + { + continue; + } + + // Filter out landmarks that lie above all the cameras that observe them + // This filtering step assumes that cameras should not be underneath the ground level + const Vec3 X = plandmark.second.X; + bool foundUnder = false; + for (const auto & pObs : plandmark.second.observations) + { + const IndexT viewId = pObs.first; + const Vec3 camCenter = sfmData.getPose(sfmData.getView(viewId)).getTransform().center(); + + if (X(1) > camCenter(1)) + { + foundUnder = true; + break; + } + } + + // Store landmark position + if (foundUnder) + { + points.push_back(X); + } + } + + if (points.empty()) + { + ALICEVISION_LOG_WARNING("Ground detection failed as there is no valid point"); + return; + } + + // Filter out statistical noise + // and take lowest point as the ground level + const double noiseRatio = 1e-4; + std::size_t idxGround = static_cast(static_cast(points.size()) * noiseRatio); + std::nth_element( + points.begin(), points.begin() + idxGround, points.end(), + [](const Vec3 & pt1, const Vec3 & pt2) { return (pt1(1) > pt2(1)); }); + const double Yground = points[idxGround](1); + + out_t = {0.0, -Yground, 0.0}; +} + void computeNewCoordinateSystemAuto(const sfmData::SfMData& sfmData, double& out_S, Mat3& out_R, Vec3& out_t) { //For reference, the update is diff --git a/src/aliceVision/sfm/utils/alignment.hpp b/src/aliceVision/sfm/utils/alignment.hpp index a81dbc74dd..d830fb61c0 100644 --- a/src/aliceVision/sfm/utils/alignment.hpp +++ b/src/aliceVision/sfm/utils/alignment.hpp @@ -210,6 +210,14 @@ void computeNewCoordinateSystemFromCamerasXAxis(const sfmData::SfMData& sfmData, */ void computeNewCoordinateSystemAuto(const sfmData::SfMData& sfmData, double& out_S, Mat3& out_R, Vec3& out_t); +/** + * @brief Compute the new coordinate system in the given SfM so that the ground is at Y=0 + * + * @param[in] sfmData + * @param[out] out_t translation + */ +void computeNewCoordinateSystemGroundAuto(const sfmData::SfMData& sfmData, Vec3& out_t); + /** * @brief Compute the new coordinate system in the given reconstruction * so that the mean of the camera centers is the origin of the world coordinate system, diff --git a/src/software/utils/main_sfmTransform.cpp b/src/software/utils/main_sfmTransform.cpp index b48a4a5ec5..4b7c06c1d4 100644 --- a/src/software/utils/main_sfmTransform.cpp +++ b/src/software/utils/main_sfmTransform.cpp @@ -22,7 +22,7 @@ // These constants define the current software version. // They must be updated when the command line is changed. #define ALICEVISION_SOFTWARE_VERSION_MAJOR 1 -#define ALICEVISION_SOFTWARE_VERSION_MINOR 0 +#define ALICEVISION_SOFTWARE_VERSION_MINOR 1 using namespace aliceVision; @@ -44,7 +44,8 @@ enum class EAlignmentMethod : unsigned char FROM_SINGLE_CAMERA, FROM_CENTER_CAMERA, FROM_MARKERS, - FROM_GPS + FROM_GPS, + ALIGN_GROUND }; /** @@ -56,16 +57,17 @@ std::string EAlignmentMethod_enumToString(EAlignmentMethod alignmentMethod) { switch(alignmentMethod) { - case EAlignmentMethod::TRANSFORMATION: return "transformation"; - case EAlignmentMethod::MANUAL: return "manual"; - case EAlignmentMethod::AUTO: return "auto"; - case EAlignmentMethod::AUTO_FROM_CAMERAS: return "auto_from_cameras"; - case EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS: return "auto_from_cameras_x_axis"; - case EAlignmentMethod::AUTO_FROM_LANDMARKS: return "auto_from_landmarks"; - case EAlignmentMethod::FROM_SINGLE_CAMERA: return "from_single_camera"; - case EAlignmentMethod::FROM_CENTER_CAMERA: return "from_center_camera"; - case EAlignmentMethod::FROM_MARKERS: return "from_markers"; - case EAlignmentMethod::FROM_GPS: return "from_gps"; + case EAlignmentMethod::TRANSFORMATION: return "transformation"; + case EAlignmentMethod::MANUAL: return "manual"; + case EAlignmentMethod::AUTO: return "auto"; + case EAlignmentMethod::AUTO_FROM_CAMERAS: return "auto_from_cameras"; + case EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS: return "auto_from_cameras_x_axis"; + case EAlignmentMethod::AUTO_FROM_LANDMARKS: return "auto_from_landmarks"; + case EAlignmentMethod::FROM_SINGLE_CAMERA: return "from_single_camera"; + case EAlignmentMethod::FROM_CENTER_CAMERA: return "from_center_camera"; + case EAlignmentMethod::FROM_MARKERS: return "from_markers"; + case EAlignmentMethod::FROM_GPS: return "from_gps"; + case EAlignmentMethod::ALIGN_GROUND: return "align_ground"; } throw std::out_of_range("Invalid EAlignmentMethod enum"); } @@ -80,16 +82,17 @@ EAlignmentMethod EAlignmentMethod_stringToEnum(const std::string& alignmentMetho std::string method = alignmentMethod; std::transform(method.begin(), method.end(), method.begin(), ::tolower); //tolower - if(method == "transformation") return EAlignmentMethod::TRANSFORMATION; - if(method == "manual") return EAlignmentMethod::MANUAL; - if(method == "auto") return EAlignmentMethod::AUTO; - if(method == "auto_from_cameras") return EAlignmentMethod::AUTO_FROM_CAMERAS; - if(method == "auto_from_cameras_x_axis") return EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS; - if(method == "auto_from_landmarks") return EAlignmentMethod::AUTO_FROM_LANDMARKS; - if(method == "from_single_camera") return EAlignmentMethod::FROM_SINGLE_CAMERA; - if(method == "from_center_camera") return EAlignmentMethod::FROM_CENTER_CAMERA; - if(method == "from_markers") return EAlignmentMethod::FROM_MARKERS; - if(method == "from_gps") return EAlignmentMethod::FROM_GPS; + if (method == "transformation") return EAlignmentMethod::TRANSFORMATION; + if (method == "manual") return EAlignmentMethod::MANUAL; + if (method == "auto") return EAlignmentMethod::AUTO; + if (method == "auto_from_cameras") return EAlignmentMethod::AUTO_FROM_CAMERAS; + if (method == "auto_from_cameras_x_axis") return EAlignmentMethod::AUTO_FROM_CAMERAS_X_AXIS; + if (method == "auto_from_landmarks") return EAlignmentMethod::AUTO_FROM_LANDMARKS; + if (method == "from_single_camera") return EAlignmentMethod::FROM_SINGLE_CAMERA; + if (method == "from_center_camera") return EAlignmentMethod::FROM_CENTER_CAMERA; + if (method == "from_markers") return EAlignmentMethod::FROM_MARKERS; + if (method == "from_gps") return EAlignmentMethod::FROM_GPS; + if (method == "align_ground") return EAlignmentMethod::ALIGN_GROUND; throw std::out_of_range("Invalid SfM alignment method : " + alignmentMethod); } @@ -265,13 +268,14 @@ int aliceVision_main(int argc, char **argv) "Transform Method:\n" "\t- transformation: Apply a given transformation\n" "\t- manual: Apply the gizmo transformation\n" - "\t- auto: Use cameras X axis, Zero centering, automatic scaling\n" - "\t- auto_from_cameras: Use cameras\n" - "\t- auto_from_cameras_x_axis: Use cameras X axis\n" - "\t- auto_from_landmarks: Use landmarks\n" - "\t- from_single_camera: Use camera specified by --tranformation\n" - "\t- from_markers: Use markers specified by --markers\n" - "\t- from_gps: use gps metadata\n") + "\t- auto: Determines scene orientation from the cameras' X axis, auto-scaling from GPS information if available, and defines ground level from the point cloud.\n" + "\t- auto_from_cameras: Defines coordinate system from cameras.\n" + "\t- auto_from_cameras_x_axis: Determines scene orientation from the cameras' X axis.\n" + "\t- auto_from_landmarks: Defines coordinate system from landmarks.\n" + "\t- from_single_camera: Refines the coordinate system from the camera specified by --tranformation\n" + "\t- from_markers: Refines the coordinate system from markers specified by --markers\n" + "\t- from_gps: Redefines coordinate system from GPS metadata\n" + "\t- align_ground: defines ground level from the point cloud density. It assumes that the scene is oriented.\n") ("transformation", po::value(&transform)->default_value(transform), "required only for 'transformation' and 'single camera' methods:\n" "Transformation: Align [X,Y,Z] to +Y-axis, rotate around Y by R deg, scale by S; syntax: X,Y,Z;R;S\n" @@ -471,6 +475,11 @@ int aliceVision_main(int argc, char **argv) } break; } + case EAlignmentMethod::ALIGN_GROUND: + { + sfm::computeNewCoordinateSystemGroundAuto(sfmData, t); + break; + } } if(!applyRotation) @@ -510,6 +519,13 @@ int aliceVision_main(int argc, char **argv) } sfm::applyTransform(sfmData, S, R, t); + + // In AUTO mode, ground detection and alignment is performed as a post-process + if (alignmentMethod == EAlignmentMethod::AUTO && applyTranslation) + { + sfm::computeNewCoordinateSystemGroundAuto(sfmData, t); + sfm::applyTransform(sfmData, 1.0, Eigen::Matrix3d::Identity(), t); + } ALICEVISION_LOG_INFO("Save into '" << outSfMDataFilename << "'");