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

Keys for poses/cameras are naked

release/4.3a0
Frank Dellaert 2022-01-31 16:22:26 -05:00
parent 2e9b51aff1
commit 7adc9898ad
4 changed files with 30 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/sfm/SfmData.cpp
View File

@ -29,7 +29,6 @@ using std::cout;
using std::endl;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
/* ************************************************************************** */
void SfmData::print(const std::string &s) const {
@ -356,7 +355,7 @@ bool writeBALfromValues(const std::string &filename, const SfmData &data,
size_t nrPoses = values.count<Pose3>();
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));
Pose3 pose = values.at<Pose3>(i);
Cal3Bundler K = dataValues.cameras[i].calibration();
Camera camera(pose, K);
dataValues.cameras[i] = camera;
@ -415,8 +414,7 @@ NonlinearFactorGraph SfmData::generalSfmFactors(
for (const SfmMeasurement &m : track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
factors.emplace_shared<ProjectionFactor>(
uv, model, X(i), P(j)); // note use of shorthand symbols X and P
factors.emplace_shared<ProjectionFactor>(uv, model, i, P(j));
}
j += 1;
}
@ -432,7 +430,7 @@ NonlinearFactorGraph SfmData::sfmFactorGraph(
NonlinearFactorGraph graph = generalSfmFactors(model);
using noiseModel::Constrained;
if (fixedCamera) {
graph.addPrior(X(*fixedCamera), cameras[0], Constrained::All(9));
graph.addPrior(*fixedCamera, cameras[0], Constrained::All(9));
}
if (fixedPoint) {
graph.addPrior(P(*fixedPoint), tracks[0].p, Constrained::All(3));
@ -452,7 +450,7 @@ Values initialCamerasEstimate(const SfmData &db) {
Values initialCamerasAndPointsEstimate(const SfmData &db) {
Values initial;
size_t i = 0, j = 0;
for (const SfmCamera &camera : db.cameras) initial.insert((i++), camera);
for (const SfmCamera &camera : db.cameras) initial.insert(i++, camera);
for (const SfmTrack &track : db.tracks) initial.insert(P(j++), track.p);
return initial;
}

16
gtsam/sfm/SfmData.h
View File

@ -83,7 +83,7 @@ struct GTSAM_EXPORT SfmData {
SfmCamera camera(size_t idx) const { return cameras[idx]; }
/**
* @brief Create projection factors using keys X(i) and P(j)
* @brief Create projection factors using keys i and P(j)
*
* @param model a noise model for projection errors
* @return NonlinearFactorGraph
@ -95,6 +95,8 @@ struct GTSAM_EXPORT SfmData {
/**
* @brief Create factor graph with projection factors and gauge fix.
*
* Note: pose keys are simply integer indices, points use Symbol('p', j).
*
* @param model a noise model for projection errors
* @param fixedCamera which camera to fix, if any (use boost::none if none)
* @param fixedPoint which point to fix, if any (use boost::none if none)
@ -177,8 +179,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, const SfmData& data);
* @param data SfM structure where the data is stored
* @param values structure where the graph values are stored (values can be
* either Pose3 or PinholeCamera<Cal3Bundler> for the cameras, and should be
* Point3 for the 3D points). Note that the current version assumes that the
* keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* Point3 for the 3D points). Note: assumes that the keys are "i" for pose i
* and "Symbol::('p',j)" for landmark j.
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
@ -212,7 +214,10 @@ GTSAM_EXPORT Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz);
GTSAM_EXPORT Pose3 gtsam2openGL(const Pose3& PoseGTSAM);
/**
* @brief This function creates initial values for cameras from db
* @brief This function creates initial values for cameras from db.
*
* No symbol is used, so camera keys are simply integer indices.
*
* @param SfmData
* @return Values
*/
@ -220,6 +225,9 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
/**
* @brief This function creates initial values for cameras and points from db
*
* Note: Pose keys are simply integer indices, points use Symbol('p', j).
*
* @param SfmData
* @return Values
*/

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

@ -25,7 +25,6 @@ using namespace std;
using namespace gtsam;
using gtsam::symbol_shorthand::P;
using gtsam::symbol_shorthand::X;
namespace gtsam {
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
@ -167,19 +166,19 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
Pose3 poseChange =
Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), Point3(0.3, 0.1, 0.3));
Values value;
Values values;
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);
values.insert(i, pose);
}
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);
values.insert(P(j), point);
}
// Write values and readData to a file
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
writeBALfromValues(filenameToWrite, readData, values);
// Read the file we wrote
SfmData writtenData = SfmData::FromBalFile(filenameToWrite);
@ -199,11 +198,11 @@ TEST(dataSet, writeBALfromValues_Dubrovnik) {
EXPECT(assert_equal(expected, actual, 12));
Pose3 expectedPose = camera0.pose();
Pose3 actualPose = value.at<Pose3>(X(0));
Pose3 actualPose = values.at<Pose3>(0);
EXPECT(assert_equal(expectedPose, actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Point3 actualPoint = value.at<Point3>(P(0));
Point3 actualPoint = values.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint, actualPoint, 1e-6));
}

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

@ -17,7 +17,7 @@ import sys
import gtsam
from gtsam import (GeneralSFMFactorCal3Bundler, SfmData,
PriorFactorPinholeCameraCal3Bundler, PriorFactorPoint3)
from gtsam.symbol_shorthand import C, P # type: ignore
from gtsam.symbol_shorthand import P # type: ignore
from gtsam.utils import plot # type: ignore
from matplotlib import pyplot as plt
@ -29,9 +29,8 @@ DEFAULT_BAL_DATASET = "dubrovnik-3-7-pre"
def plot_scene(scene_data: SfmData, result: gtsam.Values) -> None:
"""Plot the SFM results."""
plot_vals = gtsam.Values()
for cam_idx in range(scene_data.numberCameras()):
plot_vals.insert(C(cam_idx),
result.atPinholeCameraCal3Bundler(C(cam_idx)).pose())
for i in range(scene_data.numberCameras()):
plot_vals.insert(i, result.atPinholeCameraCal3Bundler(i).pose())
for j in range(scene_data.numberTracks()):
plot_vals.insert(P(j), result.atPoint3(P(j)))
@ -64,12 +63,12 @@ def run(args: argparse.Namespace) -> None:
# 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
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0),
0, scene_data.camera(0),
gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
@ -82,9 +81,9 @@ def run(args: argparse.Namespace) -> None:
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.numberCameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
for i in range(scene_data.numberCameras()):
camera = scene_data.camera(i)
initial.insert(i, camera)
i += 1
# add each SfmTrack