devbharat / gtsam Goto Github PK

I follow the documentation to compile and run examples,when i run
make CameraResectioning.run
the output is make: *** No rule to make target 'CameraResectioning.run'. Stop.
how to fix this bug?

Running the example TimeTBB I get the following results (faster using 1 thread). I'm currently looking into why but any insight in the meantime would be greatly appreciated.

numberOfProblems = 1000000
problemSize = 4
With 1 threads:
Without memory allocation, grain size = 1, time = 0.195355
Without memory allocation, grain size = 10, time = 0.194243
Without memory allocation, grain size = 100, time = 0.194409
Without memory allocation, grain size = 1000, time = 0.196936
With memory allocation, grain size = 1, time = 0.289234
With memory allocation, grain size = 10, time = 0.295508
With memory allocation, grain size = 100, time = 0.294618
With memory allocation, grain size = 1000, time = 0.29145

With 4 threads:
Without memory allocation, grain size = 1, time = 5.02581
Without memory allocation, grain size = 10, time = 4.9835
Without memory allocation, grain size = 100, time = 4.74276
Without memory allocation, grain size = 1000, time = 5.06713
With memory allocation, grain size = 1, time = 4.6808
With memory allocation, grain size = 10, time = 4.73614
With memory allocation, grain size = 100, time = 4.77174
With memory allocation, grain size = 1000, time = 4.75051

With 8 threads:
Without memory allocation, grain size = 1, time = 4.00496
Without memory allocation, grain size = 10, time = 4.06559
Without memory allocation, grain size = 100, time = 4.06971
Without memory allocation, grain size = 1000, time = 4.06233
With memory allocation, grain size = 1, time = 4.65368
With memory allocation, grain size = 10, time = 4.6617
With memory allocation, grain size = 100, time = 4.65855
With memory allocation, grain size = 1000, time = 4.65979

Summary of results:
4 threads, without allocation, grain size = 1, speedup = 0.0388704
4 threads, without allocation, grain size = 10, speedup = 0.0389772
4 threads, without allocation, grain size = 100, speedup = 0.0409907
4 threads, without allocation, grain size = 1000, speedup = 0.0388654
4 threads, with allocation, grain size = 1, speedup = 0.0617917
4 threads, with allocation, grain size = 10, speedup = 0.0623943
4 threads, with allocation, grain size = 100, speedup = 0.0617423
4 threads, with allocation, grain size = 1000, speedup = 0.0613514
8 threads, without allocation, grain size = 1, speedup = 0.0487782
8 threads, without allocation, grain size = 10, speedup = 0.0477773
8 threads, without allocation, grain size = 100, speedup = 0.0477697
8 threads, without allocation, grain size = 1000, speedup = 0.0484786
8 threads, with allocation, grain size = 1, speedup = 0.0621517
8 threads, with allocation, grain size = 10, speedup = 0.0633907
8 threads, with allocation, grain size = 100, speedup = 0.0632425
8 threads, with allocation, grain size = 1000, speedup = 0.0625457

Below I describe the issue we encounter when using BearingRangeFactor2D from the GTSAM pre-compiled toolbox to describe an inter-vehicle bearing/range measurement.

Within the current GTSAM structure I think the BearingRangeFactor2D is designed to construct factor graphs for landmarks-based SLAM i.e. to describe bearing/range measurements to a landmark to solve the SLAM problem.

The BearingRangeFactor2D constructor is defined as
BearingRangeFactor2D(size_t poseKey, size_t pointKey, Rot2 measuredBearing, double measuredRange, Base noiseModel)

As you will notice that the second argument is of type pointKey which is given by (x,y) -to describe the location of a landmark or feature- while a poseKey is defined by (x,y, theta). When using the BearingRangeFactor2D as shown in the attached pdf, it is required to provide an initial guess or estimate of the location of the landmark before executing the optimization routine. Below a piece of MATLAB script excerpt from GTSAM provided PlanarSLAMExample to describe a facotr graph for agent a and landmarks l1 and l2 as shown in the attached pdf

