12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

nr -> number

release/4.3a0
Frank Dellaert 2022-01-31 12:46:42 -05:00
parent a4fc68f759
commit 762e8097bb
21 changed files with 75 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SFMExampleExpressions_bal.cpp
View File

@ -49,7 +49,7 @@ int main(int argc, char* argv[]) {
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.nrTracks() % mydata.nrCameras();
mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
ExpressionFactorGraph graph;

2
examples/SFMExample_bal.cpp
View File

@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.nrTracks() % mydata.nrCameras();
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
NonlinearFactorGraph graph;

4
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -48,7 +48,7 @@ int main(int argc, char* argv[]) {
SfmData mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") %
mydata.nrTracks() % mydata.nrCameras();
mydata.numberTracks() % mydata.numberCameras();
// Create a factor graph
NonlinearFactorGraph graph;
@ -131,7 +131,7 @@ int main(int argc, char* argv[]) {
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") %
mydata.nrTracks() % mydata.nrCameras()
mydata.numberTracks() % mydata.numberCameras()
<< endl;
tictoc_print_();

43
gtsam/sfm/SfmData.cpp
View File

@ -32,26 +32,27 @@ using gtsam::symbol_shorthand::X;
/* ************************************************************************** */
void SfmData::print(const std::string &s) const {
std::cout << "Number of cameras = " << nrCameras() << std::endl;
std::cout << "Number of tracks = " << nrTracks() << std::endl;
std::cout << "Number of cameras = " << cameras.size() << std::endl;
std::cout << "Number of tracks = " << tracks.size() << std::endl;
}
/* ************************************************************************** */
bool SfmData::equals(const SfmData &sfmData, double tol) const {
// check number of cameras and tracks
if (nrCameras() != sfmData.nrCameras() || nrTracks() != sfmData.nrTracks()) {
if (cameras.size() != sfmData.cameras.size() ||
tracks.size() != sfmData.tracks.size()) {
return false;
}
// check each camera
for (size_t i = 0; i < nrCameras(); ++i) {
for (size_t i = 0; i < cameras.size(); ++i) {
if (!camera(i).equals(sfmData.camera(i), tol)) {
return false;
}
}
// check each track
for (size_t j = 0; j < nrTracks(); ++j) {
for (size_t j = 0; j < tracks.size(); ++j) {
if (!track(j).equals(sfmData.track(j), tol)) {
return false;
}
@ -264,19 +265,19 @@ bool writeBAL(const std::string &filename, SfmData &data) {
// Write the number of camera poses and 3D points
size_t nrObservations = 0;
for (size_t j = 0; j < data.nrTracks(); j++) {
nrObservations += data.tracks[j].nrMeasurements();
for (size_t j = 0; j < data.tracks.size(); j++) {
nrObservations += data.tracks[j].numberMeasurements();
}
// Write observations
os << data.nrCameras() << " " << data.nrTracks() << " " << nrObservations
<< endl;
os << data.cameras.size() << " " << data.tracks.size() << " "
<< nrObservations << endl;
os << endl;
for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j
for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
const SfmTrack &track = data.tracks[j];
for (size_t k = 0; k < track.nrMeasurements();
for (size_t k = 0; k < track.numberMeasurements();
k++) { // for each observation of the 3D point j
size_t i = track.measurements[k].first; // camera id
double u0 = data.cameras[i].calibration().px();
@ -301,7 +302,7 @@ bool writeBAL(const std::string &filename, SfmData &data) {
os << endl;
// Write cameras
for (size_t i = 0; i < data.nrCameras(); i++) { // for each camera
for (size_t i = 0; i < data.cameras.size(); i++) { // for each camera
Pose3 poseGTSAM = data.cameras[i].pose();
Cal3Bundler cameraCalibration = data.cameras[i].calibration();
Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
@ -316,7 +317,7 @@ bool writeBAL(const std::string &filename, SfmData &data) {
}
// Write the points
for (size_t j = 0; j < data.nrTracks(); j++) { // for each 3D point j
for (size_t j = 0; j < data.tracks.size(); j++) { // for each 3D point j
Point3 point = data.tracks[j].p;
os << point.x() << endl;
os << point.y() << endl;
@ -336,8 +337,8 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
// Store poses or cameras in SfmData
size_t nrPoses = values.count<Pose3>();
if (nrPoses == dataValues.nrCameras()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.nrCameras(); i++) { // for each camera
if (nrPoses == dataValues.cameras.size()) { // we only estimated camera poses
for (size_t i = 0; i < dataValues.cameras.size(); i++) { // for each camera
Pose3 pose = values.at<Pose3>(X(i));
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
@ -345,24 +346,24 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
}
} else {
size_t nrCameras = values.count<Camera>();
if (nrCameras == dataValues.nrCameras()) { // we only estimated camera
// poses and calibration
for (size_t i = 0; i < nrCameras; i++) { // for each camera
Key cameraKey = i; // symbol('c',i);
if (nrCameras == dataValues.cameras.size()) { // we only estimated camera
// poses and calibration
for (size_t i = 0; i < nrCameras; i++) { // for each camera
Key cameraKey = i; // symbol('c',i);
Camera camera = values.at<Camera>(cameraKey);
dataValues.cameras[i] = camera;
}
} else {
cout << "writeBALfromValues: different number of cameras in "
"SfM_dataValues (#cameras "
<< dataValues.nrCameras() << ") and values (#cameras " << nrPoses
<< dataValues.cameras.size() << ") and values (#cameras " << nrPoses
<< ", #poses " << nrCameras << ")!!" << endl;
return false;
}
}
// Store 3D points in SfmData
size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.nrTracks();
size_t nrPoints = values.count<Point3>(), nrTracks = dataValues.tracks.size();
if (nrPoints != nrTracks) {
cout << "writeBALfromValues: different number of points in "
"SfM_dataValues (#points= "

4
gtsam/sfm/SfmData.h
View File

@ -50,10 +50,10 @@ struct SfmData {
void addCamera(const SfmCamera& cam) { cameras.push_back(cam); }
/// The number of reconstructed 3D points
size_t nrTracks() const { return tracks.size(); }
size_t numberTracks() const { return tracks.size(); }
/// The number of cameras
size_t nrCameras() const { return cameras.size(); }
size_t numberCameras() const { return cameras.size(); }
/// The track formed by series of landmark measurements
SfmTrack track(size_t idx) const { return tracks[idx]; }

4
gtsam/sfm/SfmTrack.cpp
View File

@ -39,13 +39,13 @@ bool SfmTrack::equals(const SfmTrack& sfmTrack, double tol) const {
}
// compare size of vectors for measurements and siftIndices
if (nrMeasurements() != sfmTrack.nrMeasurements() ||
if (numberMeasurements() != sfmTrack.numberMeasurements() ||
siftIndices.size() != sfmTrack.siftIndices.size()) {
return false;
}
// compare measurements (order sensitive)
for (size_t idx = 0; idx < nrMeasurements(); ++idx) {
for (size_t idx = 0; idx < numberMeasurements(); ++idx) {
SfmMeasurement measurement = measurements[idx];
SfmMeasurement otherMeasurement = sfmTrack.measurements[idx];

2
gtsam/sfm/SfmTrack.h
View File

@ -68,7 +68,7 @@ struct SfmTrack {
}
/// Total number of measurements in this track
size_t nrMeasurements() const { return measurements.size(); }
size_t numberMeasurements() const { return measurements.size(); }
/// Get the measurement (camera index, Point2) at pose index `idx`
const SfmMeasurement& measurement(size_t idx) const {

2
gtsam/sfm/ShonanAveraging.h
View File

@ -165,7 +165,7 @@ class GTSAM_EXPORT ShonanAveraging {
size_t nrUnknowns() const { return nrUnknowns_; }
/// Return number of measurements
size_t nrMeasurements() const { return measurements_.size(); }
size_t numberMeasurements() const { return measurements_.size(); }
/// k^th binary measurement
const BinaryMeasurement<Rot> &measurement(size_t k) const {

10
gtsam/sfm/sfm.i
View File

@ -16,7 +16,7 @@ class SfmTrack {
std::vector<pair<size_t, gtsam::Point2>> measurements;
size_t nrMeasurements() const;
size_t numberMeasurements() const;
pair<size_t, gtsam::Point2> measurement(size_t idx) const;
pair<size_t, size_t> siftIndex(size_t idx) const;
void addMeasurement(size_t idx, const gtsam::Point2& m);
@ -31,8 +31,8 @@ class SfmTrack {
#include <gtsam/sfm/SfmData.h>
class SfmData {
SfmData();
size_t nrCameras() const;
size_t nrTracks() const;
size_t numberCameras() const;
size_t numberTracks() const;
gtsam::PinholeCamera<gtsam::Cal3Bundler> camera(size_t idx) const;
gtsam::SfmTrack track(size_t idx) const;
void addTrack(const gtsam::SfmTrack& t);
@ -136,7 +136,7 @@ class ShonanAveraging2 {
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
size_t numberMeasurements() const;
gtsam::Rot2 measured(size_t i);
gtsam::KeyVector keys(size_t i);
@ -184,7 +184,7 @@ class ShonanAveraging3 {
// Query properties
size_t nrUnknowns() const;
size_t nrMeasurements() const;
size_t numberMeasurements() const;
gtsam::Rot3 measured(size_t i);
gtsam::KeyVector keys(size_t i);

32
gtsam/sfm/tests/testSfmData.cpp
View File

@ -39,10 +39,10 @@ TEST(dataSet, Balbianello) {
CHECK(readBundler(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(5, mydata.nrCameras());
EXPECT_LONGS_EQUAL(544, mydata.nrTracks());
EXPECT_LONGS_EQUAL(5, mydata.numberCameras());
EXPECT_LONGS_EQUAL(544, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.nrMeasurements());
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
@ -60,10 +60,10 @@ TEST(dataSet, readBAL_Dubrovnik) {
CHECK(readBAL(filename, mydata));
// Check number of things
EXPECT_LONGS_EQUAL(3, mydata.nrCameras());
EXPECT_LONGS_EQUAL(7, mydata.nrTracks());
EXPECT_LONGS_EQUAL(3, mydata.numberCameras());
EXPECT_LONGS_EQUAL(7, mydata.numberTracks());
const SfmTrack& track0 = mydata.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.nrMeasurements());
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);
@ -119,16 +119,16 @@ TEST(dataSet, writeBAL_Dubrovnik) {
CHECK(readBAL(filenameToWrite, writtenData));
// Check that what we read is the same as what we wrote
EXPECT_LONGS_EQUAL(readData.nrCameras(), writtenData.nrCameras());
EXPECT_LONGS_EQUAL(readData.nrTracks(), writtenData.nrTracks());
EXPECT_LONGS_EQUAL(readData.numberCameras(), writtenData.numberCameras());
EXPECT_LONGS_EQUAL(readData.numberTracks(), writtenData.numberTracks());
for (size_t i = 0; i < readData.nrCameras(); i++) {
for (size_t i = 0; i < readData.numberCameras(); i++) {
PinholeCamera<Cal3Bundler> expectedCamera = writtenData.cameras[i];
PinholeCamera<Cal3Bundler> actualCamera = readData.cameras[i];
EXPECT(assert_equal(expectedCamera, actualCamera));
}
for (size_t j = 0; j < readData.nrTracks(); j++) {
for (size_t j = 0; j < readData.numberTracks(); j++) {
// check point
SfmTrack expectedTrack = writtenData.tracks[j];
SfmTrack actualTrack = readData.tracks[j];
@ -143,7 +143,7 @@ TEST(dataSet, writeBAL_Dubrovnik) {
EXPECT(assert_equal(expectedRGB, actualRGB));
// check measurements
for (size_t k = 0; k < actualTrack.nrMeasurements(); k++) {
for (size_t k = 0; k < actualTrack.numberMeasurements(); k++) {
EXPECT_LONGS_EQUAL(expectedTrack.measurements[k].first,
actualTrack.measurements[k].first);
EXPECT(assert_equal(expectedTrack.measurements[k].second,
@ -163,11 +163,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
Values value;
for (size_t i = 0; i < readData.nrCameras(); i++) { // for each camera
for (size_t i = 0; i < readData.numberCameras(); i++) { // for each camera
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(X(i), pose);
}
for (size_t j = 0; j < readData.nrTracks(); j++) { // for each point
for (size_t j = 0; j < readData.numberTracks(); j++) { // for each point
Point3 point = poseChange.transformFrom(readData.tracks[j].p);
value.insert(P(j), point);
}
@ -182,10 +182,10 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
// Check that the reprojection errors are the same and the poses are correct
// Check number of things
EXPECT_LONGS_EQUAL(3, writtenData.nrCameras());
EXPECT_LONGS_EQUAL(7, writtenData.nrTracks());
EXPECT_LONGS_EQUAL(3, writtenData.numberCameras());
EXPECT_LONGS_EQUAL(7, writtenData.numberTracks());
const SfmTrack& track0 = writtenData.tracks[0];
EXPECT_LONGS_EQUAL(3, track0.nrMeasurements());
EXPECT_LONGS_EQUAL(3, track0.numberMeasurements());
// Check projection of a given point
EXPECT_LONGS_EQUAL(0, track0.measurements[0].first);

2
gtsam/slam/SmartFactorBase.h
View File

@ -146,7 +146,7 @@ protected:
*/
template<class SFM_TRACK>
void add(const SFM_TRACK& trackToAdd) {
for (size_t k = 0; k < trackToAdd.nrMeasurements(); k++) {
for (size_t k = 0; k < trackToAdd.numberMeasurements(); k++) {
this->measured_.push_back(trackToAdd.measurements[k].second);
this->keys_.push_back(trackToAdd.measurements[k].first);
}

6
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -632,14 +632,14 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// We start with a factor graph and add constraints to it
// Noise sigma is 1, assuming pixel measurements
NonlinearFactorGraph graph;
for (size_t i = 0; i < data.nrTracks(); i++)
for (size_t i = 0; i < data.numberTracks(); i++)
graph.emplace_shared<EssentialMatrixFactor2>(100, i, pA(i), pB(i), model2,
K);
// Check error at ground truth
Values truth;
truth.insert(100, trueE);
for (size_t i = 0; i < data.nrTracks(); i++) {
for (size_t i = 0; i < data.numberTracks(); i++) {
Point3 P1 = data.tracks[i].p;
truth.insert(i, double(baseline / P1.z()));
}
@ -654,7 +654,7 @@ TEST(EssentialMatrixFactor2, extraMinimization) {
// Check result
EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.nrTracks(); i++)
for (size_t i = 0; i < data.numberTracks(); i++)
EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result

16
python/gtsam/examples/SFMExample_bal.py
View File

@ -29,10 +29,10 @@ DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: gtsam.SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for cam_idx in range(scene_data.nrCameras()):
for cam_idx in range(scene_data.numberCameras()):
plot_vals.insert(C(cam_idx),
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
for j in range(scene_data.nrTracks()):
for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j)))
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
scene_data = gtsam.readBal(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.nrTracks(),
scene_data.nrCameras())
logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.numberCameras())
# Create a factor graph
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
# Add measurements to the factor graph
for j in range(scene_data.nrTracks()):
for j in range(scene_data.numberTracks()):
track = scene_data.track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.nrMeasurements()):
for m_idx in range(track.numberMeasurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
@ -82,13 +82,13 @@ def run(args: argparse.Namespace) -> None:
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.nrCameras()):
for cam_idx in range(scene_data.numberCameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
i += 1
# add each SfmTrack
for j in range(scene_data.nrTracks()):
for j in range(scene_data.numberTracks()):
track = scene_data.track(j)
initial.insert(P(j), track.point3())

4
python/gtsam/tests/test_SfmData.py
View File

@ -42,7 +42,7 @@ class TestSfmData(GtsamTestCase):
self.tracks.addMeasurement(i1, uv_i1)
self.tracks.addMeasurement(i2, uv_i2)
# Number of measurements in the track is 2
self.assertEqual(self.tracks.nrMeasurements(), 2)
self.assertEqual(self.tracks.numberMeasurements(), 2)
# camera_idx in the first measurement of the track corresponds to i1
cam_idx, img_measurement = self.tracks.measurement(0)
self.assertEqual(cam_idx, i1)
@ -70,7 +70,7 @@ class TestSfmData(GtsamTestCase):
self.data.addTrack(self.tracks)
self.data.addTrack(track2)
# Number of tracks in SfmData is 2
self.assertEqual(self.data.nrTracks(), 2)
self.assertEqual(self.data.numberTracks(), 2)
# camera idx of first measurement of second track corresponds to i1
cam_idx, img_measurement = self.data.track(1).measurement(0)
self.assertEqual(cam_idx, i1)

2
tests/testGeneralSFMFactorB.cpp
View File

@ -49,7 +49,7 @@ TEST(PinholeCamera, BAL) {
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements)
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
}

2
timing/timeSFMBAL.cpp
View File

@ -36,7 +36,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;

4
timing/timeSFMBAL.h
View File

@ -73,8 +73,8 @@ int optimize(const SfmData& db, const NonlinearFactorGraph& graph,
if (gUseSchur) {
// Create Schur-complement ordering
Ordering ordering;
for (size_t j = 0; j < db.nrTracks(); j++) ordering.push_back(P(j));
for (size_t i = 0; i < db.nrCameras(); i++) {
for (size_t j = 0; j < db.numberTracks(); j++) ordering.push_back(P(j));
for (size_t i = 0; i < db.numberCameras(); i++) {
ordering.push_back(C(i));
if (separateCalibration) ordering.push_back(K(i));
}

2
timing/timeSFMBALautodiff.cpp
View File

@ -44,7 +44,7 @@ int main(int argc, char* argv[]) {
// Build graph
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;

2
timing/timeSFMBALcamTnav.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;

2
timing/timeSFMBALnavTcam.cpp
View File

@ -33,7 +33,7 @@ int main(int argc, char* argv[]) {
// Build graph using conventional GeneralSFMFactor
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
Point3_ nav_point_(P(j));
for (const SfmMeasurement& m: db.tracks[j].measurements) {
size_t i = m.first;

2
timing/timeSFMBALsmart.cpp
View File

@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
// Add smart factors to graph
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.nrTracks(); j++) {
for (size_t j = 0; j < db.numberTracks(); j++) {
auto smartFactor = boost::make_shared<SfmFactor>(gNoiseModel);
for (const SfmMeasurement& m : db.tracks[j].measurements) {
size_t i = m.first;