Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[sfmTransform] rewrite auto_from_cameras #1376

Merged
merged 2 commits into from
May 22, 2023
Merged

[sfmTransform] rewrite auto_from_cameras #1376

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
83010ba
[sfm/utils] computeNewCoordinateSystemFromCameras: rewrite and improve
servantftechnicolor Mar 13, 2023
dc2c8c0
[sfm/utils] alignment: 4-space indent
mugulmd May 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
222 changes: 106 additions & 116 deletions src/aliceVision/sfm/utils/alignment.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,20 +128,20 @@ bool computeSimilarityFromCommonCameras_viewId(const sfmData::SfMData& sfmDataA,
Mat3* out_R,
Vec3* out_t)
{
assert(out_S != nullptr);
assert(out_R != nullptr);
assert(out_t != nullptr);
std::vector<IndexT> commonViewIds;
getCommonViewsWithPoses(sfmDataA, sfmDataB, commonViewIds);
ALICEVISION_LOG_DEBUG("Found " << commonViewIds.size() << " common views.");

std::vector<std::pair<IndexT, IndexT>> commonViewIds_pairs;
for (IndexT id : commonViewIds)
{
commonViewIds_pairs.push_back(std::make_pair(id, id));
}
return computeSimilarityFromCommonViews(sfmDataA, sfmDataB, commonViewIds_pairs, randomNumberGenerator, out_S, out_R, out_t);
assert(out_S != nullptr);
assert(out_R != nullptr);
assert(out_t != nullptr);

std::vector<IndexT> commonViewIds;
getCommonViewsWithPoses(sfmDataA, sfmDataB, commonViewIds);
ALICEVISION_LOG_DEBUG("Found " << commonViewIds.size() << " common views.");

std::vector<std::pair<IndexT, IndexT>> commonViewIds_pairs;
for (IndexT id : commonViewIds)
{
commonViewIds_pairs.push_back(std::make_pair(id, id));
}
return computeSimilarityFromCommonViews(sfmDataA, sfmDataB, commonViewIds_pairs, randomNumberGenerator, out_S, out_R, out_t);
}