%-------------------------------------------------SingleAgent----------------------------------------------------------%
% Create keys for variables
% Create (instantiate) factor graph
% Add priors

%% Add odometry
graph.add(BetweenFactorPose2(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactorPose2(x2, x3, odometry, odometryNoise));

%% Add bearing/range measurement factors
graph.add(BearingRangeFactor2D(x1, l1, Rot2(45_degrees),2, brNoise));
graph.add(BearingRangeFactor2D(x2, l1, Rot2(90_degrees), sqrt(8), brNoise));
graph.add(BearingRangeFactor2D(x3, l2, Rot2(90*degrees), 2, brNoise));

%% Initialize to noisy points
initialEstimate = Values;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(0, 0)); % A required initial guess of landmark of type Point2
initialEstimate.insert(l2, Point2(0, 0)); % A required initial guess of landmark of type Point2
%% Optimize using Levenberg-Marquardt optimization
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); % initialEstimate must include a guess of landmarks
result = optimizer.optimizeSafely(); % result returns an optimized solution that includes an estimate of the landmark locations as what an EKF-SLAM would do.

%--------------------------------------------------------------------------------------------------------------------------%

When using the BearingRangeFactor2D to describe an inter-vehicle bearing/range measurement as shown in the attached pdf between agent 1 and 2, we run into the problem of having a second guess of the vehicle been measured. The below MATLAB script describes this issue

%---------------------------------------MultipleAgents-----------------------------------------------------------------%

%% Add odometry
graph.add(BetweenFactorPose2(a1, a2, odometry, odometryNoise)); % odometry factors for agent a
graph.add(BetweenFactorPose2(a2, a3, odometry, odometryNoise));

graph.add(BetweenFactorPose2(a1, a2, odometry, odometryNoise)); % odometry factors for agent b
graph.add(BetweenFactorPose2(a2, a3, odometry, odometryNoise));

%% Add bearing/range measurement factors
graph.add(BearingRangeFactor2D(a2, b2, Rot2(90*degrees), 2, brNoise));

%% Initialize to noisy points
initialEstimate = Values;
initialEstimate.insert(a1, Pose2(0.5, 0.0, 0.2)); % initial guesses for agent's a
initialEstimate.insert(a2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(a3, Pose2(4.1, 0.1, 0.1));

initialEstimate.insert(b1, Pose2(0.5, 0.0, 0.2)); % initial guesses for agent's b
initialEstimate.insert(b2, Pose2(0.3, 0.1,-0.2)); % <-------------------- 1st guess of type Pose
initialEstimate.insert(b3, Pose2(0.1, 0.1, 0.1));

initialEstimate.insert(a2, Point2(0, 0)); % <------------- 2nd guess of agent's b at b2 of type Point2

% ---------------------------------------------------------------------------------------------------------%

Work around (things we tried) & observations:

1. I used the RangeFactorPose2 to describe range measurements between agents and it works fine, the optimizer does not require an initial guess. A look into the C++ source code, I noticed it is instantiated from the same base class as the BearingRangeFactor2D (C++ class). The second argument of the RangeFacotrPose2 is of type Point2 as that of BearingRangeFactor2D. Which tells me that I may need to look into the optimizer that is using them.

2. A way to work around it, I have tried to create a virtual "landmark" that lies on the top of the agent i.e. by using a BetweenFactor between the agent say b at b2 and a virtual landmark say lb2 with odometry of zeros (0,0,0) and zero odometry noise. With that I have assumed when optimized the result location of lb2 will be on the top of b2 as defined by the ''BetweenFactor'', but that was not the case. lb2 has a separate location and I think it still considered as a landmark close to the agent b at b2.

Regards,
Laith

what does it mean when i set an inlier or outlier, it says that the input is a keyvector but that means it is setting a node as an Inlier and not an edge, Can someone clear this up. Example Code : https://github.com/AnshShah3009/DPGO_GTSAM/blob/master/notebooks/Gnc.ipynb

When I run gtsam matlab toolbox ,output error undefined function or variable 'NonlinearFactorGraph',I have added the gtsam/matlab to the matlab searching path,however,still can't fix this problem .Did I do something wrong?