Skip to content

Commit df9bafc

Browse files
g-ferrerg-ferrer
committed
fix on cmake. Adding prior factor interial with gravity
1 parent f6b990d commit df9bafc

4 files changed

Lines changed: 194 additions & 1 deletion

File tree

‎external/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,5 @@
1+
add_subdirectory(Eigen)
2+
13
if (BUILD_TESTING)
24
add_subdirectory(Catch2)
35
endif()

‎src/timeCont/factors/factorPriorInertial.cpp‎

Lines changed: 97 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,97 @@
1+
/* Copyright (c) 2022, Gonzalo Ferrer
2+
*
3+
* Licensed under the Apache License, Version 2.0 (the "License");
4+
* you may not use this file except in compliance with the License.
5+
* You may obtain a copy of the License at
6+
*
7+
* http://www.apache.org/licenses/LICENSE-2.0
8+
*
9+
* Unless required by applicable law or agreed to in writing, software
10+
* distributed under the License is distributed on an "AS IS" BASIS,
11+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
* See the License for the specific language governing permissions and
13+
* limitations under the License.
14+
*
15+
*
16+
* FactorPriorInertial.cpp
17+
*
18+
* Created on: Jan 20, 2026
19+
* Author: Gonzalo Ferrer
20+
* g.ferrer@skoltech.ru
21+
* Mobile Robotics Lab, Skoltech
22+
*/
23+
24+
25+
#include "mrob/factors/factorPriorInertial.hpp"
26+
27+
#include <iostream>
28+
29+
using namespace mrob;
30+
31+
32+
FactorPriorInertial::FactorPriorInertial(const Mat5 &observationPoseTarget,
33+
const Mat91 &obsBias,
34+
std::shared_ptr<Node> &nodePoseTarget,
35+
std::shared_ptr<Node> &nodeBiasGyro,
36+
std::shared_ptr<Node> &nodeBiasAcc,
37+
std::shared_ptr<Node> &nodeGravity,
38+
const Mat<18,18> &obsInf,
39+
Factor::robustFactorType robust_type):
40+
Factor(18,18, robust_type), W_(obsInf), r_(Vect<18>::Zero()), J_(Mat<18,18>::Zero())
41+
{
42+
43+
obs_bias_gyro_ = obsBias.head(3);
44+
obs_bias_acc_= obsBias.segment<3>(3);
45+
obs_gravity_ = obsBias.tail(3);
46+
Tobs_target_inv_ = SE3tc(observationPoseTarget).inv();
47+
// Ordering here MUST be the same. TODO handle any order.
48+
// [0] Pose Target
49+
// [1] bias gyro
50+
// [2] bias acc
51+
// [3] gravity in the global frame
52+
neighbourNodes_.push_back(nodePoseTarget);
53+
neighbourNodes_.push_back(nodeBiasGyro);
54+
neighbourNodes_.push_back(nodeBiasAcc);
55+
neighbourNodes_.push_back(nodeGravity);
56+
}
57+
58+
void FactorPriorInertial::evaluate_residuals()
59+
{
60+
// Anchor residuals as r = x - obs
61+
// r = ln(X * Tobs^-1) = - ln(Tobs * x^-1)
62+
// NOTE Tobs is a global observation (reference identity)
63+
Mat5 x_target = get_neighbour_nodes()->at(0).get()->get_state();
64+
Tr_ = SE3tc(x_target) * Tobs_target_inv_;
65+
r_.head(9) << Tr_.Ln().head(9);
66+
67+
// manifold for linear terms
68+
Mat31 bias_gyro = get_neighbour_nodes()->at(1)->get_state();
69+
r_.segment<3>(9) << bias_gyro - obs_bias_gyro_;
70+
71+
Mat31 bias_acc = get_neighbour_nodes()->at(2)->get_state();
72+
r_.segment<3>(9+3) << bias_gyro - obs_bias_acc_;
73+
74+
Mat31 bias_grav = get_neighbour_nodes()->at(3)->get_state();
75+
r_.segment<3>(9+6) << bias_grav - obs_gravity_;
76+
}
77+
78+
void FactorPriorInertial::evaluate_jacobians()
79+
{
80+
J_ = Mat<18,18>::Identity();
81+
}
82+
83+
void FactorPriorInertial::evaluate_chi2()
84+
{
85+
chi2_ = 0.5 * r_.dot(W_ * r_);
86+
}
87+
88+
void FactorPriorInertial::print() const
89+
{
90+
std::cout << "Printing Factor: " << id_ << ", obs= \n"
91+
<< "\n Residuals= \n" << r_
92+
<< " \nand Information matrix\n" << W_
93+
<< "\n Calculated Jacobian = \n" << J_
94+
<< "\n Chi2 error = " << chi2_
95+
<< " and neighbour Nodes " << neighbourNodes_.size()
96+
<< std::endl;
97+
}