bool computeSimilarityFromCommonCameras_poseId(
Expand Down Expand Up @@ -629,126 +629,116 @@ void computeNewCoordinateSystemFromCameras(const sfmData::SfMData& sfmData,
Mat3& out_R,
Vec3& out_t)
{
const std::size_t nbCameras = sfmData.getPoses().size();
Mat3X vCamCenter(3,nbCameras);

// Compute the mean of the point cloud
Vec3 meanCameraCenter = Vec3::Zero(3, 1);
std::size_t i = 0;

for(const auto & pose : sfmData.getPoses())
{
const Vec3 center = pose.second.getTransform().center();
vCamCenter.col(i) = center;
meanCameraCenter += center;
++i;
}

meanCameraCenter /= nbCameras;

std::vector<Mat3> vCamRotation; // Camera rotations
vCamRotation.reserve(nbCameras);
i=0;
double rms = 0;
for(const auto & pose : sfmData.getPoses())
{
Vec3 camCenterMean = vCamCenter.col(i) - meanCameraCenter;
rms += camCenterMean.transpose() * camCenterMean; // squared dist to the mean of camera centers

vCamRotation.push_back(pose.second.getTransform().rotation().transpose()); // Rotation in the world coordinate system
++i;
}
rms /= nbCameras;
rms = std::sqrt(rms);

// Perform an svd over vX*vXT (var-covar)
Mat3 dum = vCamCenter * vCamCenter.transpose();
Eigen::JacobiSVD<Mat3> svd(dum,Eigen::ComputeFullV|Eigen::ComputeFullU);
Mat3 U = svd.matrixU();

// Check whether the determinant is positive in order to keep
// a direct coordinate system
if(U.determinant() < 0)
{
U.col(2) = -U.col(2);
}

out_R = U.transpose();

// TODO: take the median instead of the first camera
if((out_R * vCamRotation[0])(2,2) > 0)
{
out_S = -out_S;
}

out_R = Eigen::AngleAxisd(degreeToRadian(90.0), Vec3(1,0,0)) * out_R;
out_R = Eigen::AngleAxisd(degreeToRadian(180.0), Vec3(0, 0, 1)) * out_R; // Y UP
out_S = 1.0 / rms;

out_t = - out_S * out_R * meanCameraCenter;
const std::size_t nbCameras = sfmData.getPoses().size();
Mat3X vCamCenter(3,nbCameras);

// Compute the mean of the point cloud
Vec3 meanCameraCenter = Vec3::Zero();

Vec3::Index ncol = 0;
for (const auto & pose : sfmData.getPoses())
{
const Vec3 center = pose.second.getTransform().center();
vCamCenter.col(ncol) = center;
meanCameraCenter += center;
++ncol;
}
meanCameraCenter /= nbCameras;


// Compute standard deviation
double stddev = 0;
for (Vec3::Index i = 0; i < vCamCenter.cols(); ++i)
{
Vec3 camCenterMean = vCamCenter.col(i) - meanCameraCenter;
stddev += camCenterMean.transpose() * camCenterMean;
}
stddev /= nbCameras;

// Make sure the point cloud is centered and scaled to unit deviation
for (Vec3::Index i = 0; i < vCamCenter.cols(); ++i)
{
vCamCenter.col(i) = (vCamCenter.col(i) - meanCameraCenter) / stddev;
}

// Plane fitting of the centered point cloud
// using Singular Value Decomposition (SVD)
Eigen::JacobiSVD<Mat> svd(vCamCenter.transpose(), Eigen::ComputeFullV);
Eigen::Vector3d n = svd.matrixV().col(2);

// Normal vector sign can't really be estimated. Just make sure it's the positive Y half sphere
if (n(1) < 0)
{
n = -n;
}

// We want ideal normal to be the Y axis
out_R = Matrix3d(Quaterniond().setFromTwoVectors(n, Eigen::Vector3d::UnitY()));
out_S = 1.0 / sqrt(stddev);
out_t = - out_S * out_R * meanCameraCenter;
}

IndexT getViewIdFromExpression(const sfmData::SfMData& sfmData, const std::string & camName)
{
IndexT viewId = -1;

std::regex cameraRegex = simpleFilterToRegex_noThrow(camName);

try
{
viewId = boost::lexical_cast<IndexT>(camName);
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())
IndexT viewId = -1;

std::regex cameraRegex = simpleFilterToRegex_noThrow(camName);

try
{
viewId = boost::lexical_cast<IndexT>(camName);
if (!sfmData.getViews().count(viewId))
{
bool found = false;
//check if this view is an ancestor of a view
for (auto pv : sfmData.getViews())
{
if (ancestor == viewId)
for (auto ancestor : pv.second->getAncestors())
{
if (ancestor == viewId)
{
viewId = pv.first;
found = true;
break;
}
}

if (found)
{
viewId = pv.first;
found = true;
break;
}
}

if (found)
if (!found)
{
break;
viewId = -1;
}
}
}
catch(const boost::bad_lexical_cast &)
{
viewId = -1;
}

if (!found)
if(viewId == -1)
{
for(const auto & view : sfmData.getViews())
{
viewId = -1;
const std::string path = view.second->getImagePath();
if(std::regex_match(path, cameraRegex))
{
viewId = view.second->getViewId();
break;
}
}
}
}
catch(const boost::bad_lexical_cast &)
{
viewId = -1;
}

if(viewId == -1)
{
for(const auto & view : sfmData.getViews())
{
const std::string path = view.second->getImagePath();
if(std::regex_match(path, cameraRegex))
{
viewId = view.second->getViewId();
break;
}
}
}

if(viewId == -1)
if(viewId == -1)
throw std::invalid_argument("The camera name \"" + camName + "\" is not found in the sfmData.");
else if(!sfmData.isPoseAndIntrinsicDefined(viewId))
else if(!sfmData.isPoseAndIntrinsicDefined(viewId))
throw std::invalid_argument("The camera \"" + camName + "\" exists in the sfmData but is not reconstructed.");

return viewId;
return viewId;
}

