38 using namespace gtsam;
53 emplace_shared<Projection>(
z,
model,
X(
i),
L(
j));
72 double U1 = rand() / (double) (RAND_MAX);
73 double U2 = rand() / (double) (RAND_MAX);
76 S = V1 * V1 + V2 * V2;
87 vector<Point3> landmarks;
88 landmarks.push_back(
Point3(-1., -1.,
z));
89 landmarks.push_back(
Point3(-1., 1.,
z));
90 landmarks.push_back(
Point3(1., 1.,
z));
91 landmarks.push_back(
Point3(1., -1.,
z));
92 landmarks.push_back(
Point3(-1.5, -1.5, 1.5 *
z));
93 landmarks.push_back(
Point3(-1.5, 1.5, 1.5 *
z));
94 landmarks.push_back(
Point3(1.5, 1.5, 1.5 *
z));
95 landmarks.push_back(
Point3(1.5, -1.5, 1.5 *
z));
96 landmarks.push_back(
Point3(-2., -2., 2 *
z));
97 landmarks.push_back(
Point3(-2., 2., 2 *
z));
98 landmarks.push_back(
Point3(2., 2., 2 *
z));
99 landmarks.push_back(
Point3(2., -2., 2 *
z));
104 vector<GeneralCamera>
X;
111 const Cal3_S2 K(640, 480, 0.1, 320, 240);
112 vector<GeneralCamera>
X;
119 const vector<GeneralCamera>& cameras,
const vector<Point3>& landmarks) {
121 for (
size_t i = 0;
i < landmarks.size(); ++
i)
123 for (
size_t i = 0;
i < cameras.size(); ++
i)
132 const Symbol cameraFrameNumber(
'x', 1), landmarkNumber(
'l', 1);
134 std::shared_ptr<Projection>
factor1(
137 std::shared_ptr<Projection>
factor2(
169 for (
size_t j = 0;
j < cameras.size(); ++
j) {
170 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
176 const size_t nMeasurements = cameras.
size() * landmarks.size();
179 const double noise =
baseline * 0.1;
181 for (
size_t i = 0;
i < cameras.size(); ++
i)
184 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
191 graph.addCameraConstraint(0, cameras[0]);
206 for (
size_t j = 0;
j < cameras.size(); ++
j) {
207 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
213 const size_t nMeasurements = cameras.
size() * landmarks.size();
216 const double noise =
baseline * 0.1;
218 for (
size_t i = 0;
i < cameras.size(); ++
i)
222 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
233 graph.addCameraConstraint(0, cameras[0]);
234 const double reproj_error = 1
e-5;
239 EXPECT(optimizer.
error() < 0.5 * reproj_error * nMeasurements);
249 const double noise =
baseline * 0.1;
251 for (
size_t i = 0;
i < cameras.size(); ++
i) {
252 for (
size_t j = 0;
j < landmarks.size(); ++
j) {
253 Point2 z = cameras[
i].project(landmarks[
j]);
258 const size_t nMeasurements = landmarks.
size() * cameras.size();
261 for (
size_t i = 0;
i < cameras.size(); ++
i)
264 for (
size_t j = 0;
j < landmarks.size(); ++
j) {
271 for (
size_t i = 0;
i < cameras.size(); ++
i)
272 graph.addCameraConstraint(
i, cameras[
i]);
274 const double reproj_error = 1
e-5;
279 EXPECT(optimizer.
error() < 0.5 * reproj_error * nMeasurements);
290 for (
size_t j = 0;
j < cameras.size(); ++
j) {
291 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
297 const size_t nMeasurements = landmarks.
size() * cameras.size();
300 for (
size_t i = 0;
i < cameras.size(); ++
i) {
301 const double rot_noise = 1
e-5, trans_noise = 1
e-3, focal_noise = 1,
308 trans_noise, trans_noise, trans_noise,
309 focal_noise, focal_noise,
311 trans_noise, trans_noise
317 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
322 graph.addCameraConstraint(0, cameras[0]);
323 for (
size_t i = 0;
i < landmarks.size(); ++
i)
324 graph.addPoint3Constraint(
i, landmarks[
i]);
326 const double reproj_error = 1
e-5;
331 EXPECT(optimizer.
error() < 0.5 * reproj_error * nMeasurements);
341 for (
size_t j = 0;
j < cameras.size(); ++
j) {
342 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
348 const size_t nMeasurements = cameras.
size() * landmarks.size();
351 const double noise =
baseline * 0.1;
353 for (
size_t i = 0;
i < cameras.size(); ++
i)
357 for (
size_t i = 0;
i < landmarks.size(); ++
i) {
365 graph.addCameraConstraint(0, cameras[0]);
370 noiseModel::Isotropic::Sigma(1, 10.));
372 const double reproj_error = 1
e-5;
377 EXPECT(optimizer.
error() < 0.5 * reproj_error * nMeasurements);
387 noiseModel::Isotropic::Sigma(1, 1.));
389 noiseModel::Isotropic::Sigma(6, 1.));
401 params.absoluteErrorTol = 1
e-9;
402 params.relativeErrorTol = 1
e-9;
413 noiseModel::Isotropic::Sigma(6, 1.));
416 noiseModel::Isotropic::Sigma(1, 1.));
418 noiseModel::Isotropic::Sigma(6, 1.));
430 params.absoluteErrorTol = 1
e-9;
431 params.relativeErrorTol = 1
e-9;
451 vector<SharedNoiseModel> models;
454 using namespace noiseModel;
455 Rot2 R = Rot2::fromAngle(0.3);
458 Constrained::All(2), Gaussian::Covariance(cov)};
467 factor.NoiseModelFactor::linearize(
values);
501 for (
size_t i = 0;
i < cameras.size(); ++
i)
503 for (
size_t j = 0;
j < landmarks.size(); ++
j)
506 for (
size_t i = 0;
i < cameras.size(); ++
i) {
507 for (
size_t j = 0;
j < landmarks.size(); ++
j) {
508 Point2 z = cameras[
i].project(landmarks[
j]);
510 std::make_shared<Projection>(
z,
sigma1,
X(
i),
L(
j));