‎src/timeCont/factors/nodeGravity3d.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -39,7 +39,7 @@ NodeGravity3d::NodeGravity3d(const Mat31 &initial_x, Node::nodeMode mode) :
3939
void NodeGravity3d::update(VectRefConst &dx)
4040
{
4141
Mat31 dxf = dx;
42-
// remove non-tangent compoenent to the update as (I-gg) XXX this is done in the factor
42+
// remove non-tangent compoenent to the update as (I-gg) XXX this is NOT done in the factor
4343
state_ += dxf;
4444
regenerate();
4545
}

‎src/timeCont/mrob/factors/factorPriorInertial.hpp‎

Lines changed: 94 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,94 @@
1+
/* Copyright (c) 2022, Gonzalo Ferrer
2+
*
3+
* Licensed under the Apache License, Version 2.0 (the "License");
4+
* you may not use this file except in compliance with the License.
5+
* You may obtain a copy of the License at
6+
*
7+
* http://www.apache.org/licenses/LICENSE-2.0
8+
*
9+
* Unless required by applicable law or agreed to in writing, software
10+
* distributed under the License is distributed on an "AS IS" BASIS,
11+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
* See the License for the specific language governing permissions and
13+
* limitations under the License.
14+
*
15+
*
16+
* factorPriorInertial.hpp
17+
*
18+
* Created on: Jan 20, 2026
19+
* Author: Gonzalo Ferrer
20+
* g.ferrer@skoltech.ru
21+
* Mobile Robotics Lab, Skoltech
22+
*/
23+
24+
#ifndef FACTORPRIORINERTIAL_HPP_
25+
#define FACTORPRIORINERTIAL_HPP_
26+
27+
28+
#include "mrob/matrix_base.hpp"
29+
#include "mrob/SE3tc.hpp"
30+
#include "mrob/factor.hpp"
31+
32+
namespace mrob{
33+
34+
/**
35+
* factor prior intertial, includes nodes from:
36+
* - Current pose (SE3vel) dim9
37+
* - bias gyro dim3
38+
* - bias dim3
39+
* - gravity dim3
40+
*
41+
* The factor simply provides a prior distribution, give an mean and a precision matrix (from previous optim iteration)
42+
*/
43+
44+
class FactorPriorInertial : public Factor
45+
{
46+
public:
47+
FactorPriorInertial(const Mat5 &observationPoseTarget,
48+
const Mat91 &obsBias,
49+
std::shared_ptr<Node> &nodePoseTarget,
50+
std::shared_ptr<Node> &nodeBiasGyro,
51+
std::shared_ptr<Node> &nodeBiasAcc,
52+
std::shared_ptr<Node> &nodeGravity,
53+
const Mat<18,18> &obsInf,
54+
Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
55+
~FactorPriorInertial() = default;
56+
/**
57+
* Jacobians are not evaluated, just the residuals
58+
*/
59+
void evaluate_residuals() override;
60+
/**
61+
* Evaluates residuals and Jacobians
62+
*/
63+
void evaluate_jacobians() override;
64+
void evaluate_chi2() override;
65+
66+
void print() const override;
67+
68+
MatRefConst get_obs() const override {return obs_bias_gyro_;}
69+
VectRefConst get_residual() const override {return r_;}
70+
MatRefConst get_information_matrix() const override {return W_;}
71+
MatRefConst get_jacobian(factor_id_t /*id */) const override {return J_;}
72+
73+
protected:
74+
// The Jacobians' correspondant nodes are ordered on the vector<Node>
75+
Mat31 obs_bias_gyro_,obs_bias_acc_,obs_gravity_;
76+
Vect<18> r_;
77+
Mat<18,18> W_;//inverse of observation covariance (information matrix)
78+
Mat<18,18> J_;//Joint Jacobian
79+
double delta_t_;
80+
81+
SE3tc Tobs_target_inv_, Tr_;
82+
83+
public:
84+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen
85+
86+
};
87+
88+
89+
}
90+
91+
92+
93+
94+
#endif /* FactorPriorInertial_HPP_ */

0 commit comments

Comments
 (0)