Skip to content

Commit

Permalink
Merge pull request #1438 from alicevision/dev/ground
Browse files Browse the repository at this point in the history 
Ground level alignment based on sfm point cloud
  • Loading branch information
@fabiencastan
fabiencastan authored Jun 3, 2023
2 parents 9b05a87 + e5285c8 commit afcd397
Show file tree
Hide file tree
Showing 3 changed files with 109 additions and 29 deletions.
56 changes: 56 additions & 0 deletions src/aliceVision/sfm/utils/alignment.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Vec3> 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<std::size_t>(static_cast<double>(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
Expand Down
8 changes: 8 additions & 0 deletions src/aliceVision/sfm/utils/alignment.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
74 changes: 45 additions & 29 deletions src/software/utils/main_sfmTransform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -44,7 +44,8 @@ enum class EAlignmentMethod : unsigned char
FROM_SINGLE_CAMERA,
FROM_CENTER_CAMERA,
FROM_MARKERS,
FROM_GPS
FROM_GPS,
ALIGN_GROUND
};

/**
Expand All @@ -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");
}
Expand All @@ -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);
}

Expand Down Expand Up @@ -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<std::string>(&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"
Expand Down Expand Up @@ -471,6 +475,11 @@ int aliceVision_main(int argc, char **argv)
}
break;
}
case EAlignmentMethod::ALIGN_GROUND:
{
sfm::computeNewCoordinateSystemGroundAuto(sfmData, t);
break;
}
}

if(!applyRotation)
Expand Down Expand Up @@ -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 << "'");

Expand Down

0 comments on commit afcd397

Please sign in to comment.