Skip to content

Commit

Permalink
Merge pull request #1622 from alicevision/dev/sfmdata
Browse files Browse the repository at this point in the history
Enhance controllability and readability by ensuring non direct access to data members
  • Loading branch information
cbentejac authored Dec 14, 2023
2 parents 6b018d0 + 28b8688 commit bc7c981
Show file tree
Hide file tree
Showing 64 changed files with 411 additions and 294 deletions.
14 changes: 7 additions & 7 deletions src/aliceVision/depthMap/SgmDepthList.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -298,14 +298,14 @@ void SgmDepthList::getMinMaxMidNbDepthFromSfM(float& out_min, float& out_max, fl
const Point3d point(landmark.X(0), landmark.X(1), landmark.X(2));

// find rc observation
const auto it = landmark.observations.find(viewId);
const auto it = landmark.getObservations().find(viewId);

// no rc observation
if (it == landmark.observations.end())
if (it == landmark.getObservations().end())
continue;

// get rc 2d observation
const Vec2& obs2d = it->second.x;
const Vec2& obs2d = it->second.getCoordinates();

// if we compute depth list per tile keep only observation located inside the inflated image full-size ROI
if (!_sgmParams.depthListPerTile || fullsizeRoi.contains(obs2d.x(), obs2d.y()))
Expand Down Expand Up @@ -370,18 +370,18 @@ void SgmDepthList::getRcTcDepthRangeFromSfM(int tc, double& out_zmin, double& ou
const Point3d point(landmark.X(0), landmark.X(1), landmark.X(2));

// no tc observation
if (landmark.observations.find(tcViewId) == landmark.observations.end())
if (landmark.getObservations().find(tcViewId) == landmark.getObservations().end())
continue;

// find rc observation
const auto it = landmark.observations.find(rcViewId);
const auto it = landmark.getObservations().find(rcViewId);

// no rc observation
if (it == landmark.observations.end())
if (it == landmark.getObservations().end())
continue;

// get rc 2d observation
const Vec2& obs2d = it->second.x;
const Vec2& obs2d = it->second.getCoordinates();

// observation located inside the inflated image full-size ROI
if (!_sgmParams.depthListPerTile || fullsizeRoi.contains(obs2d.x(), obs2d.y()))
Expand Down
12 changes: 6 additions & 6 deletions src/aliceVision/depthMap/volumeIO.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ void exportSimilarityVolume(const CudaHostMemoryHeap<TSim, 3>& in_volumeSim_hmh,
continue;
const rgb c = getRGBFromJetColorMap(simValue / maxValue);
pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark(
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down Expand Up @@ -235,7 +235,7 @@ void exportSimilarityVolumeCross(const CudaHostMemoryHeap<TSim, 3>& in_volumeSim

const rgb c = getRGBFromJetColorMap(simValue / maxValue);
pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark(
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down Expand Up @@ -293,7 +293,7 @@ void exportSimilarityVolumeCross(const CudaHostMemoryHeap<TSimRefine, 3>& in_vol

const rgb c = getRGBFromJetColorMap(simValue / maxValue);
pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark(
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down Expand Up @@ -365,7 +365,7 @@ void exportSimilarityVolumeTopographicCut(const CudaHostMemoryHeap<TSim, 3>& in_

const rgb c = getRGBFromJetColorMap(simValueNorm);
pointCloud.getLandmarks()[landmarkId] =
sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down Expand Up @@ -430,7 +430,7 @@ void exportSimilarityVolumeTopographicCut(const CudaHostMemoryHeap<TSimRefine, 3

const rgb c = getRGBFromJetColorMap(simValueColor);
pointCloud.getLandmarks()[landmarkId] =
sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
sfmData::Landmark(Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down Expand Up @@ -491,7 +491,7 @@ void exportColorVolume(const CudaHostMemoryHeap<float4, 3>& in_volumeSim_hmh,
float4 colorValue = *get3DBufferAt_h<float4>(in_volumeSim_hmh.getBuffer(), spitch, pitch, vx, vy, vz);
const rgb c = float4_to_rgb(colorValue); // TODO: convert Lab color into sRGB color
pointCloud.getLandmarks()[landmarkId] = sfmData::Landmark(
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, sfmData::Observations(), image::RGBColor(c.r, c.g, c.b));
Vec3(p.x, p.y, p.z), feature::EImageDescriberType::UNKNOWN, image::RGBColor(c.r, c.g, c.b));

++landmarkId;
}
Expand Down
4 changes: 2 additions & 2 deletions src/aliceVision/fuseCut/DelaunayGraphCut.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -775,10 +775,10 @@ void DelaunayGraphCut::addPointsFromSfM(const Point3d hexah[8], const StaticVect
{
*vCoordsIt = p;

vAttrIt->nrc = landmark.observations.size();
vAttrIt->nrc = landmark.getObservations().size();
vAttrIt->cams.reserve(vAttrIt->nrc);

for (const auto& observationPair : landmark.observations)
for (const auto& observationPair : landmark.getObservations())
vAttrIt->cams.push_back(_mp.getIndexFromViewId(observationPair.first));

vAttrIt->pixSize = _mp.getCamsMinPixelSize(p, vAttrIt->cams);
Expand Down
2 changes: 1 addition & 1 deletion src/aliceVision/fuseCut/DelaunayGraphCut_test.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -200,7 +200,7 @@ SfMData generateSfm(const NViewDatasetConfigurator& config, const size_t size, c
for (int j = 0; j < camsPts.size(); ++j)
{
const Vec2 pt = projectedPtsPerCam[j].col(i);
landmark.observations[j] = Observation(pt, i, unknownScale);
landmark.getObservations()[j] = Observation(pt, i, unknownScale);
}
sfm_data.getLandmarks()[i] = landmark;
}
Expand Down
10 changes: 5 additions & 5 deletions src/aliceVision/fuseCut/Fuser.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -360,7 +360,7 @@ float Fuser::computeAveragePixelSizeInHexahedron(Point3d* hexah, const sfmData::
const sfmData::Landmark& landmark = landmarkPair.second;
const Point3d p(landmark.X(0), landmark.X(1), landmark.X(2));

for (const auto& observationPair : landmark.observations)
for (const auto& observationPair : landmark.getObservations())
{
const IndexT viewId = observationPair.first;

Expand Down Expand Up @@ -506,14 +506,14 @@ void Fuser::divideSpaceFromDepthMaps(Point3d* hexah, float& minPixSize)

bool checkLandmarkMinObservationAngle(const sfmData::SfMData& sfmData, const sfmData::Landmark& landmark, float minObservationAngle)
{
for (const auto& observationPairI : landmark.observations)
for (const auto& observationPairI : landmark.getObservations())
{
const IndexT I = observationPairI.first;
const sfmData::View& viewI = *(sfmData.getViews().at(I));
const geometry::Pose3 poseI = sfmData.getPose(viewI).getTransform();
const camera::IntrinsicBase* intrinsicPtrI = sfmData.getIntrinsicPtr(viewI.getIntrinsicId());

for (const auto& observationPairJ : landmark.observations)
for (const auto& observationPairJ : landmark.getObservations())
{
const IndexT J = observationPairJ.first;

Expand All @@ -526,7 +526,7 @@ bool checkLandmarkMinObservationAngle(const sfmData::SfMData& sfmData, const sfm
const camera::IntrinsicBase* intrinsicPtrJ = sfmData.getIntrinsicPtr(viewJ.getIntrinsicId());

const double angle =
camera::angleBetweenRays(poseI, intrinsicPtrI, poseJ, intrinsicPtrJ, observationPairI.second.x, observationPairJ.second.x);
camera::angleBetweenRays(poseI, intrinsicPtrI, poseJ, intrinsicPtrJ, observationPairI.second.getCoordinates(), observationPairJ.second.getCoordinates());

// check angle between two observation
if (angle < minObservationAngle)
Expand Down Expand Up @@ -562,7 +562,7 @@ void Fuser::divideSpaceFromSfM(const sfmData::SfMData& sfmData, Point3d* hexah,
const sfmData::Landmark& landmark = landmarkPair.second;

// check number of observations
if (landmark.observations.size() < minObservations)
if (landmark.getObservations().size() < minObservations)
continue;

// check angle between observations
Expand Down
4 changes: 2 additions & 2 deletions src/aliceVision/localization/CCTagLocalizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -77,11 +77,11 @@ bool CCTagLocalizer::loadReconstructionDescriptors(const sfmData::SfMData& sfm_d
IndexT trackId = landmarkValue.first;
const sfmData::Landmark& landmark = landmarkValue.second;

for (const auto& obs : landmark.observations)
for (const auto& obs : landmark.getObservations())
{
const IndexT viewId = obs.first;
const sfmData::Observation& obs2d = obs.second;
observationsPerView[viewId][landmark.descType].emplace_back(obs2d.id_feat, trackId);
observationsPerView[viewId][landmark.descType].emplace_back(obs2d.getFeatureId(), trackId);
}
}
for (auto& featuresPerTypeInImage : observationsPerView)
Expand Down
4 changes: 2 additions & 2 deletions src/aliceVision/localization/VoctreeLocalizer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -286,11 +286,11 @@ bool VoctreeLocalizer::initDatabase(const std::string& vocTreeFilepath, const st
IndexT trackId = landmarkValue.first;
const sfmData::Landmark& landmark = landmarkValue.second;

for (const auto& obs : landmark.observations)
for (const auto& obs : landmark.getObservations())
{
const IndexT viewId = obs.first;
const sfmData::Observation& obs2d = obs.second;
observationsPerView[viewId][landmark.descType].emplace_back(obs2d.id_feat, trackId);
observationsPerView[viewId][landmark.descType].emplace_back(obs2d.getFeatureId(), trackId);
}
}
for (auto& featuresPerTypeInImage : observationsPerView)
Expand Down
18 changes: 9 additions & 9 deletions src/aliceVision/localization/optimization.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -148,11 +148,11 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,
assert(landmark.descType == match.descType);
// normally there should be no other features already associated to this
// 3D point in this view
if (landmark.observations.count(viewID) != 0)
if (landmark.getObservations().count(viewID) != 0)
{
// this is weird but it could happen when two features are really close to each other (?)
ALICEVISION_LOG_DEBUG("Point 3D " << match.landmarkId << " has multiple features "
<< "in the same view " << viewID << ", current size of obs: " << landmark.observations.size());
<< "in the same view " << viewID << ", current size of obs: " << landmark.getObservations().size());
ALICEVISION_LOG_DEBUG("its associated features are: ");
for (std::size_t i = 0; i < matches.size(); ++i)
{
Expand All @@ -169,15 +169,15 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,
}

// the 3D point exists already, add the observation
landmark.observations[viewID] = sfmData::Observation(feature, match.featId, unknownScale);
landmark.getObservations()[viewID] = sfmData::Observation(feature, match.featId, unknownScale);
}
else
{
// create a new 3D point
sfmData::Landmark newLandmark;
newLandmark.descType = match.descType;
newLandmark.X = currResult.getPt3D().col(idx);
newLandmark.observations[viewID] = sfmData::Observation(feature, match.featId, unknownScale);
newLandmark.getObservations()[viewID] = sfmData::Observation(feature, match.featId, unknownScale);
tinyScene.getLandmarks()[match.landmarkId] = std::move(newLandmark);
}
}
Expand All @@ -188,7 +188,7 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,
// sfmData::Landmarks &landmarks = tinyScene.getLandmarks();
// for(sfmData::Landmarks::iterator it = landmarks.begin(), ite = landmarks.end(); it != ite;)
// {
// if(it->second.observations.size() < 5)
// if(it->second.getObservations().size() < 5)
// {
// it = landmarks.erase(it);
// }
Expand All @@ -205,15 +205,15 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,
std::size_t maxObs = 0;
for (const auto landmark : tinyScene.getLandmarks())
{
if (landmark.second.observations.size() > maxObs)
maxObs = landmark.second.observations.size();
if (landmark.second.getObservations().size() > maxObs)
maxObs = landmark.second.getObservations().size();
}
namespace bacc = boost::accumulators;
bacc::accumulator_set<std::size_t, bacc::stats<bacc::tag::mean, bacc::tag::min, bacc::tag::max, bacc::tag::sum>> stats;
std::vector<std::size_t> hist(maxObs + 1, 0);
for (const auto landmark : tinyScene.getLandmarks())
{
const std::size_t nobs = landmark.second.observations.size();
const std::size_t nobs = landmark.second.getObservations().size();
assert(nobs < hist.size());
stats(nobs);
hist[nobs]++;
Expand Down Expand Up @@ -250,7 +250,7 @@ bool refineSequence(std::vector<LocalizationResult>& vec_localizationResult,

for (; iter != endIter;)
{
if (iter->second.observations.size() < minPointVisibility)
if (iter->second.getObservations().size() < minPointVisibility)
{
iter = landmarks.erase(iter);
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -57,7 +57,7 @@ bool refinePoseAsItShouldbe(const Mat& pt3D,
const size_t idx = vec_inliers[i];
Landmark landmark;
landmark.X = pt3D.col(idx);
landmark.observations[0] = Observation(pt2D.col(idx), UndefinedIndexT, unknownScale);
landmark.getObservations()[0] = Observation(pt2D.col(idx), UndefinedIndexT, unknownScale);
sfm_data.getLandmarks()[i] = std::move(landmark);
}

Expand Down
10 changes: 5 additions & 5 deletions src/aliceVision/mvsUtils/MultiViewParams.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -528,7 +528,7 @@ StaticVector<int> MultiViewParams::findNearestCamsFromLandmarks(int rc, int nbNe

for (const auto& landmarkPair : _sfmData.getLandmarks())
{
const auto& observations = landmarkPair.second.observations;
const auto& observations = landmarkPair.second.getObservations();

auto viewObsIt = observations.find(viewId);
if (viewObsIt == observations.end())
Expand All @@ -546,7 +546,7 @@ StaticVector<int> MultiViewParams::findNearestCamsFromLandmarks(int rc, int nbNe
const camera::IntrinsicBase* otherIntrinsicPtr = _sfmData.getIntrinsicPtr(otherView.getIntrinsicId());

const double angle =
camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.x, observationPair.second.x);
camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.getCoordinates(), observationPair.second.getCoordinates());

if (angle < _minViewAngle || angle > _maxViewAngle)
continue;
Expand Down Expand Up @@ -604,7 +604,7 @@ std::vector<int> MultiViewParams::findTileNearestCams(int rc, int nbNearestCams,

for (const auto& landmarkPair : sfmData.getLandmarks())
{
const auto& observations = landmarkPair.second.observations;
const auto& observations = landmarkPair.second.getObservations();

auto viewObsIt = observations.find(viewId);

Expand All @@ -613,7 +613,7 @@ std::vector<int> MultiViewParams::findTileNearestCams(int rc, int nbNearestCams,
continue;

// landmark R camera observation is in the image full-size ROI
if (!fullsizeRoi.contains(viewObsIt->second.x.x(), viewObsIt->second.x.y()))
if (!fullsizeRoi.contains(viewObsIt->second.getX(), viewObsIt->second.getY()))
continue;

for (const auto& observationPair : observations)
Expand All @@ -635,7 +635,7 @@ std::vector<int> MultiViewParams::findTileNearestCams(int rc, int nbNearestCams,
const camera::IntrinsicBase* otherIntrinsicPtr = sfmData.getIntrinsicPtr(otherView.getIntrinsicId());

const double angle =
camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.x, observationPair.second.x);
camera::angleBetweenRays(pose, intrinsicPtr, otherPose, otherIntrinsicPtr, viewObsIt->second.getCoordinates(), observationPair.second.getCoordinates());

tcScore[tc] += plateauFunction(1, 10, 50, 150, angle);
}
Expand Down
12 changes: 6 additions & 6 deletions src/aliceVision/photometricStereo/normalIntegration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -543,10 +543,10 @@ void adjustScale(const sfmData::SfMData& sfmData, image::Image<float>& initDepth
const sfmData::Landmark& currentLandmark = landmarks.at(currentLandmarkIndex);
knownDepths(i) = pose.depth(currentLandmark.X);

sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID);
sfmData::Observation observationInCurrentPicture = currentLandmark.getObservations().at(viewID);

int rowInd = observationInCurrentPicture.x(1);
int colInd = observationInCurrentPicture.x(0);
int rowInd = observationInCurrentPicture.getY();
int colInd = observationInCurrentPicture.getX();

estimatedDepths(i) = initDepth(rowInd, colInd);
}
Expand Down Expand Up @@ -576,10 +576,10 @@ void getZ0FromLandmarks(const sfmData::SfMData& sfmData,
{
size_t currentLandmarkIndex = visibleLandmarks.at(i);
const sfmData::Landmark& currentLandmark = landmarks.at(currentLandmarkIndex);
sfmData::Observation observationInCurrentPicture = currentLandmark.observations.at(viewID);
sfmData::Observation observationInCurrentPicture = currentLandmark.getObservations().at(viewID);

int rowInd = observationInCurrentPicture.x(1);
int colInd = observationInCurrentPicture.x(0);
int rowInd = observationInCurrentPicture.getY();
int colInd = observationInCurrentPicture.getX();

if (mask(rowInd, colInd) > 0.7)
{
Expand Down
2 changes: 1 addition & 1 deletion src/aliceVision/sfm/FrustumFilter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -169,7 +169,7 @@ void FrustumFilter::init_z_near_z_far_depth(const sfmData::SfMData& sfmData, con
{
const sfmData::Landmark& landmark = itL->second;
const Vec3& X = landmark.X;
for (sfmData::Observations::const_iterator iterO = landmark.observations.begin(); iterO != landmark.observations.end(); ++iterO)
for (sfmData::Observations::const_iterator iterO = landmark.getObservations().begin(); iterO != landmark.getObservations().end(); ++iterO)
{
const IndexT id_view = iterO->first;
const sfmData::Observation& ob = iterO->second;
Expand Down
4 changes: 2 additions & 2 deletions src/aliceVision/sfm/LocalBundleAdjustmentGraph.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -413,7 +413,7 @@ void LocalBundleAdjustmentGraph::convertDistancesToStates(sfmData::SfMData& sfmD
for (auto& itLandmark : sfmData.getLandmarks())
{
const IndexT landmarkId = itLandmark.first;
const sfmData::Observations& observations = itLandmark.second.observations;
const sfmData::Observations& observations = itLandmark.second.getObservations();

assert(observations.size() >= 2);

Expand Down Expand Up @@ -475,7 +475,7 @@ std::vector<Pair> LocalBundleAdjustmentGraph::getNewEdges(const sfmData::SfMData
// retrieve the common track Ids
for (IndexT landmarkId : newViewLandmarks)
{
for (const auto& observations : sfmData.getLandmarks().at(landmarkId).observations)
for (const auto& observations : sfmData.getLandmarks().at(landmarkId).getObservations())
{
if (observations.first == viewId)
continue; // do not compare an observation with itself
Expand Down
Loading

0 comments on commit bc7c981

Please sign in to comment.