Variable of type Point2 appears as Eigen::Matrix<double, 2, 1, 0, 2, 1>

[S Chatzizacharias]

Hello. I create a nonlinear factor graph for a mobile robot application, using Pose2 variables for robot poses and Point2 variables for landmarks, added with bearing factors. When i print the results of the optimization and the initial values, the Point2 variables appear with the type Eigen::Matrix<double, 2, 1, 0, 2, 1> and not as gtsam::Point2. Is that normal? and why does this happen?

Here is a simplified version of my code:

#include <gtsam/geometry/Pose2.h>

#include <gtsam/geometry/Point2.h>

#include <gtsam/geometry/Rot2.h>

#include <gtsam/slam/BetweenFactor.h>

#include <gtsam/sam/BearingFactor.h>

#include <gtsam/nonlinear/NonlinearFactorGraph.h>

#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

#include <gtsam/nonlinear/Marginals.h>

#include <gtsam/nonlinear/Values.h>

#include <fstream>

#include <gtsam/inference/Symbol.h>

...

// Create bearing factor from current pose to door point

Rot2 bearing_ang = Rot2::fromDegrees(13);

auto bearingNoise = noiseModel::Diagonal::Sigmas(Vector1(1));

graph.emplace_shared<BearingFactor<Pose2, Point2> >(next_pose_num, next_door_num, bearing_ang, bearingNoise);

// Initialize the door point

initial.insert<Point2>(next_door_num, Point2(current_odom[0] + 1.0, current_odom[1] + 1.0));

And here is the printed output:

Factor Graph:

size: 10

Factor 0: PriorFactor on 1

  prior mean:  (6.1, -11, -3.1)

isotropic dim=3 sigma=0.1

Factor 1: BetweenFactor(1,2)

  measured:  (2, 4.3e-05, 0.0002)

isotropic dim=3 sigma=0.1

Factor 2: BetweenFactor(2,3)

  measured:  (1.6, -0.14, -0.37)

isotropic dim=3 sigma=0.1

Factor 3: BetweenFactor(3,4)

  measured:  (0.2, -0.027, -0.24)

isotropic dim=3 sigma=0.1

Factor 4: BearingFactor

Factor 4:   keys = { 4 5 }

  noise model: unit (1) 

ExpressionFactor with measurement: : 0.23

Factor 5: BetweenFactor(4,6)

  measured:  (0.74, -0.21, -0.36)

isotropic dim=3 sigma=0.1

Initial Estimate:

Values with 10 values:

Value 1: (gtsam::Pose2)

(6.1, -11, -3.1)

Value 2: (gtsam::Pose2)

(4.1, -11, -3.1)

Value 3: (gtsam::Pose2)

(2.5, -11, 2.8)

Value 4: (gtsam::Pose2)

(2.3, -11, 2.6)

Value 5: (Eigen::Matrix<double, 2, 1, 0, 2, 1>)

[

-1.9;

-9.5

]

Value 6: (gtsam::Pose2)

(1.8, -10, 2.2)

Final Result:

Values with 10 values:

Value 1: (gtsam::Pose2)

(6.1, -11, -3.1)

Value 2: (gtsam::Pose2)

(4.1, -11, -3.1)

Value 3: (gtsam::Pose2)

(2.5, -11, 2.8)

Value 4: (gtsam::Pose2)

(2.3, -11, 2.6)

Value 5: (Eigen::Matrix<double, 2, 1, 0, 2, 1>)

[

-1.9;

-9.5

]

Value 6: (gtsam::Pose2)

(1.8, -10, 2.2)

NOTE: the point variable is not fully constrained yet. Apart from the initial value, I don't use any prior on that point and no other variable is connected to that point. The only connection so far is the bearing factor between 4 (Pose2) and 5 (Point2). (I don't know if this plays any role here.)

As mentioned here, the Vector2 is a typedef to 'Eigen::Matrix<double,2,1,0,2,1>'. Does this stand for Point2 type as well?

I compile in Ubuntu 18.04 with c++11. The code is running as a ROS node (melodic).

Any feedback is appreciated. Thanks in advance. 

[Varun Agrawal]

That's normal. GTSAM typedef's Point2 to an Eigen::Vector2 which is simply Eigen::Matrix<double, 2, 1, 0, 2, 1>.