Skip to content

Commit d828627

Browse files
AhmedBaza1AhmedBaza1
committed
Add factorLidarMap2PoseInterpolation
1 parent fd93747 commit d828627

4 files changed

Lines changed: 208 additions & 0 deletions

File tree

‎src/timeCont/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,6 +7,7 @@ SET(factors_headers
77
mrob/factors/factor1Pose3dVel.hpp
88
mrob/factors/nodePose3dVelTc.hpp
99
mrob/factors/node3d.hpp
10+
mrob/factors/factorLidarMap2PoseInterpolation.hpp
1011
)
1112

1213
SET(factors_sources
@@ -15,6 +16,7 @@ SET(factors_sources
1516
factors/nodePose3dVelTc.cpp
1617
factors/factor1Pose3dVel.cpp
1718
factors/node3d.cpp
19+
factors/factorLidarMap2PoseInterpolation.cpp
1820
)
1921

2022
# create library

‎src/timeCont/factors/factor2poseInertialGravityBais.hpp‎

Whitespace-only changes.

‎src/timeCont/factors/factorLidarMap2PoseInterpolation.cpp‎

Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
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+
* factorLidarMap2PoseInterpolation.cpp
17+
*
18+
* Created on: Jul 17, 2025
19+
* Author: Ahmed Baza
20+
Ahmed.Baza@skoltech.ru
21+
* Mobile Robotics Lab, Skoltech
22+
*/
23+
24+
#include "mrob/factors/factorLidarMap2PoseInterpolation.hpp"
25+
#include <iostream>
26+
using namespace mrob;
27+
28+
FactorLidarMap2PoseInterpolation::FactorLidarMap2PoseInterpolation(
29+
const Mat51 &observation,
30+
const Mat31 &map_point,
31+
std::shared_ptr<Node> &nodeOrigin,
32+
std::shared_ptr<Node> &nodeTarget,
33+
const Mat3 &obsInf,
34+
Factor::robustFactorType robust_type):
35+
36+
Factor(3,9,robust_type), obs_(observation), W_(obsInf), map_point_(map_point)
37+
{
38+
if (nodeOrigin->get_id() < nodeTarget->get_id())
39+
{
40+
neighbourNodes_.push_back(nodeOrigin);
41+
neighbourNodes_.push_back(nodeTarget);
42+
}
43+
else
44+
{
45+
neighbourNodes_.push_back(nodeTarget);
46+
neighbourNodes_.push_back(nodeOrigin);
47+
}
48+
49+
}
50+
51+
52+
53+
54+
Mat61 FactorLidarMap2PoseInterpolation::compute_delta(SE3 Ta, SE3 Tb, matData_t time)
55+
{
56+
Mat61 xi_delta = (Tb*Ta.inv()).ln_vee() *time;
57+
return xi_delta;
58+
}
59+
void FactorLidarMap2PoseInterpolation::interpolate_pose(const SE3tc &T_origin, const SE3tc &T_target,const matData_t &t_obs)
60+
{
61+
matData_t t1 = T_origin.time();
62+
matData_t t2 = T_target.time();
63+
matData_t taw = (t2 - t_obs)/(t2-t1);
64+
SE3 T_org = SE3(T_origin.T().block<4,4>(0,0));
65+
SE3 T_tar = SE3(T_target.T().block<4,4>(0,0));
66+
Mat61 xi_delta = compute_delta(T_org, T_tar, taw);
67+
T_taw_ = SE3(xi_delta)*T_org;
68+
69+
}
70+
void FactorLidarMap2PoseInterpolation::evaluate_residuals()
71+
{
72+
// From Origin we observe
73+
Mat5 state_origin;
74+
state_origin = get_neighbour_nodes()->at(0)->get_state();
75+
SE3tc Tx_origin = SE3tc(state_origin);
76+
Mat5 state_target = get_neighbour_nodes()->at(1)->get_state();
77+
SE3tc Tx_target = SE3tc(state_target);
78+
matData_t t_obs = obs_(4);
79+
interpolate_pose(Tx_origin, Tx_target, t_obs);
80+
r_ = T_taw_.transform(map_point_) - obs_.head(3);
81+
}
82+
83+
void FactorLidarMap2PoseInterpolation::evaluate_jacobians()
84+
{
85+
Mat5 state_origin;
86+
state_origin = get_neighbour_nodes()->at(0)->get_state();
87+
SE3tc Tx_origin = SE3tc(state_origin);
88+
Mat5 state_target = get_neighbour_nodes()->at(1)->get_state();
89+
SE3tc Tx_target = SE3tc(state_target);
90+
matData_t t_obs = obs_(4);
91+
interpolate_pose(Tx_origin, Tx_target, t_obs);
92+
matData_t t1 = Tx_origin.time();
93+
matData_t t2 = Tx_target.time();
94+
matData_t taw = (t2 - t_obs)/(t2-t1);
95+
Mat3 tmp = hat3(-T_taw_.transform(map_point_));
96+
Mat<3,6> r_T = Mat<3,6>::Zero();
97+
r_T.topLeftCorner<3,3>() = tmp;
98+
r_T.bottomRightCorner<3,3>() = Mat3::Identity();
99+
Mat<3,6> r_T2 = r_T* (taw* Mat6::Identity());
100+
J_ = Mat<3,9>::Zero();
101+
J_.block<3,6>(0,0) = r_T2;
102+
J_.block<3,3>(0,6) = Mat3::Zero();
103+
}
104+
105+
106+
void FactorLidarMap2PoseInterpolation::evaluate_chi2()
107+
{
108+
chi2_ = r_.transpose() * W_ * r_;
109+
}
110+
111+
void FactorLidarMap2PoseInterpolation::print() const
112+
{
113+
std::cout << "FactorLidarMap2PoseInterpolation" << std::endl;
114+
std::cout << "obs_: " << obs_.transpose() << std::endl;
115+
std::cout << "map_point_: " << map_point_.transpose() << std::endl;
116+
std::cout << "r_: " << r_.transpose() << std::endl;
117+
std::cout << "W_: " << W_.transpose() << std::endl;
118+
std::cout << "J_: " << J_ << std::endl;
119+
std::cout << "chi2_: " << chi2_ << std::endl;
120+
}

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

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
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+
* factorLidarMap2PoseInterpolation.hpp
17+
*
18+
* Created on: Jul 17, 2025
19+
* Author: Ahmed Baza
20+
Ahmed.Baza@skoltech.ru
21+
* Mobile Robotics Lab, Skoltech
22+
*/
23+
24+
#ifndef FACTORLIDARMAP2POSEINTERPOLATION_HPP_
25+
#define FACTORLIDARMAP2POSEINTERPOLATION_HPP_
26+
27+
28+
#include "mrob/matrix_base.hpp"
29+
#include "mrob/SE3tc.hpp"
30+
#include "mrob/SE3.hpp"
31+
#include "mrob/factor.hpp"
32+
33+
namespace mrob{
34+
35+
/**
36+
* factor lidar map 2 pose interpolation creates a high dimension interpolation between poses
37+
* and matches it with lidar observations.
38+
*/
39+
40+
class FactorLidarMap2PoseInterpolation : public Factor
41+
{
42+
public:
43+
FactorLidarMap2PoseInterpolation(const Mat51 &observation,const Mat31 &map_point, std::shared_ptr<Node> &nodeOrigin,
44+
std::shared_ptr<Node> &nodeTarget, const Mat3 &obsInf,
45+
Factor::robustFactorType robust_type = Factor::robustFactorType::QUADRATIC);
46+
~FactorLidarMap2PoseInterpolation() = default;
47+
/**
48+
* Jacobians are not evaluated, just the residuals
49+
*/
50+
void evaluate_residuals() override;
51+
/**
52+
* Evaluates residuals and Jacobians
53+
*/
54+
void evaluate_jacobians() override;
55+
void evaluate_chi2() override;
56+
Mat61 compute_delta(SE3 Ta, SE3 Tb, matData_t time);
57+
void print() const override;
58+
void interpolate_pose(const SE3tc &T_origin, const SE3tc &T_target,const matData_t &t_obs); // interpolate pose between two poses
59+
MatRefConst get_obs() const override {return obs_;}
60+
VectRefConst get_residual() const override {return r_;}
61+
MatRefConst get_information_matrix() const override {return W_;}
62+
MatRefConst get_jacobian(factor_id_t /*id */) const override {return J_;}
63+
64+
65+
protected:
66+
Mat51 obs_; // obs = [X, Y, Z, 0,t] lidar observation
67+
Mat31 r_; //residual [TP - Z(:3)]
68+
Mat3 W_;//inverse of observation covariance (information matrix)
69+
Mat<3,9> J_;//Joint Jacobian
70+
SE3 T_taw_; // interpolated pose
71+
Mat31 map_point_; // map point
72+
73+
74+
75+
public:
76+
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // as proposed by Eigen
77+
78+
};
79+
80+
81+
}
82+
83+
84+
85+
86+
#endif /* FACTOR2POSES3DTWISTMATCHING_HPP_ */

0 commit comments

Comments
 (0)