Issue with custom landmark constraint

Manohar Kuse

unread,

Sep 29, 2021, 8:23:23 AM9/29/21

to gtsam users

I am trying to add the 3d-2d constraint of the loop-pair directly into the graph.

Assume image_a <--> image_b is the loop pair. My factorgraph

is in imu frame (referred to as Ia and Ib respectively). I also have

the fixed transform between imu and the camera as T_I_C.

The pose of the imu at `a` and `b` are the factor graph optimization variable.

Following is my custom factor implementation.

I try to add 3d points of image_a (I have a stereo camera)

and 2d points of image_b as observations in the factor.

As I run the isam2 live, I do not seem to get the results I am looking for.
Is there something mathematically infeasible about what I am trying to do.

Could someone comment on how should I proceed with this sort of thing?

What this factor does (or expected to do is) convert the

optimization variables T_G_Ia and T_G_Ib to camera frame (viz T_G_Ca, T_G_Cb).

Project the 3d points of image_a frame onto image_b using T_G_Ca and T_G_Cb.

```c++

class LocalImageFramePointLandmarkProjectionFactor

: public gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3> {

public:

EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:

typedef gtsam::NoiseModelFactor2<gtsam::Pose3, gtsam::Pose3> Base;

typedef LocalImageFramePointLandmarkProjectionFactor This;

typedef boost::shared_ptr<This> shared_ptr;

// point_i : observed point uv. (of image_b)

// point_I : 3d point in imu frame of reference (of image_a)

// model : Noise Model aka sigma

// K : camera intrinsic

// T_I_C : pose of camera in imu frame of reference

LocalImageFramePointLandmarkProjectionFactor(

gtsam::Key key_a, gtsam::Key key_b, const gtsam::Point2 &point_i,

const gtsam::Point3 &point_C, const gtsam::SharedNoiseModel &model,

const boost::shared_ptr<gtsam::Cal3_S2> &K,

gtsam::Pose3 T_I_C = gtsam::Pose3())

: Base(model, key_a, key_b),

point_i_(point_i),

point_C_(point_C),

K_(K),

T_I_C_(T_I_C) {

// T_C_I_ = T_I_C_.inverse();

}

virtual ~LocalImageFramePointLandmarkProjectionFactor() override {}

virtual gtsam::NonlinearFactor::shared_ptr clone() const override {

return boost::static_pointer_cast<gtsam::NonlinearFactor>(

gtsam::NonlinearFactor::shared_ptr(new This(*this)));

}

/// Evaluate error h(x)-z and optionally derivatives

gtsam::Vector evaluateError(

const gtsam::Pose3 &T_G_Ia, const gtsam::Pose3 &T_G_Ib,

boost::optional<gtsam::Matrix &> H1 = boost::none,

boost::optional<gtsam::Matrix &> H2 = boost::none) const override {

if (H1) {

*H1 = gtsam::zeros(2, 6);

*H2 = gtsam::zeros(2, 6);

}

try {

if( H1 ) {

gtsam::Matrix D_TGCa_TGIa, D_TGCb_TGIb;

gtsam::Pose3 T_G_Ca = T_G_Ia.compose( T_I_C_, D_TGCa_TGIa );

gtsam::Pose3 T_G_Cb = T_G_Ib.compose( T_I_C_, D_TGCb_TGIb );

gtsam::Matrix D_betweenpose_TGCa, D_betweenpose_TGCb;

gtsam::Pose3 T_Cb_Ca = T_G_Cb.between( T_G_Ca, D_betweenpose_TGCb, D_betweenpose_TGCa );

gtsam::Matrix D_campose;

gtsam::PinholeCamera<gtsam::Cal3_S2> camera( T_Cb_Ca, *K_);

gtsam::Point2 _projected_pt = camera.project(point_C_, D_campose );

*H1 = D_campose * D_betweenpose_TGCa * D_TGCa_TGIa;

*H2 = D_campose * D_betweenpose_TGCb * D_TGCb_TGIb;

return (_projected_pt - point_i_);

}else {

gtsam::Pose3 T_G_Ca = T_G_Ia.compose( T_I_C_ );

gtsam::Pose3 T_G_Cb = T_G_Ib.compose( T_I_C_ );

gtsam::Pose3 T_Cb_Ca = T_G_Cb.between( T_G_Ca );

gtsam::PinholeCamera<gtsam::Cal3_S2> camera( T_Cb_Ca, *K_);

gtsam::Point2 residuals =

camera.project(point_C_) - point_i_;

return residuals.vector();

}

} catch (gtsam::CheiralityException &e) {

// LOG(FATAL) << "Landmark moved behind camera "

// << gtsam::DefaultKeyFormatter(this->key() );

}

// This only gets called in case a cheirality exc was thrown,

// error handling from GTSAM's ProjectionFactor.h

return gtsam::ones(2) * 2.0 * K_->fx();

}

private:

gtsam::Point2 point_i_; //< observed point uv. (of image_b)

gtsam::Point3 point_C_; //< 3d point in camera frame of reference (of image_a)

boost::shared_ptr<gtsam::Cal3_S2> K_;

gtsam::Pose3 T_I_C_;

// gtsam::Pose3 T_C_I_;

public:

};

} // namespace graph_optimization

```

