Problem & ProcessorMotion initialization

Created by: joanvallve

Mostly, we want to add a initial covariance to the problem. To do this, there is the CaptureFix (we can change its name), adding it to the first frame and processing it, it creates a constraintFix.

But the ProcessorMotion::setOrigin(vectorXs state, ts) creates a key frame that should be the first, also. Maybe it should accept a frame instead of a ts...?

Created by: joanvallve

I added a ProcessorMotion::setOrigin(FrameBase*, ts) to be used in case the first keyframe is already created before the processor motion initialization.

However, it is still undefined the wolf problem initialization procedure, so I leave this issue opened...

Created by: joanvallve

There are 2 things that must be done in order to initialize the problem and the processMotion:

1. Problem::setProcessorMotion(ProcessorMotion*)
2. ProcessorMotion::setOrigin(FrameBase*, TimeStamp)

This may be related with issue #58 (closed)

Created by: joansola

I think we should only provide one function call for initialization, in Problem. the rest should be automatic. Can you come up with an API proposition that makes sense from the user point of view? Please consider #58 (closed)

Created by: joansola

IMO, we could do:

• Problem.setProcessorMotion("Processor name").
• Problem.setOrigin(FrameBase& frame_0)
• Problem.setOrigin(const TimeStamp& ts, const VectorXs& x_0)

or even, with just one call (two overloaded APIs),

• Problem.setProcessorMotion("Processor name", FrameBase& frame_0).
• Problem.setProcessorMotion("Processor name", const TimeStamp& ts, const VectorXs& x_0).

Created by: joanvallve

I think that the first frame is more an input parameter in the problem constructor (with the option to provide an initial covariance too) and the processorMotion should take it as origin when it is added (or setted or installed... XD)

Created by: joanvallve

I'm moving wolf tests to the factory API. These two things should be discussed:

A. Initial pose There should be a problem constructor accepting an _init_pose and _init_covariance, executing the following:

1. origin_frame_ptr = createFrame(KEY_FRAME, _init_pose, ts)
2. SensorFix2D* fix_sensor_ptr = createSensor("FIX_2D_POSE", "initial pose")
3. CaptureFix* init_capture = new CaptureFix(ts, fix_sensor_ptr, _init_pose, _init_covariance)
4. origin_frame_ptr->addCapture(initial_covariance)
5. init_capture->process()

B. ProcessorMotion initialization Initializing the processor motion means 2 things:

• ProcessorMotion::setOrigin(_origin_frame_ptr)
• Problem::setProcessorMotion(_processor_motion_ptr)

I think both should be done in the installation process...

Other alternative is doing A and B.1 in something like: Problem::initialize(_init_pose,_init_covariance) But it should be called after the installation of the motion sensor.

Created by: joansola

Good point.

I think we need problem.setOrigin(mean, covariance).

Can we guarantee integrity no matter the order in which we initialize things? I think this should be doable:

• If problem.setOrigin() is after problem.installProcessor(), we need to initialize the processor just after installing it, in the installer.
• If problem.installProcessor is after problem.setOrigin(), we need to propagate the initialization down to the processor.

Created by: joanvallve

So to be sure, do you mean that you would discard the alternative of setting the origin in the problem constructor?

Then, in the installProcessor there should be a Problem::setProcessorMotion(_processor_motion_ptr) call. I see two problems:

• Is there a way to use polymorphism to force that processor motion installers should call Problem::setProcessorMotion(this)?
• Dealing with more than one processor motion: The processor motion installer should accept somehow specifying if the Problem::setProcessorMotion(this) should be called.

Created by: joansola

No no, I just say that whatever the order of initializations, everything should end up OK. Then I put an example, but I did not think a lot about it.

Created by: joansola

The one processor motion needs to be known by problem, and it's one of the parameters of the configurations. I think specifying the origin means:

• set mean and cov
• set which is the processor motion

and that this should be done only once, for example in problem:

• problem.setOrigin(mean, cov)
• problem.setProcessorMotion("Processor Name")

but, again I did not think about it enough...

