Add gyroscope constrain into pose graph

[Nhat Nguyen Duy]

Hi everyone,
Currently, I am researching about the pose graph optimization (gtsam). I want to modify the loss function to add gyroscope into pose graph to limit the roll and pitch angles of vertices in pose graph. The gyroscope only has roll, pitch, yaw information.  Could anyone help me check my code? (below)
After I apply the GyroFactor to pose graph, the vertex still has the large angle although the roll-pitch -yaw of gyro is 0-0-0 (degree).
Thank you very much. hope to get feedback from everyone

All the best,
Duy Nhat

[Dellaert, Frank]

I did not at your code but note there is an AHRS factor that is designed to work with gyroscopes 

Best!

Frank

---

From: gtsam...@googlegroups.com <gtsam...@googlegroups.com> on behalf of Nhat Nguyen Duy <duynhat...@gmail.com>
Sent: Tuesday, June 20, 2023 12:30:07 AM
To: gtsam users <gtsam...@googlegroups.com>
Subject: [GTSAM] Add gyroscope constrain into pose graph

 

--
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/c4a81a98-0c3e-408c-b546-7ffdb22ac1bbn%40googlegroups.com.

[Nhat Nguyen Duy]

Hi Frank,
I am conducting an experiment involving a simple pose graph consisting of 6 nodes and 5 edges. Among these vertices, I have created 5 that align with the gravity direction, while one vertex does not align with the gravity direction (refer to image 1). My objective is to define a factor that utilizes gyroscope information for optimization. I expect that after optimization, all the vertices will align with the gravity direction, as depicted in image 2. To achieve this, I intend to modify the loss function to incorporate the gyroscope data and impose limits on the roll and pitch angles of the vertices in the pose graph. and this is my loss function:
--------------------------------------------------------------------------------------------------------------------------------
class Gyrofactor : public NoiseModelFactorN<Pose3> {
    double mx_, my_, mz_;  // roll, pitch, yaw of gyro

   public:
    using NoiseModelFactor1<Pose3>::evaluateError;

    typedef std::shared_ptr<Gyrofactor> shared_ptr;

    Gyrofactor(Key j, double x, double y, double z, const SharedNoiseModel& model) : NoiseModelFactorN<Pose3>(model, j), mx_(x), my_(y), mz_(z) {}

    ~Gyrofactor() override {}
    Vector evaluateError(const Pose3& q, OptionalMatrixType H) const override {
        Rot3 rotation = q.rotation(H);
        Vector3 rpy = rotation.rpy();
        double roll = rpy[0];
        double pitch = rpy[1];

        return (Vector(3) << roll - mx_, pitch - my_, 0).finished(); 
    }
    gtsam::NonlinearFactor::shared_ptr clone() const override { return std::static_pointer_cast<gtsam::NonlinearFactor>(gtsam::NonlinearFactor::shared_ptr(new Gyrofactor(*this))); }
};
--------------------------------------------------------------------------------------------------------------------------------

When I use this function to test in the complicated pose graph, all the vertices align with the gravity direction but the its translations change a lot. I think my function have a problem.  Could you help me ? 
Thank you very much. hope to get feedback from you

Vào lúc 01:47:27 UTC+7 ngày Thứ Tư, 21 tháng 6, 2023, Dellaert, Frank đã viết:

[Russell Buchanan]

I think the problem is that you are trying to solve Pose3 (Position and Orientation) states using only orientation data. This leaves the translation component completely unconstrained. As Frank suggested, you likely don't need to define your own factor as you are doing here and you can instead use the AHRSFactor built into GTSAM. Additionally, you should find a way to either constrain the Position or only estimate orientation.

Best