IndexT getCenterCameraView(const sfmData::SfMData& sfmData)
Expand Down Expand Up @@ -834,8 +824,8 @@ void computeNewCoordinateSystemFromLandmarks(const sfmData::SfMData& sfmData,
// Center the point cloud in [0;0;0]
for(int i = 0; i < landmarksCount; ++i)
{
vX.col(i) -= meanPoints;
accDist(vX.col(i).norm());
vX.col(i) -= meanPoints;
accDist(vX.col(i).norm());
}

// Perform an svd over vX*vXT (var-covar)
Expand All @@ -847,7 +837,7 @@ void computeNewCoordinateSystemFromLandmarks(const sfmData::SfMData& sfmData,
// a direct coordinate system
if(U.determinant() < 0)
{
U.col(2) = -U.col(2);
U.col(2) = -U.col(2);
}

const double distMax = quantile(accDist, quantile_probability = percentile);
Expand Down
97 changes: 49 additions & 48 deletions src/aliceVision/sfm/utils/alignment.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,32 +17,32 @@ inline void getCommonViews(const sfmData::SfMData& sfmDataA,
const sfmData::SfMData& sfmDataB,
std::vector<IndexT>& outIndexes)
{
for(const auto& viewA: sfmDataA.getViews())
{
if(sfmDataB.getViews().find(viewA.first) != sfmDataB.getViews().end())
for(const auto& viewA: sfmDataA.getViews())
{
outIndexes.push_back(viewA.first);
if(sfmDataB.getViews().find(viewA.first) != sfmDataB.getViews().end())
{
outIndexes.push_back(viewA.first);
}
}
}
}

inline void getCommonViewsWithPoses(const sfmData::SfMData& sfmDataA,
const sfmData::SfMData& sfmDataB,
std::vector<IndexT>& outIndexes)
{
for(const auto& viewA: sfmDataA.getViews())
{
// check there is a view with the same ID and both of them have pose and
// intrinsics defined
if(!sfmDataA.isPoseAndIntrinsicDefined(viewA.second.get()))
continue;

if(sfmDataB.getViews().find(viewA.first) != sfmDataB.getViews().end() &&
sfmDataB.isPoseAndIntrinsicDefined(viewA.first))
for(const auto& viewA: sfmDataA.getViews())
{
outIndexes.push_back(viewA.first);
// check there is a view with the same ID and both of them have pose and
// intrinsics defined
if(!sfmDataA.isPoseAndIntrinsicDefined(viewA.second.get()))
continue;

if(sfmDataB.getViews().find(viewA.first) != sfmDataB.getViews().end() &&
sfmDataB.isPoseAndIntrinsicDefined(viewA.first))
{
outIndexes.push_back(viewA.first);
}
}
}
}

inline void getCommonPoseId(const sfmData::SfMData& sfmDataA,
Expand Down Expand Up @@ -153,32 +153,33 @@ inline void applyTransform(sfmData::SfMData& sfmData,
const Vec3& t,
bool transformControlPoints = false)
{
for(auto& poseIt: sfmData.getPoses())
{
geometry::Pose3 pose = poseIt.second.getTransform();
pose = pose.transformSRt(S, R, t);
poseIt.second.setTransform(pose);
}
for (auto& rigIt : sfmData.getRigs())
{
for (auto& subPose : rigIt.second.getSubPoses())
{
subPose.pose.center() *= S;
}
}

for(auto& landmark: sfmData.structure)
{
landmark.second.X = S * R * landmark.second.X + t;
}

if(!transformControlPoints)
return;

for(auto& controlPts: sfmData.control_points)
{
controlPts.second.X = S * R * controlPts.second.X + t;
}
for(auto& poseIt: sfmData.getPoses())
{
geometry::Pose3 pose = poseIt.second.getTransform();
pose = pose.transformSRt(S, R, t);
poseIt.second.setTransform(pose);
}

for (auto& rigIt : sfmData.getRigs())
{
for (auto& subPose : rigIt.second.getSubPoses())
{
subPose.pose.center() *= S;
}
}

for(auto& landmark: sfmData.structure)
{
landmark.second.X = S * R * landmark.second.X + t;
}

if(!transformControlPoints)
return;

for(auto& controlPts: sfmData.control_points)
{
controlPts.second.X = S * R * controlPts.second.X + t;
}
}

/**
Expand Down Expand Up @@ -210,12 +211,12 @@ 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 reconstruction so that the mean
* of the camera centers is the origin of the world coordinate system, a
* dominant plane P is fitted to the set of the optical centers and the scene
* aligned so that P roughly define the (x,y) plane, and the scale is set so
* that the optical centers RMS is "1.0".
* (Hartley-like normalization, p.180)
* @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,
* a dominant plane P is fitted to the set of the optical centers
* and the scene aligned so that P roughly define the (x,y) plane,
* and the scale is set so that the optical centers standard deviation is "1.0".
* @see https://www.ltu.se/cms_fs/1.51590!/svd-fitting.pdf
*
* @param[in] sfmData
* @param[out] out_S scale
Expand Down