Deprecated incorrectly named methods
parent
0fd49efaba
commit
3d51989f2e
|
@ -49,7 +49,7 @@ int main(int argc, char* argv[]) {
|
||||||
SfmData mydata;
|
SfmData mydata;
|
||||||
readBAL(filename, mydata);
|
readBAL(filename, mydata);
|
||||||
cout << boost::format("read %1% tracks on %2% cameras\n") %
|
cout << boost::format("read %1% tracks on %2% cameras\n") %
|
||||||
mydata.number_tracks() % mydata.number_cameras();
|
mydata.nrTracks() % mydata.nrCameras();
|
||||||
|
|
||||||
// Create a factor graph
|
// Create a factor graph
|
||||||
ExpressionFactorGraph graph;
|
ExpressionFactorGraph graph;
|
||||||
|
|
|
@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
|
||||||
// Load the SfM data from file
|
// Load the SfM data from file
|
||||||
SfmData mydata;
|
SfmData mydata;
|
||||||
readBAL(filename, mydata);
|
readBAL(filename, mydata);
|
||||||
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
|
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.nrTracks() % mydata.nrCameras();
|
||||||
|
|
||||||
// Create a factor graph
|
// Create a factor graph
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
|
|
|
@ -48,7 +48,7 @@ int main(int argc, char* argv[]) {
|
||||||
SfmData mydata;
|
SfmData mydata;
|
||||||
readBAL(filename, mydata);
|
readBAL(filename, mydata);
|
||||||
cout << boost::format("read %1% tracks on %2% cameras\n") %
|
cout << boost::format("read %1% tracks on %2% cameras\n") %
|
||||||
mydata.number_tracks() % mydata.number_cameras();
|
mydata.nrTracks() % mydata.nrCameras();
|
||||||
|
|
||||||
// Create a factor graph
|
// Create a factor graph
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
|
@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
cout << "Time comparison by solving " << filename << " results:" << endl;
|
cout << "Time comparison by solving " << filename << " results:" << endl;
|
||||||
cout << boost::format("%1% point tracks and %2% cameras\n") %
|
cout << boost::format("%1% point tracks and %2% cameras\n") %
|
||||||
mydata.number_tracks() % mydata.number_cameras()
|
mydata.nrTracks() % mydata.nrCameras()
|
||||||
<< endl;
|
<< endl;
|
||||||
|
|
||||||
tictoc_print_();
|
tictoc_print_();
|
||||||
|
|
|
@ -146,7 +146,7 @@ protected:
|
||||||
*/
|
*/
|
||||||
template<class SFM_TRACK>
|
template<class SFM_TRACK>
|
||||||
void add(const SFM_TRACK& trackToAdd) {
|
void add(const SFM_TRACK& trackToAdd) {
|
||||||
for (size_t k = 0; k < trackToAdd.number_measurements(); k++) {
|
for (size_t k = 0; k < trackToAdd.nrMeasurements(); k++) {
|
||||||
this->measured_.push_back(trackToAdd.measurements[k].second);
|
this->measured_.push_back(trackToAdd.measurements[k].second);
|
||||||
this->keys_.push_back(trackToAdd.measurements[k].first);
|
this->keys_.push_back(trackToAdd.measurements[k].first);
|
||||||
}
|
}
|
||||||
|
|
|
@ -220,10 +220,10 @@ class SfmTrack {
|
||||||
|
|
||||||
std::vector<pair<size_t, gtsam::Point2>> measurements;
|
std::vector<pair<size_t, gtsam::Point2>> measurements;
|
||||||
|
|
||||||
size_t number_measurements() const;
|
size_t nrMeasurements() const;
|
||||||
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
|
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
|
||||||
pair<size_t, size_t> siftIndex(size_t idx) const;
|
pair<size_t, size_t> siftIndex(size_t idx) const;
|
||||||
void add_measurement(size_t idx, const gtsam::Point2& m);
|
void addMeasurement(size_t idx, const gtsam::Point2& m);
|
||||||
|
|
||||||
// enabling serialization functionality
|
// enabling serialization functionality
|
||||||
void serialize() const;
|
void serialize() const;
|
||||||
|
@ -234,12 +234,12 @@ class SfmTrack {
|
||||||
|
|
||||||
class SfmData {
|
class SfmData {
|
||||||
SfmData();
|
SfmData();
|
||||||
size_t number_cameras() const;
|
size_t nrCameras() const;
|
||||||
size_t number_tracks() const;
|
size_t nrTracks() const;
|
||||||
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
|
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
|
||||||
gtsam::SfmTrack track(size_t idx) const;
|
gtsam::SfmTrack track(size_t idx) const;
|
||||||
void add_track(const gtsam::SfmTrack& t);
|
void addTrack(const gtsam::SfmTrack& t);
|
||||||
void add_camera(const gtsam::SfmCamera& cam);
|
void addCamera(const gtsam::SfmCamera& cam);
|
||||||
|
|
||||||
// enabling serialization functionality
|
// enabling serialization functionality
|
||||||
void serialize() const;
|
void serialize() const;
|
||||||
|
|
|
@ -160,10 +160,10 @@ TEST( dataSet, Balbianello)
|
||||||
CHECK(readBundler(filename, mydata));
|
CHECK(readBundler(filename, mydata));
|
||||||
|
|
||||||
// Check number of things
|
// Check number of things
|
||||||
EXPECT_LONGS_EQUAL(5,mydata.number_cameras());
|
EXPECT_LONGS_EQUAL(5,mydata.nrCameras());
|
||||||
EXPECT_LONGS_EQUAL(544,mydata.number_tracks());
|
EXPECT_LONGS_EQUAL(544,mydata.nrTracks());
|
||||||
const SfmTrack& track0 = mydata.tracks[0];
|
const SfmTrack& track0 = mydata.tracks[0];
|
||||||
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
|
EXPECT_LONGS_EQUAL(3,track0.nrMeasurements());
|
||||||
|
|
||||||
// Check projection of a given point
|
// Check projection of a given point
|
||||||
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
||||||
|
@ -470,10 +470,10 @@ TEST( dataSet, readBAL_Dubrovnik)
|
||||||
CHECK(readBAL(filename, mydata));
|
CHECK(readBAL(filename, mydata));
|
||||||
|
|
||||||
// Check number of things
|
// Check number of things
|
||||||
EXPECT_LONGS_EQUAL(3,mydata.number_cameras());
|
EXPECT_LONGS_EQUAL(3,mydata.nrCameras());
|
||||||
EXPECT_LONGS_EQUAL(7,mydata.number_tracks());
|
EXPECT_LONGS_EQUAL(7,mydata.nrTracks());
|
||||||
const SfmTrack& track0 = mydata.tracks[0];
|
const SfmTrack& track0 = mydata.tracks[0];
|
||||||
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
|
EXPECT_LONGS_EQUAL(3,track0.nrMeasurements());
|
||||||
|
|
||||||
// Check projection of a given point
|
// Check projection of a given point
|
||||||
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
||||||
|
@ -533,16 +533,16 @@ TEST( dataSet, writeBAL_Dubrovnik)
|
||||||
CHECK(readBAL(filenameToWrite, writtenData));
|
CHECK(readBAL(filenameToWrite, writtenData));
|
||||||
|
|
||||||
// Check that what we read is the same as what we wrote
|
// Check that what we read is the same as what we wrote
|
||||||
EXPECT_LONGS_EQUAL(readData.number_cameras(),writtenData.number_cameras());
|
EXPECT_LONGS_EQUAL(readData.nrCameras(),writtenData.nrCameras());
|
||||||
EXPECT_LONGS_EQUAL(readData.number_tracks(),writtenData.number_tracks());
|
EXPECT_LONGS_EQUAL(readData.nrTracks(),writtenData.nrTracks());
|
||||||
|
|
||||||
for (size_t i = 0; i < readData.number_cameras(); i++){
|
for (size_t i = 0; i < readData.nrCameras(); i++){
|
||||||
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
|
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
|
||||||
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
|
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
|
||||||
EXPECT(assert_equal(expectedCamera,actualCamera));
|
EXPECT(assert_equal(expectedCamera,actualCamera));
|
||||||
}
|
}
|
||||||
|
|
||||||
for (size_t j = 0; j < readData.number_tracks(); j++){
|
for (size_t j = 0; j < readData.nrTracks(); j++){
|
||||||
// check point
|
// check point
|
||||||
SfmTrack expectedTrack = writtenData.tracks[j];
|
SfmTrack expectedTrack = writtenData.tracks[j];
|
||||||
SfmTrack actualTrack = readData.tracks[j];
|
SfmTrack actualTrack = readData.tracks[j];
|
||||||
|
@ -556,7 +556,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
|
||||||
EXPECT(assert_equal(expectedRGB,actualRGB));
|
EXPECT(assert_equal(expectedRGB,actualRGB));
|
||||||
|
|
||||||
// check measurements
|
// check measurements
|
||||||
for (size_t k = 0; k < actualTrack.number_measurements(); k++){
|
for (size_t k = 0; k < actualTrack.nrMeasurements(); k++){
|
||||||
EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first);
|
EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,actualTrack.measurements[k].first);
|
||||||
EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
|
EXPECT(assert_equal(expectedTrack.measurements[k].second,actualTrack.measurements[k].second));
|
||||||
}
|
}
|
||||||
|
@ -575,11 +575,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
|
||||||
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
|
Pose3 poseChange = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.3,0.1,0.3));
|
||||||
|
|
||||||
Values value;
|
Values value;
|
||||||
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
|
for(size_t i=0; i < readData.nrCameras(); i++){ // for each camera
|
||||||
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
|
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
|
||||||
value.insert(X(i), pose);
|
value.insert(X(i), pose);
|
||||||
}
|
}
|
||||||
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
|
for(size_t j=0; j < readData.nrTracks(); j++){ // for each point
|
||||||
Point3 point = poseChange.transformFrom( readData.tracks[j].p );
|
Point3 point = poseChange.transformFrom( readData.tracks[j].p );
|
||||||
value.insert(P(j), point);
|
value.insert(P(j), point);
|
||||||
}
|
}
|
||||||
|
@ -594,10 +594,10 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
|
||||||
|
|
||||||
// Check that the reprojection errors are the same and the poses are correct
|
// Check that the reprojection errors are the same and the poses are correct
|
||||||
// Check number of things
|
// Check number of things
|
||||||
EXPECT_LONGS_EQUAL(3,writtenData.number_cameras());
|
EXPECT_LONGS_EQUAL(3,writtenData.nrCameras());
|
||||||
EXPECT_LONGS_EQUAL(7,writtenData.number_tracks());
|
EXPECT_LONGS_EQUAL(7,writtenData.nrTracks());
|
||||||
const SfmTrack& track0 = writtenData.tracks[0];
|
const SfmTrack& track0 = writtenData.tracks[0];
|
||||||
EXPECT_LONGS_EQUAL(3,track0.number_measurements());
|
EXPECT_LONGS_EQUAL(3,track0.nrMeasurements());
|
||||||
|
|
||||||
// Check projection of a given point
|
// Check projection of a given point
|
||||||
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
EXPECT_LONGS_EQUAL(0,track0.measurements[0].first);
|
||||||
|
|
|
@ -632,14 +632,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
|
||||||
// We start with a factor graph and add constraints to it
|
// We start with a factor graph and add constraints to it
|
||||||
// Noise sigma is 1, assuming pixel measurements
|
// Noise sigma is 1, assuming pixel measurements
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t i = 0; i < data.number_tracks(); i++)
|
for (size_t i = 0; i < data.nrTracks(); i++)
|
||||||
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
|
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
|
||||||
K);
|
K);
|
||||||
|
|
||||||
// Check error at ground truth
|
// Check error at ground truth
|
||||||
Values truth;
|
Values truth;
|
||||||
truth.insert(100, trueE);
|
truth.insert(100, trueE);
|
||||||
for (size_t i = 0; i < data.number_tracks(); i++) {
|
for (size_t i = 0; i < data.nrTracks(); i++) {
|
||||||
Point3 P1 = data.tracks[i].p;
|
Point3 P1 = data.tracks[i].p;
|
||||||
truth.insert(i, double(baseline / P1.z()));
|
truth.insert(i, double(baseline / P1.z()));
|
||||||
}
|
}
|
||||||
|
@ -654,7 +654,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
|
||||||
// Check result
|
// Check result
|
||||||
EssentialMatrix actual = result.at<EssentialMatrix>(100);
|
EssentialMatrix actual = result.at<EssentialMatrix>(100);
|
||||||
EXPECT(assert_equal(trueE, actual, 1e-1));
|
EXPECT(assert_equal(trueE, actual, 1e-1));
|
||||||
for (size_t i = 0; i < data.number_tracks(); i++)
|
for (size_t i = 0; i < data.nrTracks(); i++)
|
||||||
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
|
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
|
||||||
|
|
||||||
// Check error at result
|
// Check error at result
|
||||||
|
|
|
@ -29,10 +29,10 @@ DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
|
||||||
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
|
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
|
||||||
"""Plot the SFM results."""
|
"""Plot the SFM results."""
|
||||||
plot_vals = gtsam.Values()
|
plot_vals = gtsam.Values()
|
||||||
for cam_idx in range(scene_data.number_cameras()):
|
for cam_idx in range(scene_data.nrCameras()):
|
||||||
plot_vals.insert(C(cam_idx),
|
plot_vals.insert(C(cam_idx),
|
||||||
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
|
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
|
||||||
for j in range(scene_data.number_tracks()):
|
for j in range(scene_data.nrTracks()):
|
||||||
plot_vals.insert(P(j), result.atPoint3(P(j)))
|
plot_vals.insert(P(j), result.atPoint3(P(j)))
|
||||||
|
|
||||||
plot.plot_3d_points(0, plot_vals, linespec="g.")
|
plot.plot_3d_points(0, plot_vals, linespec="g.")
|
||||||
|
@ -47,8 +47,8 @@ def run(args: argparse.Namespace) -> None:
|
||||||
|
|
||||||
# Load the SfM data from file
|
# Load the SfM data from file
|
||||||
scene_data = gtsam.readBal(input_file)
|
scene_data = gtsam.readBal(input_file)
|
||||||
logging.info("read %d tracks on %d cameras\n", scene_data.number_tracks(),
|
logging.info("read %d tracks on %d cameras\n", scene_data.nrTracks(),
|
||||||
scene_data.number_cameras())
|
scene_data.nrCameras())
|
||||||
|
|
||||||
# Create a factor graph
|
# Create a factor graph
|
||||||
graph = gtsam.NonlinearFactorGraph()
|
graph = gtsam.NonlinearFactorGraph()
|
||||||
|
@ -57,10 +57,10 @@ def run(args: argparse.Namespace) -> None:
|
||||||
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
|
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
|
||||||
|
|
||||||
# Add measurements to the factor graph
|
# Add measurements to the factor graph
|
||||||
for j in range(scene_data.number_tracks()):
|
for j in range(scene_data.nrTracks()):
|
||||||
track = scene_data.track(j) # SfmTrack
|
track = scene_data.track(j) # SfmTrack
|
||||||
# retrieve the SfmMeasurement objects
|
# retrieve the SfmMeasurement objects
|
||||||
for m_idx in range(track.number_measurements()):
|
for m_idx in range(track.nrMeasurements()):
|
||||||
# i represents the camera index, and uv is the 2d measurement
|
# i represents the camera index, and uv is the 2d measurement
|
||||||
i, uv = track.measurement(m_idx)
|
i, uv = track.measurement(m_idx)
|
||||||
# note use of shorthand symbols C and P
|
# note use of shorthand symbols C and P
|
||||||
|
@ -82,13 +82,13 @@ def run(args: argparse.Namespace) -> None:
|
||||||
|
|
||||||
i = 0
|
i = 0
|
||||||
# add each PinholeCameraCal3Bundler
|
# add each PinholeCameraCal3Bundler
|
||||||
for cam_idx in range(scene_data.number_cameras()):
|
for cam_idx in range(scene_data.nrCameras()):
|
||||||
camera = scene_data.camera(cam_idx)
|
camera = scene_data.camera(cam_idx)
|
||||||
initial.insert(C(i), camera)
|
initial.insert(C(i), camera)
|
||||||
i += 1
|
i += 1
|
||||||
|
|
||||||
# add each SfmTrack
|
# add each SfmTrack
|
||||||
for j in range(scene_data.number_tracks()):
|
for j in range(scene_data.nrTracks()):
|
||||||
track = scene_data.track(j)
|
track = scene_data.track(j)
|
||||||
initial.insert(P(j), track.point3())
|
initial.insert(P(j), track.point3())
|
||||||
|
|
||||||
|
|
|
@ -39,10 +39,10 @@ class TestSfmData(GtsamTestCase):
|
||||||
# translating point uv_i1 along X-axis
|
# translating point uv_i1 along X-axis
|
||||||
uv_i2 = gtsam.Point2(24.88, 82)
|
uv_i2 = gtsam.Point2(24.88, 82)
|
||||||
# add measurements to the track
|
# add measurements to the track
|
||||||
self.tracks.add_measurement(i1, uv_i1)
|
self.tracks.addMeasurement(i1, uv_i1)
|
||||||
self.tracks.add_measurement(i2, uv_i2)
|
self.tracks.addMeasurement(i2, uv_i2)
|
||||||
# Number of measurements in the track is 2
|
# Number of measurements in the track is 2
|
||||||
self.assertEqual(self.tracks.number_measurements(), 2)
|
self.assertEqual(self.tracks.nrMeasurements(), 2)
|
||||||
# camera_idx in the first measurement of the track corresponds to i1
|
# camera_idx in the first measurement of the track corresponds to i1
|
||||||
cam_idx, img_measurement = self.tracks.measurement(0)
|
cam_idx, img_measurement = self.tracks.measurement(0)
|
||||||
self.assertEqual(cam_idx, i1)
|
self.assertEqual(cam_idx, i1)
|
||||||
|
@ -64,13 +64,13 @@ class TestSfmData(GtsamTestCase):
|
||||||
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
|
measurements = [(i1, uv_i1), (i2, uv_i2), (i3, uv_i3)]
|
||||||
pt = gtsam.Point3(1.0, 6.0, 2.0)
|
pt = gtsam.Point3(1.0, 6.0, 2.0)
|
||||||
track2 = gtsam.SfmTrack(pt)
|
track2 = gtsam.SfmTrack(pt)
|
||||||
track2.add_measurement(i1, uv_i1)
|
track2.addMeasurement(i1, uv_i1)
|
||||||
track2.add_measurement(i2, uv_i2)
|
track2.addMeasurement(i2, uv_i2)
|
||||||
track2.add_measurement(i3, uv_i3)
|
track2.addMeasurement(i3, uv_i3)
|
||||||
self.data.add_track(self.tracks)
|
self.data.addTrack(self.tracks)
|
||||||
self.data.add_track(track2)
|
self.data.addTrack(track2)
|
||||||
# Number of tracks in SfmData is 2
|
# Number of tracks in SfmData is 2
|
||||||
self.assertEqual(self.data.number_tracks(), 2)
|
self.assertEqual(self.data.nrTracks(), 2)
|
||||||
# camera idx of first measurement of second track corresponds to i1
|
# camera idx of first measurement of second track corresponds to i1
|
||||||
cam_idx, img_measurement = self.data.track(1).measurement(0)
|
cam_idx, img_measurement = self.data.track(1).measurement(0)
|
||||||
self.assertEqual(cam_idx, i1)
|
self.assertEqual(cam_idx, i1)
|
||||||
|
|
|
@ -37,8 +37,8 @@ class TestPickle(GtsamTestCase):
|
||||||
|
|
||||||
def test_sfmTrack_roundtrip(self):
|
def test_sfmTrack_roundtrip(self):
|
||||||
obj = SfmTrack(Point3(1, 1, 0))
|
obj = SfmTrack(Point3(1, 1, 0))
|
||||||
obj.add_measurement(0, Point2(-1, 5))
|
obj.addMeasurement(0, Point2(-1, 5))
|
||||||
obj.add_measurement(1, Point2(6, 2))
|
obj.addMeasurement(1, Point2(6, 2))
|
||||||
self.assertEqualityOnPickleRoundtrip(obj)
|
self.assertEqualityOnPickleRoundtrip(obj)
|
||||||
|
|
||||||
def test_unit3_roundtrip(self):
|
def test_unit3_roundtrip(self):
|
||||||
|
|
|
@ -49,7 +49,7 @@ TEST(PinholeCamera, BAL) {
|
||||||
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
|
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
|
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
for (const SfmMeasurement& m: db.tracks[j].measurements)
|
for (const SfmMeasurement& m: db.tracks[j].measurements)
|
||||||
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
|
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
|
||||||
}
|
}
|
||||||
|
|
|
@ -36,7 +36,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
// Build graph using conventional GeneralSFMFactor
|
// Build graph using conventional GeneralSFMFactor
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
||||||
size_t i = m.first;
|
size_t i = m.first;
|
||||||
Point2 z = m.second;
|
Point2 z = m.second;
|
||||||
|
|
|
@ -73,8 +73,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
|
||||||
if (gUseSchur) {
|
if (gUseSchur) {
|
||||||
// Create Schur-complement ordering
|
// Create Schur-complement ordering
|
||||||
Ordering ordering;
|
Ordering ordering;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
|
for (size_t j = 0; j < db.nrTracks(); j++) ordering.push_back(P(j));
|
||||||
for (size_t i = 0; i < db.number_cameras(); i++) {
|
for (size_t i = 0; i < db.nrCameras(); i++) {
|
||||||
ordering.push_back(C(i));
|
ordering.push_back(C(i));
|
||||||
if (separateCalibration) ordering.push_back(K(i));
|
if (separateCalibration) ordering.push_back(K(i));
|
||||||
}
|
}
|
||||||
|
|
|
@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
// Build graph
|
// Build graph
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
||||||
size_t i = m.first;
|
size_t i = m.first;
|
||||||
Point2 z = m.second;
|
Point2 z = m.second;
|
||||||
|
|
|
@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
// Build graph using conventional GeneralSFMFactor
|
// Build graph using conventional GeneralSFMFactor
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
||||||
size_t i = m.first;
|
size_t i = m.first;
|
||||||
Point2 z = m.second;
|
Point2 z = m.second;
|
||||||
|
|
|
@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
// Build graph using conventional GeneralSFMFactor
|
// Build graph using conventional GeneralSFMFactor
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
Point3_ nav_point_(P(j));
|
Point3_ nav_point_(P(j));
|
||||||
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
for (const SfmMeasurement& m: db.tracks[j].measurements) {
|
||||||
size_t i = m.first;
|
size_t i = m.first;
|
||||||
|
|
|
@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
|
||||||
|
|
||||||
// Add smart factors to graph
|
// Add smart factors to graph
|
||||||
NonlinearFactorGraph graph;
|
NonlinearFactorGraph graph;
|
||||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
for (size_t j = 0; j < db.nrTracks(); j++) {
|
||||||
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
|
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
|
||||||
for (const SfmMeasurement& m : db.tracks[j].measurements) {
|
for (const SfmMeasurement& m : db.tracks[j].measurements) {
|
||||||
size_t i = m.first;
|
size_t i = m.first;
|
||||||
|
|
Loading…
Reference in New Issue