Then, if later, you install sensors and processors, things should get configured automatically. And also it you installed them previously. There must be a system to ensure that everything ends up OK no matter the order of the configuration.

Created by: joansola

Otherwise, we can enforce an order. I see this as the logical order:

• problem.installSensor()
• problem.InstallProcessor()
• problem.setProcessorMotion("Name")
• problem.setOrigin(mean,cov)

Created by: joanvallve

My point is that if the origin is setted in the problem constructor, then we force the order (we can't install anything in a problem that is not created yet).

However, I'm not sure if always in the constructor call time, the final user will always know the origin..

Let's do it separately. I'll figure out if the order is really an issue.

Created by: joanvallve

I think we can avoid your 3rd point if when installing the processor motion, setProcessorMotion is automatically done. We can specify that, for instance, the first processor motion installed is the main one.

added External API update To Do (release) labels

changed milestone to %First private release 0.1.0

In my opinion, this issue is important since it defines how Wolf should be set up by the external users. I added in the milestone of the first private release.

assigned to @joanvallve

I agree it is important. However, the discussion above is 3 years old. Is it still relevant? I mean, do we need to consider the discussion above to go for a better design, or do we have newer/different considerations or needs?

So to center a little bit the discussion to the status of WOLF today.

First, what we have:

Problem::setPrior()

• Emplace a KF
• Assign "P" and "O" of the KF, and disregard "V" even if the FrameStructure has "V".
• Emplace a CapturePose
• Emplace a FeaturePose and a FactorPose. Use the provided covariance for the factor.
• Call setOrigin() on all motion processors
• Call keyFrameCallback() on all non-motion processors

ProcessorMotion::setOrigin()

• Emplace the origin CaptureMotion
• Emplace the last Frame and Captures at the same time stamp than origin
• Do not create features or factors at all

Second, what we would want:

• Initializing also "V" when required by the frame structure.
  • What what to do if a value for 'v' is not provided?
• Options for setting the prior:
  • initial guess: just an initial value for the solver, with no constraints
  • fix state: use this state as a fixed parameter
  • prior factor: via covariance
• Option to control each state block differently (maybe through a second setPrior overload.
  • per-block initial guess / fix / factor

Once these options are in place in Problem::setPrior(), the operation I think must proceed the same as before:

• Call setOrigin() on all motion processors
• Call keyFrameCallback() on all non-motion processors
• ProcessorMotion::setOrigin() would not change

Agreed on skype discussion with @jsola and @mederic_fourmy

Functionality

There will be four types of prior (from user side):

1. Factor: a KF will be created with the provided state and an absolute factor with this state and a provided covariance.
2. Fix: a KF will be created with the provided state and fixed.
3. Initial guess: A KF will be created with the provided state.
4. Nothing: No KF will be created.

In the yaml, there will be:

• prior/mode = {factor, fix, initial_guess, nothing}
• prior/state a vector (not required in mode nothing).
• prior/time_tolerance (not required in mode nothing).
• prior/covariance a covariance matrix (only required in mode factor).

Implementation

About the TimeStamp and the creation of this KF:

• New struct in Problem containing all the previous information.
• New function Problem::isPriorCreated() returning false if not created or nothing prior mode.
• New function Problem::emplacePriorKeyFrame(TimeStamp) that emplaces the KF with the stored information and calls Problem::keyFrameCallback(KF,nullptr).
• In ProcessorBase::captureCallback(capture), before anything else:

if (!getProblem()->isPriorCreated()) getProblem()->emplacePriorKeyFrame(capture->getTimeStamp())

Open issues:

• setOrigin() functions, most of them to be removed or made private.
• Check that processors work ok geting the origin via keyFrameCallback(), IMPORTANT: this implementation ensures that before any processCapture(), keyFrameCallback() will be called before (either calling storeKF() or processKF() or both).

created merge request !353 (merged) to address this issue

mentioned in merge request !353 (merged)

added ~209 label and removed To Do (release) label

mentioned in issue #273

closed via merge request !353 (merged)

mentioned in commit 0a402f61

mentioned in issue #319 (closed)

mentioned in issue #339