FixedFramePoseData optimize translation and rotation

[slamer-run]

Does FixedFramePoseData optimize not only translation but also rotation?

At "SensorBridge::HandleNavSatFixMessage" function below file, it seems like that "carto::sensor::FixedFramePoseDatalike" set just "Rigid3d::Translation". cartographer-repo/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc

However, FixedFramePoseData optimize both translation and rotation below file. cartographer/cartographer/mapping/internal/optimization/optimization_problem_3d.cc What I want to point out in the code is followed.

problem.AddResidualBlock( SpaCostFunction3D::CreateAutoDiffCostFunction(constraint_pose), nullptr /* loss function */, C_fixed_frames.at(trajectory_id).rotation(), C_fixed_frames.at(trajectory_id).translation(), C_nodes.at(node_id).rotation(), C_nodes.at(node_id).translation());

Thanks in advance.

[wohe]

Fixed frame poses can contain both a position and orientation and thus in libcartographer both are introduced into the optimization problem.

NavSatFix from ROS only contains position and thus uses a fixed frame pose only using translation. There is no weight given to the orientation for this reason.

Other users of the fixed frame pose mechanism can and do use the orientation as well. It's not only intended to implement GPS support in Cartographer ROS, but more general.

[slamer-run]

@wohe thanks for your reply!

There is no weight given to the orientation for this reason.

Which line in the code do you talk about? In the option at pose_graph.lua, there is a parameter of "fixed_frame_pose_rotation_weight". What is more, it seems that orientation from FixedFrameis also optimized, even if it is not set from NavsatFix.

[wohe]

Sorry, my mistake. There is no public example yet it seems. The parameter you found is the one I was talking about: you have to set the weight to zero in the configuration, by adding a line like this:

POSE_GRAPH.optimization_problem.fixed_frame_pose_rotation_weight = 0 -- GPS provides no orientation.

That the orientation is optimized is correct, since libcartographer is supporting more use cases than just Cartographer ROS. Might make sense to check that this configuration is set to zero at Cartographer ROS. @cschuet What do you think?

[slamer-run]

@wohe

The parameter you found is the one I was talking about: you have to set the weight to zero in the configuration

That's what I expect from your answer!

That the orientation is optimized is correct, since libcartographer is supporting more use cases than just Cartographer ROS.

Actually, I have already tried using orientation of FixedFramePose from Multi-GPS input in the pose graph optimization. The optimization of orientation worked correctly!

[Fanny-one]

Hi @slamer-run,

That the orientation is optimized is correct, since libcartographer is supporting more use cases than just Cartographer ROS.

Actually, I have already tried using orientation of FixedFramePose from Multi-GPS input in the pose graph optimization. The optimization of orientation worked correctly!

Now I'm planning to employ GPS/INS integrated navigation in the pose graph optimization. But I don't know how to realize it. I have get GPS localization by nmea_navsat_driver and is it correct ? Hope for your reply. Thank you very much!

[chenxianbo]

@wohe I derived optimazition formula for GPS translation, but I find the implementation in cartographer is not agree with the true. ！image