Skip to content

Commit da3593a

Browse files
CedricThebaultCedricThebault
committed
Add unit tests for the temporal constraint
1 parent 962821a commit da3593a

File tree

2 files changed

+217
-0
lines changed

2 files changed

+217
-0
lines changed

‎src/aliceVision/sfm/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -160,6 +160,12 @@ alicevision_add_test(bundle/bundleAdjustment_Enhanced_test.cpp
160160
${LEMON_LIBRARY}
161161
)
162162

163+
alicevision_add_test(bundle/bundleAdjustment_temporalConstraint_test.cpp
164+
NAME "sfm_bundleAdjustment_temporalConstraint"
165+
LINKS aliceVision_sfm
166+
aliceVision_track
167+
)
168+
163169
alicevision_add_test(utils/alignment_test.cpp
164170
NAME "sfm_alignment"
165171
LINKS

‎src/aliceVision/sfm/bundle/bundleAdjustment_temporalConstraint_test.cpp‎

Lines changed: 211 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,211 @@
1+
// This file is part of the AliceVision project.
2+
// Copyright (c) 2023 AliceVision contributors.
3+
// This Source Code Form is subject to the terms of the Mozilla Public License,
4+
// v. 2.0. If a copy of the MPL was not distributed with this file,
5+
// You can obtain one at https://mozilla.org/MPL/2.0/.
6+
7+
#define BOOST_TEST_MODULE bundleAdjustment_temporalConstraint
8+
9+
#include <aliceVision/sfmData/SfMData.hpp>
10+
#include <aliceVision/sfm/bundle/BundleAdjustmentCeres.hpp>
11+
#include <aliceVision/sfmDataIO/sceneSample.hpp>
12+
#include <aliceVision/sfm/utils/poseNoise.hpp>
13+
#include <aliceVision/track/tracksUtils.hpp>
14+
15+
#include <aliceVision/sfm/utils/statistics.hpp>
16+
#include <boost/test/unit_test.hpp>
17+
#include <boost/test/tools/floating_point_comparison.hpp>
18+
19+
using namespace aliceVision;
20+
21+
void computeTemporalSmoothness(const sfmData::SfMData& sfmData, double& diff0Mean, double& diff1Mean, double& diff2Mean,
22+
double& angleDiff0Mean, double& angleDiff1Mean, double& angleDiff2Mean)
23+
{
24+
using namespace Eigen;
25+
using namespace Eigen::indexing;
26+
27+
const int viewCount = sfmData.getViews().size();
28+
29+
MatrixXd viewCenters(3, viewCount);
30+
31+
int frameIdx = 0;
32+
for (auto& [_, pose] : sfmData.getPoses().valueRange())
33+
viewCenters.col(frameIdx++) = pose.getTransform().center();
34+
35+
Vector3d viewsCenter = viewCenters.rowwise().mean();
36+
37+
double radiusMean = (viewCenters.colwise() - viewsCenter).colwise().norm().mean();
38+
39+
diff0Mean = (viewCenters(all,seq(1, last)) - viewCenters(all,seq(0, last-1))).array().abs().mean();
40+
diff0Mean = diff0Mean / radiusMean;
41+
42+
diff1Mean = (2. * viewCenters(all,seq(1, last-1))
43+
- viewCenters(all,seq(0, last-2))
44+
- viewCenters(all,seq(2, last))).array().abs().mean();
45+
diff1Mean = diff1Mean / radiusMean;
46+
47+
diff2Mean = (3. * viewCenters(all,seq(1, last-2))
48+
- 3. * viewCenters(all,seq(2, last-1))
49+
+ viewCenters(all,seq(3, last))
50+
- viewCenters(all,seq(0, last-3))).array().abs().mean();
51+
diff2Mean = diff2Mean / radiusMean;
52+
53+
MatrixXd angleDiff(3, viewCount-1);
54+
MatrixXd quaterDiff(4, viewCount-1);
55+
56+
frameIdx = -1;
57+
Quaterniond prevRot, currRot;
58+
for (auto& [_, pose] : sfmData.getPoses().valueRange())
59+
{
60+
prevRot = currRot;
61+
currRot = Quaterniond(pose.getTransform().rotation());
62+
if (frameIdx == -1)
63+
{
64+
frameIdx++;
65+
continue;
66+
}
67+
Quaterniond quatDiff = currRot * prevRot.conjugate();
68+
quaterDiff.col(frameIdx) = quatDiff.coeffs();
69+
AngleAxisd aa(quatDiff);
70+
angleDiff.col(frameIdx++) = aa.angle() * aa.axis();
71+
}
72+
73+
angleDiff0Mean = std::sqrt(angleDiff.array().pow(2).mean());
74+
75+
MatrixXd angleDiff1(3, viewCount-2);
76+
MatrixXd quaterDiff1(4, viewCount-2);
77+
for (frameIdx = 0; frameIdx < viewCount-2; frameIdx++)
78+
{
79+
Quaterniond quatDiff = Quaterniond(quaterDiff.col(frameIdx+1).data()) * Quaterniond(quaterDiff.col(frameIdx).data()).conjugate();
80+
quaterDiff1.col(frameIdx) = quatDiff.coeffs();
81+
AngleAxisd aa(quatDiff);
82+
angleDiff1.col(frameIdx) = aa.angle() * aa.axis();
83+
}
84+
85+
angleDiff1Mean = std::sqrt(angleDiff1.array().pow(2).mean());
86+
87+
MatrixXd angleDiff2(3, viewCount-3);
88+
for (frameIdx = 0; frameIdx < viewCount-3; frameIdx++)
89+
{
90+
Quaterniond quatDiff = Quaterniond(quaterDiff1.col(frameIdx+1).data()) * Quaterniond(quaterDiff1.col(frameIdx).data()).conjugate();
91+
AngleAxisd aa(quatDiff);
92+
angleDiff2.col(frameIdx) = aa.angle() * aa.axis();
93+
}
94+
95+
angleDiff2Mean = std::sqrt(angleDiff2.array().pow(2).mean());
96+
}
97+
98+
BOOST_AUTO_TEST_CASE(BA_temporalConstraint)
99+
{
100+
sfmData::SfMData sfmData;
101+
sfmDataIO::generateSphereScene(sfmData, 100, 240);
102+
103+
track::TracksMap mapTracks;
104+
track::simulateTracks(sfmData, 20., 0., 0., false, mapTracks);
105+
for (auto& [landmarkId, landmark] : sfmData.getLandmarks())
106+
for (auto& [viewId, obs] : mapTracks[landmarkId].featPerView)
107+
landmark.getObservations().at(viewId).setCoordinates(obs.coords);
108+
109+
double diff0MeanClean, diff1MeanClean, diff2MeanClean;
110+
double angleDiff0MeanClean, angleDiff1MeanClean, angleDiff2MeanClean;
111+
computeTemporalSmoothness(sfmData, diff0MeanClean, diff1MeanClean, diff2MeanClean,
112+
angleDiff0MeanClean, angleDiff1MeanClean, angleDiff2MeanClean);
113+
114+
sfm::addPoseNoise(sfmData, 0.3, 0.05);
115+
116+
double diff0MeanNoise, diff1MeanNoise, diff2MeanNoise;
117+
double angleDiff0MeanNoise, angleDiff1MeanNoise, angleDiff2MeanNoise;
118+
computeTemporalSmoothness(sfmData, diff0MeanNoise, diff1MeanNoise, diff2MeanNoise,
119+
angleDiff0MeanNoise, angleDiff1MeanNoise, angleDiff2MeanNoise);
120+
121+
BOOST_CHECK_LT(diff0MeanClean, diff0MeanNoise);
122+
BOOST_CHECK_LT(diff1MeanClean, diff1MeanNoise);
123+
BOOST_CHECK_LT(diff2MeanClean, diff2MeanNoise);
124+
125+
BOOST_CHECK_LT(angleDiff0MeanClean, angleDiff0MeanNoise);
126+
BOOST_CHECK_LT(angleDiff1MeanClean, angleDiff1MeanNoise);
127+
BOOST_CHECK_LT(angleDiff2MeanClean, angleDiff2MeanNoise);
128+
129+
sfm::BundleAdjustmentCeres::CeresOptions options;
130+
sfm::BundleAdjustment::ERefineOptions refineOptions;
131+
refineOptions |= sfm::BundleAdjustment::REFINE_ROTATION;
132+
refineOptions |= sfm::BundleAdjustment::REFINE_STRUCTURE;
133+
refineOptions |= sfm::BundleAdjustment::REFINE_TRANSLATION;
134+
refineOptions |= sfm::BundleAdjustment::REFINE_INTRINSICS_ALL;
135+
options.summary = false;
136+
137+
// Bundle adjustment without temporal constraint
138+
double rmseBeforeBA = sfm::RMSE(sfmData);
139+
sfm::BundleAdjustmentCeres BA(options);
140+
BA.adjust(sfmData, refineOptions);
141+
double rmseAfterBA_noTC = sfm::RMSE(sfmData);
142+
BOOST_CHECK_LT(rmseAfterBA_noTC, .5 * rmseBeforeBA);
143+
144+
double diff0MeanNoTC, diff1MeanNoTC, diff2MeanNoTC;
145+
double angleDiff0MeanNoTC, angleDiff1MeanNoTC, angleDiff2MeanNoTC;
146+
computeTemporalSmoothness(sfmData, diff0MeanNoTC, diff1MeanNoTC, diff2MeanNoTC,
147+
angleDiff0MeanNoTC, angleDiff1MeanNoTC, angleDiff2MeanNoTC);
148+
149+
BOOST_CHECK_LT(angleDiff0MeanNoTC, angleDiff0MeanNoise);
150+
BOOST_CHECK_LT(angleDiff1MeanNoTC, angleDiff1MeanNoise);
151+
BOOST_CHECK_LT(angleDiff2MeanNoTC, angleDiff2MeanNoise);
152+
153+
sfmData::SfMData sfmData_noTC = sfmData::SfMData(sfmData);
154+
155+
// Add the temporal constraint
156+
refineOptions |= sfm::BundleAdjustment::REFINE_TEMPORAL_SMOOTHNESS_CONSTRAINT;
157+
options.temporalConstraintParams.positionWeight = 10.;
158+
options.temporalConstraintParams.c0positionWeight = 1.0;
159+
options.temporalConstraintParams.orientationWeight = 0.;
160+
BA = sfm::BundleAdjustmentCeres(options);
161+
162+
// Bundle adjustment with temporal constraint on positions
163+
BA.adjust(sfmData, refineOptions);
164+
double rmseAfterBA_TCP = sfm::RMSE(sfmData);
165+
BOOST_CHECK_LT(rmseAfterBA_TCP, 1.2 * rmseAfterBA_noTC);
166+
167+
double diff0MeanTCP, diff1MeanTCP, diff2MeanTCP;
168+
double angleDiff0MeanTCP, angleDiff1MeanTCP, angleDiff2MeanTCP;
169+
computeTemporalSmoothness(sfmData, diff0MeanTCP, diff1MeanTCP, diff2MeanTCP,
170+
angleDiff0MeanTCP, angleDiff1MeanTCP, angleDiff2MeanTCP);
171+
172+
BOOST_CHECK_LT(diff0MeanTCP, 2. * diff0MeanClean);
173+
BOOST_CHECK_LT(angleDiff0MeanTCP, 2. * angleDiff0MeanClean);
174+
175+
BOOST_CHECK_LT(diff1MeanTCP, .1 * diff1MeanNoTC);
176+
BOOST_CHECK_LT(diff2MeanTCP, .1 * diff2MeanNoTC);
177+
178+
BOOST_CHECK_LT(angleDiff1MeanTCP, .9 * angleDiff1MeanNoTC);
179+
BOOST_CHECK_LT(angleDiff2MeanTCP, .9 * angleDiff2MeanNoTC);
180+
181+
options.temporalConstraintParams.positionWeight = 0.;
182+
options.temporalConstraintParams.orientationWeight = 10.;
183+
BA = sfm::BundleAdjustmentCeres(options);
184+
185+
sfmData = sfmData::SfMData(sfmData_noTC);
186+
187+
// Bundle adjustment with temporal constraint on orientations
188+
BA.adjust(sfmData, refineOptions);
189+
double rmseAfterBA_TCO = sfm::RMSE(sfmData);
190+
BOOST_CHECK_LT(rmseAfterBA_TCO, 1.2 * rmseAfterBA_noTC);
191+
192+
double diff0MeanTCO, diff1MeanTCO, diff2MeanTCO;
193+
double angleDiff0MeanTCO, angleDiff1MeanTCO, angleDiff2MeanTCO;
194+
computeTemporalSmoothness(sfmData, diff0MeanTCO, diff1MeanTCO, diff2MeanTCO,
195+
angleDiff0MeanTCO, angleDiff1MeanTCO, angleDiff2MeanTCO);
196+
197+
BOOST_CHECK_LT(diff0MeanTCO, 2. * diff0MeanClean);
198+
BOOST_CHECK_LT(angleDiff0MeanTCO, 1.5 * angleDiff0MeanClean);
199+
200+
BOOST_CHECK_LT(diff1MeanTCO, .75 * diff1MeanNoTC);
201+
BOOST_CHECK_LT(diff2MeanTCO, .75 * diff2MeanNoTC);
202+
203+
BOOST_CHECK_LT(angleDiff1MeanTCO, .9 * angleDiff1MeanNoTC);
204+
BOOST_CHECK_LT(angleDiff2MeanTCO, .9 * angleDiff2MeanNoTC);
205+
206+
BOOST_CHECK_LT(diff1MeanTCP, .5 * diff1MeanTCO);
207+
BOOST_CHECK_LT(diff2MeanTCP, .5 * diff2MeanTCO);
208+
209+
BOOST_CHECK_LT(angleDiff1MeanTCO, .9 * angleDiff1MeanTCP);
210+
BOOST_CHECK_LT(angleDiff2MeanTCO, .9 * angleDiff2MeanTCP);
211+
}

0 commit comments

Comments
 (0)