Dellaert, Frank

unread,

Sep 29, 2021, 8:44:27 AM9/29/21

to Manohar Kuse, gtsam users

Unit tests on Jacobians all pass?

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Manohar Kuse <swaroo...@gmail.com>
Sent: Wednesday, September 29, 2021 08:23
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Issue with custom landmark constraint

--
You received this message because you are subscribed to the Google Groups "gtsam users" group.
To unsubscribe from this group and stop receiving emails from it, send an email to gtsam-users...@googlegroups.com.
To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/7f823144-d6a3-423a-93aa-e3faad5153d3n%40googlegroups.com.

Manohar Kuse

unread,

Sep 29, 2021, 9:08:06 AM9/29/21

to gtsam users

Sorry, could you elaborate?

Are you referring to this by any chance? https://gtsam.org/doxygen/a00851.html#details

Are there unit tests I can run on my custom factors to check

for the correctness of jacobians? I have tried to go over manually

the equations of my residue and try to get to the hopefully correct jacobians.

Manohar Kuse

unread,

Sep 29, 2021, 10:47:03 AM9/29/21

to gtsam users

Following an earlier discussion on this forum https://groups.google.com/g/gtsam-users/c/GRC47Og3rH8/m/w79B2pmuDAAJ

I wrote up a quick unit test.

```c++

#include <iostream>

#include <gtsam/slam/PriorFactor.h>

#include <gtsam/geometry/Pose2.h>

#include <gtsam/inference/Symbol.h>

#include <gtsam/nonlinear/factorTesting.h>

#include <CppUnitLite/TestHarness.h>

TEST(SampleTests, PriorFactor) {

gtsam::Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin

gtsam::noiseModel::Diagonal::shared_ptr priorNoise =

gtsam::noiseModel::Diagonal::Sigmas(gtsam::Vector3(0.3, 0.3, 0.1));

auto factor = gtsam::PriorFactor<gtsam::Pose2>(gtsam::Symbol('x'), priorMean, priorNoise );

gtsam::Values values;

values.insert( gtsam::Symbol('x'), gtsam::Pose2(0.002, 0.01, 0.57));

EXPECT_CORRECT_FACTOR_JACOBIANS( factor, values, 1e-7, 1e-5 );

}

int main(int argc, char **argv) {

TestResult tr;

return TestRegistry::runAllTests(tr);

}

```

As I run this unit test on an internal (and well used factor, the PriorFactor), I seem to get the following

test-failed message. How do I interpret this?

```

Not equal:

expected:

A[120] = [

3.33333 0 0.0328994;

-3.46945e-11 3.33333 -0.00855481;

0 0 10

]

b = [ -0.00855481 -0.0328994 -0.57 ]

No noise model

actual:

A[120] = [

3.33333 0 0;

0 3.33333 0;

0 0 10

]

b = [ -0.00855481 -0.0328994 -0.57 ]

No noise model

not equal:

expected = [

3.33333, 0, 0.0328994;

-3.46945e-11, 3.33333, -0.00855481;

0, 0, 10

]

actual = [

3.33333, 0, 0;

0, 3.33333, 0;

0, 0, 10

]

actual - expected = [

2.44915e-12, 0, -0.0328994;

3.46945e-11, 1.97962e-11, 0.00855481;

0, 0, 2.67555e-10

]

Mismatch in Jacobian 1 (base 1), as shown above

/home/manohar/Downloads/gtsam_standalone/unittests/sampletest.cpp:25: Failure: "gtsam::internal::testFactorJacobians(name_, factor, values, 1e-7, 1e-5)"

There were 1 failures

```

Dellaert, Frank

unread,

Sep 29, 2021, 1:14:28 PM9/29/21

to Manohar Kuse, gtsam users

That’s a good test. Try it on your own factor?

Frank

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Manohar Kuse <swaroo...@gmail.com>

Sent: Wednesday, September 29, 2021 10:47:03 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: Re: [GTSAM] Issue with custom landmark constraint

To view this discussion on the web visit https://groups.google.com/d/msgid/gtsam-users/fc43d000-23ba-45e6-91ac-6aa36a63a9fen%40googlegroups.com.

Reply all

Reply to author

Forward