diff --git a/CMakeLists.txt b/CMakeLists.txt
index b44f68d79c1ea47dd5b9cc8879b6b98ab44e7f6a..fef41cbba74fb13c7d02841c2ddbb876a0c28959 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -100,6 +100,7 @@ option(_WOLF_TRACE "Enable wolf tracing macro" ON)
# option(BUILD_EXAMPLES "Build examples" OFF)
set(BUILD_TESTS true)
+set(BUILD_EXAMPLES true)
# Does this has any other interest
# but for the examples ?
@@ -305,51 +306,51 @@ SET(HDRS_CAPTURE
include/base/capture/capture_velocity.h
include/base/capture/capture_wheel_joint_position.h
)
-SET(HDRS_CONSTRAINT
- include/base/constraint/constraint_block_absolute.h
- include/base/constraint/constraint_container.h
- include/base/constraint/constraint_corner_2D.h
- include/base/constraint/constraint_AHP.h
- include/base/constraint/constraint_epipolar.h
- include/base/constraint/constraint_IMU.h
- include/base/constraint/constraint_fix_bias.h
- include/base/constraint/constraint_GPS_2D.h
- include/base/constraint/constraint_GPS_pseudorange_3D.h
- include/base/constraint/constraint_GPS_pseudorange_2D.h
- include/base/constraint/constraint_odom_2D.h
- include/base/constraint/constraint_odom_2D_analytic.h
- include/base/constraint/constraint_odom_3D.h
- include/base/constraint/constraint_point_2D.h
- include/base/constraint/constraint_point_to_line_2D.h
- include/base/constraint/constraint_pose_2D.h
- include/base/constraint/constraint_pose_3D.h
- include/base/constraint/constraint_quaternion_absolute.h
- include/base/constraint/constraint_relative_2D_analytic.h
- include/base/constraint/constraint_autodiff_trifocal.h
- include/base/constraint/constraint_autodiff_distance_3D.h
- include/base/constraint/constraint_AHP.h
- include/base/constraint/constraint_block_absolute.h
- include/base/constraint/constraint_container.h
- include/base/constraint/constraint_corner_2D.h
- include/base/constraint/constraint_diff_drive.h
- include/base/constraint/constraint_epipolar.h
- include/base/constraint/constraint_IMU.h
- include/base/constraint/constraint_fix_bias.h
- include/base/constraint/constraint_GPS_2D.h
- include/base/constraint/constraint_GPS_pseudorange_3D.h
- include/base/constraint/constraint_GPS_pseudorange_2D.h
- include/base/constraint/constraint_odom_2D.h
- include/base/constraint/constraint_odom_2D_analytic.h
- include/base/constraint/constraint_odom_3D.h
- include/base/constraint/constraint_point_2D.h
- include/base/constraint/constraint_point_to_line_2D.h
- include/base/constraint/constraint_pose_2D.h
- include/base/constraint/constraint_pose_3D.h
- include/base/constraint/constraint_quaternion_absolute.h
- include/base/constraint/constraint_relative_2D_analytic.h
- include/base/constraint/constraint_analytic.h
- include/base/constraint/constraint_autodiff.h
- include/base/constraint/constraint_base.h
+SET(HDRS_FACTOR
+ include/base/factor/factor_block_absolute.h
+ include/base/factor/factor_container.h
+ include/base/factor/factor_corner_2D.h
+ include/base/factor/factor_AHP.h
+ include/base/factor/factor_epipolar.h
+ include/base/factor/factor_IMU.h
+ include/base/factor/factor_fix_bias.h
+ include/base/factor/factor_GPS_2D.h
+ include/base/factor/factor_GPS_pseudorange_3D.h
+ include/base/factor/factor_GPS_pseudorange_2D.h
+ include/base/factor/factor_odom_2D.h
+ include/base/factor/factor_odom_2D_analytic.h
+ include/base/factor/factor_odom_3D.h
+ include/base/factor/factor_point_2D.h
+ include/base/factor/factor_point_to_line_2D.h
+ include/base/factor/factor_pose_2D.h
+ include/base/factor/factor_pose_3D.h
+ include/base/factor/factor_quaternion_absolute.h
+ include/base/factor/factor_relative_2D_analytic.h
+ include/base/factor/factor_autodiff_trifocal.h
+ include/base/factor/factor_autodiff_distance_3D.h
+ include/base/factor/factor_AHP.h
+ include/base/factor/factor_block_absolute.h
+ include/base/factor/factor_container.h
+ include/base/factor/factor_corner_2D.h
+ include/base/factor/factor_diff_drive.h
+ include/base/factor/factor_epipolar.h
+ include/base/factor/factor_IMU.h
+ include/base/factor/factor_fix_bias.h
+ include/base/factor/factor_GPS_2D.h
+ include/base/factor/factor_GPS_pseudorange_3D.h
+ include/base/factor/factor_GPS_pseudorange_2D.h
+ include/base/factor/factor_odom_2D.h
+ include/base/factor/factor_odom_2D_analytic.h
+ include/base/factor/factor_odom_3D.h
+ include/base/factor/factor_point_2D.h
+ include/base/factor/factor_point_to_line_2D.h
+ include/base/factor/factor_pose_2D.h
+ include/base/factor/factor_pose_3D.h
+ include/base/factor/factor_quaternion_absolute.h
+ include/base/factor/factor_relative_2D_analytic.h
+ include/base/factor/factor_analytic.h
+ include/base/factor/factor_autodiff.h
+ include/base/factor/factor_base.h
)
SET(HDRS_FEATURE
include/base/feature/feature_corner_2D.h
@@ -441,9 +442,9 @@ SET(HDRS_CORE
include/base/capture/capture_buffer.h
include/base/capture/capture_pose.h
include/base/capture/capture_void.h
- include/base/constraint/constraint_analytic.h
- include/base/constraint/constraint_autodiff.h
- include/base/constraint/constraint_base.h
+ include/base/factor/factor_analytic.h
+ include/base/factor/factor_autodiff.h
+ include/base/factor/factor_base.h
include/base/processor/processor_factory.h
include/base/feature/feature_base.h
include/base/feature/feature_match.h
@@ -485,8 +486,8 @@ SET(SRCS_CORE
src/capture/capture_base.cpp
src/capture/capture_pose.cpp
src/capture/capture_void.cpp
- src/constraint/constraint_analytic.cpp
- src/constraint/constraint_base.cpp
+ src/factor/factor_analytic.cpp
+ src/factor/factor_base.cpp
src/feature/feature_base.cpp
src/feature/feature_pose.cpp
src/frame_base.cpp
@@ -708,7 +709,7 @@ ADD_LIBRARY(${PROJECT_NAME}
${SRCS_CORE}
${SRCS}
${SRCS_CAPTURE}
- ${SRCS_CONSTRAINT}
+ ${SRCS_FACTOR}
${SRCS_FEATURE}
${SRCS_LANDMARK}
${SRCS_PROCESSOR}
@@ -777,8 +778,8 @@ INSTALL(FILES ${HDRS_DTASSC}
DESTINATION include/iri-algorithms/wolf/base/association)
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/base/capture)
-INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/base/constraint)
+INSTALL(FILES ${HDRS_FACTOR}
+ DESTINATION include/iri-algorithms/wolf/base/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/base/feature)
INSTALL(FILES ${HDRS_SENSOR}
diff --git a/README.md b/README.md
index fb08051ca2f57199c0d91870e30db96ebb381b43..69055e1fda60d7623404761607c6bc5f531c7b80 100644
--- a/README.md
+++ b/README.md
@@ -37,15 +37,22 @@ Dependencies
! Please notice that we are detailing two installation procedures below. If you are familiar with `ROS` and more especially the [`catkin_tools`](https://catkin-tools.readthedocs.io/en/latest/index.html) package then you may jump directly to the 'Using the `catkin_tools` package' section.
+#### CMake
+Building tool used by Wolf and by some of its dependencies. In order to install *cmake* please follow the instructions at [cmake site](https://cmake.org/install/)
+
+#### Autoreconf
+
+ $ sudo apt install dh-autoreconf
+
#### Eigen
[Eigen](http://eigen.tuxfamily.org). Linear algebra, header library. Eigen 3.2 is also a depencency of ROS-Hydro. In case you don't have ROS in your machine, you can install Eigen by typing:
$ sudo apt-get install libeigen3-dev
-
+
#### Ceres (5 steps)
-[Ceres](http://www.ceres-solver.org/) is an optimization library. Currently, this dependency is optional, so the build procedure of Wolf skips part of compilation in case this dependency is not found on the system. **Installation** is desctibed at [Ceres site](http://www.ceres-solver.org/building.html). However we report here an alternative step by step procedure to install Ceres.
+[Ceres](http://www.ceres-solver.org/) is an optimization library. Currently, this dependency is optional, so the build procedure of Wolf skips part of compilation in case this dependency is not found on the system. **Installation** is described at [Ceres site](http://www.ceres-solver.org/building.html). However we report here an alternative step by step procedure to install Ceres.
**(1)** Skip this step if Cmake 2.8.0+ and Eigen3.0+ are already installed. Otherwise install them with *apt-get*.
@@ -74,6 +81,7 @@ libgflags.a will be installed at **/usr/local/lib**
- Build and install with:
+ $ cd glog
$ ./autogen.sh
$ ./configure --with-gflags=/usr/local/
$ make
@@ -81,7 +89,7 @@ libgflags.a will be installed at **/usr/local/lib**
libglog.so will be installed at **/usr/local/lib**
-- Tourbleshooting:
+- Troubleshooting:
* If ./autogen.sh fails with './autogen.sh: autoreconf: not found'
@@ -157,9 +165,11 @@ Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0
#### Optional: Vision Utils (Install only if you want to use IRI's vision utils)
+This library requires OpenCV. If it is not installed in your system or you are unsure, please follow the installation steps at https://gitlab.iri.upc.edu/mobile_robotics/vision_utils
+
**(1)** Git clone the source:
- $ git clone ssh://git@gitlab.iri.upc.edu:2202/asantamaria/vision_utils.git
+ $ git clone https://gitlab.iri.upc.edu/mobile_robotics/vision_utils.git
**(2)** Build and install:
@@ -180,7 +190,8 @@ Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0
**(2)** Build and install:
- $ cd laser_scan_utils/build
+ $ cd laser_scan_utils
+ $ mkdir build && cd build
$ cmake ..
$ make
$ sudo make install
@@ -193,7 +204,8 @@ Wolf uses spdlog macros. Right now Wolf is only compatible with spdlog version 0
**(2)** Build and install:
- $ cd raw_gps_utils/build
+ $ cd raw_gps_utils
+ $ mkdir build && cd build
$ cmake ..
$ make
$ sudo make install
@@ -203,9 +215,9 @@ Download and build
#### Wolf C++ Library
-**Download:**
+**Clone:**
- $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf.git
+ $ git clone https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/wolf.git
**Build:**
diff --git a/hello_wolf/CMakeLists.txt b/hello_wolf/CMakeLists.txt
index 96b4a4b48ba807ce96c9e2e639bb51093ad29b47..df4b2b136f5215ead121440939fedff4e428af32 100644
--- a/hello_wolf/CMakeLists.txt
+++ b/hello_wolf/CMakeLists.txt
@@ -4,8 +4,8 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
${CMAKE_CURRENT_SOURCE_DIR}/capture_range_bearing.h
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_bearing.h
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_range_bearing.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/factor_bearing.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/factor_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/feature_range_bearing.h
${CMAKE_CURRENT_SOURCE_DIR}/landmark_point_2D.h
${CMAKE_CURRENT_SOURCE_DIR}/processor_range_bearing.h
diff --git a/hello_wolf/constraint_bearing.h b/hello_wolf/factor_bearing.h
similarity index 75%
rename from hello_wolf/constraint_bearing.h
rename to hello_wolf/factor_bearing.h
index 7849f17d4a2dc3847e49f019cc6f538f859cf6db..5c958ba175327d9ae30061ec71605996561505f4 100644
--- a/hello_wolf/constraint_bearing.h
+++ b/hello_wolf/factor_bearing.h
@@ -1,37 +1,37 @@
/*
- * ConstraintBearing.h
+ * FactorBearing.h
*
* Created on: Nov 30, 2017
* Author: jsola
*/
-#ifndef HELLO_WOLF_CONSTRAINT_BEARING_H_
-#define HELLO_WOLF_CONSTRAINT_BEARING_H_
+#ifndef HELLO_WOLF_FACTOR_BEARING_H_
+#define HELLO_WOLF_FACTOR_BEARING_H_
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
namespace wolf
{
using namespace Eigen;
-class ConstraintBearing : public ConstraintAutodiff<ConstraintBearing, 1, 2, 1, 2>
+class FactorBearing : public FactorAutodiff<FactorBearing, 1, 2, 1, 2>
{
public:
- ConstraintBearing(const LandmarkBasePtr& _landmark_other_ptr,
+ FactorBearing(const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
+ bool _apply_loss_function, FactorStatus _status) :
+ FactorAutodiff<FactorBearing, 1, 2, 1, 2>("BEARING", nullptr, nullptr, nullptr,
_landmark_other_ptr, _processor_ptr,
_apply_loss_function, _status,
- getCapturePtr()->getFramePtr()->getPPtr(),
- getCapturePtr()->getFramePtr()->getOPtr(),
- _landmark_other_ptr->getPPtr())
+ getCapture()->getFrame()->getP(),
+ getCapture()->getFrame()->getO(),
+ _landmark_other_ptr->getP())
{
//
}
- virtual ~ConstraintBearing()
+ virtual ~FactorBearing()
{
//
}
@@ -52,7 +52,7 @@ namespace wolf
{
template<typename T>
-inline bool ConstraintBearing::operator ()(const T* const _p1, const T* const _o1,
+inline bool FactorBearing::operator ()(const T* const _p1, const T* const _o1,
const T* const _p2, T* _res) const
{
@@ -88,4 +88,4 @@ inline bool ConstraintBearing::operator ()(const T* const _p1, const T* const _o
} // namespace wolf
-#endif /* HELLO_WOLF_CONSTRAINT_BEARING_H_ */
+#endif /* HELLO_WOLF_FACTOR_BEARING_H_ */
diff --git a/hello_wolf/constraint_range_bearing.h b/hello_wolf/factor_range_bearing.h
similarity index 79%
rename from hello_wolf/constraint_range_bearing.h
rename to hello_wolf/factor_range_bearing.h
index cf7af72a4574f0006c68c582e85bb151b81f9bf0..1414d61a20252cd76402af416e4a484812d57c8a 100644
--- a/hello_wolf/constraint_range_bearing.h
+++ b/hello_wolf/factor_range_bearing.h
@@ -1,49 +1,49 @@
/**
- * \file constraint_range_bearing.h
+ * \file factor_range_bearing.h
*
* Created on: Dec 1, 2017
* \author: jsola
*/
-#ifndef HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
-#define HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_
+#ifndef HELLO_WOLF_FACTOR_RANGE_BEARING_H_
+#define HELLO_WOLF_FACTOR_RANGE_BEARING_H_
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(ConstraintRangeBearing);
+WOLF_PTR_TYPEDEFS(FactorRangeBearing);
using namespace Eigen;
-class ConstraintRangeBearing : public ConstraintAutodiff<ConstraintRangeBearing, 2, 2, 1, 2, 1, 2>
+class FactorRangeBearing : public FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>
{
public:
- ConstraintRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
+ FactorRangeBearing(const CaptureBasePtr& _capture_own, // own capture's pointer
const LandmarkBasePtr& _landmark_other_ptr, // other landmark's pointer
const ProcessorBasePtr& _processor_ptr, // processor having created this
bool _apply_loss_function, // apply loss function to residual?
- ConstraintStatus _status) : // active constraint?
- ConstraintAutodiff<ConstraintRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
- "RANGE BEARING", // constraint type enum (see wolf.h)
+ FactorStatus _status) : // active factor?
+ FactorAutodiff<FactorRangeBearing, 2, 2, 1, 2, 1, 2>( // sizes of: residual, rob pos, rob ori, sen pos, sen ori, lmk pos
+ "RANGE BEARING", // factor type enum (see wolf.h)
nullptr, // other frame's pointer
nullptr, // other capture's pointer
nullptr, // other feature's pointer
_landmark_other_ptr, // other landmark's pointer
_processor_ptr, // processor having created this
_apply_loss_function, // apply loss function to residual?
- _status, // active constraint?
- _capture_own->getFramePtr()->getPPtr(), // robot position
- _capture_own->getFramePtr()->getOPtr(), // robot orientation state block
- _capture_own->getSensorPtr()->getPPtr(), // sensor position state block
- _capture_own->getSensorPtr()->getOPtr(), // sensor orientation state block
- _landmark_other_ptr->getPPtr()) // landmark position state block
+ _status, // active factor?
+ _capture_own->getFrame()->getP(), // robot position
+ _capture_own->getFrame()->getO(), // robot orientation state block
+ _capture_own->getSensor()->getP(), // sensor position state block
+ _capture_own->getSensor()->getO(), // sensor orientation state block
+ _landmark_other_ptr->getP()) // landmark position state block
{
//
}
- virtual ~ConstraintRangeBearing()
+ virtual ~FactorRangeBearing()
{
//
}
@@ -66,7 +66,7 @@ namespace wolf
{
template<typename T>
-inline bool ConstraintRangeBearing::operator ()(const T* const _p_w_r, // robot position
+inline bool FactorRangeBearing::operator ()(const T* const _p_w_r, // robot position
const T* const _o_w_r, // robot orientation
const T* const _p_r_s, // sensor position
const T* const _o_r_s, // sensor orientation
@@ -132,4 +132,4 @@ inline bool ConstraintRangeBearing::operator ()(const T* const _p_w_r, // robot
}
} // namespace wolf
-#endif /* HELLO_WOLF_CONSTRAINT_RANGE_BEARING_H_ */
+#endif /* HELLO_WOLF_FACTOR_RANGE_BEARING_H_ */
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 646f1c2d6a6254b28748adbd5fe1933701857842..f7c8d1c0d158f13cc8065dd8a077b8bc7a0c49bf 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -20,7 +20,7 @@
#include "processor_range_bearing.h"
#include "capture_range_bearing.h"
#include "feature_range_bearing.h"
-#include "constraint_range_bearing.h"
+#include "factor_range_bearing.h"
#include "landmark_point_2D.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -141,11 +141,11 @@ int main()
// NOTE: SELF-CALIBRATION OF SENSOR ORIENTATION
// Uncomment this line below to achieve sensor self-calibration (of the orientation only, since the position is not observable)
- sensor_rb->getOPtr()->unfix();
+ sensor_rb->getO()->unfix();
// NOTE: SELF-CALIBRATION OF SENSOR POSITION
// The position is however not observable, and thus self-calibration would not work. You can try uncommenting it too.
- // sensor_rb->getPPtr()->unfix();
+ // sensor_rb->getP()->unfix();
// CONFIGURE ==========================================================
@@ -220,16 +220,16 @@ int main()
// PERTURB initial guess
WOLF_TRACE("======== PERTURB PROBLEM PRIORS =======")
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
for (auto sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+ for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
for (auto sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
- for (auto lmk : problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : problem->getMap()->getLandmarkList())
for (auto sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
@@ -244,10 +244,10 @@ int main()
// GET COVARIANCES of all states
WOLF_TRACE("======== COVARIANCES OF SOLVED PROBLEM =======")
ceres->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- for (auto kf : problem->getTrajectoryPtr()->getFrameList())
+ for (auto kf : problem->getTrajectory()->getFrameList())
if (kf->isKey())
WOLF_TRACE("KF", kf->id(), "_cov = \n", kf->getCovariance());
- for (auto lmk : problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : problem->getMap()->getLandmarkList())
WOLF_TRACE("L", lmk->id(), "_cov = \n", lmk->getCovariance());
std::cout << std::endl;
@@ -292,25 +292,25 @@ int main()
Intr [Sta] = [ ] // Static intrinsics, but no intrinsics anyway
pt2 RANGE BEARING // Processor 2: type Range and Bearing
Trajectory
- KF1 <-- c3 // KeyFrame 1, constrained by Constraint 3
+ KF1 <-- c3 // KeyFrame 1, constrained by Factor 3
Est, ts=0, x = ( -1.6e-13 9.4e-11 1.4e-10 ) // State is estimated; time stamp and state vector
sb: Est Est // State's pos and orient are estimated
C1 FIX -> S- [ <-- // Capture 1, type FIX or Absolute
f1 FIX <-- // Feature 1, type Fix
m = ( 0 0 0 ) // The absolute measurement for this frame is (0,0,0) --> origin
- c1 FIX --> A // Constraint 1, type FIX, it is Absolute
+ c1 FIX --> A // Factor 1, type FIX, it is Absolute
CM2 ODOM 2D -> S1 [Sta, Sta] <-- // Capture 2, type ODOM, from Sensor 1 (static extr and intr)
C5 RANGE BEARING -> S2 [Sta, Sta] <-- // Capture 5, type R+B, from Sensor 2 (static extr and intr)
f2 RANGE BEARING <-- // Feature 2, type R+B
m = ( 1 1.57 ) // The feature's measurement is 1m, 1.57rad
- c2 RANGE BEARING --> L1 // Constraint 2 against Landmark 1
+ c2 RANGE BEARING --> L1 // Factor 2 against Landmark 1
KF2 <-- c6
Est, ts=1, x = ( 1 2.5e-10 1.6e-10 )
sb: Est Est
CM3 ODOM 2D -> S1 [Sta, Sta] <--
f3 ODOM 2D <--
m = ( 1 0 0 )
- c3 ODOM 2D --> F1 // Constraint 3, type ODOM, against Frame 1
+ c3 ODOM 2D --> F1 // Factor 3, type ODOM, against Frame 1
C9 RANGE BEARING -> S2 [Sta, Sta] <--
f4 RANGE BEARING <--
m = ( 1.41 2.36 )
@@ -337,7 +337,7 @@ int main()
sb: Est Est
CM10 ODOM 2D -> S1 [Sta, Sta] <--
Map
- L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Constraints 2 and 4
+ L1 POINT 2D <-- c2 c4 // Landmark 1, constrained by Factors 2 and 4
Est, x = ( 1 2 ) // L4 state is estimated, state vector
sb: Est // L4 has 1 state block estimated
L2 POINT 2D <-- c5 c7
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index 670a61ea61917ef371a6b5a7f31687dcc7a18a0e..25d7fd4067c2401b46db2c247961b54a8213f71f 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -9,7 +9,7 @@
#include "capture_range_bearing.h"
#include "landmark_point_2D.h"
#include "feature_range_bearing.h"
-#include "constraint_range_bearing.h"
+#include "factor_range_bearing.h"
namespace wolf
{
@@ -17,7 +17,7 @@ namespace wolf
ProcessorRangeBearing::ProcessorRangeBearing(const SensorRangeBearingPtr _sensor_ptr, ProcessorParamsBasePtr _params) :
ProcessorBase("RANGE BEARING", _params)
{
- H_r_s = transform(_sensor_ptr->getPPtr()->getState(), _sensor_ptr->getOPtr()->getState());
+ H_r_s = transform(_sensor_ptr->getP()->getState(), _sensor_ptr->getO()->getState());
}
void ProcessorRangeBearing::process(CaptureBasePtr _capture)
@@ -64,15 +64,15 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
{
// known landmarks : recover landmark
lmk = std::static_pointer_cast<LandmarkPoint2D>(lmk_it->second);
- WOLF_TRACE("known lmk(", id, "): ", lmk->getPPtr()->getState().transpose());
+ WOLF_TRACE("known lmk(", id, "): ", lmk->getP()->getState().transpose());
}
else
{
// new landmark:
// - create landmark
lmk = std::make_shared<LandmarkPoint2D>(id, invObserve(range, bearing));
- WOLF_TRACE("new lmk(", id, "): ", lmk->getPPtr()->getState().transpose());
- getProblem()->getMapPtr()->addLandmark(lmk);
+ WOLF_TRACE("new lmk(", id, "): ", lmk->getP()->getState().transpose());
+ getProblem()->getMap()->addLandmark(lmk);
// - add to known landmarks
known_lmks.emplace(id, lmk);
}
@@ -80,17 +80,17 @@ void ProcessorRangeBearing::process(CaptureBasePtr _capture)
// 5. create feature
Vector2s rb(range,bearing);
auto ftr = std::make_shared<FeatureRangeBearing>(rb,
- getSensorPtr()->getNoiseCov());
+ getSensor()->getNoiseCov());
capture_rb->addFeature(ftr);
- // 6. create constraint
+ // 6. create factor
auto prc = shared_from_this();
- auto ctr = std::make_shared<ConstraintRangeBearing>(capture_rb,
+ auto ctr = std::make_shared<FactorRangeBearing>(capture_rb,
lmk,
prc,
false,
- CTR_ACTIVE);
- ftr->addConstraint(ctr);
+ FAC_ACTIVE);
+ ftr->addFactor(ctr);
lmk->addConstrainedBy(ctr);
}
@@ -141,7 +141,7 @@ Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s)
Eigen::Vector2s ProcessorRangeBearing::toSensor(const Eigen::Vector2s& lmk_w) const
{
-// Trf H_w_r = transform(getSensorPtr()->getPPtr()->getState(), getSensorPtr()->getOPtr()->getState());
+// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
Trf H_w_r = transform(getProblem()->getCurrentState());
return (H_w_r * H_r_s).inverse() * lmk_w;
}
diff --git a/include/base/association/association_node.h b/include/base/association/association_node.h
index 21a10d4801bc3aea1487c39e5357e08f86552700..c652ba6c48878fb43801cfc546733f7abc0790bb 100644
--- a/include/base/association/association_node.h
+++ b/include/base/association/association_node.h
@@ -102,7 +102,7 @@ class AssociationNode
* Returns a copy of up_node_ptr_
*
**/
- AssociationNode * upNodePtr() const;
+ AssociationNode * upNode() const;
/** \brief Computes node probability
*
diff --git a/include/base/capture/capture_base.h b/include/base/capture/capture_base.h
index 293ab9d0303aa493930ae11336b7abfe58d7d0f9..0082d9d314fa28cf9f26a64b0cb455ab07102244 100644
--- a/include/base/capture/capture_base.h
+++ b/include/base/capture/capture_base.h
@@ -21,8 +21,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
{
private:
FrameBaseWPtr frame_ptr_;
- FeatureBaseList feature_list_;
- ConstraintBaseList constrained_by_list_;
+ FeatureBasePtrList feature_list_;
+ FactorBasePtrList constrained_by_list_;
SensorBaseWPtr sensor_ptr_; ///< Pointer to sensor
// Deal with sensors with dynamic extrinsics (check dynamic_extrinsic_ in SensorBase)
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
@@ -56,35 +56,35 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
void setTimeStamp(const TimeStamp& _ts);
void setTimeStampToNow();
- FrameBasePtr getFramePtr() const;
- void setFramePtr(const FrameBasePtr _frm_ptr);
+ FrameBasePtr getFrame() const;
+ void setFrame(const FrameBasePtr _frm_ptr);
void unlinkFromFrame(){frame_ptr_.reset();}
virtual void setProblem(ProblemPtr _problem) final;
FeatureBasePtr addFeature(FeatureBasePtr _ft_ptr);
- FeatureBaseList& getFeatureList();
- void addFeatureList(FeatureBaseList& _new_ft_list);
+ FeatureBasePtrList& getFeatureList();
+ void addFeatureList(FeatureBasePtrList& _new_ft_list);
- void getConstraintList(ConstraintBaseList& _ctr_list);
+ void getFactorList(FactorBasePtrList& _fac_list);
- SensorBasePtr getSensorPtr() const;
- virtual void setSensorPtr(const SensorBasePtr sensor_ptr);
+ SensorBasePtr getSensor() const;
+ virtual void setSensor(const SensorBasePtr sensor_ptr);
// constrained by
- virtual ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- ConstraintBaseList& getConstrainedByList();
+ FactorBasePtrList& getConstrainedByList();
// State blocks
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
- StateBlockPtr getSensorPPtr() const;
- StateBlockPtr getSensorOPtr() const;
- StateBlockPtr getSensorIntrinsicPtr() const;
+ StateBlockPtr getSensorP() const;
+ StateBlockPtr getSensorO() const;
+ StateBlockPtr getSensorIntrinsic() const;
void removeStateBlocks();
virtual void registerNewStateBlocks();
@@ -143,24 +143,24 @@ inline std::vector<StateBlockPtr>& CaptureBase::getStateBlockVec()
return state_block_vec_;
}
-inline void CaptureBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void CaptureBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
{
state_block_vec_[_i] = _sb_ptr;
}
-inline StateBlockPtr CaptureBase::getSensorPPtr() const
+inline StateBlockPtr CaptureBase::getSensorP() const
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
-inline StateBlockPtr CaptureBase::getSensorOPtr() const
+inline StateBlockPtr CaptureBase::getSensorO() const
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
-inline StateBlockPtr CaptureBase::getSensorIntrinsicPtr() const
+inline StateBlockPtr CaptureBase::getSensorIntrinsic() const
{
- return getStateBlockPtr(2);
+ return getStateBlock(2);
}
inline unsigned int CaptureBase::id()
@@ -168,17 +168,17 @@ inline unsigned int CaptureBase::id()
return capture_id_;
}
-inline FrameBasePtr CaptureBase::getFramePtr() const
+inline FrameBasePtr CaptureBase::getFrame() const
{
return frame_ptr_.lock();
}
-inline void CaptureBase::setFramePtr(const FrameBasePtr _frm_ptr)
+inline void CaptureBase::setFrame(const FrameBasePtr _frm_ptr)
{
frame_ptr_ = _frm_ptr;
}
-inline FeatureBaseList& CaptureBase::getFeatureList()
+inline FeatureBasePtrList& CaptureBase::getFeatureList()
{
return feature_list_;
}
@@ -188,7 +188,7 @@ inline unsigned int CaptureBase::getHits() const
return constrained_by_list_.size();
}
-inline ConstraintBaseList& CaptureBase::getConstrainedByList()
+inline FactorBasePtrList& CaptureBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -198,12 +198,12 @@ inline TimeStamp CaptureBase::getTimeStamp() const
return time_stamp_;
}
-inline SensorBasePtr CaptureBase::getSensorPtr() const
+inline SensorBasePtr CaptureBase::getSensor() const
{
return sensor_ptr_.lock();
}
-inline void CaptureBase::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureBase::setSensor(const SensorBasePtr sensor_ptr)
{
sensor_ptr_ = sensor_ptr;
}
@@ -220,9 +220,9 @@ inline void CaptureBase::setTimeStampToNow()
inline bool CaptureBase::process()
{
- assert (getSensorPtr() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
+ assert (getSensor() != nullptr && "Attempting to process a capture with no associated sensor. Either set the capture's sensor or call sensor->process(capture) instead.");
- return getSensorPtr()->process(shared_from_this());
+ return getSensor()->process(shared_from_this());
}
diff --git a/include/base/capture/capture_laser_2D.h b/include/base/capture/capture_laser_2D.h
index 9abd6bf4b874a8970d71929355aed095e52c3838..96a434547b5e802376472f472be0aeb0e27af940 100644
--- a/include/base/capture/capture_laser_2D.h
+++ b/include/base/capture/capture_laser_2D.h
@@ -28,7 +28,7 @@ class CaptureLaser2D : public CaptureBase
laserscanutils::LaserScan& getScan();
- void setSensorPtr(const SensorBasePtr sensor_ptr);
+ void setSensor(const SensorBasePtr sensor_ptr);
private:
SensorLaser2DPtr laser_ptr_; //specific pointer to sensor laser 2D object
@@ -41,9 +41,9 @@ inline laserscanutils::LaserScan& CaptureLaser2D::getScan()
return scan_;
}
-inline void CaptureLaser2D::setSensorPtr(const SensorBasePtr sensor_ptr)
+inline void CaptureLaser2D::setSensor(const SensorBasePtr sensor_ptr)
{
- CaptureBase::setSensorPtr(sensor_ptr);
+ CaptureBase::setSensor(sensor_ptr);
laser_ptr_ = std::static_pointer_cast<SensorLaser2D>(sensor_ptr);
}
diff --git a/include/base/capture/capture_motion.h b/include/base/capture/capture_motion.h
index a93b627aa0120820128a9127f88a457b42152544..ed71171e99ef6d221777f1ab89a7e0ba4a8dc0d7 100644
--- a/include/base/capture/capture_motion.h
+++ b/include/base/capture/capture_motion.h
@@ -96,8 +96,8 @@ class CaptureMotion : public CaptureBase
virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error);
// Origin frame
- FrameBasePtr getOriginFramePtr();
- void setOriginFramePtr(FrameBasePtr _frame_ptr);
+ FrameBasePtr getOriginFrame();
+ void setOriginFrame(FrameBasePtr _frame_ptr);
// member data:
private:
@@ -156,12 +156,12 @@ inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const
return _delta + _delta_error;
}
-inline FrameBasePtr CaptureMotion::getOriginFramePtr()
+inline FrameBasePtr CaptureMotion::getOriginFrame()
{
return origin_frame_ptr_.lock();
}
-inline void CaptureMotion::setOriginFramePtr(FrameBasePtr _frame_ptr)
+inline void CaptureMotion::setOriginFrame(FrameBasePtr _frame_ptr)
{
origin_frame_ptr_ = _frame_ptr;
}
diff --git a/include/base/capture/capture_pose.h b/include/base/capture/capture_pose.h
index 51fb833e43a8f525fefa559be72e99fa7906549c..e96bda8643dcafabc0b1d8ff003708c3ad5cdd31 100644
--- a/include/base/capture/capture_pose.h
+++ b/include/base/capture/capture_pose.h
@@ -3,8 +3,8 @@
//Wolf includes
#include "base/capture/capture_base.h"
-#include "base/constraint/constraint_pose_2D.h"
-#include "base/constraint/constraint_pose_3D.h"
+#include "base/factor/factor_pose_2D.h"
+#include "base/factor/factor_pose_3D.h"
#include "base/feature/feature_pose.h"
//std includes
@@ -27,7 +27,7 @@ class CapturePose : public CaptureBase
virtual ~CapturePose();
- virtual void emplaceFeatureAndConstraint();
+ virtual void emplaceFeatureAndFactor();
};
diff --git a/include/base/capture/capture_wheel_joint_position.h b/include/base/capture/capture_wheel_joint_position.h
index 0bade4029f499ba78b6cc71a7f260f05df44a79e..a90cdb205178dd4c36cd302a7793e7e74c668df1 100644
--- a/include/base/capture/capture_wheel_joint_position.h
+++ b/include/base/capture/capture_wheel_joint_position.h
@@ -155,7 +155,7 @@ protected:
// ~CaptureWheelJointPosition() = default;
-//// void setSensorPtr(const SensorBasePtr sensor_ptr) override;
+//// void setSensor(const SensorBasePtr sensor_ptr) override;
// std::size_t getNumWheels() const noexcept
// {
diff --git a/include/base/ceres_wrapper/ceres_manager.h b/include/base/ceres_wrapper/ceres_manager.h
index 5eb3436f55bf2ce49f814cd6e26302b0f90a9796..f937332f15aec0235ff3b9ada7254cffba47461d 100644
--- a/include/base/ceres_wrapper/ceres_manager.h
+++ b/include/base/ceres_wrapper/ceres_manager.h
@@ -28,8 +28,8 @@ class CeresManager : public SolverManager
{
protected:
- std::map<ConstraintBasePtr, ceres::ResidualBlockId> ctr_2_residual_idx_;
- std::map<ConstraintBasePtr, ceres::CostFunctionPtr> ctr_2_costfunction_;
+ std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
+ std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
std::map<StateBlockPtr, LocalParametrizationWrapperPtr> state_blocks_local_param_;
@@ -51,7 +51,7 @@ class CeresManager : public SolverManager
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks
= CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
- virtual void computeCovariances(const StateBlockList& st_list) override;
+ virtual void computeCovariances(const StateBlockPtrList& st_list) override;
virtual bool hasConverged() override;
@@ -69,9 +69,9 @@ class CeresManager : public SolverManager
std::string solveImpl(const ReportVerbosity report_level) override;
- void addConstraint(const ConstraintBasePtr& ctr_ptr) override;
+ void addFactor(const FactorBasePtr& fac_ptr) override;
- void removeConstraint(const ConstraintBasePtr& ctr_ptr) override;
+ void removeFactor(const FactorBasePtr& fac_ptr) override;
void addStateBlock(const StateBlockPtr& state_ptr) override;
@@ -81,7 +81,7 @@ class CeresManager : public SolverManager
void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr) override;
- ceres::CostFunctionPtr createCostFunction(const ConstraintBasePtr& _ctr_ptr);
+ ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
};
inline ceres::Solver::Summary CeresManager::getSummary()
diff --git a/include/base/ceres_wrapper/cost_function_wrapper.h b/include/base/ceres_wrapper/cost_function_wrapper.h
index 119839dee31898d2efcf06782e2c2c4b7df6d4f5..4a3f42dcbbc1aeea006e7843941707d4a2b9e5c5 100644
--- a/include/base/ceres_wrapper/cost_function_wrapper.h
+++ b/include/base/ceres_wrapper/cost_function_wrapper.h
@@ -3,7 +3,7 @@
// WOLF
#include "base/wolf.h"
-#include "base/constraint/constraint_analytic.h"
+#include "base/factor/factor_analytic.h"
// CERES
#include "ceres/cost_function.h"
@@ -19,25 +19,25 @@ class CostFunctionWrapper : public ceres::CostFunction
{
private:
- ConstraintBasePtr constraint_ptr_;
+ FactorBasePtr factor_ptr_;
public:
- CostFunctionWrapper(ConstraintBasePtr _constraint_ptr);
+ CostFunctionWrapper(FactorBasePtr _factor_ptr);
virtual ~CostFunctionWrapper();
virtual bool Evaluate(const double* const * parameters, double* residuals, double** jacobians) const;
- ConstraintBasePtr getConstraintPtr() const;
+ FactorBasePtr getFactor() const;
};
-inline CostFunctionWrapper::CostFunctionWrapper(ConstraintBasePtr _constraint_ptr) :
- ceres::CostFunction(), constraint_ptr_(_constraint_ptr)
+inline CostFunctionWrapper::CostFunctionWrapper(FactorBasePtr _factor_ptr) :
+ ceres::CostFunction(), factor_ptr_(_factor_ptr)
{
- for (auto st_block_size : constraint_ptr_->getStateSizes())
+ for (auto st_block_size : factor_ptr_->getStateSizes())
mutable_parameter_block_sizes()->push_back(st_block_size);
- set_num_residuals(constraint_ptr_->getSize());
+ set_num_residuals(factor_ptr_->getSize());
}
inline CostFunctionWrapper::~CostFunctionWrapper()
@@ -46,12 +46,12 @@ inline CostFunctionWrapper::~CostFunctionWrapper()
inline bool CostFunctionWrapper::Evaluate(const double* const * parameters, double* residuals, double** jacobians) const
{
- return constraint_ptr_->evaluate(parameters, residuals, jacobians);
+ return factor_ptr_->evaluate(parameters, residuals, jacobians);
}
-inline ConstraintBasePtr CostFunctionWrapper::getConstraintPtr() const
+inline FactorBasePtr CostFunctionWrapper::getFactor() const
{
- return constraint_ptr_;
+ return factor_ptr_;
}
} // namespace wolf
diff --git a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
index 8f8e9686b753c8912842b311cb9186010fad36f1..fafa62aae9d0fae3f0a26e4bc08816dc33b9e73b 100644
--- a/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
+++ b/include/base/ceres_wrapper/create_numeric_diff_cost_function.h
@@ -11,9 +11,9 @@
#include "ceres/cost_function.h"
#include "ceres/numeric_diff_cost_function.h"
-// Constraints
-#include "base/constraint/constraint_odom_2D.h"
-#include "base/constraint/constraint_base.h"
+// Factors
+#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_base.h"
namespace wolf {
@@ -21,28 +21,28 @@ namespace wolf {
template <class T>
std::shared_ptr<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(ConstraintBasePtr _constraint_ptr)
+ T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> > createNumericDiffCostFunctionCeres(FactorBasePtr _factor_ptr)
{
return std::make_shared<ceres::NumericDiffCostFunction<T, ceres::CENTRAL, T::residualSize,
T::block0Size,T::block1Size,T::block2Size,T::block3Size,T::block4Size,
- T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_constraint_ptr).get());
+ T::block5Size,T::block6Size,T::block7Size,T::block8Size,T::block9Size> >(std::static_pointer_cast<T>(_factor_ptr).get());
};
-inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(ConstraintBasePtr _ctr_ptr)
+inline std::shared_ptr<ceres::CostFunction> createNumericDiffCostFunction(FactorBasePtr _fac_ptr)
{
-// switch (_ctr_ptr->getTypeId())
+// switch (_fac_ptr->getTypeId())
// {
// // just for testing
-// case CTR_ODOM_2D:
-// return createNumericDiffCostFunctionCeres<ConstraintOdom2D>(_ctr_ptr);
+// case FAC_ODOM_2D:
+// return createNumericDiffCostFunctionCeres<FactorOdom2D>(_fac_ptr);
//
-// /* For adding a new constraint, add the #include and a case:
-// case CTR_ENUM:
-// return createNumericDiffCostFunctionCeres<ConstraintType>(_ctr_ptr);
+// /* For adding a new factor, add the #include and a case:
+// case FAC_ENUM:
+// return createNumericDiffCostFunctionCeres<FactorType>(_fac_ptr);
// */
//
// default:
- throw std::invalid_argument( "Unknown constraint type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
+ throw std::invalid_argument( "Unknown factor type! Please add it in the file: ceres_wrapper/create_Numeric_diff_cost_function.h" );
// }
}
diff --git a/include/base/ceres_wrapper/local_parametrization_wrapper.h b/include/base/ceres_wrapper/local_parametrization_wrapper.h
index 4ae4182cc525ee5224d74c8287a9b5b6a9189b6f..fc046a7ec002c29b702133e89fb950b456a764d1 100644
--- a/include/base/ceres_wrapper/local_parametrization_wrapper.h
+++ b/include/base/ceres_wrapper/local_parametrization_wrapper.h
@@ -31,7 +31,7 @@ class LocalParametrizationWrapper : public ceres::LocalParameterization
virtual int LocalSize() const;
- LocalParametrizationBasePtr getLocalParametrizationPtr() const;
+ LocalParametrizationBasePtr getLocalParametrization() const;
};
using LocalParametrizationWrapperPtr = std::shared_ptr<LocalParametrizationWrapper>;
@@ -57,7 +57,7 @@ inline int LocalParametrizationWrapper::LocalSize() const
return local_parametrization_ptr_->getLocalSize();
}
-inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrizationPtr() const
+inline LocalParametrizationBasePtr LocalParametrizationWrapper::getLocalParametrization() const
{
return local_parametrization_ptr_;
}
diff --git a/include/base/ceres_wrapper/qr_manager.h b/include/base/ceres_wrapper/qr_manager.h
index d09a5147b96a63bab51c72cbf69316e4326d0723..d4945e066a372d5ec95578a87552e4369eec1b2e 100644
--- a/include/base/ceres_wrapper/qr_manager.h
+++ b/include/base/ceres_wrapper/qr_manager.h
@@ -22,7 +22,7 @@ class QRManager : public SolverManager
Eigen::SparseMatrixs A_;
Eigen::VectorXs b_;
std::map<StateBlockPtr, unsigned int> sb_2_col_;
- std::map<ConstraintBasePtr, unsigned int> ctr_2_row_;
+ std::map<FactorBasePtr, unsigned int> fac_2_row_;
bool any_state_block_removed_;
unsigned int new_state_blocks_;
unsigned int N_batch_;
@@ -38,15 +38,15 @@ class QRManager : public SolverManager
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS);
- virtual void computeCovariances(const StateBlockList& _sb_list);
+ virtual void computeCovariances(const StateBlockPtrList& _sb_list);
private:
bool computeDecomposition();
- virtual void addConstraint(ConstraintBasePtr _ctr_ptr);
+ virtual void addFactor(FactorBasePtr _fac_ptr);
- virtual void removeConstraint(ConstraintBasePtr _ctr_ptr);
+ virtual void removeFactor(FactorBasePtr _fac_ptr);
virtual void addStateBlock(StateBlockPtr _st_ptr);
@@ -54,7 +54,7 @@ class QRManager : public SolverManager
virtual void updateStateBlockStatus(StateBlockPtr _st_ptr);
- void relinearizeConstraint(ConstraintBasePtr _ctr_ptr);
+ void relinearizeFactor(FactorBasePtr _fac_ptr);
};
} /* namespace wolf */
diff --git a/include/base/ceres_wrapper/solver_manager.h b/include/base/ceres_wrapper/solver_manager.h
index 6ffda3f7c32763c6896a7e5cb2e4c90f8bffce92..7252c409d2bf0c503e06b6fe74dec895e3754150 100644
--- a/include/base/ceres_wrapper/solver_manager.h
+++ b/include/base/ceres_wrapper/solver_manager.h
@@ -4,7 +4,7 @@
//wolf includes
#include "base/wolf.h"
#include "base/state_block.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
namespace wolf {
@@ -40,17 +40,17 @@ class SolverManager
virtual void computeCovariances(CovarianceBlocksToBeComputed _blocks = ROBOT_LANDMARKS) = 0;
- virtual void computeCovariances(const StateBlockList& st_list) = 0;
+ virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
virtual void update();
- virtual ProblemPtr getProblemPtr();
+ virtual ProblemPtr getProblem();
private:
- virtual void addConstraint(ConstraintBasePtr _ctr_ptr) = 0;
+ virtual void addFactor(FactorBasePtr _fac_ptr) = 0;
- virtual void removeConstraint(ConstraintBasePtr _ctr_ptr) = 0;
+ virtual void removeFactor(FactorBasePtr _fac_ptr) = 0;
virtual void addStateBlock(StateBlockPtr _st_ptr) = 0;
diff --git a/include/base/constraint/constraint_GPS_2D.h b/include/base/constraint/constraint_GPS_2D.h
deleted file mode 100644
index 2f85d06631f3d09b5ba587c442238ba70e545242..0000000000000000000000000000000000000000
--- a/include/base/constraint/constraint_GPS_2D.h
+++ /dev/null
@@ -1,41 +0,0 @@
-
-#ifndef CONSTRAINT_GPS_2D_H_
-#define CONSTRAINT_GPS_2D_H_
-
-//Wolf includes
-#include "base/wolf.h"
-#include "base/constraint/constraint_autodiff.h"
-#include "base/frame_base.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ConstraintGPS2D);
-
-class ConstraintGPS2D : public ConstraintAutodiff<ConstraintGPS2D, 2, 2>
-{
- public:
-
- ConstraintGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr())
- {
- //
- }
-
- virtual ~ConstraintGPS2D() = default;
-
- template<typename T>
- bool operator ()(const T* const _x, T* _residuals) const;
-
-};
-
-template<typename T>
-inline bool ConstraintGPS2D::operator ()(const T* const _x, T* _residuals) const
-{
- _residuals[0] = (T(getMeasurement()(0)) - _x[0]) / T(sqrt(getMeasurementCovariance()(0, 0)));
- _residuals[1] = (T(getMeasurement()(1)) - _x[1]) / T(sqrt(getMeasurementCovariance()(1, 1)));
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/constraint/constraint_autodiff_distance_3D.h b/include/base/constraint/constraint_autodiff_distance_3D.h
deleted file mode 100644
index 6e9452ff6686a1eb44d5f6d7ebddd3ad79e784d5..0000000000000000000000000000000000000000
--- a/include/base/constraint/constraint_autodiff_distance_3D.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/**
- * \file constraint_autodiff_distance_3D.h
- *
- * Created on: Apr 10, 2018
- * \author: jsola
- */
-
-#ifndef CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
-#define CONSTRAINT_AUTODIFF_DISTANCE_3D_H_
-
-#include "base/constraint/constraint_autodiff.h"
-
-namespace wolf
-{
-
-WOLF_PTR_TYPEDEFS(ConstraintAutodiffDistance3D);
-
-class ConstraintAutodiffDistance3D : public ConstraintAutodiff<ConstraintAutodiffDistance3D, 1, 3, 3>
-{
- public:
- ConstraintAutodiffDistance3D(const FeatureBasePtr& _feat,
- const FrameBasePtr& _frm_other,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status) :
- ConstraintAutodiff("DISTANCE 3D",
- _frm_other,
- nullptr,
- nullptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feat->getFramePtr()->getPPtr(),
- _frm_other->getPPtr())
- {
- setFeaturePtr(_feat);
- }
-
- virtual ~ConstraintAutodiffDistance3D() { /* nothing */ }
-
- template<typename T>
- bool operator () (const T* const _pos1,
- const T* const _pos2,
- T* _residual) const
- {
- using namespace Eigen;
-
- Map<const Matrix<T,3,1>> pos1(_pos1);
- Map<const Matrix<T,3,1>> pos2(_pos2);
- Map<Matrix<T,1,1>> res(_residual);
-
- Matrix<T,1,1> dist_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) );
- Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
- Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
-
- res = sqrt_info_upper * (dist_meas - dist_exp);
-
- return true;
- }
-};
-
-} /* namespace wolf */
-
-#endif /* CONSTRAINT_AUTODIFF_DISTANCE_3D_H_ */
diff --git a/include/base/constraint/constraint_corner_2D.h b/include/base/constraint/constraint_corner_2D.h
deleted file mode 100644
index 4692ad29a31a437492805b479a510a5e0e7383e9..0000000000000000000000000000000000000000
--- a/include/base/constraint/constraint_corner_2D.h
+++ /dev/null
@@ -1,114 +0,0 @@
-#ifndef CONSTRAINT_CORNER_2D_THETA_H_
-#define CONSTRAINT_CORNER_2D_THETA_H_
-
-//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
-#include "base/landmark/landmark_corner_2D.h"
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(ConstraintCorner2D);
-
-class ConstraintCorner2D: public ConstraintAutodiff<ConstraintCorner2D, 3,2,1,2,1>
-{
- public:
-
- ConstraintCorner2D(const FeatureBasePtr _ftr_ptr,
- const LandmarkCorner2DPtr _lmk_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintCorner2D,3,2,1,2,1>("CORNER 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr())
- {
- //
- }
-
- virtual ~ConstraintCorner2D() = default;
-
- LandmarkCorner2DPtr getLandmarkPtr()
- {
- return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
- }
-
- template <typename T>
- bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const;
-
- static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
- {
- return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
- }
-
-};
-
-template<typename T>
-inline bool ConstraintCorner2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
- const T* const _landmarkO, T* _residuals) const
-{
- // Mapping
- Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
- Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
- Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
-
- //std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl;
- //std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
-
- // sensor transformation
- Eigen::Matrix<T, 2, 1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
- // robot transformation
- Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
-
- // Expected measurement
- Eigen::Matrix<T, 2, 1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapturePtr()->getSensorPtr()->getOPtr()->getState()(0));
-
- // Error
- residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
- residuals_map(2) = expected_measurement_orientation - T(getMeasurement()(2));
-
- // pi 2 pi
- while (_residuals[2] > T(M_PI))
- _residuals[2] = _residuals[2] - T(2*M_PI);
- while (_residuals[2] <= T(-M_PI))
- _residuals[2] = _residuals[2] + T(2*M_PI);
-
- // Residuals
- residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
-
- //std::cout << "\nCONSTRAINT: " << id() << std::endl;
- //std::cout << "Feature: " << getFeaturePtr()->id() << std::endl;
- //std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
- //std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
- //
- //std::cout << "robot pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _robotP[i];
- //std::cout << "\n\t" << _robotO[0];
- //std::cout << std::endl;
- //
- //std::cout << "landmark pose:";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << _landmarkP[i];
- //std::cout << "\n\t" << _landmarkO[0];
- //std::cout << std::endl;
- //
- //std::cout << "expected_measurement: ";
- //for (int i=0; i < 2; i++)
- // std::cout << "\n\t" << expected_measurement_position.data()[i];
- //std::cout << std::endl;
- //
- //std::cout << "_residuals: "<< std::endl;
- //for (int i=0; i < 3; i++)
- // std::cout << "\n\t" << _residuals[i] << " ";
- //std::cout << std::endl;
- return true;
-}
-
-} // namespace wolf
-
-#endif
diff --git a/include/base/constraint/constraint_AHP.h b/include/base/factor/factor_AHP.h
similarity index 71%
rename from include/base/constraint/constraint_AHP.h
rename to include/base/factor/factor_AHP.h
index a0ab7db1fa497ac7f9a9f87db97ecb128650c306..a6448b06458658949bff014aa93da139f74f0e83 100644
--- a/include/base/constraint/constraint_AHP.h
+++ b/include/base/factor/factor_AHP.h
@@ -1,8 +1,8 @@
-#ifndef CONSTRAINT_AHP_H
-#define CONSTRAINT_AHP_H
+#ifndef FACTOR_AHP_H
+#define FACTOR_AHP_H
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/landmark/landmark_AHP.h"
#include "base/sensor/sensor_camera.h"
//#include "base/feature/feature_point_image.h"
@@ -12,10 +12,10 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintAHP);
+WOLF_PTR_TYPEDEFS(FactorAHP);
//class
-class ConstraintAHP : public ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>
+class FactorAHP : public FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>
{
protected:
Eigen::Vector3s anchor_sensor_extrinsics_p_;
@@ -25,13 +25,13 @@ class ConstraintAHP : public ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>
public:
- ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
+ FactorAHP(const FeatureBasePtr& _ftr_ptr,
const LandmarkAHPPtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~ConstraintAHP() = default;
+ virtual ~FactorAHP() = default;
template<typename T>
void expectation(const T* const _current_frame_p,
@@ -52,20 +52,20 @@ class ConstraintAHP : public ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>
T* _residuals) const;
// Static creator method
- static ConstraintAHPPtr create(const FeatureBasePtr& _ftr_ptr,
+ static FactorAHPPtr create(const FeatureBasePtr& _ftr_ptr,
const LandmarkAHPPtr& _lmk_ahp_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
};
-inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
+inline FactorAHP::FactorAHP(const FeatureBasePtr& _ftr_ptr,
const LandmarkAHPPtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintAHP, 2, 3, 4, 3, 4, 4>("AHP",
+ FactorStatus _status) :
+ FactorAutodiff<FactorAHP, 2, 3, 4, 3, 4, 4>("AHP",
_landmark_ptr->getAnchorFrame(),
nullptr,
nullptr,
@@ -73,30 +73,30 @@ inline ConstraintAHP::ConstraintAHP(const FeatureBasePtr& _ftr_ptr,
_processor_ptr,
_apply_loss_function,
_status,
- _ftr_ptr->getCapturePtr()->getFramePtr()->getPPtr(),
- _ftr_ptr->getCapturePtr()->getFramePtr()->getOPtr(),
- _landmark_ptr->getAnchorFrame()->getPPtr(),
- _landmark_ptr->getAnchorFrame()->getOPtr(),
- _landmark_ptr->getPPtr()),
- anchor_sensor_extrinsics_p_(_ftr_ptr->getCapturePtr()->getSensorPPtr()->getState()),
- anchor_sensor_extrinsics_o_(_ftr_ptr->getCapturePtr()->getSensorOPtr()->getState()),
- intrinsic_(_ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr()->getState())
+ _ftr_ptr->getCapture()->getFrame()->getP(),
+ _ftr_ptr->getCapture()->getFrame()->getO(),
+ _landmark_ptr->getAnchorFrame()->getP(),
+ _landmark_ptr->getAnchorFrame()->getO(),
+ _landmark_ptr->getP()),
+ anchor_sensor_extrinsics_p_(_ftr_ptr->getCapture()->getSensorP()->getState()),
+ anchor_sensor_extrinsics_o_(_ftr_ptr->getCapture()->getSensorO()->getState()),
+ intrinsic_(_ftr_ptr->getCapture()->getSensor()->getIntrinsic()->getState())
{
// obtain some intrinsics from provided sensor
- distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getDistortionVector();
+ distortion_ = (std::static_pointer_cast<SensorCamera>(_ftr_ptr->getCapture()->getSensor()))->getDistortionVector();
}
-inline Eigen::VectorXs ConstraintAHP::expectation() const
+inline Eigen::VectorXs FactorAHP::expectation() const
{
- FrameBasePtr frm_current = getFeaturePtr()->getCapturePtr()->getFramePtr();
- FrameBasePtr frm_anchor = getFrameOtherPtr();
- LandmarkBasePtr lmk = getLandmarkOtherPtr();
+ FrameBasePtr frm_current = getFeature()->getCapture()->getFrame();
+ FrameBasePtr frm_anchor = getFrameOther();
+ LandmarkBasePtr lmk = getLandmarkOther();
- const Eigen::MatrixXs frame_current_pos = frm_current->getPPtr()->getState();
- const Eigen::MatrixXs frame_current_ori = frm_current->getOPtr()->getState();
- const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getPPtr()->getState();
- const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getOPtr()->getState();
- const Eigen::MatrixXs lmk_pos_hmg = lmk ->getPPtr()->getState();
+ const Eigen::MatrixXs frame_current_pos = frm_current->getP()->getState();
+ const Eigen::MatrixXs frame_current_ori = frm_current->getO()->getState();
+ const Eigen::MatrixXs frame_anchor_pos = frm_anchor ->getP()->getState();
+ const Eigen::MatrixXs frame_anchor_ori = frm_anchor ->getO()->getState();
+ const Eigen::MatrixXs lmk_pos_hmg = lmk ->getP()->getState();
Eigen::Vector2s exp;
expectation(frame_current_pos.data(), frame_current_ori.data(), frame_anchor_pos.data(),
@@ -106,7 +106,7 @@ inline Eigen::VectorXs ConstraintAHP::expectation() const
}
template<typename T>
-inline void ConstraintAHP::expectation(const T* const _current_frame_p,
+inline void FactorAHP::expectation(const T* const _current_frame_p,
const T* const _current_frame_o,
const T* const _anchor_frame_p,
const T* const _anchor_frame_o,
@@ -136,9 +136,9 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
TransformType T_R0_C0 = t_r0_c0 * q_r0_c0;
// current robot to current camera transform
- CaptureBasePtr current_capture = this->getFeaturePtr()->getCapturePtr();
- Translation<T, 3> t_r1_c1 (current_capture->getSensorPPtr()->getState().cast<T>());
- Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorOPtr()->getState()));
+ CaptureBasePtr current_capture = this->getFeature()->getCapture();
+ Translation<T, 3> t_r1_c1 (current_capture->getSensorP()->getState().cast<T>());
+ Quaternions q_r1_c1_s(Eigen::Vector4s(current_capture->getSensorO()->getState()));
Quaternion<T> q_r1_c1 = q_r1_c1_s.cast<T>();
TransformType T_R1_C1 = t_r1_c1 * q_r1_c1;
@@ -163,7 +163,7 @@ inline void ConstraintAHP::expectation(const T* const _current_frame_p,
}
template<typename T>
-inline bool ConstraintAHP::operator ()(const T* const _current_frame_p,
+inline bool FactorAHP::operator ()(const T* const _current_frame_p,
const T* const _current_frame_o,
const T* const _anchor_frame_p,
const T* const _anchor_frame_o,
@@ -183,18 +183,18 @@ inline bool ConstraintAHP::operator ()(const T* const _current_frame_p,
return true;
}
-inline ConstraintAHPPtr ConstraintAHP::create(const FeatureBasePtr& _ftr_ptr,
+inline FactorAHPPtr FactorAHP::create(const FeatureBasePtr& _ftr_ptr,
const LandmarkAHPPtr& _lmk_ahp_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status)
+ FactorStatus _status)
{
- // construct constraint
- ConstraintAHPPtr ctr_ahp = std::make_shared<ConstraintAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
+ // construct factor
+ FactorAHPPtr fac_ahp = std::make_shared<FactorAHP>(_ftr_ptr, _lmk_ahp_ptr, _processor_ptr, _apply_loss_function, _status);
- return ctr_ahp;
+ return fac_ahp;
}
} // namespace wolf
-#endif // CONSTRAINT_AHP_H
+#endif // FACTOR_AHP_H
diff --git a/include/base/factor/factor_GPS_2D.h b/include/base/factor/factor_GPS_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..a3ce0e60979b6241fbf2d4f871543764430c8015
--- /dev/null
+++ b/include/base/factor/factor_GPS_2D.h
@@ -0,0 +1,41 @@
+
+#ifndef FACTOR_GPS_2D_H_
+#define FACTOR_GPS_2D_H_
+
+//Wolf includes
+#include "base/wolf.h"
+#include "base/factor/factor_autodiff.h"
+#include "base/frame_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorGPS2D);
+
+class FactorGPS2D : public FactorAutodiff<FactorGPS2D, 2, 2>
+{
+ public:
+
+ FactorGPS2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGPS2D, 2, 2>("GPS 2D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP())
+ {
+ //
+ }
+
+ virtual ~FactorGPS2D() = default;
+
+ template<typename T>
+ bool operator ()(const T* const _x, T* _residuals) const;
+
+};
+
+template<typename T>
+inline bool FactorGPS2D::operator ()(const T* const _x, T* _residuals) const
+{
+ _residuals[0] = (T(getMeasurement()(0)) - _x[0]) / T(sqrt(getMeasurementCovariance()(0, 0)));
+ _residuals[1] = (T(getMeasurement()(1)) - _x[1]) / T(sqrt(getMeasurementCovariance()(1, 1)));
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/base/constraint/constraint_GPS_pseudorange_2D.h b/include/base/factor/factor_GPS_pseudorange_2D.h
similarity index 85%
rename from include/base/constraint/constraint_GPS_pseudorange_2D.h
rename to include/base/factor/factor_GPS_pseudorange_2D.h
index d76e51c4d2eff9cbd09d8a7cb8edb3e2a89d98dc..82b186fc65e43f124bf1fd9e4f905b4d14928379 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_2D.h
+++ b/include/base/factor/factor_GPS_pseudorange_2D.h
@@ -1,12 +1,12 @@
-#ifndef CONSTRAINT_GPS_PSEUDORANGE_2D_H_
-#define CONSTRAINT_GPS_PSEUDORANGE_2D_H_
+#ifndef FACTOR_GPS_PSEUDORANGE_2D_H_
+#define FACTOR_GPS_PSEUDORANGE_2D_H_
#define LIGHT_SPEED_ 299792458
//Wolf includes
#include "base/sensor/sensor_GPS.h"
#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
//std
#include <string>
@@ -14,7 +14,7 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange2D);
+WOLF_PTR_TYPEDEFS(FactorGPSPseudorange2D);
/*
* NB:
@@ -24,13 +24,13 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange2D);
*
* TODO maybe is no more true
*/
-class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
+class FactorGPSPseudorange2D : public FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>
{
public:
- ConstraintGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
+ FactorGPSPseudorange2D(const FeatureBasePtr& _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGPSPseudorange2D, 1, 2, 1, 3, 1, 3, 1>("GPS PR 2D",
nullptr,
nullptr,
nullptr,
@@ -38,20 +38,20 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo
_pr_ptr,
_apply_loss_function,
_status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to the initial pos frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to the vehicle frame
+ _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to the initial pos frame
+ _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame
+ _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to the vehicle frame
// orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapPPtr()), // map position with respect to ecef
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getMapOPtr())) // map orientation with respect to ecef
+ _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapP()), // map position with respect to ecef
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor())->getMapO())) // map orientation with respect to ecef
{
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
- //std::cout << "ConstraintGPSPseudorange2D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
+ //std::cout << "FactorGPSPseudorange2D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
}
- virtual ~ConstraintGPSPseudorange2D() = default;
+ virtual ~FactorGPSPseudorange2D() = default;
template<typename T>
bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
@@ -75,7 +75,7 @@ class ConstraintGPSPseudorange2D : public ConstraintAutodiff<ConstraintGPSPseudo
*/
template<typename T>
-inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
+inline bool FactorGPSPseudorange2D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
const T* const _sensor_p, const T* const _bias,
const T* const _map_p, const T* const _map_o,
T* _residual) const
@@ -238,4 +238,4 @@ inline bool ConstraintGPSPseudorange2D::operator ()(const T* const _vehicle_p, c
} // namespace wolf
-#endif //CONSTRAINT_GPS_PSEUDORANGE_2D_H_
+#endif //FACTOR_GPS_PSEUDORANGE_2D_H_
diff --git a/include/base/constraint/constraint_GPS_pseudorange_3D.h b/include/base/factor/factor_GPS_pseudorange_3D.h
similarity index 73%
rename from include/base/constraint/constraint_GPS_pseudorange_3D.h
rename to include/base/factor/factor_GPS_pseudorange_3D.h
index c768855399cd89305563d10a600ff77149cd65f7..72e44d4f62f55358e33cf2ebb14254ca532f1bd3 100644
--- a/include/base/constraint/constraint_GPS_pseudorange_3D.h
+++ b/include/base/factor/factor_GPS_pseudorange_3D.h
@@ -1,17 +1,17 @@
-#ifndef CONSTRAINT_GPS_PSEUDORANGE_3D_H_
-#define CONSTRAINT_GPS_PSEUDORANGE_3D_H_
+#ifndef FACTOR_GPS_PSEUDORANGE_3D_H_
+#define FACTOR_GPS_PSEUDORANGE_3D_H_
#define LIGHT_SPEED 299792458
//Wolf includes
#include "base/sensor/sensor_GPS.h"
#include "base/feature/feature_GPS_pseudorange.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
namespace wolf {
// Set ClassPtr, ClassConstPtr and ClassWPtr typedefs;
-WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange3D);
+WOLF_PTR_TYPEDEFS(FactorGPSPseudorange3D);
/*
* NB:
@@ -21,31 +21,31 @@ WOLF_PTR_TYPEDEFS(ConstraintGPSPseudorange3D);
*
* TODO maybe is no more true
*/
-class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>
+class FactorGPSPseudorange3D: public FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>
{
public:
- ConstraintGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
+ FactorGPSPseudorange3D(FeatureBasePtr _ftr_ptr, const ProcessorBasePtr& _pr_ptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorGPSPseudorange3D, 1, 3, 4, 3, 1, 3, 4>("GPS PR 3D",
nullptr, nullptr, nullptr, nullptr, _pr_ptr, _apply_loss_function, _status,
- _ftr_ptr->getFramePtr()->getPPtr(), // position of the vehicle's frame with respect to map frame
- _ftr_ptr->getFramePtr()->getOPtr(), // orientation of the vehicle's frame wrt map frame
- _ftr_ptr->getCapturePtr()->getSensorPPtr(), // position of the sensor (gps antenna) with respect to base frame
+ _ftr_ptr->getFrame()->getP(), // position of the vehicle's frame with respect to map frame
+ _ftr_ptr->getFrame()->getO(), // orientation of the vehicle's frame wrt map frame
+ _ftr_ptr->getCapture()->getSensorP(), // position of the sensor (gps antenna) with respect to base frame
// orientation of antenna is not needed, because omnidirectional
- _ftr_ptr->getCapturePtr()->getSensorPtr()->getIntrinsicPtr(), //intrinsic parameter = receiver time bias
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapPPtr(), // initial vehicle position wrt ecef frame
- (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapturePtr()->getSensorPtr()))->getMapOPtr()) // initial vehicle orientation wrt ecef frame
+ _ftr_ptr->getCapture()->getSensor()->getIntrinsic(), //intrinsic parameter = receiver time bias
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapP(), // initial vehicle position wrt ecef frame
+ (std::static_pointer_cast<SensorGPS>(_ftr_ptr->getCapture()->getSensor()))->getMapO()) // initial vehicle orientation wrt ecef frame
{
sat_position_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getSatPosition();
pseudorange_ = (std::static_pointer_cast<FeatureGPSPseudorange>(_ftr_ptr))->getPseudorange();
- //std::cout << "ConstraintGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
+ //std::cout << "FactorGPSPseudorange3D() pr=" << pseudorange_ << "\tsat_pos=(" << sat_position_[0] << ", " << sat_position_[1] << ", " << sat_position_[2] << ")" << std::endl;
}
- virtual ~ConstraintGPSPseudorange3D() = default;
+ virtual ~FactorGPSPseudorange3D() = default;
template<typename T>
bool operator ()(const T* const _vehicle_p, const T* const _vehicle_o, const T* const _sensor_p,
@@ -59,10 +59,10 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
- static ConstraintBasePtr create(FeatureBasePtr _feature_ptr, //
+ static FactorBasePtr create(FeatureBasePtr _feature_ptr, //
NodeBasePtr _correspondant_ptr = nullptr)
{
- return std::make_shared<ConstraintGPSPseudorange3D>(_feature_ptr);
+ return std::make_shared<FactorGPSPseudorange3D>(_feature_ptr);
}
};
@@ -75,7 +75,7 @@ class ConstraintGPSPseudorange3D: public ConstraintAutodiff<ConstraintGPSPseudor
*/
template<typename T>
-inline bool ConstraintGPSPseudorange3D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
+inline bool FactorGPSPseudorange3D::operator ()(const T* const _vehicle_p, const T* const _vehicle_o,
const T* const _sensor_p, const T* const _bias,
const T* const _map_p, const T* const _map_o,
T* _residual) const
@@ -138,4 +138,4 @@ inline bool ConstraintGPSPseudorange3D::operator ()(const T* const _vehicle_p, c
} // namespace wolf
-#endif //CONSTRAINT_GPS_PSEUDORANGE_3D_H_
+#endif //FACTOR_GPS_PSEUDORANGE_3D_H_
diff --git a/include/base/constraint/constraint_IMU.h b/include/base/factor/factor_IMU.h
similarity index 83%
rename from include/base/constraint/constraint_IMU.h
rename to include/base/factor/factor_IMU.h
index 87dd49f0f6e7f5e35fe036e91704936e0f836daf..fbb29d1642e91d9fe90f55da673f3b8a87b6c335 100644
--- a/include/base/constraint/constraint_IMU.h
+++ b/include/base/factor/factor_IMU.h
@@ -1,29 +1,29 @@
-#ifndef CONSTRAINT_IMU_THETA_H_
-#define CONSTRAINT_IMU_THETA_H_
+#ifndef FACTOR_IMU_THETA_H_
+#define FACTOR_IMU_THETA_H_
//Wolf includes
#include "base/feature/feature_IMU.h"
#include "base/sensor/sensor_IMU.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/rotations.h"
//Eigen include
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintIMU);
+WOLF_PTR_TYPEDEFS(FactorIMU);
//class
-class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
+class FactorIMU : public FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>
{
public:
- ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
+ FactorIMU(const FeatureIMUPtr& _ftr_ptr,
const CaptureIMUPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~ConstraintIMU() = default;
+ virtual ~FactorIMU() = default;
/* \brief : compute the residual from the state blocks being iterated by the solver.
-> computes the expected measurement
@@ -140,28 +140,28 @@ class ConstraintIMU : public ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3
///////////////////// IMPLEMENTAITON ////////////////////////////
-inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
+inline FactorIMU::FactorIMU(const FeatureIMUPtr& _ftr_ptr,
const CaptureIMUPtr& _cap_origin_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
+ FactorStatus _status) :
+ FactorAutodiff<FactorIMU, 15, 3, 4, 3, 6, 3, 4, 3, 6>( // ...
"IMU",
- _cap_origin_ptr->getFramePtr(),
+ _cap_origin_ptr->getFrame(),
_cap_origin_ptr,
nullptr,
nullptr,
_processor_ptr,
_apply_loss_function,
_status,
- _cap_origin_ptr->getFramePtr()->getPPtr(),
- _cap_origin_ptr->getFramePtr()->getOPtr(),
- _cap_origin_ptr->getFramePtr()->getVPtr(),
- _cap_origin_ptr->getSensorIntrinsicPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr(),
- _ftr_ptr->getFramePtr()->getVPtr(),
- _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
+ _cap_origin_ptr->getFrame()->getP(),
+ _cap_origin_ptr->getFrame()->getO(),
+ _cap_origin_ptr->getFrame()->getV(),
+ _cap_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getCapture()->getSensorIntrinsic()),
dp_preint_(_ftr_ptr->getMeasurement().head<3>()), // dp, dv, dq at preintegration time
dq_preint_(_ftr_ptr->getMeasurement().data()+3),
dv_preint_(_ftr_ptr->getMeasurement().tail<3>()),
@@ -172,9 +172,9 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
dDp_dwb_(_ftr_ptr->getJacobianBias().block(0,3,3,3)),
dDv_dwb_(_ftr_ptr->getJacobianBias().block(6,3,3,3)),
dDq_dwb_(_ftr_ptr->getJacobianBias().block(3,3,3,3)),
- dt_(_ftr_ptr->getFramePtr()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
- ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getAbRateStdev()),
- wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapturePtr()->getSensorPtr())->getWbRateStdev()),
+ dt_(_ftr_ptr->getFrame()->getTimeStamp() - _cap_origin_ptr->getTimeStamp()),
+ ab_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getAbRateStdev()),
+ wb_rate_stdev_(std::static_pointer_cast<SensorIMU>(_ftr_ptr->getCapture()->getSensor())->getWbRateStdev()),
sqrt_A_r_dt_inv((Eigen::Matrix3s::Identity() * ab_rate_stdev_ * sqrt(dt_)).inverse()),
sqrt_W_r_dt_inv((Eigen::Matrix3s::Identity() * wb_rate_stdev_ * sqrt(dt_)).inverse())
{
@@ -182,7 +182,7 @@ inline ConstraintIMU::ConstraintIMU(const FeatureIMUPtr& _ftr_ptr,
}
template<typename T>
-inline bool ConstraintIMU::operator ()(const T* const _p1,
+inline bool FactorIMU::operator ()(const T* const _p1,
const T* const _q1,
const T* const _v1,
const T* const _b1,
@@ -215,7 +215,7 @@ inline bool ConstraintIMU::operator ()(const T* const _p1,
}
template<typename D1, typename D2, typename D3>
-inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
+inline bool FactorIMU::residual(const Eigen::MatrixBase<D1> & _p1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _v1,
const Eigen::MatrixBase<D1> & _ab1,
@@ -391,20 +391,20 @@ inline bool ConstraintIMU::residual(const Eigen::MatrixBase<D1> & _p1,
return true;
}
-inline Eigen::VectorXs ConstraintIMU::expectation() const
+inline Eigen::VectorXs FactorIMU::expectation() const
{
- FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
- FrameBasePtr frm_previous = getFrameOtherPtr();
+ FrameBasePtr frm_current = getFeature()->getFrame();
+ FrameBasePtr frm_previous = getFrameOther();
- //get information on current_frame in the ConstraintIMU
- const Eigen::Vector3s frame_current_pos = (frm_current->getPPtr()->getState());
- const Eigen::Quaternions frame_current_ori (frm_current->getOPtr()->getState().data());
- const Eigen::Vector3s frame_current_vel = (frm_current->getVPtr()->getState());
+ //get information on current_frame in the FactorIMU
+ const Eigen::Vector3s frame_current_pos = (frm_current->getP()->getState());
+ const Eigen::Quaternions frame_current_ori (frm_current->getO()->getState().data());
+ const Eigen::Vector3s frame_current_vel = (frm_current->getV()->getState());
- // get info on previous frame in the ConstraintIMU
- const Eigen::Vector3s frame_previous_pos = (frm_previous->getPPtr()->getState());
- const Eigen::Quaternions frame_previous_ori (frm_previous->getOPtr()->getState().data());
- const Eigen::Vector3s frame_previous_vel = (frm_previous->getVPtr()->getState());
+ // get info on previous frame in the FactorIMU
+ const Eigen::Vector3s frame_previous_pos = (frm_previous->getP()->getState());
+ const Eigen::Quaternions frame_previous_ori (frm_previous->getO()->getState().data());
+ const Eigen::Vector3s frame_previous_vel = (frm_previous->getV()->getState());
// Define results vector and Map bits over it
Eigen::Matrix<Scalar, 10, 1> exp;
@@ -421,7 +421,7 @@ inline Eigen::VectorXs ConstraintIMU::expectation() const
}
template<typename D1, typename D2, typename D3, typename D4>
-inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
+inline void FactorIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
const Eigen::QuaternionBase<D2> & _q1,
const Eigen::MatrixBase<D1> & _v1,
const Eigen::MatrixBase<D1> & _p2,
@@ -451,48 +451,48 @@ inline void ConstraintIMU::expectation(const Eigen::MatrixBase<D1> & _p1,
*
* With Method 1:
*
-[ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
[trace][10:58:16] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251480e-06, Termination: CONVERGENCE
[trace][10:58:16] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
-[ RUN ] Process_Constraint_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (53 ms)
+[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
[trace][10:58:16] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238969e+03, Final cost: 1.059256e-19, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
-[ RUN ] Process_Constraint_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (56 ms)
+[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
[trace][10:58:16] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769588e+03, Final cost: 3.767740e-19, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
-[----------] 3 tests from Process_Constraint_IMU (159 ms total)
+[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (50 ms)
+[----------] 3 tests from Process_Factor_IMU (159 ms total)
-[----------] 2 tests from Process_Constraint_IMU_ODO
-[ RUN ] Process_Constraint_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[----------] 2 tests from Process_Factor_IMU_ODO
+[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
[trace][10:58:16] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.867678e-22, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
[trace][10:58:16] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363681e+04, Final cost: 1.879880e-20, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
-[----------] 2 tests from Process_Constraint_IMU_ODO (120 ms total)
+[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (52 ms)
+[----------] 2 tests from Process_Factor_IMU_ODO (120 ms total)
*
* With Method 2:
*
-[ RUN ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
+[ RUN ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2
[trace][11:15:43] [gtest_IMU.cpp L488 : TestBody] Ceres Solver Report: Iterations: 3, Initial cost: 1.092909e+02, Final cost: 1.251479e-06, Termination: CONVERGENCE
[trace][11:15:43] [gtest_IMU.cpp L490 : TestBody] w * DT (rather be lower than 1.57 approx) = 0.5 1 1.5
-[ OK ] Process_Constraint_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
-[ RUN ] Process_Constraint_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
+[ OK ] Process_Factor_IMU.Var_B1_B2_Invar_P1_Q1_V1_P2_Q2_V2 (37 ms)
+[ RUN ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2
[trace][11:15:43] [gtest_IMU.cpp L564 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.238985e+03, Final cost: 1.058935e-19, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
-[ RUN ] Process_Constraint_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
+[ OK ] Process_Factor_IMU.Var_P1_Q1_V1_B1_B2_Invar_P2_Q2_V2 (48 ms)
+[ RUN ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2
[trace][11:15:43] [gtest_IMU.cpp L638 : TestBody] Ceres Solver Report: Iterations: 17, Initial cost: 4.769603e+03, Final cost: 3.762091e-19, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
-[----------] 3 tests from Process_Constraint_IMU (133 ms total)
+[ OK ] Process_Factor_IMU.Var_P1_Q1_B1_V2_B2_Invar_V1_P2_Q2 (47 ms)
+[----------] 3 tests from Process_Factor_IMU (133 ms total)
-[----------] 2 tests from Process_Constraint_IMU_ODO
-[ RUN ] Process_Constraint_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
+[----------] 2 tests from Process_Factor_IMU_ODO
+[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1
[trace][11:15:43] [gtest_IMU.cpp L711 : TestBody] Ceres Solver Report: Iterations: 19, Initial cost: 6.842446e+03, Final cost: 1.855814e-22, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
-[ RUN ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
+[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_V0_B0_P1_Q1_B1__Invar_V1 (68 ms)
+[ RUN ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0
[trace][11:15:43] [gtest_IMU.cpp L783 : TestBody] Ceres Solver Report: Iterations: 16, Initial cost: 1.363675e+04, Final cost: 1.880084e-20, Termination: CONVERGENCE
-[ OK ] Process_Constraint_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
-[----------] 2 tests from Process_Constraint_IMU_ODO (127 ms total)
+[ OK ] Process_Factor_IMU_ODO.Var_P0_Q0_B0_P1_Q1_V1_B1__Invar_V0 (59 ms)
+[----------] 2 tests from Process_Factor_IMU_ODO (127 ms total)
*
*/
diff --git a/include/base/constraint/constraint_analytic.h b/include/base/factor/factor_analytic.h
similarity index 88%
rename from include/base/constraint/constraint_analytic.h
rename to include/base/factor/factor_analytic.h
index 2e3d110b133925f4032370c971e4bd344d8a1ae5..14604bdb21ff2d0ed8d13b0d5fcf55055a637bf7 100644
--- a/include/base/constraint/constraint_analytic.h
+++ b/include/base/factor/factor_analytic.h
@@ -1,16 +1,16 @@
-#ifndef CONSTRAINT_ANALYTIC_H_
-#define CONSTRAINT_ANALYTIC_H_
+#ifndef FACTOR_ANALYTIC_H_
+#define FACTOR_ANALYTIC_H_
//Wolf includes
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include <Eigen/StdVector>
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintAnalytic);
+WOLF_PTR_TYPEDEFS(FactorAnalytic);
-class ConstraintAnalytic: public ConstraintBase
+class FactorAnalytic: public FactorBase
{
protected:
std::vector<StateBlockPtr> state_ptr_vector_;
@@ -18,14 +18,14 @@ class ConstraintAnalytic: public ConstraintBase
public:
- /** \brief Constructor of category CTR_ABSOLUTE
+ /** \brief Constructor of category FAC_ABSOLUTE
*
- * Constructor of category CTR_ABSOLUTE
+ * Constructor of category FAC_ABSOLUTE
*
**/
- ConstraintAnalytic(const std::string& _tp,
+ FactorAnalytic(const std::string& _tp,
bool _apply_loss_function,
- ConstraintStatus _status,
+ FactorStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr = nullptr,
StateBlockPtr _state2Ptr = nullptr,
@@ -37,14 +37,14 @@ class ConstraintAnalytic: public ConstraintBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr ) ;
- ConstraintAnalytic(const std::string& _tp,
+ FactorAnalytic(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status,
+ FactorStatus _status,
StateBlockPtr _state0Ptr,
StateBlockPtr _state1Ptr = nullptr,
StateBlockPtr _state2Ptr = nullptr,
@@ -56,11 +56,11 @@ class ConstraintAnalytic: public ConstraintBase
StateBlockPtr _state8Ptr = nullptr,
StateBlockPtr _state9Ptr = nullptr );
- virtual ~ConstraintAnalytic() = default;
+ virtual ~FactorAnalytic() = default;
/** \brief Returns a vector of pointers to the states
*
- * Returns a vector of pointers to the state in which this constraint depends
+ * Returns a vector of pointers to the state in which this factor depends
*
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override;
@@ -72,14 +72,14 @@ class ConstraintAnalytic: public ConstraintBase
**/
virtual std::vector<unsigned int> getStateSizes() const override;
- /** \brief Returns the constraint residual size
+ /** \brief Returns the factor residual size
*
- * Returns the constraint residual size
+ * Returns the factor residual size
*
**/
// virtual unsigned int getSize() const = 0;
- /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override
{
@@ -134,7 +134,7 @@ class ConstraintAnalytic: public ConstraintBase
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
- * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
diff --git a/include/base/constraint/constraint_autodiff.h b/include/base/factor/factor_autodiff.h
similarity index 81%
rename from include/base/constraint/constraint_autodiff.h
rename to include/base/factor/factor_autodiff.h
index e6a68703678e5dc9c3a3fcf8519d1867cbfea014..be35c772aacb9c7fce4e780d413fa0df675c1655 100644
--- a/include/base/constraint/constraint_autodiff.h
+++ b/include/base/factor/factor_autodiff.h
@@ -1,9 +1,9 @@
-#ifndef CONSTRAINT_AUTODIFF_H_
-#define CONSTRAINT_AUTODIFF_H_
+#ifndef FACTOR_AUTODIFF_H_
+#define FACTOR_AUTODIFF_H_
//Wolf includes
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/state_block.h"
// CERES
@@ -14,9 +14,9 @@
namespace wolf {
-//template class ConstraintAutodiff
-template <class CtrT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
-class ConstraintAutodiff : public ConstraintBase
+//template class FactorAutodiff
+template <class FacT, unsigned int RES, unsigned int B0, unsigned int B1 = 0, unsigned int B2 = 0, unsigned int B3 = 0, unsigned int B4 = 0, unsigned int B5 = 0, unsigned int B6 = 0, unsigned int B7 = 0, unsigned int B8 = 0, unsigned int B9 = 0>
+class FactorAutodiff : public FactorBase
{
public:
@@ -58,25 +58,25 @@ class ConstraintAutodiff : public ConstraintBase
public:
/** \brief Constructor valid for all categories (ABSOLUTE, FRAME, FEATURE, LANDMARK)
**/
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr,
- StateBlockPtr _state9Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr,
+ StateBlockPtr _state9Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -114,7 +114,7 @@ class ConstraintAutodiff : public ConstraintBase
(*jets_9_)[i] = WolfJet(0, last_jet_idx++);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -144,7 +144,7 @@ class ConstraintAutodiff : public ConstraintBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -165,7 +165,7 @@ class ConstraintAutodiff : public ConstraintBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -241,7 +241,7 @@ class ConstraintAutodiff : public ConstraintBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -272,7 +272,7 @@ class ConstraintAutodiff : public ConstraintBase
/** \brief Returns a vector of pointers to the states
*
- * Returns a vector of pointers to the state in which this constraint depends
+ * Returns a vector of pointers to the state in which this factor depends
*
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const
@@ -301,8 +301,8 @@ class ConstraintAutodiff : public ConstraintBase
////////////////// SPECIALIZATION 9 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
{
public:
@@ -343,24 +343,24 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr,
- StateBlockPtr _state8Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr,
+ StateBlockPtr _state8Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -396,7 +396,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -420,7 +420,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -440,7 +440,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -507,7 +507,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -553,8 +553,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public Constra
////////////////// SPECIALIZATION 8 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
{
public:
@@ -594,23 +594,23 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr,
- StateBlockPtr _state7Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr,
+ StateBlockPtr _state7Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -643,7 +643,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -666,7 +666,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -685,7 +685,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -749,7 +749,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -794,8 +794,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public Constrai
////////////////// SPECIALIZATION 7 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
{
public:
@@ -834,22 +834,22 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr,
- StateBlockPtr _state6Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr,
+ StateBlockPtr _state6Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -879,7 +879,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -901,7 +901,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -919,7 +919,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -980,7 +980,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1024,8 +1024,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public Constrain
////////////////// SPECIALIZATION 6 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
{
public:
@@ -1063,21 +1063,21 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr,
- StateBlockPtr _state5Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr,
+ StateBlockPtr _state5Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -1104,7 +1104,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -1125,7 +1125,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1142,7 +1142,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1200,7 +1200,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1239,8 +1239,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public Constraint
////////////////// SPECIALIZATION 5 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
{
public:
@@ -1276,20 +1276,20 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr,
- StateBlockPtr _state4Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr,
+ StateBlockPtr _state4Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -1313,7 +1313,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -1333,7 +1333,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1349,7 +1349,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1404,7 +1404,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1442,8 +1442,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public ConstraintB
////////////////// SPECIALIZATION 4 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1478,19 +1478,19 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr,
- StateBlockPtr _state3Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr,
+ StateBlockPtr _state3Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -1511,7 +1511,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -1530,7 +1530,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
parameters[3],
@@ -1545,7 +1545,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1597,7 +1597,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
jets_3_->data(),
@@ -1638,8 +1638,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public ConstraintBa
////////////////// SPECIALIZATION 3 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1673,18 +1673,18 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr,
- StateBlockPtr _state2Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr,
+ StateBlockPtr _state2Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr,_state2Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -1702,7 +1702,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -1720,7 +1720,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
parameters[2],
residuals);
@@ -1734,7 +1734,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
residuals_jets_->data());
@@ -1783,7 +1783,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
jets_2_->data(),
residuals_jets_->data());
@@ -1823,8 +1823,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public ConstraintBas
////////////////// SPECIALIZATION 2 BLOCKS ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -1857,17 +1857,17 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr,
- StateBlockPtr _state1Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr,
+ StateBlockPtr _state1Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr,_state1Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>),
@@ -1882,7 +1882,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete jets_1_;
@@ -1899,7 +1899,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
parameters[1],
residuals);
}
@@ -1912,7 +1912,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
residuals_jets_->data());
@@ -1958,7 +1958,7 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
jets_1_->data(),
residuals_jets_->data());
@@ -1997,8 +1997,8 @@ class ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0> : public ConstraintBase
////////////////// SPECIALIZATION 1 BLOCK ////////////////////////////////////////////////////////////////////////
-template <class CtrT,unsigned int RES,unsigned int B0>
-class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
+template <class FacT,unsigned int RES,unsigned int B0>
+class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
{
public:
@@ -2030,16 +2030,16 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
public:
- ConstraintAutodiff(const std::string& _tp,
- const FrameBasePtr& _frame_other_ptr,
- const CaptureBasePtr& _capture_other_ptr,
- const FeatureBasePtr& _feature_other_ptr,
- const LandmarkBasePtr& _landmark_other_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status,
- StateBlockPtr _state0Ptr) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorAutodiff(const std::string& _tp,
+ const FrameBasePtr& _frame_other_ptr,
+ const CaptureBasePtr& _capture_other_ptr,
+ const FeatureBasePtr& _feature_other_ptr,
+ const LandmarkBasePtr& _landmark_other_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status,
+ StateBlockPtr _state0Ptr) :
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptrs_({_state0Ptr}),
residuals_jets_(new std::array<WolfJet, RES>),
jets_0_(new std::array<WolfJet, B0>)
@@ -2051,7 +2051,7 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
state_ptrs_.resize(n_blocks);
}
- virtual ~ConstraintAutodiff()
+ virtual ~FactorAutodiff()
{
delete jets_0_;
delete residuals_jets_;
@@ -2067,7 +2067,7 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
// only residuals
if (jacobians == nullptr)
{
- (*static_cast<CtrT const*>(this))(parameters[0],
+ (*static_cast<FacT const*>(this))(parameters[0],
residuals);
}
// also compute jacobians
@@ -2079,7 +2079,7 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
updateJetsRealPart(param_vec);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
residuals_jets_->data());
// fill the residual array
@@ -2122,7 +2122,7 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
updateJetsRealPart(_states_ptr);
// call functor
- (*static_cast<CtrT const*>(this))(jets_0_->data(),
+ (*static_cast<FacT const*>(this))(jets_0_->data(),
residuals_jets_->data());
// fill the residual vector
@@ -2163,112 +2163,112 @@ class ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0> : public ConstraintBase
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// state_block_sizes_
// 10 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8,B9};
// 9 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7,B8};
// 8 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6,B7};
// 7 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5,B6};
// 6 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4,B5};
// 5 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3,B4};
// 4 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2,B3};
// 3 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1,B2};
// 2 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0,B1};
// 1 BLOCK
-template <class CtrT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
+template <class FacT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::state_block_sizes_ = {B0};
// jacobian_locations_
// 10 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7,
- B0+B1+B2+B3+B4+B5+B6+B7+B8};
-// 9 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6,
- B0+B1+B2+B3+B4+B5+B6+B7};
-// 8 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5,
- B0+B1+B2+B3+B4+B5+B6};
-// 7 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
- B0,
- B0+B1,
- B0+B1+B2,
- B0+B1+B2+B3,
- B0+B1+B2+B3+B4,
- B0+B1+B2+B3+B4+B5};
-// 6 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8,unsigned int B9>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,B9>::jacobian_locations_ = {0,
B0,
B0+B1,
B0+B1+B2,
B0+B1+B2+B3,
- B0+B1+B2+B3+B4};
-// 5 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7,
+ B0+B1+B2+B3+B4+B5+B6+B7+B8};
+// 9 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7,unsigned int B8>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0>::jacobian_locations_ = {0,
B0,
B0+B1,
B0+B1+B2,
- B0+B1+B2+B3};
-// 4 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6,
+ B0+B1+B2+B3+B4+B5+B6+B7};
+// 8 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6,unsigned int B7>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0>::jacobian_locations_ = {0,
B0,
B0+B1,
- B0+B1+B2};
-// 3 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5,
+ B0+B1+B2+B3+B4+B5+B6};
+// 7 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5,unsigned int B6>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0>::jacobian_locations_ = {0,
B0,
- B0+B1};
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4,
+ B0+B1+B2+B3+B4+B5};
+// 6 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4,unsigned int B5>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3,
+ B0+B1+B2+B3+B4};
+// 5 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3,unsigned int B4>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2,
+ B0+B1+B2+B3};
+// 4 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2,unsigned int B3>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1,
+ B0+B1+B2};
+// 3 BLOCKS
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1,unsigned int B2>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0,
+ B0+B1};
// 2 BLOCKS
-template <class CtrT,unsigned int RES,unsigned int B0,unsigned int B1>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
- B0};
+template <class FacT,unsigned int RES,unsigned int B0,unsigned int B1>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0,
+ B0};
// 1 BLOCK
-template <class CtrT,unsigned int RES,unsigned int B0>
-const std::vector<unsigned int> ConstraintAutodiff<CtrT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
+template <class FacT,unsigned int RES,unsigned int B0>
+const std::vector<unsigned int> FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0>::jacobian_locations_ = {0};
} // namespace wolf
diff --git a/include/base/factor/factor_autodiff_distance_3D.h b/include/base/factor/factor_autodiff_distance_3D.h
new file mode 100644
index 0000000000000000000000000000000000000000..77eb08e2ae049978c7f2978eee3106fcec66ba9c
--- /dev/null
+++ b/include/base/factor/factor_autodiff_distance_3D.h
@@ -0,0 +1,65 @@
+/**
+ * \file factor_autodiff_distance_3D.h
+ *
+ * Created on: Apr 10, 2018
+ * \author: jsola
+ */
+
+#ifndef FACTOR_AUTODIFF_DISTANCE_3D_H_
+#define FACTOR_AUTODIFF_DISTANCE_3D_H_
+
+#include "base/factor/factor_autodiff.h"
+
+namespace wolf
+{
+
+WOLF_PTR_TYPEDEFS(FactorAutodiffDistance3D);
+
+class FactorAutodiffDistance3D : public FactorAutodiff<FactorAutodiffDistance3D, 1, 3, 3>
+{
+ public:
+ FactorAutodiffDistance3D(const FeatureBasePtr& _feat,
+ const FrameBasePtr& _frm_other,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff("DISTANCE 3D",
+ _frm_other,
+ nullptr,
+ nullptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feat->getFrame()->getP(),
+ _frm_other->getP())
+ {
+ setFeature(_feat);
+ }
+
+ virtual ~FactorAutodiffDistance3D() { /* nothing */ }
+
+ template<typename T>
+ bool operator () (const T* const _pos1,
+ const T* const _pos2,
+ T* _residual) const
+ {
+ using namespace Eigen;
+
+ Map<const Matrix<T,3,1>> pos1(_pos1);
+ Map<const Matrix<T,3,1>> pos2(_pos2);
+ Map<Matrix<T,1,1>> res(_residual);
+
+ Matrix<T,1,1> dist_exp ( sqrt( ( pos2 - pos1 ).squaredNorm() ) );
+ Matrix<T,1,1> dist_meas (getMeasurement().cast<T>() );
+ Matrix<T,1,1> sqrt_info_upper = getMeasurementSquareRootInformationUpper().cast<T>();
+
+ res = sqrt_info_upper * (dist_meas - dist_exp);
+
+ return true;
+ }
+};
+
+} /* namespace wolf */
+
+#endif /* FACTOR_AUTODIFF_DISTANCE_3D_H_ */
diff --git a/include/base/constraint/constraint_autodiff_trifocal.h b/include/base/factor/factor_autodiff_trifocal.h
similarity index 63%
rename from include/base/constraint/constraint_autodiff_trifocal.h
rename to include/base/factor/factor_autodiff_trifocal.h
index e10c59649055b18b3cd4b2cca93f1add68f950e5..d7c2091fb2c547b1511d8f5985e66723a24fb247 100644
--- a/include/base/constraint/constraint_autodiff_trifocal.h
+++ b/include/base/factor/factor_autodiff_trifocal.h
@@ -1,9 +1,9 @@
-#ifndef _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
-#define _CONSTRAINT_AUTODIFF_TRIFOCAL_H_
+#ifndef _FACTOR_AUTODIFF_TRIFOCAL_H_
+#define _FACTOR_AUTODIFF_TRIFOCAL_H_
//Wolf includes
//#include "base/wolf.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/sensor/sensor_camera.h"
#include <common_class/trifocaltensor.h>
@@ -12,28 +12,28 @@
namespace wolf
{
-WOLF_PTR_TYPEDEFS(ConstraintAutodiffTrifocal);
+WOLF_PTR_TYPEDEFS(FactorAutodiffTrifocal);
using namespace Eigen;
-class ConstraintAutodiffTrifocal : public ConstraintAutodiff<ConstraintAutodiffTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
+class FactorAutodiffTrifocal : public FactorAutodiff<FactorAutodiffTrifocal, 3, 3, 4, 3, 4, 3, 4, 3, 4>
{
public:
/** \brief Class constructor
*/
- ConstraintAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status);
+ FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
+ const FeatureBasePtr& _feature_origin_ptr,
+ const FeatureBasePtr& _feature_last_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status);
/** \brief Class Destructor
*/
- virtual ~ConstraintAutodiffTrifocal();
+ virtual ~FactorAutodiffTrifocal();
- FeatureBasePtr getFeaturePrevPtr();
+ FeatureBasePtr getFeaturePrev();
const Vector3s& getPixelCanonicalLast() const
{
@@ -126,33 +126,33 @@ namespace wolf
using namespace Eigen;
// Constructor
-ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
- const FeatureBasePtr& _feature_prev_ptr,
- const FeatureBasePtr& _feature_origin_ptr,
- const FeatureBasePtr& _feature_last_ptr,
- const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function,
- ConstraintStatus _status) : ConstraintAutodiff( "TRIFOCAL PLP",
- nullptr,
- nullptr,
- _feature_origin_ptr,
- nullptr,
- _processor_ptr,
- _apply_loss_function,
- _status,
- _feature_prev_ptr->getFramePtr()->getPPtr(),
- _feature_prev_ptr->getFramePtr()->getOPtr(),
- _feature_origin_ptr->getFramePtr()->getPPtr(),
- _feature_origin_ptr->getFramePtr()->getOPtr(),
- _feature_last_ptr->getFramePtr()->getPPtr(),
- _feature_last_ptr->getFramePtr()->getOPtr(),
- _feature_last_ptr->getCapturePtr()->getSensorPPtr(),
- _feature_last_ptr->getCapturePtr()->getSensorOPtr() ),
- feature_prev_ptr_(_feature_prev_ptr),
- camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensorPtr())),
- sqrt_information_upper(Matrix3s::Zero())
+FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_prev_ptr,
+ const FeatureBasePtr& _feature_origin_ptr,
+ const FeatureBasePtr& _feature_last_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function,
+ FactorStatus _status) :
+ FactorAutodiff( "TRIFOCAL PLP",
+ nullptr,
+ nullptr,
+ _feature_origin_ptr,
+ nullptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _feature_prev_ptr->getFrame()->getP(),
+ _feature_prev_ptr->getFrame()->getO(),
+ _feature_origin_ptr->getFrame()->getP(),
+ _feature_origin_ptr->getFrame()->getO(),
+ _feature_last_ptr->getFrame()->getP(),
+ _feature_last_ptr->getFrame()->getO(),
+ _feature_last_ptr->getCapture()->getSensorP(),
+ _feature_last_ptr->getCapture()->getSensorO() ),
+ feature_prev_ptr_(_feature_prev_ptr),
+ camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
+ sqrt_information_upper(Matrix3s::Zero())
{
- setFeaturePtr(_feature_last_ptr);
+ setFeature(_feature_last_ptr);
Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
pixel_canonical_prev_ = K_inv * Vector3s(_feature_prev_ptr ->getMeasurement(0), _feature_prev_ptr ->getMeasurement(1), 1.0);
pixel_canonical_origin_ = K_inv * Vector3s(_feature_origin_ptr->getMeasurement(0), _feature_origin_ptr->getMeasurement(1), 1.0);
@@ -160,14 +160,14 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
Matrix<Scalar,3,2> J_m_u = K_inv.block(0,0,3,2);
// extract relevant states
- Vector3s wtr1 = _feature_prev_ptr ->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr1 = Quaternions(_feature_prev_ptr ->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s wtr2 = _feature_origin_ptr->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr2 = Quaternions(_feature_origin_ptr->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s wtr3 = _feature_last_ptr ->getFramePtr() ->getPPtr() ->getState();
- Quaternions wqr3 = Quaternions(_feature_last_ptr ->getFramePtr() ->getOPtr() ->getState().data() );
- Vector3s rtc = _feature_last_ptr ->getCapturePtr()->getSensorPPtr()->getState();
- Quaternions rqc = Quaternions(_feature_last_ptr ->getCapturePtr()->getSensorOPtr()->getState().data() );
+ Vector3s wtr1 = _feature_prev_ptr ->getFrame() ->getP() ->getState();
+ Quaternions wqr1 = Quaternions(_feature_prev_ptr ->getFrame() ->getO() ->getState().data() );
+ Vector3s wtr2 = _feature_origin_ptr->getFrame() ->getP() ->getState();
+ Quaternions wqr2 = Quaternions(_feature_origin_ptr->getFrame() ->getO() ->getState().data() );
+ Vector3s wtr3 = _feature_last_ptr ->getFrame() ->getP() ->getState();
+ Quaternions wqr3 = Quaternions(_feature_last_ptr ->getFrame() ->getO() ->getState().data() );
+ Vector3s rtc = _feature_last_ptr ->getCapture()->getSensorP()->getState();
+ Quaternions rqc = Quaternions(_feature_last_ptr ->getCapture()->getSensorO()->getState().data() );
// expectation // canonical units
vision_utils::TrifocalTensorBase<Scalar> tensor;
@@ -188,9 +188,9 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
Matrix<Scalar,3,2> J_e_u3 = J_e_m3 * J_m_u;
// Error covariances induced by each of the measurement covariance // canonical units
- Matrix3s Q1 = J_e_u1 * getFeaturePrevPtr() ->getMeasurementCovariance() * J_e_u1.transpose();
- Matrix3s Q2 = J_e_u2 * getFeatureOtherPtr()->getMeasurementCovariance() * J_e_u2.transpose();
- Matrix3s Q3 = J_e_u3 * getFeaturePtr() ->getMeasurementCovariance() * J_e_u3.transpose();
+ Matrix3s Q1 = J_e_u1 * getFeaturePrev() ->getMeasurementCovariance() * J_e_u1.transpose();
+ Matrix3s Q2 = J_e_u2 * getFeatureOther()->getMeasurementCovariance() * J_e_u2.transpose();
+ Matrix3s Q3 = J_e_u3 * getFeature() ->getMeasurementCovariance() * J_e_u3.transpose();
// Total error covariance // canonical units
Matrix3s Q = Q1 + Q2 + Q3;
@@ -201,29 +201,29 @@ ConstraintAutodiffTrifocal::ConstraintAutodiffTrifocal(
// Re-write info matrix (for debug only)
// Scalar pix_noise = 1.0;
- // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3s::Identity(); // one PLP (2D) and one epipolar (1D) constraint
+ // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3s::Identity(); // one PLP (2D) and one epipolar (1D) factor
}
// Destructor
-ConstraintAutodiffTrifocal::~ConstraintAutodiffTrifocal()
+FactorAutodiffTrifocal::~FactorAutodiffTrifocal()
{
}
-inline FeatureBasePtr ConstraintAutodiffTrifocal::getFeaturePrevPtr()
+inline FeatureBasePtr FactorAutodiffTrifocal::getFeaturePrev()
{
return feature_prev_ptr_.lock();
}
template<typename T>
-bool ConstraintAutodiffTrifocal::operator ()( const T* const _prev_pos,
- const T* const _prev_quat,
- const T* const _origin_pos,
- const T* const _origin_quat,
- const T* const _last_pos,
- const T* const _last_quat,
- const T* const _sen_pos,
- const T* const _sen_quat,
- T* _residuals) const
+bool FactorAutodiffTrifocal::operator ()( const T* const _prev_pos,
+ const T* const _prev_quat,
+ const T* const _origin_pos,
+ const T* const _origin_quat,
+ const T* const _last_pos,
+ const T* const _last_quat,
+ const T* const _sen_pos,
+ const T* const _sen_quat,
+ T* _residuals) const
{
// MAPS
@@ -245,16 +245,16 @@ bool ConstraintAutodiffTrifocal::operator ()( const T* const _prev_pos,
}
template<typename D1, typename D2, class T, typename D3>
-inline void ConstraintAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
- const QuaternionBase<D2>& _wqr1,
- const MatrixBase<D1>& _wtr2,
- const QuaternionBase<D2>& _wqr2,
- const MatrixBase<D1>& _wtr3,
- const QuaternionBase<D2>& _wqr3,
- const MatrixBase<D1>& _rtc,
- const QuaternionBase<D2>& _rqc,
- vision_utils::TrifocalTensorBase<T>& _tensor,
- MatrixBase<D3>& _c2Ec1) const
+inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
+ const QuaternionBase<D2>& _wqr1,
+ const MatrixBase<D1>& _wtr2,
+ const QuaternionBase<D2>& _wqr2,
+ const MatrixBase<D1>& _wtr3,
+ const QuaternionBase<D2>& _wqr3,
+ const MatrixBase<D1>& _rtc,
+ const QuaternionBase<D2>& _rqc,
+ vision_utils::TrifocalTensorBase<T>& _tensor,
+ MatrixBase<D3>& _c2Ec1) const
{
typedef Translation<T, 3> TranslationType;
@@ -301,12 +301,12 @@ inline void ConstraintAutodiffTrifocal::expectation(const MatrixBase<D1>& _w
*
* P1' * (T x (R*P2)) = 0
* P1' * [T]x * R * P2 = 0
- * P1' * c1Ec2 * P2 = 0 <--- Epipolar constraint
+ * P1' * c1Ec2 * P2 = 0 <--- Epipolar factor
*
* therefore:
* c1Ec2 = [T]x * R <--- Essential matrix
*
- * or, if we prefer the constraint P2' * c2Ec1 * P1 = 0,
+ * or, if we prefer the factor P2' * c2Ec1 * P1 = 0,
* c2Ec1 = c1Ec2' = R' * [T]x (we obviate the sign change)
*/
Matrix<T,3,3> Rtr = c1Hc2.matrix().block(0,0,3,3).transpose();
@@ -316,8 +316,8 @@ inline void ConstraintAutodiffTrifocal::expectation(const MatrixBase<D1>& _w
}
template<typename T, typename D1>
-inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1) const
+inline Matrix<T, 3, 1> FactorAutodiffTrifocal::residual(const vision_utils::TrifocalTensorBase<T>& _tensor,
+ const MatrixBase<D1>& _c2Ec1) const
{
// 1. COMMON COMPUTATIONS
@@ -346,11 +346,11 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::residual(const vision_utils::
// Helper functions to be used by the above
template<class T, typename D1, typename D2, typename D3, typename D4>
-inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
- const MatrixBase<D1>& _c2Ec1,
- MatrixBase<D2>& _J_e_m1,
- MatrixBase<D3>& _J_e_m2,
- MatrixBase<D4>& _J_e_m3)
+inline Matrix<T, 3, 1> FactorAutodiffTrifocal::error_jacobians(const vision_utils::TrifocalTensorBase<T>& _tensor,
+ const MatrixBase<D1>& _c2Ec1,
+ MatrixBase<D2>& _J_e_m1,
+ MatrixBase<D3>& _J_e_m2,
+ MatrixBase<D4>& _J_e_m3)
{
// 1. COMMON COMPUTATIONS
@@ -394,4 +394,4 @@ inline Matrix<T, 3, 1> ConstraintAutodiffTrifocal::error_jacobians(const vision_
} // namespace wolf
-#endif /* _CONSTRAINT_AUTODIFF_TRIFOCAL_H_ */
+#endif /* _FACTOR_AUTODIFF_TRIFOCAL_H_ */
diff --git a/include/base/constraint/constraint_base.h b/include/base/factor/factor_base.h
similarity index 65%
rename from include/base/constraint/constraint_base.h
rename to include/base/factor/factor_base.h
index 1a0b4f14f77f559f983b7dbce0b4028bea45baa9..4562d5f75b39995fc6578b8498fc5ee95e223e61 100644
--- a/include/base/constraint/constraint_base.h
+++ b/include/base/factor/factor_base.h
@@ -1,5 +1,5 @@
-#ifndef CONSTRAINT_BASE_H_
-#define CONSTRAINT_BASE_H_
+#ifndef FACTOR_BASE_H_
+#define FACTOR_BASE_H_
// Forward declarations for node templates
namespace wolf{
@@ -14,15 +14,15 @@ class FeatureBase;
namespace wolf {
-/** \brief Enumeration of constraint status
+/** \brief Enumeration of factor status
*
* You may add items to this list as needed. Be concise with names, and document your entries.
*/
typedef enum
{
- CTR_INACTIVE = 0, ///< Constraint established with a frame (odometry).
- CTR_ACTIVE = 1 ///< Constraint established with absolute reference.
-} ConstraintStatus;
+ FAC_INACTIVE = 0, ///< Factor established with a frame (odometry).
+ FAC_ACTIVE = 1 ///< Factor established with absolute reference.
+} FactorStatus;
/** \brief Enumeration of jacobian computation method
*
@@ -35,50 +35,50 @@ typedef enum
JAC_ANALYTIC ///< Analytic jacobians.
} JacobianMethod;
-//class ConstraintBase
-class ConstraintBase : public NodeBase, public std::enable_shared_from_this<ConstraintBase>
+//class FactorBase
+class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBase>
{
private:
FeatureBaseWPtr feature_ptr_; ///< FeatureBase pointer (upper node)
- static unsigned int constraint_id_count_;
+ static unsigned int factor_id_count_;
protected:
- unsigned int constraint_id_;
- ConstraintStatus status_; ///< status of constraint (types defined at wolf.h)
- bool apply_loss_function_; ///< flag for applying loss function to this constraint
- FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category CTR_FRAME)
+ unsigned int factor_id_;
+ FactorStatus status_; ///< status of factor (types defined at wolf.h)
+ bool apply_loss_function_; ///< flag for applying loss function to this factor
+ FrameBaseWPtr frame_other_ptr_; ///< FrameBase pointer (for category FAC_FRAME)
CaptureBaseWPtr capture_other_ptr_; ///< CaptureBase pointer
- FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category CTR_FEATURE)
- LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category CTR_LANDMARK)
+ FeatureBaseWPtr feature_other_ptr_; ///< FeatureBase pointer (for category FAC_FEATURE)
+ LandmarkBaseWPtr landmark_other_ptr_; ///< LandmarkBase pointer (for category FAC_LANDMARK)
ProcessorBaseWPtr processor_ptr_; ///< ProcessorBase pointer
public:
- /** \brief Constructor of category CTR_ABSOLUTE
+ /** \brief Constructor of category FAC_ABSOLUTE
**/
- ConstraintBase(const std::string& _tp,
+ FactorBase(const std::string& _tp,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
/** \brief Constructor valid for all categories (FRAME, FEATURE, LANDMARK)
**/
- ConstraintBase(const std::string& _tp,
+ FactorBase(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~ConstraintBase() = default;
+ virtual ~FactorBase() = default;
virtual void remove();
unsigned int id() const;
- /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(Scalar const* const* _parameters, Scalar* _residuals, Scalar** _jacobians) const = 0;
@@ -90,7 +90,7 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
**/
virtual JacobianMethod getJacobianMethod() const = 0;
- /** \brief Returns a vector of pointers to the states in which this constraint depends
+ /** \brief Returns a vector of pointers to the states in which this factor depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const = 0;
@@ -112,24 +112,24 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
/** \brief Returns a pointer to the feature constrained from
**/
- FeatureBasePtr getFeaturePtr() const;
- void setFeaturePtr(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
+ FeatureBasePtr getFeature() const;
+ void setFeature(const FeatureBasePtr _ft_ptr){feature_ptr_ = _ft_ptr;}
/** \brief Returns a pointer to its capture
**/
- CaptureBasePtr getCapturePtr() const;
+ CaptureBasePtr getCapture() const;
- /** \brief Returns the constraint residual size
+ /** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const = 0;
/** \brief Gets the status
*/
- ConstraintStatus getStatus() const;
+ FactorStatus getStatus() const;
/** \brief Sets the status
*/
- void setStatus(ConstraintStatus _status);
+ void setStatus(FactorStatus _status);
/** \brief Gets the apply loss function flag
*/
@@ -141,23 +141,23 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
/** \brief Returns a pointer to the frame constrained to
**/
- FrameBasePtr getFrameOtherPtr() const { return frame_other_ptr_.lock(); }
- void setFrameOtherPtr(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
+ FrameBasePtr getFrameOther() const { return frame_other_ptr_.lock(); }
+ void setFrameOther(FrameBasePtr _frm_o) { frame_other_ptr_ = _frm_o; }
/** \brief Returns a pointer to the frame constrained to
**/
- CaptureBasePtr getCaptureOtherPtr() const { return capture_other_ptr_.lock(); }
- void setCaptureOtherPtr(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
+ CaptureBasePtr getCaptureOther() const { return capture_other_ptr_.lock(); }
+ void setCaptureOther(CaptureBasePtr _cap_o) { capture_other_ptr_ = _cap_o; }
/** \brief Returns a pointer to the feature constrained to
**/
- FeatureBasePtr getFeatureOtherPtr() const { return feature_other_ptr_.lock(); }
- void setFeatureOtherPtr(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
+ FeatureBasePtr getFeatureOther() const { return feature_other_ptr_.lock(); }
+ void setFeatureOther(FeatureBasePtr _ftr_o) { feature_other_ptr_ = _ftr_o; }
/** \brief Returns a pointer to the landmark constrained to
**/
- LandmarkBasePtr getLandmarkOtherPtr() const { return landmark_other_ptr_.lock(); }
- void setLandmarkOtherPtr(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
+ LandmarkBasePtr getLandmarkOther() const { return landmark_other_ptr_.lock(); }
+ void setLandmarkOther(LandmarkBasePtr _lmk_o){ landmark_other_ptr_ = _lmk_o; }
/**
* @brief getProcessor
@@ -191,9 +191,9 @@ class ConstraintBase : public NodeBase, public std::enable_shared_from_this<Cons
namespace wolf{
//template<typename D>
-//inline void ConstraintBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
+//inline void FactorBase::print(const std::string& name, const Eigen::MatrixBase<D>& mat) const {} // Do nothing if input Scalar type is ceres::Jet
//template<int R, int C>
-//inline void ConstraintBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
+//inline void FactorBase::print(const std::string& name, const Eigen::Matrix<Scalar, R, C>& mat) const // Normal print if Scalar type is wolf::Scalar
//{
// if (mat.cols() == 1)
// {
@@ -209,32 +209,32 @@ namespace wolf{
// }
//}
-inline unsigned int ConstraintBase::id() const
+inline unsigned int FactorBase::id() const
{
- return constraint_id_;
+ return factor_id_;
}
-inline FeatureBasePtr ConstraintBase::getFeaturePtr() const
+inline FeatureBasePtr FactorBase::getFeature() const
{
return feature_ptr_.lock();
}
-inline ConstraintStatus ConstraintBase::getStatus() const
+inline FactorStatus FactorBase::getStatus() const
{
return status_;
}
-inline bool ConstraintBase::getApplyLossFunction()
+inline bool FactorBase::getApplyLossFunction()
{
return apply_loss_function_;
}
-inline ProcessorBasePtr ConstraintBase::getProcessor() const
+inline ProcessorBasePtr FactorBase::getProcessor() const
{
return processor_ptr_.lock();
}
-inline void ConstraintBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
+inline void FactorBase::setProcessor(const ProcessorBasePtr& _processor_ptr)
{
processor_ptr_ = _processor_ptr;
}
diff --git a/include/base/constraint/constraint_block_absolute.h b/include/base/factor/factor_block_absolute.h
similarity index 78%
rename from include/base/constraint/constraint_block_absolute.h
rename to include/base/factor/factor_block_absolute.h
index 85a76c664ab5c3113c91ec34ff1df2785e1b910e..4e0da9d764f50d5f55c14c596696b138c9b76035 100644
--- a/include/base/constraint/constraint_block_absolute.h
+++ b/include/base/factor/factor_block_absolute.h
@@ -1,24 +1,24 @@
/**
- * \file constraint_block_absolute.h
+ * \file factor_block_absolute.h
*
* Created on: Dec 15, 2017
* \author: AtDinesh
*/
-#ifndef CONSTRAINT_BLOCK_ABSOLUTE_H_
-#define CONSTRAINT_BLOCK_ABSOLUTE_H_
+#ifndef FACTOR_BLOCK_ABSOLUTE_H_
+#define FACTOR_BLOCK_ABSOLUTE_H_
//Wolf includes
-#include "base/constraint/constraint_analytic.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_analytic.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintBlockAbsolute);
+WOLF_PTR_TYPEDEFS(FactorBlockAbsolute);
//class
-class ConstraintBlockAbsolute : public ConstraintAnalytic
+class FactorBlockAbsolute : public FactorAnalytic
{
private:
SizeEigen sb_size_; // the size of the state block
@@ -35,15 +35,15 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
* \param _start_idx (default=-1) the size of the state segment that is constrained, -1 = all the
*
*/
- ConstraintBlockAbsolute(StateBlockPtr _sb_ptr,
- unsigned int _start_idx = 0,
- int _size = -1,
- bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAnalytic("BLOCK ABS",
- _apply_loss_function,
- _status,
- _sb_ptr),
+ FactorBlockAbsolute(StateBlockPtr _sb_ptr,
+ unsigned int _start_idx = 0,
+ int _size = -1,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic("BLOCK ABS",
+ _apply_loss_function,
+ _status,
+ _sb_ptr),
sb_size_(_sb_ptr->getSize()),
sb_constrained_start_(_start_idx),
sb_constrained_size_(_size == -1 ? sb_size_ : _size)
@@ -60,7 +60,7 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
}
}
- virtual ~ConstraintBlockAbsolute() = default;
+ virtual ~FactorBlockAbsolute() = default;
/** \brief Returns the residual evaluated in the states provided
*
@@ -74,7 +74,7 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
* Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
- * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
@@ -92,12 +92,12 @@ class ConstraintBlockAbsolute : public ConstraintAnalytic
**/
virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const;
- /** \brief Returns the constraint residual size
+ /** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const;
};
-inline Eigen::VectorXs ConstraintBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
+inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
@@ -109,7 +109,7 @@ inline Eigen::VectorXs ConstraintBlockAbsolute::evaluateResiduals(const std::vec
return getMeasurementSquareRootInformationUpper() * (_st_vector.front().segment(sb_constrained_start_,sb_constrained_size_) - getMeasurement());
}
-inline void ConstraintBlockAbsolute::evaluateJacobians(
+inline void FactorBlockAbsolute::evaluateJacobians(
const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
{
@@ -124,14 +124,14 @@ inline void ConstraintBlockAbsolute::evaluateJacobians(
jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
}
-inline void ConstraintBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
{
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
jacobians.front() = J_;
}
-inline unsigned int ConstraintBlockAbsolute::getSize() const
+inline unsigned int FactorBlockAbsolute::getSize() const
{
return sb_constrained_size_;
}
diff --git a/include/base/constraint/constraint_container.h b/include/base/factor/factor_container.h
similarity index 59%
rename from include/base/constraint/constraint_container.h
rename to include/base/factor/factor_container.h
index 1e578f10dc4628031c94b3654264c2a90a435190..93d455a8e7e36ce85e1688c5c70a91b020d963c9 100644
--- a/include/base/constraint/constraint_container.h
+++ b/include/base/factor/factor_container.h
@@ -1,16 +1,16 @@
-#ifndef CONSTRAINT_CONTAINER_H_
-#define CONSTRAINT_CONTAINER_H_
+#ifndef FACTOR_CONTAINER_H_
+#define FACTOR_CONTAINER_H_
//Wolf includes
#include "base/wolf.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/landmark/landmark_container.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintContainer);
+WOLF_PTR_TYPEDEFS(FactorContainer);
-class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>
+class FactorContainer: public FactorAutodiff<FactorContainer,3,2,1,2,1>
{
protected:
LandmarkContainerWPtr lmk_ptr_;
@@ -18,24 +18,34 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
public:
- ConstraintContainer(const FeatureBasePtr& _ftr_ptr,
+ FactorContainer(const FeatureBasePtr& _ftr_ptr,
const LandmarkContainerPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
const unsigned int _corner,
- bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintContainer,3,2,1,2,1>("CONTAINER",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(),_ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr()),
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorContainer,3,2,1,2,1>("CONTAINER",
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO()),
lmk_ptr_(_lmk_ptr),
corner_(_corner)
{
- assert(/*_corner >= 0 &&*/ _corner <= 3 && "Wrong corner id in constraint container constructor");
+ assert(/*_corner >= 0 &&*/ _corner <= 3 && "Wrong corner id in factor container constructor");
- std::cout << "new constraint container: corner idx = " << corner_ << std::endl;
+ std::cout << "new factor container: corner idx = " << corner_ << std::endl;
}
- virtual ~ConstraintContainer() = default;
+ virtual ~FactorContainer() = default;
- LandmarkContainerPtr getLandmarkPtr()
+ LandmarkContainerPtr getLandmark()
{
return lmk_ptr_.lock();
}
@@ -49,14 +59,14 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
//std::cout << "getSensorPosition: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorPosition()->head(2).transpose() << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
//std::cout << "getSensorRotation: " << std::endl;
- //std::cout << getCapturePtr()->getSensorPtr()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
- //std::cout << "atan2: " << atan2(getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,1),getCapturePtr()->getSensorPtr()->getSensorRotation()->transpose()(0,0)) << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
+ //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot information
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
Eigen::Matrix<T,2,2> R_landmark = Eigen::Rotation2D<T>(_landmarkO[0]).matrix();
@@ -64,7 +74,7 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
// Expected measurement
Eigen::Matrix<T,2,1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map + R_landmark * corner_position) - sensor_position);
- T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapturePtr()->getSensorPtr()->getOPtr()->getState()(0)) + T(lmk_ptr_.lock()->getCorner(corner_)(2));
+ T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0)) + T(lmk_ptr_.lock()->getCorner(corner_)(2));
// Error
residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
@@ -79,8 +89,8 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
// Residuals
residuals_map = getMeasurementSquareRootInformationUpper().cast<T>() * residuals_map;
- //std::cout << "\nCONSTRAINT: " << id() << std::endl;
- //std::cout << "Feature: " << getFeaturePtr()->id() << std::endl;
+ //std::cout << "\nFACTOR: " << id() << std::endl;
+ //std::cout << "Feature: " << getFeature()->id() << std::endl;
//std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
//std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
//
@@ -100,7 +110,7 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
//Eigen::Matrix<T,2,1> relative_landmark_position = inverse_R_sensor * (inverse_R_robot * (landmark_position - robot_position) - sensor_position);
//for (int i=0; i < 2; i++)
// std::cout << "\n\t" << relative_landmark_position.data()[i];
- //std::cout << "\n\t" << _landmarkO[0] - _robotO[0] - T( *(getCapturePtr()->getSensorPtr()->getOPtr()->getPtr()) );
+ //std::cout << "\n\t" << _landmarkO[0] - _robotO[0] - T( *(getCapture()->getSensor()->getO()->get()) );
//std::cout << std::endl;
//
//std::cout << "expected_measurement: ";
@@ -117,13 +127,13 @@ class ConstraintContainer: public ConstraintAutodiff<ConstraintContainer,3,2,1,2
}
public:
- static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
- const NodeBasePtr& _correspondant_ptr,
- const ProcessorBasePtr& _processor_ptr = nullptr)
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
+ const NodeBasePtr& _correspondant_ptr,
+ const ProcessorBasePtr& _processor_ptr = nullptr)
{
- unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
+ unsigned int corner = 0; // Hard-coded, but this class is nevertheless deprecated.
- return std::make_shared<ConstraintContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
+ return std::make_shared<FactorContainer>(_feature_ptr, std::static_pointer_cast<LandmarkContainer>(_correspondant_ptr), _processor_ptr, corner);
}
};
diff --git a/include/base/factor/factor_corner_2D.h b/include/base/factor/factor_corner_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..2bcacaac7600e55cdbcfe672e6c1d4c88697a18e
--- /dev/null
+++ b/include/base/factor/factor_corner_2D.h
@@ -0,0 +1,124 @@
+#ifndef FACTOR_CORNER_2D_THETA_H_
+#define FACTOR_CORNER_2D_THETA_H_
+
+//Wolf includes
+#include "base/factor/factor_autodiff.h"
+#include "base/landmark/landmark_corner_2D.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(FactorCorner2D);
+
+class FactorCorner2D: public FactorAutodiff<FactorCorner2D, 3,2,1,2,1>
+{
+ public:
+
+ FactorCorner2D(const FeatureBasePtr _ftr_ptr,
+ const LandmarkCorner2DPtr _lmk_ptr,
+ const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function = false,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorCorner2D,3,2,1,2,1>("CORNER 2D",
+ nullptr,
+ nullptr,
+ nullptr,
+ _lmk_ptr,
+ _processor_ptr,
+ _apply_loss_function,
+ _status,
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _lmk_ptr->getP(),
+ _lmk_ptr->getO())
+ {
+ //
+ }
+
+ virtual ~FactorCorner2D() = default;
+
+ LandmarkCorner2DPtr getLandmark()
+ {
+ return std::static_pointer_cast<LandmarkCorner2D>( landmark_other_ptr_.lock() );
+ }
+
+ template <typename T>
+ bool operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
+ const T* const _landmarkO, T* _residuals) const;
+
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr = nullptr)
+ {
+ return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_correspondant_ptr), _processor_ptr);
+ }
+
+};
+
+template<typename T>
+inline bool FactorCorner2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkP,
+ const T* const _landmarkO, T* _residuals) const
+{
+ // Mapping
+ Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkP);
+ Eigen::Map<const Eigen::Matrix<T,2,1>> robot_position_map(_robotP);
+ Eigen::Map<Eigen::Matrix<T,3,1>> residuals_map(_residuals);
+
+ //std::cout << "getSensorPosition: " << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorPosition()->head(2).transpose() << std::endl;
+ //std::cout << "getSensorRotation: " << std::endl;
+ //std::cout << getCapture()->getSensor()->getSensorRotation()->topLeftCorner<2,2>() << std::endl;
+ //std::cout << "atan2: " << atan2(getCapture()->getSensor()->getSensorRotation()->transpose()(0,1),getCapture()->getSensor()->getSensorRotation()->transpose()(0,0)) << std::endl;
+
+ // sensor transformation
+ Eigen::Matrix<T, 2, 1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
+ // robot transformation
+ Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
+
+ // Expected measurement
+ Eigen::Matrix<T, 2, 1> expected_measurement_position = inverse_R_sensor * (inverse_R_robot * (landmark_position_map - robot_position_map) - sensor_position);
+ T expected_measurement_orientation = _landmarkO[0] - _robotO[0] - T(getCapture()->getSensor()->getO()->getState()(0));
+
+ // Error
+ residuals_map.head(2) = expected_measurement_position - getMeasurement().head<2>().cast<T>();
+ residuals_map(2) = expected_measurement_orientation - T(getMeasurement()(2));
+
+ // pi 2 pi
+ while (_residuals[2] > T(M_PI))
+ _residuals[2] = _residuals[2] - T(2*M_PI);
+ while (_residuals[2] <= T(-M_PI))
+ _residuals[2] = _residuals[2] + T(2*M_PI);
+
+ // Residuals
+ residuals_map = getMeasurementSquareRootInformationUpper().topLeftCorner<3,3>().cast<T>() * residuals_map;
+
+ //std::cout << "\nFACTOR: " << id() << std::endl;
+ //std::cout << "Feature: " << getFeature()->id() << std::endl;
+ //std::cout << "Landmark: " << lmk_ptr_->id() << std::endl;
+ //std::cout << "measurement:\n\t" << getMeasurement().transpose() << std::endl;
+ //
+ //std::cout << "robot pose:";
+ //for (int i=0; i < 2; i++)
+ // std::cout << "\n\t" << _robotP[i];
+ //std::cout << "\n\t" << _robotO[0];
+ //std::cout << std::endl;
+ //
+ //std::cout << "landmark pose:";
+ //for (int i=0; i < 2; i++)
+ // std::cout << "\n\t" << _landmarkP[i];
+ //std::cout << "\n\t" << _landmarkO[0];
+ //std::cout << std::endl;
+ //
+ //std::cout << "expected_measurement: ";
+ //for (int i=0; i < 2; i++)
+ // std::cout << "\n\t" << expected_measurement_position.data()[i];
+ //std::cout << std::endl;
+ //
+ //std::cout << "_residuals: "<< std::endl;
+ //for (int i=0; i < 3; i++)
+ // std::cout << "\n\t" << _residuals[i] << " ";
+ //std::cout << std::endl;
+ return true;
+}
+
+} // namespace wolf
+
+#endif
diff --git a/include/base/constraint/constraint_diff_drive.h b/include/base/factor/factor_diff_drive.h
similarity index 72%
rename from include/base/constraint/constraint_diff_drive.h
rename to include/base/factor/factor_diff_drive.h
index 01722378488cc6a02c2fae0fff0f78635516731a..b495859c7409031153025fe507c0f10113c92a54 100644
--- a/include/base/constraint/constraint_diff_drive.h
+++ b/include/base/factor/factor_diff_drive.h
@@ -1,15 +1,15 @@
/**
- * \file constraint_diff_drive.h
+ * \file factor_diff_drive.h
*
* Created on: Oct 27, 2017
* \author: Jeremie Deray
*/
-#ifndef WOLF_CONSTRAINT_DIFF_DRIVE_H_
-#define WOLF_CONSTRAINT_DIFF_DRIVE_H_
+#ifndef WOLF_FACTOR_DIFF_DRIVE_H_
+#define WOLF_FACTOR_DIFF_DRIVE_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
#include "base/feature/feature_diff_drive.h"
#include "base/capture/capture_wheel_joint_position.h"
@@ -28,34 +28,34 @@ constexpr std::size_t INTRINSICS_STATE_BLOCK_SIZE = 3;
namespace wolf {
-class ConstraintDiffDrive :
- public ConstraintAutodiff< ConstraintDiffDrive,
+class FactorDiffDrive :
+ public FactorAutodiff< FactorDiffDrive,
RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
INTRINSICS_STATE_BLOCK_SIZE >
{
- using Base = ConstraintAutodiff<ConstraintDiffDrive,
+ using Base = FactorAutodiff<FactorDiffDrive,
RESIDUAL_SIZE, POSITION_STATE_BLOCK_SIZE, ORIENTATION_STATE_BLOCK_SIZE,
INTRINSICS_STATE_BLOCK_SIZE>;
public:
- ConstraintDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr,
+ FactorDiffDrive(const FeatureDiffDrivePtr& _ftr_ptr,
const CaptureWheelJointPositionPtr& _capture_origin_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
const bool _apply_loss_function = false,
- const ConstraintStatus _status = CTR_ACTIVE) :
- Base("DIFF DRIVE", _capture_origin_ptr->getFramePtr(), _capture_origin_ptr,
+ const FactorStatus _status = FAC_ACTIVE) :
+ Base("DIFF DRIVE", _capture_origin_ptr->getFrame(), _capture_origin_ptr,
nullptr, nullptr, _processor_ptr,
_apply_loss_function, _status,
- _frame_ptr->getPPtr(), _frame_ptr->getOPtr(),
- _capture_origin_ptr->getFramePtr()->getPPtr(),
- _capture_origin_ptr->getFramePtr()->getOPtr(),
- _capture_origin_ptr->getFramePtr()->getVPtr(),
- _capture_origin_ptr->getSensorIntrinsicPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr(),
- _ftr_ptr->getFramePtr()->getVPtr(),
- _ftr_ptr->getCapturePtr()->getSensorIntrinsicPtr()),
+ _frame_ptr->getP(), _frame_ptr->getO(),
+ _capture_origin_ptr->getFrame()->getP(),
+ _capture_origin_ptr->getFrame()->getO(),
+ _capture_origin_ptr->getFrame()->getV(),
+ _capture_origin_ptr->getSensorIntrinsic(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO(),
+ _ftr_ptr->getFrame()->getV(),
+ _ftr_ptr->getCapture()->getSensorIntrinsic()),
J_delta_calib_(_ftr_ptr->getJacobianFactor())
{
//
@@ -67,7 +67,7 @@ public:
* Default destructor.
*
**/
- virtual ~ConstraintDiffDrive() = default;
+ virtual ~FactorDiffDrive() = default;
template<typename T>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
@@ -89,7 +89,7 @@ protected:
template<typename T>
inline bool
-ConstraintDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
+FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T* const _c1,
const T* const _p2, const T* const _o2, const T* const _c2,
T* _residuals) const
{
@@ -136,4 +136,4 @@ ConstraintDiffDrive::operator ()(const T* const _p1, const T* const _o1, const T
} // namespace wolf
-#endif /* WOLF_CONSTRAINT_DIFF_DRIVE_H_ */
+#endif /* WOLF_FACTOR_DIFF_DRIVE_H_ */
diff --git a/include/base/constraint/constraint_epipolar.h b/include/base/factor/factor_epipolar.h
similarity index 57%
rename from include/base/constraint/constraint_epipolar.h
rename to include/base/factor/factor_epipolar.h
index 2887af376692cce2ca2420074657a9ab4af92fee..42f0e855894c6f247ef30f15c77508157824fb68 100644
--- a/include/base/constraint/constraint_epipolar.h
+++ b/include/base/factor/factor_epipolar.h
@@ -1,25 +1,25 @@
-#ifndef CONSTRAINT_EPIPOLAR_H
-#define CONSTRAINT_EPIPOLAR_H
+#ifndef FACTOR_EPIPOLAR_H
+#define FACTOR_EPIPOLAR_H
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintEpipolar);
+WOLF_PTR_TYPEDEFS(FactorEpipolar);
-class ConstraintEpipolar : public ConstraintBase
+class FactorEpipolar : public FactorBase
{
public:
- ConstraintEpipolar(const FeatureBasePtr& _feature_ptr,
+ FactorEpipolar(const FeatureBasePtr& _feature_ptr,
const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~ConstraintEpipolar() = default;
+ virtual ~FactorEpipolar() = default;
- /** \brief Evaluate the constraint given the input parameters and returning the residuals and jacobians
+ /** \brief Evaluate the factor given the input parameters and returning the residuals and jacobians
**/
virtual bool evaluate(Scalar const* const* parameters, Scalar* residuals, Scalar** jacobians) const override {return true;};
@@ -31,39 +31,39 @@ class ConstraintEpipolar : public ConstraintBase
**/
virtual JacobianMethod getJacobianMethod() const override {return JAC_ANALYTIC;}
- /** \brief Returns a vector of pointers to the states in which this constraint depends
+ /** \brief Returns a vector of pointers to the states in which this factor depends
**/
virtual std::vector<StateBlockPtr> getStateBlockPtrVector() const override {return std::vector<StateBlockPtr>(0);}
- /** \brief Returns the constraint residual size
+ /** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const override {return 0;}
- /** \brief Returns the constraint states sizes
+ /** \brief Returns the factor states sizes
**/
virtual std::vector<unsigned int> getStateSizes() const override {return std::vector<unsigned int>({1});}
public:
- static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr);
};
-inline ConstraintEpipolar::ConstraintEpipolar(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
+inline FactorEpipolar::FactorEpipolar(const FeatureBasePtr& /*_feature_ptr*/, const FeatureBasePtr& _feature_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status) :
- ConstraintBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
+ bool _apply_loss_function, FactorStatus _status) :
+ FactorBase("EPIPOLAR", nullptr, nullptr, _feature_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status)
{
//
}
-inline ConstraintBasePtr ConstraintEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
+inline FactorBasePtr FactorEpipolar::create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<ConstraintEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorEpipolar>(_feature_ptr, std::static_pointer_cast<FeatureBase>(_correspondant_ptr), _processor_ptr);
}
} // namespace wolf
-#endif // CONSTRAINT_EPIPOLAR_H
+#endif // FACTOR_EPIPOLAR_H
diff --git a/include/base/constraint/constraint_fix_bias.h b/include/base/factor/factor_fix_bias.h
similarity index 65%
rename from include/base/constraint/constraint_fix_bias.h
rename to include/base/factor/factor_fix_bias.h
index 55dbbf8db732dbc9f18bade35b4b2dce790b3c5d..6c1f321dc0f7fcaf965476dc802080e3d3bb23b5 100644
--- a/include/base/constraint/constraint_fix_bias.h
+++ b/include/base/factor/factor_fix_bias.h
@@ -1,11 +1,11 @@
-#ifndef CONSTRAINT_FIX_BIAS_H_
-#define CONSTRAINT_FIX_BIAS_H_
+#ifndef FACTOR_FIX_BIAS_H_
+#define FACTOR_FIX_BIAS_H_
//Wolf includes
#include "base/capture/capture_IMU.h"
#include "base/feature/feature_IMU.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
#include "base/rotations.h"
@@ -13,21 +13,21 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintFixBias);
+WOLF_PTR_TYPEDEFS(FactorFixBias);
//class
-class ConstraintFixBias: public ConstraintAutodiff<ConstraintFixBias,6,3,3>
+class FactorFixBias: public FactorAutodiff<FactorFixBias,6,3,3>
{
public:
- ConstraintFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintFixBias, 6, 3, 3>("FIX BIAS",
- nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getAccBiasPtr(),
- std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapturePtr())->getGyroBiasPtr())
+ FactorFixBias(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorFixBias, 6, 3, 3>("FIX BIAS",
+ nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getAccBias(),
+ std::static_pointer_cast<CaptureIMU>(_ftr_ptr->getCapture())->getGyroBias())
{
- // std::cout << "created ConstraintFixBias " << std::endl;
+ // std::cout << "created FactorFixBias " << std::endl;
}
- virtual ~ConstraintFixBias() = default;
+ virtual ~FactorFixBias() = default;
template<typename T>
bool operator ()(const T* const _ab, const T* const _wb, T* _residuals) const;
@@ -35,7 +35,7 @@ class ConstraintFixBias: public ConstraintAutodiff<ConstraintFixBias,6,3,3>
};
template<typename T>
-inline bool ConstraintFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
+inline bool FactorFixBias::operator ()(const T* const _ab, const T* const _wb, T* _residuals) const
{
// measurement
Eigen::Matrix<T,6,1> meas = getMeasurement().cast<T>();
@@ -49,7 +49,7 @@ inline bool ConstraintFixBias::operator ()(const T* const _ab, const T* const _w
// residual
Eigen::Map<Eigen::Matrix<T,6,1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
@@ -71,8 +71,8 @@ inline bool ConstraintFixBias::operator ()(const T* const _ab, const T* const _w
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "ConstraintFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "ConstraintFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorFixBias::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorFixBias::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/base/constraint/constraint_odom_2D.h b/include/base/factor/factor_odom_2D.h
similarity index 64%
rename from include/base/constraint/constraint_odom_2D.h
rename to include/base/factor/factor_odom_2D.h
index 5df37fbb98b330d561a3658bf07ed6c83e1ae74a..e45570d79a8debe284310998cc87f48032521a10 100644
--- a/include/base/constraint/constraint_odom_2D.h
+++ b/include/base/factor/factor_odom_2D.h
@@ -1,52 +1,52 @@
-#ifndef CONSTRAINT_ODOM_2D_THETA_H_
-#define CONSTRAINT_ODOM_2D_THETA_H_
+#ifndef FACTOR_ODOM_2D_THETA_H_
+#define FACTOR_ODOM_2D_THETA_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
//#include "ceres/jet.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintOdom2D);
+WOLF_PTR_TYPEDEFS(FactorOdom2D);
//class
-class ConstraintOdom2D : public ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>
+class FactorOdom2D : public FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>
{
public:
- ConstraintOdom2D(const FeatureBasePtr& _ftr_ptr,
+ FactorOdom2D(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
- bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorOdom2D, 3, 2, 1, 2, 1>("ODOM 2D",
_frame_other_ptr, nullptr, nullptr, nullptr,
_processor_ptr,
_apply_loss_function, _status,
- _frame_other_ptr->getPPtr(),
- _frame_other_ptr->getOPtr(),
- _ftr_ptr->getFramePtr()->getPPtr(),
- _ftr_ptr->getFramePtr()->getOPtr())
+ _frame_other_ptr->getP(),
+ _frame_other_ptr->getO(),
+ _ftr_ptr->getFrame()->getP(),
+ _ftr_ptr->getFrame()->getO())
{
//
}
- virtual ~ConstraintOdom2D() = default;
+ virtual ~FactorOdom2D() = default;
template<typename T>
bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2,
T* _residuals) const;
public:
- static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr, const NodeBasePtr& _correspondant_ptr, const ProcessorBasePtr& _processor_ptr)
{
- return std::make_shared<ConstraintOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorOdom2D>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
};
template<typename T>
-inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
+inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, T* _residuals) const
{
@@ -85,9 +85,9 @@ inline bool ConstraintOdom2D::operator ()(const T* const _p1, const T* const _o1
// J.row(2) = ((Jet<Scalar, 6>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "ConstraintOdom2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "ConstraintOdom2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "ConstraintOdom2D::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
+// std::cout << "FactorOdom2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorOdom2D::Sqrt Info(c" << id() << ") = \n" << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/base/constraint/constraint_odom_2D_analytic.h b/include/base/factor/factor_odom_2D_analytic.h
similarity index 81%
rename from include/base/constraint/constraint_odom_2D_analytic.h
rename to include/base/factor/factor_odom_2D_analytic.h
index 43048d3ffa702042cc022ddebd499d7251b21d10..2eb3296240c9b28766919748875eaea9720d87e0 100644
--- a/include/base/constraint/constraint_odom_2D_analytic.h
+++ b/include/base/factor/factor_odom_2D_analytic.h
@@ -1,34 +1,34 @@
-#ifndef CONSTRAINT_ODOM_2D_ANALYTIC_H_
-#define CONSTRAINT_ODOM_2D_ANALYTIC_H_
+#ifndef FACTOR_ODOM_2D_ANALYTIC_H_
+#define FACTOR_ODOM_2D_ANALYTIC_H_
//Wolf includes
-#include "base/constraint/constraint_relative_2D_analytic.h"
+#include "base/factor/factor_relative_2D_analytic.h"
#include <Eigen/StdVector>
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintOdom2DAnalytic);
+WOLF_PTR_TYPEDEFS(FactorOdom2DAnalytic);
//class
-class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
+class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
{
public:
- ConstraintOdom2DAnalytic(const FeatureBasePtr& _ftr_ptr,
+ FactorOdom2DAnalytic(const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintRelative2DAnalytic("ODOM_2D", _ftr_ptr,
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorRelative2DAnalytic("ODOM_2D", _ftr_ptr,
_frame_ptr, _processor_ptr, _apply_loss_function, _status)
{
//
}
- virtual ~ConstraintOdom2DAnalytic() = default;
+ virtual ~FactorOdom2DAnalytic() = default;
-// /** \brief Returns the constraint residual size
+// /** \brief Returns the factor residual size
// *
-// * Returns the constraint residual size
+// * Returns the factor residual size
// *
// **/
// virtual unsigned int getSize() const
@@ -67,7 +67,7 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
// * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
// * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
// *
-// * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+// * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
// * \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
// * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
// *
@@ -94,11 +94,11 @@ class ConstraintOdom2DAnalytic : public ConstraintRelative2DAnalytic
// }
public:
- static ConstraintBasePtr create(const FeatureBasePtr& _feature_ptr,
+ static FactorBasePtr create(const FeatureBasePtr& _feature_ptr,
const NodeBasePtr& _correspondant_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr)
{
- return std::make_shared<ConstraintOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
+ return std::make_shared<FactorOdom2DAnalytic>(_feature_ptr, std::static_pointer_cast<FrameBase>(_correspondant_ptr), _processor_ptr);
}
};
diff --git a/include/base/constraint/constraint_odom_3D.h b/include/base/factor/factor_odom_3D.h
similarity index 78%
rename from include/base/constraint/constraint_odom_3D.h
rename to include/base/factor/factor_odom_3D.h
index 9047b7891cec383e1575c4fbab3b772b7efea610..ce3456ef0e8c286c7b082a79a41fa268e4608a8e 100644
--- a/include/base/constraint/constraint_odom_3D.h
+++ b/include/base/factor/factor_odom_3D.h
@@ -1,32 +1,32 @@
/*
- * constraint_odom_3D.h
+ * factor_odom_3D.h
*
* Created on: Oct 7, 2016
* Author: jsola
*/
-#ifndef CONSTRAINT_ODOM_3D_H_
-#define CONSTRAINT_ODOM_3D_H_
+#ifndef FACTOR_ODOM_3D_H_
+#define FACTOR_ODOM_3D_H_
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/rotations.h"
namespace wolf
{
-WOLF_PTR_TYPEDEFS(ConstraintOdom3D);
+WOLF_PTR_TYPEDEFS(FactorOdom3D);
//class
-class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4>
+class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
{
public:
- ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr,
+ FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE);
+ FactorStatus _status = FAC_ACTIVE);
- virtual ~ConstraintOdom3D() = default;
+ virtual ~FactorOdom3D() = default;
template<typename T>
bool operator ()(const T* const _p_current,
@@ -52,7 +52,7 @@ class ConstraintOdom3D : public ConstraintAutodiff<ConstraintOdom3D,6,3,4,3,4>
};
template<typename T>
-inline void ConstraintOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
+inline void FactorOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
{
std::cout << "Jet residual = " << std::endl;
std::cout << r(0) << std::endl;
@@ -64,18 +64,18 @@ inline void ConstraintOdom3D::printRes(const Eigen::Matrix<T, 6, 1>& r) const
}
template<>
-inline void ConstraintOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) const
+inline void FactorOdom3D::printRes (const Eigen::Matrix<Scalar,6,1> & r) const
{
std::cout << "Scalar residual = " << std::endl;
std::cout << r.transpose() << std::endl;
}
-inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr,
+inline FactorOdom3D::FactorOdom3D(const FeatureBasePtr& _ftr_current_ptr,
const FrameBasePtr& _frame_past_ptr,
const ProcessorBasePtr& _processor_ptr,
bool _apply_loss_function,
- ConstraintStatus _status) :
- ConstraintAutodiff<ConstraintOdom3D, 6, 3, 4, 3, 4>("ODOM 3D", // type
+ FactorStatus _status) :
+ FactorAutodiff<FactorOdom3D, 6, 3, 4, 3, 4>("ODOM 3D", // type
_frame_past_ptr, // frame other
nullptr, // capture other
nullptr, // feature other
@@ -83,13 +83,13 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
_processor_ptr, // processor
_apply_loss_function,
_status,
- _ftr_current_ptr->getFramePtr()->getPPtr(), // current frame P
- _ftr_current_ptr->getFramePtr()->getOPtr(), // current frame Q
- _frame_past_ptr->getPPtr(), // past frame P
- _frame_past_ptr->getOPtr()) // past frame Q
+ _ftr_current_ptr->getFrame()->getP(), // current frame P
+ _ftr_current_ptr->getFrame()->getO(), // current frame Q
+ _frame_past_ptr->getP(), // past frame P
+ _frame_past_ptr->getO()) // past frame Q
{
// WOLF_TRACE("Constr ODOM 3D (f", _ftr_current_ptr->id(),
-// " F", _ftr_current_ptr->getCapturePtr()->getFramePtr()->id(),
+// " F", _ftr_current_ptr->getCapture()->getFrame()->id(),
// ") (Fo", _frame_past_ptr->id(), ")");
//
// WOLF_TRACE("delta preint: ", _ftr_current_ptr->getMeasurement().transpose());
@@ -99,7 +99,7 @@ inline ConstraintOdom3D::ConstraintOdom3D(const FeatureBasePtr& _ftr_current_ptr
}
template<typename T>
-inline bool ConstraintOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _expectation_dp, T* _expectation_dq) const
{
Eigen::Map<const Eigen::Matrix<T,3,1> > p_current(_p_current);
@@ -139,18 +139,18 @@ inline bool ConstraintOdom3D::expectation(const T* const _p_current, const T* co
return true;
}
-inline Eigen::VectorXs ConstraintOdom3D::expectation() const
+inline Eigen::VectorXs FactorOdom3D::expectation() const
{
Eigen::VectorXs exp(7);
- FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
- FrameBasePtr frm_past = getFrameOtherPtr();
- const Eigen::VectorXs frame_current_pos = frm_current->getPPtr()->getState();
- const Eigen::VectorXs frame_current_ori = frm_current->getOPtr()->getState();
- const Eigen::VectorXs frame_past_pos = frm_past->getPPtr()->getState();
- const Eigen::VectorXs frame_past_ori = frm_past->getOPtr()->getState();
+ FrameBasePtr frm_current = getFeature()->getFrame();
+ FrameBasePtr frm_past = getFrameOther();
+ const Eigen::VectorXs frame_current_pos = frm_current->getP()->getState();
+ const Eigen::VectorXs frame_current_ori = frm_current->getO()->getState();
+ const Eigen::VectorXs frame_past_pos = frm_past->getP()->getState();
+ const Eigen::VectorXs frame_past_ori = frm_past->getO()->getState();
-// std::cout << "frame_current_pos: " << frm_current->getPPtr()->getState().transpose() << std::endl;
-// std::cout << "frame_past_pos: " << frm_past->getPPtr()->getState().transpose() << std::endl;
+// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
+// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
expectation(frame_current_pos.data(),
frame_current_ori.data(),
@@ -162,7 +162,7 @@ inline Eigen::VectorXs ConstraintOdom3D::expectation() const
}
template<typename T>
-inline bool ConstraintOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
+inline bool FactorOdom3D::operator ()(const T* const _p_current, const T* const _q_current, const T* const _p_past,
const T* const _q_past, T* _residuals) const
{
@@ -228,4 +228,4 @@ inline bool ConstraintOdom3D::operator ()(const T* const _p_current, const T* co
} /* namespace wolf */
-#endif /* CONSTRAINT_ODOM_3D_H_ */
+#endif /* FACTOR_ODOM_3D_H_ */
diff --git a/include/base/constraint/constraint_point_2D.h b/include/base/factor/factor_point_2D.h
similarity index 72%
rename from include/base/constraint/constraint_point_2D.h
rename to include/base/factor/factor_point_2D.h
index 1900991f7d47b01d0486a7ffca5465056033fbf1..4d9686e3805c9fb5d337ed033e90a904cbb4ae53 100644
--- a/include/base/constraint/constraint_point_2D.h
+++ b/include/base/factor/factor_point_2D.h
@@ -1,19 +1,19 @@
-#ifndef CONSTRAINT_POINT_2D_THETA_H_
-#define CONSTRAINT_POINT_2D_THETA_H_
+#ifndef FACTOR_POINT_2D_THETA_H_
+#define FACTOR_POINT_2D_THETA_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/feature/feature_polyline_2D.h"
#include "base/landmark/landmark_polyline_2D.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintPoint2D);
+WOLF_PTR_TYPEDEFS(FactorPoint2D);
/**
- * @brief The ConstraintPoint2D class
+ * @brief The FactorPoint2D class
*/
-class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,2>
+class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
{
protected:
unsigned int feature_point_id_;
@@ -25,28 +25,28 @@ class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,
public:
- ConstraintPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
+ FactorPoint2D(const FeaturePolyline2DPtr& _ftr_ptr,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
- unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPoint2D,2,2,1,2,1,2>("POINT 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)),
- feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ unsigned int _ftr_point_id, int _lmk_point_id, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPoint2D,2,2,1,2,1,2>("POINT 2D",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id)),
+ feature_point_id_(_ftr_point_id), landmark_point_id_(_lmk_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
- //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlockPtr(_lmk_point_id)->getVector().transpose() << std::endl;
+ //std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
- virtual ~ConstraintPoint2D() = default;
+ virtual ~FactorPoint2D() = default;
/**
* @brief getLandmarkPtr
* @return
*/
- LandmarkPolyline2DPtr getLandmarkPtr()
+ LandmarkPolyline2DPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2D>(landmark_other_ptr_.lock());
}
@@ -73,7 +73,7 @@ class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,
* @brief getLandmarkPointPtr
* @return
*/
- StateBlockPtr getLandmarkPointPtr()
+ StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
@@ -107,9 +107,9 @@ class ConstraintPoint2D: public ConstraintAutodiff<ConstraintPoint2D, 2,2,1,2,1,
};
template<typename T>
-inline bool ConstraintPoint2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
+inline bool FactorPoint2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginP, const T* const _landmarkOriginO, const T* const _landmarkPoint, T* _residuals) const
{
- //std::cout << "ConstraintPointToLine2D::operator" << std::endl;
+ //std::cout << "FactorPointToLine2D::operator" << std::endl;
// Mapping
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginP);
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
@@ -120,8 +120,8 @@ inline bool ConstraintPoint2D::operator ()(const T* const _robotP, const T* cons
Eigen::Matrix<T,2,1> landmark_point = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginO) * landmark_position_map;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot transformation
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
diff --git a/include/base/constraint/constraint_point_to_line_2D.h b/include/base/factor/factor_point_to_line_2D.h
similarity index 73%
rename from include/base/constraint/constraint_point_to_line_2D.h
rename to include/base/factor/factor_point_to_line_2D.h
index 842b84a800996c8bdb1ca6cf2adf3607dc53d358..d4b891f3ee5d055d3e7500464e9ce59b1780f9f9 100644
--- a/include/base/constraint/constraint_point_to_line_2D.h
+++ b/include/base/factor/factor_point_to_line_2D.h
@@ -1,17 +1,17 @@
-#ifndef CONSTRAINT_POINT_TO_LINE_2D_H_
-#define CONSTRAINT_POINT_TO_LINE_2D_H_
+#ifndef FACTOR_POINT_TO_LINE_2D_H_
+#define FACTOR_POINT_TO_LINE_2D_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/feature/feature_polyline_2D.h"
#include "base/landmark/landmark_polyline_2D.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintPointToLine2D);
+WOLF_PTR_TYPEDEFS(FactorPointToLine2D);
//class
-class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>
+class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>
{
protected:
int landmark_point_id_;
@@ -25,24 +25,24 @@ class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D
public:
- ConstraintPointToLine2D(const FeaturePolyline2DPtr& _ftr_ptr,
+ FactorPointToLine2D(const FeaturePolyline2DPtr& _ftr_ptr,
const LandmarkPolyline2DPtr& _lmk_ptr,
const ProcessorBasePtr& _processor_ptr,
unsigned int _ftr_point_id, int _lmk_point_id, int _lmk_point_aux_id,
- bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
- nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _lmk_ptr->getPPtr(), _lmk_ptr->getOPtr(), _lmk_ptr->getPointStateBlockPtr(_lmk_point_id), _lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)),
- landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlockPtr(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
+ bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,2,2>("POINT TO LINE 2D",
+ nullptr, nullptr, nullptr, _lmk_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _lmk_ptr->getP(), _lmk_ptr->getO(), _lmk_ptr->getPointStateBlock(_lmk_point_id), _lmk_ptr->getPointStateBlock(_lmk_point_aux_id)),
+ landmark_point_id_(_lmk_point_id), landmark_point_aux_id_(_lmk_point_aux_id), feature_point_id_(_ftr_point_id), point_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_id)), point_aux_state_ptr_(_lmk_ptr->getPointStateBlock(_lmk_point_aux_id)), measurement_(_ftr_ptr->getPoints().col(_ftr_point_id)), measurement_covariance_(_ftr_ptr->getPointsCov().middleCols(_ftr_point_id*2,2))
{
- //std::cout << "ConstraintPointToLine2D" << std::endl;
+ //std::cout << "FactorPointToLine2D" << std::endl;
Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
- virtual ~ConstraintPointToLine2D() = default;
+ virtual ~FactorPointToLine2D() = default;
- LandmarkPolyline2DPtr getLandmarkPtr()
+ LandmarkPolyline2DPtr getLandmark()
{
return std::static_pointer_cast<LandmarkPolyline2D>( landmark_other_ptr_.lock() );
}
@@ -62,12 +62,12 @@ class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D
return feature_point_id_;
}
- StateBlockPtr getLandmarkPointPtr()
+ StateBlockPtr getLandmarkPoint()
{
return point_state_ptr_;
}
- StateBlockPtr getLandmarkPointAuxPtr()
+ StateBlockPtr getLandmarkPointAux()
{
return point_state_ptr_;
}
@@ -98,9 +98,9 @@ class ConstraintPointToLine2D: public ConstraintAutodiff<ConstraintPointToLine2D
};
template<typename T>
-inline bool ConstraintPointToLine2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
+inline bool FactorPointToLine2D::operator ()(const T* const _robotP, const T* const _robotO, const T* const _landmarkOriginPosition, const T* const _landmarkOriginOrientation, const T* const _landmarkPoint, const T* const _landmarkPointAux, T* _residuals) const
{
- //std::cout << "ConstraintPointToLine2D::operator" << std::endl;
+ //std::cout << "FactorPointToLine2D::operator" << std::endl;
// Mapping
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_origin_position_map(_landmarkOriginPosition);
Eigen::Map<const Eigen::Matrix<T,2,1>> landmark_position_map(_landmarkPoint);
@@ -111,8 +111,8 @@ inline bool ConstraintPointToLine2D::operator ()(const T* const _robotP, const T
Eigen::Matrix<T,2,1> landmark_point_aux = landmark_origin_position_map + Eigen::Rotation2D<T>(*_landmarkOriginOrientation) * landmark_aux_position_map;
// sensor transformation
- Eigen::Matrix<T,2,1> sensor_position = getCapturePtr()->getSensorPtr()->getPPtr()->getState().head(2).cast<T>();
- Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapturePtr()->getSensorOPtr()->getState()(0))).matrix();
+ Eigen::Matrix<T,2,1> sensor_position = getCapture()->getSensor()->getP()->getState().head(2).cast<T>();
+ Eigen::Matrix<T,2,2> inverse_R_sensor = Eigen::Rotation2D<T>(T(-getCapture()->getSensorO()->getState()(0))).matrix();
// robot transformation
Eigen::Matrix<T,2,2> inverse_R_robot = Eigen::Rotation2D<T>(-_robotO[0]).matrix();
diff --git a/include/base/constraint/constraint_pose_2D.h b/include/base/factor/factor_pose_2D.h
similarity index 58%
rename from include/base/constraint/constraint_pose_2D.h
rename to include/base/factor/factor_pose_2D.h
index b6fe79ef378c0b1bfb6e722b786678ae195f274c..306b5e15ebf9e2e67eb41c8e5072aa050f7d6b32 100644
--- a/include/base/constraint/constraint_pose_2D.h
+++ b/include/base/factor/factor_pose_2D.h
@@ -1,9 +1,9 @@
-#ifndef CONSTRAINT_POSE_2D_H_
-#define CONSTRAINT_POSE_2D_H_
+#ifndef FACTOR_POSE_2D_H_
+#define FACTOR_POSE_2D_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
#include "base/rotations.h"
@@ -11,19 +11,19 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintPose2D);
+WOLF_PTR_TYPEDEFS(FactorPose2D);
//class
-class ConstraintPose2D: public ConstraintAutodiff<ConstraintPose2D,3,2,1>
+class FactorPose2D: public FactorAutodiff<FactorPose2D,3,2,1>
{
public:
- ConstraintPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose2D, 3, 2, 1>("POSE 2D", nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
-// std::cout << "created ConstraintPose2D " << std::endl;
+// std::cout << "created FactorPose2D " << std::endl;
}
- virtual ~ConstraintPose2D() = default;
+ virtual ~FactorPose2D() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -31,7 +31,7 @@ class ConstraintPose2D: public ConstraintAutodiff<ConstraintPose2D,3,2,1>
};
template<typename T>
-inline bool ConstraintPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
@@ -48,7 +48,7 @@ inline bool ConstraintPose2D::operator ()(const T* const _p, const T* const _o,
// residual
Eigen::Map<Eigen::Matrix<T,3,1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
@@ -62,8 +62,8 @@ inline bool ConstraintPose2D::operator ()(const T* const _p, const T* const _o,
// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "ConstraintPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "ConstraintPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "FactorPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/include/base/constraint/constraint_pose_3D.h b/include/base/factor/factor_pose_3D.h
similarity index 50%
rename from include/base/constraint/constraint_pose_3D.h
rename to include/base/factor/factor_pose_3D.h
index 5fa1ec6c25e606cc4cb451e41c18b421fe38b98d..b9c4da39516235081a4ec91334fa06958478264c 100644
--- a/include/base/constraint/constraint_pose_3D.h
+++ b/include/base/factor/factor_pose_3D.h
@@ -1,28 +1,28 @@
-#ifndef CONSTRAINT_POSE_3D_H_
-#define CONSTRAINT_POSE_3D_H_
+#ifndef FACTOR_POSE_3D_H_
+#define FACTOR_POSE_3D_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/frame_base.h"
#include "base/rotations.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintPose3D);
+WOLF_PTR_TYPEDEFS(FactorPose3D);
//class
-class ConstraintPose3D: public ConstraintAutodiff<ConstraintPose3D,6,3,4>
+class FactorPose3D: public FactorAutodiff<FactorPose3D,6,3,4>
{
public:
- ConstraintPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorPose3D,6,3,4>("POSE 3D", nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
- virtual ~ConstraintPose3D() = default;
+ virtual ~FactorPose3D() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -30,7 +30,7 @@ class ConstraintPose3D: public ConstraintAutodiff<ConstraintPose3D,6,3,4>
};
template<typename T>
-inline bool ConstraintPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// states
@@ -48,7 +48,7 @@ inline bool ConstraintPose3D::operator ()(const T* const _p, const T* const _o,
// residual
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/include/base/constraint/constraint_quaternion_absolute.h b/include/base/factor/factor_quaternion_absolute.h
similarity index 61%
rename from include/base/constraint/constraint_quaternion_absolute.h
rename to include/base/factor/factor_quaternion_absolute.h
index eb2e2431a983f14c6300db4b621c85348cf1a334..1864a6c8eede4bd07190c4da737a7e1f25da117f 100644
--- a/include/base/constraint/constraint_quaternion_absolute.h
+++ b/include/base/factor/factor_quaternion_absolute.h
@@ -1,36 +1,36 @@
/**
- * \file constraint_quaternion_absolute.h
+ * \file factor_quaternion_absolute.h
*
* Created on: Dec 15, 2017
* \author: AtDinesh
*/
-#ifndef CONSTRAINT_QUATERNION_ABSOLUTE_H_
-#define CONSTRAINT_QUATERNION_ABSOLUTE_H_
+#ifndef FACTOR_QUATERNION_ABSOLUTE_H_
+#define FACTOR_QUATERNION_ABSOLUTE_H_
//Wolf includes
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_autodiff.h"
#include "base/local_parametrization_quaternion.h"
#include "base/frame_base.h"
#include "base/rotations.h"
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintQuaternionAbsolute);
+WOLF_PTR_TYPEDEFS(FactorQuaternionAbsolute);
//class
-class ConstraintQuaternionAbsolute: public ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4>
+class FactorQuaternionAbsolute: public FactorAutodiff<FactorQuaternionAbsolute,3,4>
{
public:
- ConstraintQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintQuaternionAbsolute,3,4>("QUATERNION ABS",
+ FactorQuaternionAbsolute(StateBlockPtr _sb_ptr, bool _apply_loss_function = false, FactorStatus _status = FAC_ACTIVE) :
+ FactorAutodiff<FactorQuaternionAbsolute,3,4>("QUATERNION ABS",
nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _sb_ptr)
{
//
}
- virtual ~ConstraintQuaternionAbsolute() = default;
+ virtual ~FactorQuaternionAbsolute() = default;
template<typename T>
bool operator ()(const T* const _o, T* _residuals) const;
@@ -38,7 +38,7 @@ class ConstraintQuaternionAbsolute: public ConstraintAutodiff<ConstraintQuaterni
};
template<typename T>
-inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) const
+inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residuals) const
{
// states
@@ -63,7 +63,7 @@ inline bool ConstraintQuaternionAbsolute::operator ()(const T* const _o, T* _res
// residual
Eigen::Map<Eigen::Matrix<T, 3, 1>> res(_residuals);
- res = getFeaturePtr()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
+ res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * er;
return true;
}
diff --git a/include/base/constraint/constraint_relative_2D_analytic.h b/include/base/factor/factor_relative_2D_analytic.h
similarity index 75%
rename from include/base/constraint/constraint_relative_2D_analytic.h
rename to include/base/factor/factor_relative_2D_analytic.h
index 125537e5a3dde3771d9d7c8da20c97c09b96f520..5c3aec2333bb85999c6892c6ce365e0de3c5ac2e 100644
--- a/include/base/constraint/constraint_relative_2D_analytic.h
+++ b/include/base/factor/factor_relative_2D_analytic.h
@@ -1,62 +1,62 @@
-#ifndef CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
-#define CONSTRAINT_RELATIVE_2D_ANALYTIC_H_
+#ifndef FACTOR_RELATIVE_2D_ANALYTIC_H_
+#define FACTOR_RELATIVE_2D_ANALYTIC_H_
//Wolf includes
-#include "base/constraint/constraint_analytic.h"
+#include "base/factor/factor_analytic.h"
#include "base/landmark/landmark_base.h"
#include <Eigen/StdVector>
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintRelative2DAnalytic);
+WOLF_PTR_TYPEDEFS(FactorRelative2DAnalytic);
//class
-class ConstraintRelative2DAnalytic : public ConstraintAnalytic
+class FactorRelative2DAnalytic : public FactorAnalytic
{
public:
- /** \brief Constructor of category CTR_FRAME
+ /** \brief Constructor of category FAC_FRAME
**/
- ConstraintRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FrameBasePtr& _frame_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getPPtr(), _frame_ptr->getOPtr(), _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp, _frame_ptr, nullptr, nullptr, nullptr, _processor_ptr, _apply_loss_function, _status, _frame_ptr->getP(), _frame_ptr->getO(), _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO())
{
//
}
- /** \brief Constructor of category CTR_FEATURE
+ /** \brief Constructor of category FAC_FEATURE
**/
- ConstraintRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const FeatureBasePtr& _ftr_other_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _ftr_other_ptr->getFramePtr()->getPPtr(), _ftr_other_ptr->getFramePtr()->getOPtr() )
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp, nullptr, nullptr, _ftr_other_ptr, nullptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _ftr_other_ptr->getFrame()->getP(), _ftr_other_ptr->getFrame()->getO() )
{
//
}
- /** \brief Constructor of category CTR_LANDMARK
+ /** \brief Constructor of category FAC_LANDMARK
**/
- ConstraintRelative2DAnalytic(const std::string& _tp,
+ FactorRelative2DAnalytic(const std::string& _tp,
const FeatureBasePtr& _ftr_ptr,
const LandmarkBasePtr& _landmark_ptr,
const ProcessorBasePtr& _processor_ptr = nullptr,
bool _apply_loss_function = false,
- ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr(), _landmark_ptr->getPPtr(), _landmark_ptr->getOPtr())
+ FactorStatus _status = FAC_ACTIVE) :
+ FactorAnalytic(_tp, nullptr, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status, _ftr_ptr->getFrame()->getP(), _ftr_ptr->getFrame()->getO(), _landmark_ptr->getP(), _landmark_ptr->getO())
{
//
}
- virtual ~ConstraintRelative2DAnalytic() = default;
+ virtual ~FactorRelative2DAnalytic() = default;
- /** \brief Returns the constraint residual size
+ /** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const override
{
@@ -75,7 +75,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
* Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
- * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the constraint
+ * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
@@ -94,7 +94,7 @@ class ConstraintRelative2DAnalytic : public ConstraintAnalytic
/// IMPLEMENTATION ///
-inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals(
+inline Eigen::VectorXs FactorRelative2DAnalytic::evaluateResiduals(
const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
{
Eigen::VectorXs residual(3);
@@ -113,7 +113,7 @@ inline Eigen::VectorXs ConstraintRelative2DAnalytic::evaluateResiduals(
return residual;
}
-inline void ConstraintRelative2DAnalytic::evaluateJacobians(
+inline void FactorRelative2DAnalytic::evaluateJacobians(
const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
{
@@ -130,7 +130,7 @@ inline void ConstraintRelative2DAnalytic::evaluateJacobians(
jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
}
-inline void ConstraintRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
{
jacobians[0]
<< -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)), sin(
diff --git a/include/base/feature/feature_GPS_fix.h b/include/base/feature/feature_GPS_fix.h
index 688176fdfac976bd483e3fb4694155d7843af754..d694465480d2dbc47eb886c27ed0444ca47e7616 100644
--- a/include/base/feature/feature_GPS_fix.h
+++ b/include/base/feature/feature_GPS_fix.h
@@ -2,7 +2,7 @@
#define FEATURE_GPS_FIX_H_
//Wolf includes
-#include "base/constraint/constraint_GPS_2D.h"
+#include "base/factor/factor_GPS_2D.h"
#include "base/feature/feature_base.h"
//std includes
diff --git a/include/base/feature/feature_base.h b/include/base/feature/feature_base.h
index 1ab46cccceebfae070368341aa2edde0000e694a..3732bc55247d9d39b90992a07afe7bb04d7f7681 100644
--- a/include/base/feature/feature_base.h
+++ b/include/base/feature/feature_base.h
@@ -4,7 +4,7 @@
// Forward declarations for node templates
namespace wolf{
class CaptureBase;
-class ConstraintBase;
+class FactorBase;
}
//Wolf includes
@@ -20,8 +20,8 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
{
private:
CaptureBaseWPtr capture_ptr_;
- ConstraintBaseList constraint_list_;
- ConstraintBaseList constrained_by_list_;
+ FactorBasePtrList factor_list_;
+ FactorBasePtrList constrained_by_list_;
static unsigned int feature_id_count_;
@@ -73,20 +73,20 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
void setExpectation(const Eigen::VectorXs& expectation);
// wolf tree access
- FrameBasePtr getFramePtr() const;
+ FrameBasePtr getFrame() const;
- CaptureBasePtr getCapturePtr() const;
- void setCapturePtr(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
+ CaptureBasePtr getCapture() const;
+ void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
- ConstraintBasePtr addConstraint(ConstraintBasePtr _co_ptr);
- ConstraintBaseList& getConstraintList();
+ FactorBasePtr addFactor(FactorBasePtr _co_ptr);
+ FactorBasePtrList& getFactorList();
- virtual ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- ConstraintBaseList& getConstrainedByList();
+ FactorBasePtrList& getConstrainedByList();
- // all constraints
- void getConstraintList(ConstraintBaseList & _ctr_list);
+ // all factors
+ void getFactorList(FactorBasePtrList & _fac_list);
private:
Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
@@ -96,7 +96,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
// IMPLEMENTATION
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
namespace wolf{
@@ -105,7 +105,7 @@ inline unsigned int FeatureBase::getHits() const
return constrained_by_list_.size();
}
-inline ConstraintBaseList& FeatureBase::getConstrainedByList()
+inline FactorBasePtrList& FeatureBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -115,7 +115,7 @@ inline unsigned int FeatureBase::id()
return feature_id_;
}
-inline CaptureBasePtr FeatureBase::getCapturePtr() const
+inline CaptureBasePtr FeatureBase::getCapture() const
{
return capture_ptr_.lock();
}
diff --git a/include/base/feature/feature_odom_2D.h b/include/base/feature/feature_odom_2D.h
index 92fd84fa7482903beb435672d4c480f6708d1d09..27db3e538b7dfd8172a6f4a815db21ca31969e32 100644
--- a/include/base/feature/feature_odom_2D.h
+++ b/include/base/feature/feature_odom_2D.h
@@ -3,8 +3,8 @@
//Wolf includes
#include "base/feature/feature_base.h"
-#include "base/constraint/constraint_odom_2D.h"
-#include "base/constraint/constraint_odom_2D_analytic.h"
+#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_odom_2D_analytic.h"
//std includes
@@ -27,13 +27,13 @@ class FeatureOdom2D : public FeatureBase
virtual ~FeatureOdom2D();
- /** \brief Generic interface to find constraints
+ /** \brief Generic interface to find factors
*
* TBD
- * Generic interface to find constraints between this feature and a map (static/slam) or a previous feature
+ * Generic interface to find factors between this feature and a map (static/slam) or a previous feature
*
**/
- virtual void findConstraints();
+ virtual void findFactors();
};
diff --git a/include/base/frame_base.h b/include/base/frame_base.h
index 16d13dbee1f0484dee1f911df14b07fd476a007d..be97eb80a333fac1391f7b19bf12d1895e9dbb6d 100644
--- a/include/base/frame_base.h
+++ b/include/base/frame_base.h
@@ -31,8 +31,8 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
{
private:
TrajectoryBaseWPtr trajectory_ptr_;
- CaptureBaseList capture_list_;
- ConstraintBaseList constrained_by_list_;
+ CaptureBasePtrList capture_list_;
+ FactorBasePtrList constrained_by_list_;
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order: Position, Orientation, Velocity.
static unsigned int frame_id_count_;
@@ -86,17 +86,17 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
protected:
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
public:
- StateBlockPtr getPPtr() const;
- StateBlockPtr getOPtr() const;
- StateBlockPtr getVPtr() const;
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
- void setVPtr(const StateBlockPtr _v_ptr);
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ StateBlockPtr getV() const;
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
+ void setV(const StateBlockPtr _v_ptr);
void registerNewStateBlocks();
void removeStateBlocks();
@@ -119,26 +119,26 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
public:
virtual void setProblem(ProblemPtr _problem) final;
- TrajectoryBasePtr getTrajectoryPtr() const;
- void setTrajectoryPtr(TrajectoryBasePtr _trj_ptr);
+ TrajectoryBasePtr getTrajectory() const;
+ void setTrajectory(TrajectoryBasePtr _trj_ptr);
FrameBasePtr getPreviousFrame() const;
FrameBasePtr getNextFrame() const;
- CaptureBaseList& getCaptureList();
+ CaptureBasePtrList& getCaptureList();
CaptureBasePtr addCapture(CaptureBasePtr _capt_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr);
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type);
- CaptureBaseList getCapturesOf(const SensorBasePtr _sensor_ptr);
+ CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr);
void unlinkCapture(CaptureBasePtr _cap_ptr);
- ConstraintBasePtr getConstraintOf(const ProcessorBasePtr _processor_ptr);
- ConstraintBasePtr getConstraintOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr);
+ FactorBasePtr getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type);
- void getConstraintList(ConstraintBaseList& _ctr_list);
- virtual ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
+ void getFactorList(FactorBasePtrList& _fac_list);
+ virtual FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- ConstraintBaseList& getConstrainedByList();
+ FactorBasePtrList& getConstrainedByList();
public:
static FrameBasePtr create_PO_2D (const FrameType & _tp,
@@ -158,7 +158,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
#include "base/trajectory_base.h"
#include "base/capture/capture_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/state_block.h"
namespace wolf {
@@ -193,52 +193,52 @@ inline std::vector<StateBlockPtr>& FrameBase::getStateBlockVec()
return state_block_vec_;
}
-inline StateBlockPtr FrameBase::getPPtr() const
+inline StateBlockPtr FrameBase::getP() const
{
return state_block_vec_[0];
}
-inline void FrameBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void FrameBase::setP(const StateBlockPtr _p_ptr)
{
state_block_vec_[0] = _p_ptr;
}
-inline StateBlockPtr FrameBase::getOPtr() const
+inline StateBlockPtr FrameBase::getO() const
{
return state_block_vec_[1];
}
-inline void FrameBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void FrameBase::setO(const StateBlockPtr _o_ptr)
{
state_block_vec_[1] = _o_ptr;
}
-inline StateBlockPtr FrameBase::getVPtr() const
+inline StateBlockPtr FrameBase::getV() const
{
return state_block_vec_[2];
}
-inline void FrameBase::setVPtr(const StateBlockPtr _v_ptr)
+inline void FrameBase::setV(const StateBlockPtr _v_ptr)
{
state_block_vec_[2] = _v_ptr;
}
-inline StateBlockPtr FrameBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr FrameBase::getStateBlock(unsigned int _i) const
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
-inline void FrameBase::setStateBlockPtr(unsigned int _i, const StateBlockPtr _sb_ptr)
+inline void FrameBase::setStateBlock(unsigned int _i, const StateBlockPtr _sb_ptr)
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
state_block_vec_[_i] = _sb_ptr;
}
-inline TrajectoryBasePtr FrameBase::getTrajectoryPtr() const
+inline TrajectoryBasePtr FrameBase::getTrajectory() const
{
return trajectory_ptr_.lock();
}
-inline CaptureBaseList& FrameBase::getCaptureList()
+inline CaptureBasePtrList& FrameBase::getCaptureList()
{
return capture_list_;
}
@@ -261,12 +261,12 @@ inline void FrameBase::setProblem(ProblemPtr _problem)
cap->setProblem(_problem);
}
-inline ConstraintBaseList& FrameBase::getConstrainedByList()
+inline FactorBasePtrList& FrameBase::getConstrainedByList()
{
return constrained_by_list_;
}
-inline void FrameBase::setTrajectoryPtr(TrajectoryBasePtr _trj_ptr)
+inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
{
trajectory_ptr_ = _trj_ptr;
}
diff --git a/include/base/hardware_base.h b/include/base/hardware_base.h
index f3679dc4ab7d7694bd3058eaf86a6158e9218283..6dd60648b3961dd8f88244b240eebf7c4d88e37d 100644
--- a/include/base/hardware_base.h
+++ b/include/base/hardware_base.h
@@ -17,14 +17,14 @@ namespace wolf {
class HardwareBase : public NodeBase, public std::enable_shared_from_this<HardwareBase>
{
private:
- SensorBaseList sensor_list_;
+ SensorBasePtrList sensor_list_;
public:
HardwareBase();
virtual ~HardwareBase();
virtual SensorBasePtr addSensor(SensorBasePtr _sensor_ptr);
- SensorBaseList& getSensorList();
+ SensorBasePtrList& getSensorList();
};
} // namespace wolf
@@ -33,7 +33,7 @@ class HardwareBase : public NodeBase, public std::enable_shared_from_this<Hardwa
namespace wolf {
-inline SensorBaseList& HardwareBase::getSensorList()
+inline SensorBasePtrList& HardwareBase::getSensorList()
{
return sensor_list_;
}
diff --git a/include/base/landmark/landmark_base.h b/include/base/landmark/landmark_base.h
index 8b365e9be506f9271571542970683032326819f9..9019ee85efd04f54ddfa920e98413c8aabf3846e 100644
--- a/include/base/landmark/landmark_base.h
+++ b/include/base/landmark/landmark_base.h
@@ -24,7 +24,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
{
private:
MapBaseWPtr map_ptr_;
- ConstraintBaseList constrained_by_list_;
+ FactorBasePtrList constrained_by_list_;
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O.
static unsigned int landmark_id_count_;
@@ -62,12 +62,12 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
std::vector<StateBlockPtr> getUsedStateBlockVec() const;
- StateBlockPtr getStateBlockPtr(unsigned int _i) const;
- void setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr);
- StateBlockPtr getPPtr() const;
- StateBlockPtr getOPtr() const;
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
+ StateBlockPtr getStateBlock(unsigned int _i) const;
+ void setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr);
+ StateBlockPtr getP() const;
+ StateBlockPtr getO() const;
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
virtual void registerNewStateBlocks();
Eigen::VectorXs getState() const;
void getState(Eigen::VectorXs& _state) const;
@@ -86,19 +86,19 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Navigate wolf tree
virtual void setProblem(ProblemPtr _problem) final;
- ConstraintBasePtr addConstrainedBy(ConstraintBasePtr _ctr_ptr);
+ FactorBasePtr addConstrainedBy(FactorBasePtr _fac_ptr);
unsigned int getHits() const;
- ConstraintBaseList& getConstrainedByList();
+ FactorBasePtrList& getConstrainedByList();
- void setMapPtr(const MapBasePtr _map_ptr);
- MapBasePtr getMapPtr();
+ void setMap(const MapBasePtr _map_ptr);
+ MapBasePtr getMap();
};
}
#include "base/map_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/state_block.h"
namespace wolf{
@@ -108,12 +108,12 @@ inline void LandmarkBase::setProblem(ProblemPtr _problem)
NodeBase::setProblem(_problem);
}
-inline MapBasePtr LandmarkBase::getMapPtr()
+inline MapBasePtr LandmarkBase::getMap()
{
return map_ptr_.lock();
}
-inline void LandmarkBase::setMapPtr(const MapBasePtr _map_ptr)
+inline void LandmarkBase::setMap(const MapBasePtr _map_ptr)
{
map_ptr_ = _map_ptr;
}
@@ -135,7 +135,7 @@ inline unsigned int LandmarkBase::getHits() const
return constrained_by_list_.size();
}
-inline ConstraintBaseList& LandmarkBase::getConstrainedByList()
+inline FactorBasePtrList& LandmarkBase::getConstrainedByList()
{
return constrained_by_list_;
}
@@ -150,35 +150,35 @@ inline std::vector<StateBlockPtr>& LandmarkBase::getStateBlockVec()
return state_block_vec_;
}
-inline StateBlockPtr LandmarkBase::getStateBlockPtr(unsigned int _i) const
+inline StateBlockPtr LandmarkBase::getStateBlock(unsigned int _i) const
{
assert (_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
-inline void LandmarkBase::setStateBlockPtr(unsigned int _i, StateBlockPtr _sb_ptr)
+inline void LandmarkBase::setStateBlock(unsigned int _i, StateBlockPtr _sb_ptr)
{
state_block_vec_[_i] = _sb_ptr;
}
-inline StateBlockPtr LandmarkBase::getPPtr() const
+inline StateBlockPtr LandmarkBase::getP() const
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
-inline StateBlockPtr LandmarkBase::getOPtr() const
+inline StateBlockPtr LandmarkBase::getO() const
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
-inline void LandmarkBase::setPPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setP(const StateBlockPtr _st_ptr)
{
- setStateBlockPtr(0, _st_ptr);
+ setStateBlock(0, _st_ptr);
}
-inline void LandmarkBase::setOPtr(const StateBlockPtr _st_ptr)
+inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
{
- setStateBlockPtr(1, _st_ptr);
+ setStateBlock(1, _st_ptr);
}
inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index 4414a69f6175f36280656c2df33e4e94697d0d7d..d6b5d51f9edee7b70a7376a2e0771b34c1de612e 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -74,7 +74,7 @@ class LandmarkPolyline2D : public LandmarkBase
const Eigen::VectorXs getPointVector(int _i) const;
- StateBlockPtr getPointStateBlockPtr(int _i);
+ StateBlockPtr getPointStateBlock(int _i);
/** \brief Gets a vector of all state blocks pointers
**/
diff --git a/include/base/map_base.h b/include/base/map_base.h
index f9e5d32b6d344fa25077b30dd035835bb5378299..a8b447905d59c8ffb30be66cd1b91a6b36dc9ae6 100644
--- a/include/base/map_base.h
+++ b/include/base/map_base.h
@@ -20,15 +20,15 @@ namespace wolf {
class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
{
private:
- LandmarkBaseList landmark_list_;
+ LandmarkBasePtrList landmark_list_;
public:
MapBase();
~MapBase();
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
- virtual void addLandmarkList(LandmarkBaseList& _landmark_list);
- LandmarkBaseList& getLandmarkList();
+ virtual void addLandmarkList(LandmarkBasePtrList& _landmark_list);
+ LandmarkBasePtrList& getLandmarkList();
void load(const std::string& _map_file_yaml);
void save(const std::string& _map_file_yaml, const std::string& _map_name = "Map automatically saved by Wolf");
@@ -37,7 +37,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
std::string dateTimeNow();
};
-inline LandmarkBaseList& MapBase::getLandmarkList()
+inline LandmarkBasePtrList& MapBase::getLandmarkList()
{
return landmark_list_;
}
diff --git a/include/base/node_base.h b/include/base/node_base.h
index 2426f53a53357005f9e0fcab1ed0af72b7494b7a..ebbc640825d3d6775da395b2fe1d02c03abf8073 100644
--- a/include/base/node_base.h
+++ b/include/base/node_base.h
@@ -22,7 +22,7 @@ namespace wolf {
* - "FRAME"
* - "CAPTURE"
* - "FEATURE"
- * - "CONSTRAINT"
+ * - "FACTOR"
* - "MAP"
* - "LANDMARK"
*
diff --git a/include/base/problem.h b/include/base/problem.h
index f0ade3c321d356a81367fbc1a9dc22d32a4d8da1..84b51c2a6ce9638fef140c84c86240ebb596738d 100644
--- a/include/base/problem.h
+++ b/include/base/problem.h
@@ -38,10 +38,10 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
ProcessorMotionPtr processor_motion_ptr_;
- StateBlockList state_block_list_;
+ StateBlockPtrList state_block_list_;
std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
- std::map<ConstraintBasePtr, Notification> constraint_notification_map_;
+ std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
bool prior_is_set_;
@@ -59,7 +59,7 @@ class Problem : public std::enable_shared_from_this<Problem>
SizeEigen getDim() const;
// Hardware branch ------------------------------------
- HardwareBasePtr getHardwarePtr();
+ HardwareBasePtr getHardware();
void addSensor(SensorBasePtr _sen_ptr);
/** \brief Factory method to install (create and add) sensors only from its properties
@@ -87,7 +87,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief get a sensor pointer by its name
* \param _sensor_name The sensor name, as it was installed with installSensor()
*/
- SensorBasePtr getSensorPtr(const std::string& _sensor_name);
+ SensorBasePtr getSensor(const std::string& _sensor_name);
/** \brief Factory method to install (create, and add to sensor) processors only from its properties
*
@@ -124,10 +124,10 @@ class Problem : public std::enable_shared_from_this<Problem>
void setProcessorMotion(ProcessorMotionPtr _processor_motion_ptr);
ProcessorMotionPtr setProcessorMotion(const std::string& _unique_processor_name);
void clearProcessorMotion();
- ProcessorMotionPtr& getProcessorMotionPtr();
+ ProcessorMotionPtr& getProcessorMotion();
// Trajectory branch ----------------------------------
- TrajectoryBasePtr getTrajectoryPtr();
+ TrajectoryBasePtr getTrajectory();
virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
const Eigen::MatrixXs& _prior_cov, //
const TimeStamp& _ts,
@@ -190,8 +190,8 @@ class Problem : public std::enable_shared_from_this<Problem>
const TimeStamp& _time_stamp);
// Frame getters
- FrameBasePtr getLastFramePtr ( );
- FrameBasePtr getLastKeyFramePtr ( );
+ FrameBasePtr getLastFrame ( );
+ FrameBasePtr getLastKeyFrame ( );
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
/** \brief Give the permission to a processor to create a new keyFrame
@@ -221,9 +221,9 @@ class Problem : public std::enable_shared_from_this<Problem>
bool priorIsSet() const;
// Map branch -----------------------------------------
- MapBasePtr getMapPtr();
+ MapBasePtr getMap();
LandmarkBasePtr addLandmark(LandmarkBasePtr _lmk_ptr);
- void addLandmarkList(LandmarkBaseList& _lmk_list);
+ void addLandmarkList(LandmarkBasePtrList& _lmk_list);
void loadMap(const std::string& _filename_dot_yaml);
void saveMap(const std::string& _filename_dot_yaml, //
const std::string& _map_name = "Map automatically saved by Wolf");
@@ -245,7 +245,7 @@ class Problem : public std::enable_shared_from_this<Problem>
/** \brief Gets a reference to the state blocks list
*/
- StateBlockList& getStateBlockList();
+ StateBlockPtrList& getStateBlockPtrList();
/** \brief Notifies a new state block to be added to the solver manager
*/
@@ -255,27 +255,27 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
void removeStateBlock(StateBlockPtr _state_ptr);
- /** \brief Gets a map of constraint notification to be handled by the solver
+ /** \brief Gets a map of factor notification to be handled by the solver
*/
std::map<StateBlockPtr,Notification>& getStateBlockNotificationMap();
- /** \brief Notifies a new constraint to be added to the solver manager
+ /** \brief Notifies a new factor to be added to the solver manager
*/
- ConstraintBasePtr addConstraint(ConstraintBasePtr _constraint_ptr);
+ FactorBasePtr addFactor(FactorBasePtr _factor_ptr);
- /** \brief Notifies a constraint to be removed from the solver manager
+ /** \brief Notifies a factor to be removed from the solver manager
*/
- void removeConstraint(ConstraintBasePtr _constraint_ptr);
+ void removeFactor(FactorBasePtr _factor_ptr);
- /** \brief Gets a map of constraint notification to be handled by the solver
+ /** \brief Gets a map of factor notification to be handled by the solver
*/
- std::map<ConstraintBasePtr, Notification>& getConstraintNotificationMap();
+ std::map<FactorBasePtr, Notification>& getFactorNotificationMap();
// Print and check ---------------------------------------
/**
* \brief print wolf tree
* \param depth : levels to show ( 0: H, T, M : 1: H:S:p, T:F, M:L ; 2: T:F:C ; 3: T:F:C:f ; 4: T:F:C:f:c )
- * \param constr_by: show constraints pointing to F, f and L.
+ * \param constr_by: show factors pointing to F, f and L.
* \param metric : show metric info (status, time stamps, state vectors, measurements)
* \param state_blocks : show state blocks
*/
@@ -303,7 +303,7 @@ inline bool Problem::priorIsSet() const
return prior_is_set_;
}
-inline ProcessorMotionPtr& Problem::getProcessorMotionPtr()
+inline ProcessorMotionPtr& Problem::getProcessorMotion()
{
return processor_motion_ptr_;
}
@@ -313,9 +313,9 @@ inline std::map<StateBlockPtr,Notification>& Problem::getStateBlockNotificationM
return state_block_notification_map_;
}
-inline std::map<ConstraintBasePtr,Notification>& Problem::getConstraintNotificationMap()
+inline std::map<FactorBasePtr,Notification>& Problem::getFactorNotificationMap()
{
- return constraint_notification_map_;
+ return factor_notification_map_;
}
} // namespace wolf
diff --git a/include/base/processor/processor_IMU.h b/include/base/processor/processor_IMU.h
index 98c7cffeef534a179e2127eb4b775e437a92c8d6..53204c6a00bce51200b555c2e2fff61f189c4599 100644
--- a/include/base/processor/processor_IMU.h
+++ b/include/base/processor/processor_IMU.h
@@ -56,7 +56,7 @@ class ProcessorIMU : public ProcessorMotion{
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion,
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
protected:
diff --git a/include/base/processor/processor_base.h b/include/base/processor/processor_base.h
index 23ac82daa6ff6ea65bc3172d4fa0726890e5dff1..84fee5343c63ec3c01b28de1dfbc46c37125f337 100644
--- a/include/base/processor/processor_base.h
+++ b/include/base/processor/processor_base.h
@@ -182,9 +182,9 @@ class ProcessorBase : public NodeBase, public std::enable_shared_from_this<Proce
virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr, const Scalar& _time_tol_other);
- SensorBasePtr getSensorPtr();
- const SensorBasePtr getSensorPtr() const;
- void setSensorPtr(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
+ SensorBasePtr getSensor();
+ const SensorBasePtr getSensor() const;
+ void setSensor(SensorBasePtr _sen_ptr){sensor_ptr_ = _sen_ptr;}
virtual bool isMotion();
@@ -208,7 +208,7 @@ inline void ProcessorBase::setVotingActive(bool _voting_active)
}
#include "base/sensor/sensor_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
namespace wolf {
@@ -222,12 +222,12 @@ inline unsigned int ProcessorBase::id()
return processor_id_;
}
-inline SensorBasePtr ProcessorBase::getSensorPtr()
+inline SensorBasePtr ProcessorBase::getSensor()
{
return sensor_ptr_.lock();
}
-inline const SensorBasePtr ProcessorBase::getSensorPtr() const
+inline const SensorBasePtr ProcessorBase::getSensor() const
{
return sensor_ptr_.lock();
}
diff --git a/include/base/processor/processor_diff_drive.h b/include/base/processor/processor_diff_drive.h
index e219b552f566153659b3f3643ce9a3210b89efe4..d0c8411f285b11b1f727eccd813f80ae092b9fbb 100644
--- a/include/base/processor/processor_diff_drive.h
+++ b/include/base/processor/processor_diff_drive.h
@@ -101,7 +101,7 @@ protected:
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
- virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature,
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
diff --git a/include/base/processor/processor_loopclosure_base.h b/include/base/processor/processor_loopclosure_base.h
index 1ded95bf036f2569e95f1ec9f41b5141789f70d5..a9d8e878eeabdf9e97db4f659ad45a403d9acaca 100644
--- a/include/base/processor/processor_loopclosure_base.h
+++ b/include/base/processor/processor_loopclosure_base.h
@@ -20,7 +20,7 @@ struct ProcessorParamsLoopClosure : public ProcessorParamsBase
*
* This is an abstract class.
*
- * It establishes constraints XXX
+ * It establishes factors XXX
*
* Should you need extra functionality for your derived types,
* you can overload these two methods,
diff --git a/include/base/processor/processor_motion.h b/include/base/processor/processor_motion.h
index df43f9f841e51c7d8d0cae06354b04c0752b159d..b66feb38face40a05ce5c96696920ece1ca86c1b 100644
--- a/include/base/processor/processor_motion.h
+++ b/include/base/processor/processor_motion.h
@@ -38,7 +38,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* This data is, in the general case, in the reference frame of the sensor:
*
* - Beware of the frame transformations Map to Robot, and Robot to Sensor, so that your produced
- * motion Constraints are correctly expressed.
+ * motion Factors are correctly expressed.
* - The robot state R is expressed in a global or 'Map' reference frame, named M.
* - The sensor frame S is expressed in the robot frame R.
* - The motion data data_ is expressed in the sensor frame S.
@@ -47,7 +47,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* - Transform incoming data from sensor frame to robot frame, and then integrate motion in robot frame.
* - Integrate motion directly in sensor frame, and transform to robot frame at the time of:
* - Publishing the robot state (see getCurrentState() and similar functions)
- * - Creating Keyframes and Constraints (see emplaceConstraint() ).
+ * - Creating Keyframes and Factors (see emplaceFactor() ).
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
@@ -421,11 +421,11 @@ class ProcessorMotion : public ProcessorBase
*/
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_own) = 0;
- /** \brief create a constraint and link it in the wolf tree
+ /** \brief create a factor and link it in the wolf tree
* \param _feature_motion: the parent feature
- * \param _frame_origin: the frame constrained by this motion constraint
+ * \param _frame_origin: the frame constrained by this motion factor
*/
- virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin) = 0;
Motion motionZero(const TimeStamp& _ts);
@@ -433,9 +433,9 @@ class ProcessorMotion : public ProcessorBase
public:
//getters
- CaptureMotionPtr getOriginPtr();
- CaptureMotionPtr getLastPtr();
- CaptureMotionPtr getIncomingPtr();
+ CaptureMotionPtr getOrigin();
+ CaptureMotionPtr getLast();
+ CaptureMotionPtr getIncoming();
Scalar getMaxTimeSpan() const;
Scalar getMaxBuffLength() const;
@@ -512,7 +512,7 @@ inline Eigen::VectorXs ProcessorMotion::getCurrentState()
inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x)
{
Scalar Dt = getCurrentTimeStamp() - origin_ptr_->getTimeStamp();
- statePlusDelta(origin_ptr_->getFramePtr()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
+ statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov()
@@ -568,17 +568,17 @@ inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts)
);
}
-inline CaptureMotionPtr ProcessorMotion::getOriginPtr()
+inline CaptureMotionPtr ProcessorMotion::getOrigin()
{
return origin_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getLastPtr()
+inline CaptureMotionPtr ProcessorMotion::getLast()
{
return last_ptr_;
}
-inline CaptureMotionPtr ProcessorMotion::getIncomingPtr()
+inline CaptureMotionPtr ProcessorMotion::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/base/processor/processor_odom_2D.h b/include/base/processor/processor_odom_2D.h
index 83e2100111973f0ae8f7544325ba31032a452cab..d18c9591e5abd9932a7781144f172b9a72826487 100644
--- a/include/base/processor/processor_odom_2D.h
+++ b/include/base/processor/processor_odom_2D.h
@@ -10,7 +10,7 @@
#include "base/processor/processor_motion.h"
#include "base/capture/capture_odom_2D.h"
-#include "base/constraint/constraint_odom_2D.h"
+#include "base/factor/factor_odom_2D.h"
#include "base/rotations.h"
namespace wolf {
@@ -70,7 +70,7 @@ class ProcessorOdom2D : public ProcessorMotion
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
- virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature,
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin) override;
protected:
diff --git a/include/base/processor/processor_odom_3D.h b/include/base/processor/processor_odom_3D.h
index 491ae23c7593d2b9c13ad52ed2b90f046a91274a..2cbc26a068f13c8e316bf876dd8d483b7eb0a125 100644
--- a/include/base/processor/processor_odom_3D.h
+++ b/include/base/processor/processor_odom_3D.h
@@ -11,7 +11,7 @@
#include "base/processor/processor_motion.h"
#include "base/sensor/sensor_odom_3D.h"
#include "base/capture/capture_odom_3D.h"
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
#include "base/rotations.h"
#include <cmath>
@@ -91,7 +91,7 @@ class ProcessorOdom3D : public ProcessorMotion
const VectorXs& _data,
const MatrixXs& _data_cov,
const FrameBasePtr& _frame_origin) override;
- virtual ConstraintBasePtr emplaceConstraint(FeatureBasePtr _feature_motion,
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
CaptureBasePtr _capture_origin) override;
virtual FeatureBasePtr createFeature(CaptureMotionPtr _capture_motion) override;
diff --git a/include/base/processor/processor_tracker.h b/include/base/processor/processor_tracker.h
index f132626d7a028855fbaae0856e09c0995665f9f9..2461a69fe70597ff5a4b97f9b331a8254f66c6cf 100644
--- a/include/base/processor/processor_tracker.h
+++ b/include/base/processor/processor_tracker.h
@@ -42,7 +42,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTracker);
* Each successful correspondence
* results in an extension of the track of the Feature up to the \b incoming Capture.
*
- * It establishes constraints Feature-Feature or Feature-Landmark;
+ * It establishes factors Feature-Feature or Feature-Landmark;
* Implement these options in two separate derived classes:
* - ProcessorTrackerFeature : for Feature-Feature correspondences (no landmarks)
* - ProcessorTrackerLandmark : for Feature-Landmark correspondences (with landmarks)
@@ -55,7 +55,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTracker);
* - if voteForKeyFrame() <=== IMPLEMENT
* - Populate the tracker with new Features : processNew() <=== IMPLEMENT
* - Make a KeyFrame with the \b last Capture: makeFrame(), setKey()
- * - Establish constraints of the new Features: establishConstraints() <=== IMPLEMENT
+ * - Establish factors of the new Features: establishFactors() <=== IMPLEMENT
* - Reset the tracker with the \b last Capture as the new \b origin: reset() <=== IMPLEMENT
* - else
* - Advance the tracker one Capture ahead: advance() <=== IMPLEMENT
@@ -86,8 +86,8 @@ class ProcessorTracker : public ProcessorBase
CaptureBasePtr origin_ptr_; ///< Pointer to the origin of the tracker.
CaptureBasePtr last_ptr_; ///< Pointer to the last tracked capture.
CaptureBasePtr incoming_ptr_; ///< Pointer to the incoming capture being processed.
- FeatureBaseList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
- FeatureBaseList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
+ FeatureBasePtrList new_features_last_; ///< List of new features in \b last for landmark initialization and new key-frame creation.
+ FeatureBasePtrList new_features_incoming_; ///< list of the new features of \b last successfully tracked in \b incoming
SizeStd number_of_tracks_;
@@ -108,9 +108,9 @@ class ProcessorTracker : public ProcessorBase
bool checkTimeTolerance(const FrameBasePtr _frm, const TimeStamp& _ts);
bool checkTimeTolerance(const FrameBasePtr _frm, const CaptureBasePtr _cap);
- virtual CaptureBasePtr getOriginPtr();
- virtual CaptureBasePtr getLastPtr();
- virtual CaptureBasePtr getIncomingPtr();
+ virtual CaptureBasePtr getOrigin();
+ virtual CaptureBasePtr getLast();
+ virtual CaptureBasePtr getIncoming();
protected:
/** Pre-process incoming Capture
@@ -147,18 +147,18 @@ class ProcessorTracker : public ProcessorBase
* This should do one of the following, depending on the design of the tracker -- see use_landmarks_:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last.
+ * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
- * - The Constraints in \b last need to be transferred to \b incoming (moved, not copied).
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Track Features against Landmarks in the Map. Tips:
- * - The links to the Landmarks are in the Features' Constraints in last.
- * - The Constraints in \b last need to be transferred to \b incoming (moved, not copied).
+ * - The links to the Landmarks are in the Features' Factors in last.
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
*
* The function must generate the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
*
- * It must also generate the constraints, of the correct type, derived from ConstraintBase
- * (through ConstraintAnalytic or ConstraintSparse).
+ * It must also generate the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
*/
virtual unsigned int processKnown() = 0;
@@ -183,10 +183,10 @@ class ProcessorTracker : public ProcessorBase
*/
virtual unsigned int processNew(const unsigned int& _max_features) = 0;
- /**\brief Creates and adds constraints from last_ to origin_
+ /**\brief Creates and adds factors from last_ to origin_
*
*/
- virtual void establishConstraints() = 0;
+ virtual void establishFactors() = 0;
/** \brief Reset the tracker using the \b last Capture as the new \b origin.
*/
@@ -194,7 +194,7 @@ class ProcessorTracker : public ProcessorBase
public:
- FeatureBaseList& getNewFeaturesListLast();
+ FeatureBasePtrList& getNewFeaturesListLast();
const SizeStd& previousNumberOfTracks() const
{
@@ -212,13 +212,13 @@ class ProcessorTracker : public ProcessorBase
void addNewFeatureLast(FeatureBasePtr _feature_ptr);
- FeatureBaseList& getNewFeaturesListIncoming();
+ FeatureBasePtrList& getNewFeaturesListIncoming();
void addNewFeatureIncoming(FeatureBasePtr _feature_ptr);
};
-inline FeatureBaseList& ProcessorTracker::getNewFeaturesListLast()
+inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListLast()
{
return new_features_last_;
}
@@ -228,7 +228,7 @@ inline void ProcessorTracker::addNewFeatureLast(FeatureBasePtr _feature_ptr)
new_features_last_.push_back(_feature_ptr);
}
-inline FeatureBaseList& ProcessorTracker::getNewFeaturesListIncoming()
+inline FeatureBasePtrList& ProcessorTracker::getNewFeaturesListIncoming()
{
return new_features_incoming_;
}
@@ -258,17 +258,17 @@ inline void ProcessorTracker::addNewFeatureIncoming(FeatureBasePtr _feature_ptr)
new_features_incoming_.push_back(_feature_ptr);
}
-inline CaptureBasePtr ProcessorTracker::getOriginPtr()
+inline CaptureBasePtr ProcessorTracker::getOrigin()
{
return origin_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getLastPtr()
+inline CaptureBasePtr ProcessorTracker::getLast()
{
return last_ptr_;
}
-inline CaptureBasePtr ProcessorTracker::getIncomingPtr()
+inline CaptureBasePtr ProcessorTracker::getIncoming()
{
return incoming_ptr_;
}
diff --git a/include/base/processor/processor_tracker_feature.h b/include/base/processor/processor_tracker_feature.h
index 52bd493f6870a1424621f5085ebed10b9558df60..d3b0eedd6c9627bb0f56c86689f6f3dada9220f5 100644
--- a/include/base/processor/processor_tracker_feature.h
+++ b/include/base/processor/processor_tracker_feature.h
@@ -43,7 +43,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
* Each successful correspondence
* results in an extension of the track of the Feature up to the \b incoming Capture.
*
- * It establishes constraints Feature-Feature or Feature-Landmark.
+ * It establishes factors Feature-Feature or Feature-Landmark.
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
@@ -55,7 +55,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
* - if voteForKeyFrame() <=== IMPLEMENT
* - Populate the tracker with new Features : processNew() <--- IMPLEMENTED
* - Make a KeyFrame with the \b last Capture: makeFrame(), setKey()
- * - Establish constraints of the new Features: establishConstraints() <--- IMPLEMENTED
+ * - Establish factors of the new Features: establishFactors() <--- IMPLEMENTED
* - Reset the tracker with the \b last Capture as the new \b origin: reset() <--- IMPLEMENTED
* - else
* - Advance the tracker one Capture ahead: advance() <--- IMPLEMENTED
@@ -67,8 +67,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerFeature);
* - processNew() : which calls the pure virtuals:
* - detectNewFeatures() : detects new Features in \b last <=== IMPLEMENT
* - trackFeatures() : track these new Features again in \b incoming <=== IMPLEMENT
- * - establishConstraints() : which calls the pure virtual:
- * - createConstraint() : create constraint of the correct derived type <=== IMPLEMENT
+ * - establishFactors() : which calls the pure virtual:
+ * - createFactor() : create factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can override these two methods,
*
@@ -92,7 +92,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
ProcessorParamsTrackerFeaturePtr params_tracker_feature_;
TrackMatrix track_matrix_;
- FeatureBaseList known_features_incoming_;
+ FeatureBasePtrList known_features_incoming_;
FeatureMatchMap matches_last_from_incoming_;
/** \brief Process known Features
@@ -103,13 +103,13 @@ class ProcessorTrackerFeature : public ProcessorTracker
* This function does:
* - Track Features against other Features in the \b origin Capture. Tips:
* - An intermediary step of matching against Features in the \b last Capture makes tracking easier.
- * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Constraints in \b last.
+ * - Once tracked against last, then the link to Features in \b origin is provided by the Features' Factors in \b last.
* - If required, correct the drift by re-comparing against the Features in origin.
- * - The Constraints in \b last need to be transferred to \b incoming (moved, not copied).
+ * - The Factors in \b last need to be transferred to \b incoming (moved, not copied).
* - Create the necessary Features in the \b incoming Capture,
* of the correct type, derived from FeatureBase.
- * - Create the constraints, of the correct type, derived from ConstraintBase
- * (through ConstraintAnalytic or ConstraintSparse).
+ * - Create the factors, of the correct type, derived from FactorBase
+ * (through FactorAnalytic or FactorSparse).
*/
virtual unsigned int processKnown();
@@ -118,7 +118,7 @@ class ProcessorTrackerFeature : public ProcessorTracker
* \param _features_incoming_out returned list of features found in \b incoming
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
*/
- virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) = 0;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
@@ -153,21 +153,21 @@ class ProcessorTrackerFeature : public ProcessorTracker
*
* The function sets the member new_features_last_, the list of newly detected features.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) = 0;
- /** \brief Create a new constraint and link it to the wolf tree
+ /** \brief Create a new factor and link it to the wolf tree
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
- * This function creates a constraint of the appropriate type for the derived processor.
+ * This function creates a factor of the appropriate type for the derived processor.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) = 0;
- /** \brief Establish constraints between features in Captures \b last and \b origin
+ /** \brief Establish factors between features in Captures \b last and \b origin
*/
- virtual void establishConstraints();
+ virtual void establishFactors();
};
} // namespace wolf
diff --git a/include/base/processor/processor_tracker_feature_corner.h b/include/base/processor/processor_tracker_feature_corner.h
index 5bc33bf47a51b4b267c64ecf916ccc271047c280..e0c2ecbad2f1b89d8e11b15ea174714066ea4f58 100644
--- a/include/base/processor/processor_tracker_feature_corner.h
+++ b/include/base/processor/processor_tracker_feature_corner.h
@@ -13,7 +13,7 @@
#include "base/capture/capture_laser_2D.h"
#include "base/feature/feature_corner_2D.h"
#include "base/landmark/landmark_corner_2D.h"
-#include "base/constraint/constraint_corner_2D.h"
+#include "base/factor/factor_corner_2D.h"
#include "base/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_feature.h"
@@ -56,8 +56,8 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
laserscanutils::CornerFinder corner_finder_;
//TODO: add corner_finder_params
- FeatureBaseList corners_incoming_;
- FeatureBaseList corners_last_;
+ FeatureBasePtrList corners_incoming_;
+ FeatureBasePtrList corners_last_;
Eigen::Matrix3s R_world_sensor_, R_world_sensor_prev_;
Eigen::Matrix3s R_robot_sensor_;
@@ -86,7 +86,7 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
* \param _features_incoming_out returned list of features found in \b incoming
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
*/
- virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences);
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -115,13 +115,13 @@ class ProcessorTrackerFeatureCorner : public ProcessorTrackerFeature
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
private:
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _corner_list);
+ void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
};
diff --git a/include/base/processor/processor_tracker_feature_dummy.h b/include/base/processor/processor_tracker_feature_dummy.h
index dbc024b7c1187c5a9cf4b0753aaa8d0fc56fe050..586cd57d716519a45f01f66050f39f2bb45f6bee 100644
--- a/include/base/processor/processor_tracker_feature_dummy.h
+++ b/include/base/processor/processor_tracker_feature_dummy.h
@@ -10,7 +10,7 @@
#include "base/wolf.h"
#include "base/processor/processor_tracker_feature.h"
-#include "base/constraint/constraint_epipolar.h"
+#include "base/factor/factor_epipolar.h"
namespace wolf
{
@@ -36,7 +36,7 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* \param _features_incoming_out returned list of features found in \b incoming
* \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
*/
- virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences);
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -65,9 +65,9 @@ class ProcessorTrackerFeatureDummy : public ProcessorTrackerFeature
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
};
diff --git a/include/base/processor/processor_tracker_feature_image.h b/include/base/processor/processor_tracker_feature_image.h
index 48f7e7a705bd0b2006f5ec009653b895e211b11d..800ca1211fbdcdd3d8f99781be90d4ddfdbfad11 100644
--- a/include/base/processor/processor_tracker_feature_image.h
+++ b/include/base/processor/processor_tracker_feature_image.h
@@ -8,7 +8,7 @@
#include "base/state_block.h"
#include "base/state_quaternion.h"
#include "base/processor/processor_tracker_feature.h"
-#include "base/constraint/constraint_epipolar.h"
+#include "base/factor/factor_epipolar.h"
#include "base/processor/processor_params_image.h"
// vision_utils
@@ -91,7 +91,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
image_last_ = image_incoming_;
}
- virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences);
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -119,9 +119,9 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
*
* \return The number of detected Features.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
private:
@@ -151,7 +151,7 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
virtual void drawFeatures(cv::Mat _image);
virtual void drawTarget(cv::Mat _image, std::list<cv::Point> _target_list);
virtual void drawRoi(cv::Mat _image, std::list<cv::Rect> _roi_list, cv::Scalar _color);
- virtual void resetVisualizationFlag(FeatureBaseList& _feature_list_last);
+ virtual void resetVisualizationFlag(FeatureBasePtrList& _feature_list_last);
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params,
diff --git a/include/base/processor/processor_tracker_feature_trifocal.h b/include/base/processor/processor_tracker_feature_trifocal.h
index dca3b791a6885b5e89296b6f988838a511c212fa..dba9e91714a2736b7bae8a970dc5f8cde9cc05d6 100644
--- a/include/base/processor/processor_tracker_feature_trifocal.h
+++ b/include/base/processor/processor_tracker_feature_trifocal.h
@@ -26,7 +26,7 @@ struct ProcessorParamsTrackerFeatureTrifocal : public ProcessorParamsTrackerFeat
int min_response_new_feature;
Scalar max_euclidean_distance;
Scalar pixel_noise_std; ///< std noise of the pixel
- int min_track_length_for_constraint; ///< Minimum track length of a matched feature to create a constraint
+ int min_track_length_for_factor; ///< Minimum track length of a matched feature to create a factor
};
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeatureTrifocal);
@@ -66,7 +66,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
* \param _features_incoming_out returned list of features found in \b incoming
* \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr
*/
- virtual unsigned int trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
@@ -92,17 +92,17 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*
* The function sets the member new_features_last_, the list of newly detected features.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out) override;
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out) override;
- /** \brief Create a new constraint and link it to the wolf tree
+ /** \brief Create a new factor and link it to the wolf tree
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
* Implement this method in derived classes.
*
- * This function creates a constraint of the appropriate type for the derived processor.
+ * This function creates a factor of the appropriate type for the derived processor.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -116,7 +116,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/
virtual void resetDerived() override;
- /** \brief Pre-process: check if all captures (prev-origin, origin, last) are initialized to allow constraints creation
+ /** \brief Pre-process: check if all captures (prev-origin, origin, last) are initialized to allow factors creation
*
*/
virtual void preProcess() override;
@@ -126,11 +126,11 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
*/
virtual void postProcess() override;
- /** \brief Establish constraints between features in Captures \b last and \b origin
+ /** \brief Establish factors between features in Captures \b last and \b origin
*/
- virtual void establishConstraints() override;
+ virtual void establishFactors() override;
- CaptureBasePtr getPrevOriginPtr();
+ CaptureBasePtr getPrevOrigin();
bool isInlier(const cv::KeyPoint& _kp_incoming, const cv::KeyPoint& _kp_last);
@@ -144,7 +144,7 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
const SensorBasePtr _sensor_ptr);
};
-inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOriginPtr()
+inline CaptureBasePtr ProcessorTrackerFeatureTrifocal::getPrevOrigin()
{
return prev_origin_ptr_;
}
diff --git a/include/base/processor/processor_tracker_landmark.h b/include/base/processor/processor_tracker_landmark.h
index 293570afa29406dfc8515ef9c7d684af2da35cf9..23e8132e308138b41b9c2e79553c82e4cb4b03ba 100644
--- a/include/base/processor/processor_tracker_landmark.h
+++ b/include/base/processor/processor_tracker_landmark.h
@@ -41,7 +41,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* Each successful correspondence
* results in an extension of the track of the Feature up to the \b incoming Capture.
*
- * This processor creates landmarks for new detected Features, and establishes constraints Feature-Landmark.
+ * This processor creates landmarks for new detected Features, and establishes factors Feature-Landmark.
*
* This tracker builds on top of the ProcessorTracker by implementing some of its pure virtual functions.
* As a reminder, we sketch here the pipeline of the parent ProcessorTracker process() function.
@@ -53,7 +53,7 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - if voteForKeyFrame() <=== IMPLEMENT
* - processNew() : Populate the tracker with new Features <--- IMPLEMENTED
* - makeFrame(), setKey() : Make a KeyFrame with the \b last Capture <--- IMPLEMENTED
- * - establishConstraints() : Establish constraints of the new Features <--- IMPLEMENTED
+ * - establishFactors() : Establish factors of the new Features <--- IMPLEMENTED
* - reset() : Reset the tracker with the \b last Capture as the new \b origin<--- IMPLEMENTED
* - else
* - advance() : Advance the tracker one Capture ahead <--- IMPLEMENTED
@@ -65,8 +65,8 @@ WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmark);
* - detectNewFeatures() : detects new Features in \b last <=== IMPLEMENT
* - createLandmark() : creates a Landmark using a new Feature <=== IMPLEMENT
* - findLandmarks() : find the new Landmarks again in \b incoming <=== IMPLEMENT
- * - establishConstraints() : which calls the pure virtual:
- * - createConstraint() : create a Feature-Landmark constraint of the correct derived type <=== IMPLEMENT
+ * - establishFactors() : which calls the pure virtual:
+ * - createFactor() : create a Feature-Landmark factor of the correct derived type <=== IMPLEMENT
*
* Should you need extra functionality for your derived types, you can overload these two methods,
*
@@ -86,7 +86,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
protected:
ProcessorParamsTrackerLandmarkPtr params_tracker_landmark_;
- LandmarkBaseList new_landmarks_; ///< List of new detected landmarks
+ LandmarkBasePtrList new_landmarks_; ///< List of new detected landmarks
LandmarkMatchMap matches_landmark_from_incoming_;
LandmarkMatchMap matches_landmark_from_last_;
@@ -108,8 +108,8 @@ class ProcessorTrackerLandmark : public ProcessorTracker
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*/
- virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in,
- FeatureBaseList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences) = 0;
/** \brief Vote for KeyFrame generation
@@ -139,7 +139,7 @@ class ProcessorTrackerLandmark : public ProcessorTracker
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out) = 0;
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out) = 0;
/** \brief Creates a landmark for each of new_features_last_
**/
@@ -151,17 +151,17 @@ class ProcessorTrackerLandmark : public ProcessorTracker
*/
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) = 0;
- /** \brief Create a new constraint
+ /** \brief Create a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) = 0;
- /** \brief Establish constraints between features in Capture \b last and landmarks
+ /** \brief Establish factors between features in Capture \b last and landmarks
*/
- virtual void establishConstraints();
+ virtual void establishFactors();
};
}// namespace wolf
diff --git a/include/base/processor/processor_tracker_landmark_corner.h b/include/base/processor/processor_tracker_landmark_corner.h
index bd81b1643980f41468180735b85d9dcabd550346..a25a2646f21efd437201b8cac0c0899164a07597 100644
--- a/include/base/processor/processor_tracker_landmark_corner.h
+++ b/include/base/processor/processor_tracker_landmark_corner.h
@@ -13,7 +13,7 @@
#include "base/capture/capture_laser_2D.h"
#include "base/feature/feature_corner_2D.h"
#include "base/landmark/landmark_corner_2D.h"
-#include "base/constraint/constraint_corner_2D.h"
+#include "base/factor/factor_corner_2D.h"
#include "base/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_landmark.h"
@@ -61,8 +61,8 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
laserscanutils::CornerFinder corner_finder_;
//TODO: add corner_finder_params
- FeatureBaseList corners_incoming_;
- FeatureBaseList corners_last_;
+ FeatureBasePtrList corners_incoming_;
+ FeatureBasePtrList corners_last_;
unsigned int new_corners_th_;
unsigned int loop_frames_th_;
@@ -122,7 +122,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*/
- virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Vote for KeyFrame generation
@@ -144,7 +144,7 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
@@ -152,17 +152,17 @@ class ProcessorTrackerLandmarkCorner : public ProcessorTrackerLandmark
*/
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
- /** \brief Create a new constraint
+ /** \brief Create a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
private:
- void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _corner_list);
+ void extractCorners(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _corner_list);
void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Vector4s& expected_feature_,
Eigen::Matrix3s& expected_feature_cov_);
diff --git a/include/base/processor/processor_tracker_landmark_dummy.h b/include/base/processor/processor_tracker_landmark_dummy.h
index 1d9d66bbaeef70cdf9366180ff55b796d78b73ac..82b446c47d15b282d985a5d6020a9f33c9d79936 100644
--- a/include/base/processor/processor_tracker_landmark_dummy.h
+++ b/include/base/processor/processor_tracker_landmark_dummy.h
@@ -35,7 +35,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*/
- virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Vote for KeyFrame generation
@@ -56,7 +56,7 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
@@ -64,13 +64,13 @@ class ProcessorTrackerLandmarkDummy : public ProcessorTrackerLandmark
*/
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
- /** \brief Create a new constraint
+ /** \brief Create a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
};
inline void ProcessorTrackerLandmarkDummy::postProcess()
diff --git a/include/base/processor/processor_tracker_landmark_image.h b/include/base/processor/processor_tracker_landmark_image.h
index 1b7a0dbd6d5d33d3adb40742768b5470bb2a3c11..d58920cc10b943c74a27dd1c3f61946624747133 100644
--- a/include/base/processor/processor_tracker_landmark_image.h
+++ b/include/base/processor/processor_tracker_landmark_image.h
@@ -76,7 +76,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
std::list<cv::Rect> tracker_roi_;
std::list<cv::Rect> detector_roi_;
std::list<cv::Point> tracker_target_;
- FeatureBaseList feat_lmk_found_;
+ FeatureBasePtrList feat_lmk_found_;
ProcessorTrackerLandmarkImage(ProcessorParamsTrackerLandmarkImagePtr _params_tracker_landmark_image);
virtual ~ProcessorTrackerLandmarkImage();
@@ -115,7 +115,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*/
- virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Vote for KeyFrame generation
@@ -135,7 +135,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
*
* \return The number of detected Features.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Create one landmark
*
@@ -146,11 +146,11 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
- /** \brief Create a new constraint
+ /** \brief Create a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
//Other functions
private:
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
index e8d70824a898a85130775aed2bb712945ef4e6fc..a6f902c930d696dc595ae19f133a0df4fd0e00c2 100644
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ b/include/base/processor/processor_tracker_landmark_polyline.h
@@ -13,8 +13,8 @@
#include "base/capture/capture_laser_2D.h"
#include "base/feature/feature_polyline_2D.h"
#include "base/landmark/landmark_polyline_2D.h"
-#include "base/constraint/constraint_point_2D.h"
-#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/factor/factor_point_2D.h"
+#include "base/factor/factor_point_to_line_2D.h"
#include "base/state_block.h"
#include "base/association/association_tree.h"
#include "base/processor/processor_tracker_landmark.h"
@@ -96,8 +96,8 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
laserscanutils::LineFinderIterative line_finder_;
ProcessorParamsPolylinePtr params_;
- FeatureBaseList polylines_incoming_;
- FeatureBaseList polylines_last_;
+ FeatureBasePtrList polylines_incoming_;
+ FeatureBasePtrList polylines_last_;
Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
Eigen::Matrix2s R_robot_sensor_;
@@ -116,7 +116,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
virtual ~ProcessorTrackerLandmarkPolyline();
virtual void configure(SensorBasePtr _sensor) { };
- const FeatureBaseList& getLastPolylines() const;
+ const FeatureBasePtrList& getLastPolylines() const;
protected:
@@ -133,7 +133,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
* \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
*/
- virtual unsigned int findLandmarks(const LandmarkBaseList& _landmarks_in, FeatureBaseList& _features_incoming_out,
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences);
/** \brief Vote for KeyFrame generation
@@ -155,7 +155,7 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
* The function sets the member new_features_list_, the list of newly detected features,
* to be used for landmark initialization.
*/
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out);
+ virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
/** \brief Creates a landmark for each of new_features_last_
**/
@@ -167,25 +167,25 @@ class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
*/
virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
- /** \brief Establish constraints between features in Captures \b last and \b origin
+ /** \brief Establish factors between features in Captures \b last and \b origin
*/
- virtual void establishConstraints();
+ virtual void establishFactors();
/** \brief look for known objects in the list of unclassified polylines
*/
- void classifyPolilines(LandmarkBaseList& _lmk_list);
+ void classifyPolilines(LandmarkBasePtrList& _lmk_list);
- /** \brief Create a new constraint
+ /** \brief Create a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*
* Implement this method in derived classes.
*/
- virtual ConstraintBasePtr createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
private:
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBaseList& _polyline_list);
+ void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
Eigen::MatrixXs& expected_feature_cov_);
@@ -210,7 +210,7 @@ inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(Proces
{
}
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
// already computed since each scan is computed in preprocess()
_features_incoming_out = std::move(polylines_last_);
@@ -238,7 +238,7 @@ inline void ProcessorTrackerLandmarkPolyline::resetDerived()
polylines_last_ = std::move(polylines_incoming_);
}
-inline const FeatureBaseList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
+inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
{
return polylines_last_;
}
diff --git a/include/base/sensor/sensor_GPS.h b/include/base/sensor/sensor_GPS.h
index 17b6f20331e6e990b876cb52b1b83277d404f404..a56b8c26776e054dc469d563b113df7c4b68c4d3 100644
--- a/include/base/sensor/sensor_GPS.h
+++ b/include/base/sensor/sensor_GPS.h
@@ -2,7 +2,7 @@
#define SENSOR_GPS_H_
/* WARNING: from here you cannot include gps_scan_utils
- * because is included in constraintGPS, and constraintGPS is included in wolf.h (or some other wolf file)
+ * because is included in factorGPS, and factorGPS is included in wolf.h (or some other wolf file)
* Otherwise wolf will not build without my library installed
*
* --- MAYBE IS NO MORE TRUE, AFTER THE INCLUDES FIX!! ---
@@ -31,9 +31,9 @@ public:
//pointer to sensor position, orientation, bias, init vehicle position and orientation
SensorGPS(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, StateBlockPtr _bias_ptr, StateBlockPtr _map_position_ptr, StateBlockPtr _map_orientation_ptr);
- StateBlockPtr getMapPPtr() const;
+ StateBlockPtr getMapP() const;
- StateBlockPtr getMapOPtr() const;
+ StateBlockPtr getMapO() const;
virtual ~SensorGPS();
diff --git a/include/base/sensor/sensor_IMU.h b/include/base/sensor/sensor_IMU.h
index 2c8efa2c9312f62bca4d9d1c2578a458914403bb..c62ec6fc11e2f3e0c194744843bb0b5b8556e460 100644
--- a/include/base/sensor/sensor_IMU.h
+++ b/include/base/sensor/sensor_IMU.h
@@ -38,7 +38,7 @@ class SensorIMU : public SensorBase
Scalar a_noise; //Power Spectral Density (same for all the axis) in micro_g/ sqrt(Hz)
Scalar w_noise; //Rate Noise Spectral Density (same for all the axis) in deg/sec/ sqrt(Hz)
- //This is a trial to constraint how much can the bias change in 1 sec at most
+ //This is a trial to factor how much can the bias change in 1 sec at most
Scalar ab_initial_stdev; //accelerometer micro_g/sec
Scalar wb_initial_stdev; //gyroscope rad/sec
Scalar ab_rate_stdev; //accelerometer micro_g/sec
diff --git a/include/base/sensor/sensor_base.h b/include/base/sensor/sensor_base.h
index 01df97dc63db1cbd6a02abce6625dc6b24247d47..64ed42ba22597606c616af57c9ba23984ad3b2a7 100644
--- a/include/base/sensor/sensor_base.h
+++ b/include/base/sensor/sensor_base.h
@@ -30,7 +30,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
{
private:
HardwareBaseWPtr hardware_ptr_;
- ProcessorBaseList processor_list_;
+ ProcessorBasePtrList processor_list_;
std::vector<StateBlockPtr> state_block_vec_; ///< vector of state blocks, in the order P, O, intrinsic.
SizeEigen calib_size_;
@@ -93,11 +93,11 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
virtual void setProblem(ProblemPtr _problem) final;
- HardwareBasePtr getHardwarePtr();
- void setHardwarePtr(const HardwareBasePtr _hw_ptr);
+ HardwareBasePtr getHardware();
+ void setHardware(const HardwareBasePtr _hw_ptr);
ProcessorBasePtr addProcessor(ProcessorBasePtr _proc_ptr);
- ProcessorBaseList& getProcessorList();
+ ProcessorBasePtrList& getProcessorList();
CaptureBasePtr lastKeyCapture(void);
CaptureBasePtr lastCapture(const TimeStamp& _ts);
@@ -108,8 +108,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
const std::vector<StateBlockPtr>& getStateBlockVec() const;
std::vector<StateBlockPtr>& getStateBlockVec();
StateBlockPtr getStateBlockPtrStatic(unsigned int _i) const;
- StateBlockPtr getStateBlockPtr(unsigned int _i);
- StateBlockPtr getStateBlockPtr(unsigned int _i, const TimeStamp& _ts);
+ StateBlockPtr getStateBlock(unsigned int _i);
+ StateBlockPtr getStateBlock(unsigned int _i, const TimeStamp& _ts);
void setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr);
void resizeStateBlockVec(unsigned int _size);
@@ -118,15 +118,15 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
bool isStateBlockDynamic(unsigned int _i, const TimeStamp& _ts);
bool isStateBlockDynamic(unsigned int _i);
- StateBlockPtr getPPtr(const TimeStamp _ts);
- StateBlockPtr getOPtr(const TimeStamp _ts);
- StateBlockPtr getIntrinsicPtr(const TimeStamp _ts);
- StateBlockPtr getPPtr() ;
- StateBlockPtr getOPtr();
- StateBlockPtr getIntrinsicPtr();
- void setPPtr(const StateBlockPtr _p_ptr);
- void setOPtr(const StateBlockPtr _o_ptr);
- void setIntrinsicPtr(const StateBlockPtr _intr_ptr);
+ StateBlockPtr getP(const TimeStamp _ts);
+ StateBlockPtr getO(const TimeStamp _ts);
+ StateBlockPtr getIntrinsic(const TimeStamp _ts);
+ StateBlockPtr getP() ;
+ StateBlockPtr getO();
+ StateBlockPtr getIntrinsic();
+ void setP(const StateBlockPtr _p_ptr);
+ void setO(const StateBlockPtr _o_ptr);
+ void setIntrinsic(const StateBlockPtr _intr_ptr);
void removeStateBlocks();
void fix();
@@ -136,9 +136,9 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
void fixIntrinsics();
void unfixIntrinsics();
- /** \brief Add an absolute constraint to a parameter
+ /** \brief Add an absolute factor to a parameter
*
- * Add an absolute constraint to a parameter
+ * Add an absolute factor to a parameter
* \param _i state block index (in state_block_vec_) of the parameter to be constrained
* \param _x prior value
* \param _cov covariance
@@ -202,7 +202,7 @@ inline unsigned int SensorBase::id()
return sensor_id_;
}
-inline ProcessorBaseList& SensorBase::getProcessorList()
+inline ProcessorBasePtrList& SensorBase::getProcessorList()
{
return processor_list_;
}
@@ -226,7 +226,7 @@ inline StateBlockPtr SensorBase::getStateBlockPtrStatic(unsigned int _i) const
inline void SensorBase::setStateBlockPtrStatic(unsigned int _i, const StateBlockPtr _sb_ptr)
{
assert (_i < state_block_vec_.size() && "Setting a state block pointer out of the vector range!");
- assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute constraint");
+ assert((params_prior_map_.find(_i) == params_prior_map_.end() || _sb_ptr == nullptr) && "overwriting a state block that has an absolute factor");
state_block_vec_[_i] = _sb_ptr;
}
@@ -258,27 +258,27 @@ inline Eigen::MatrixXs SensorBase::getNoiseCov()
return noise_cov_;
}
-inline HardwareBasePtr SensorBase::getHardwarePtr()
+inline HardwareBasePtr SensorBase::getHardware()
{
return hardware_ptr_.lock();
}
-inline void SensorBase::setPPtr(const StateBlockPtr _p_ptr)
+inline void SensorBase::setP(const StateBlockPtr _p_ptr)
{
setStateBlockPtrStatic(0, _p_ptr);
}
-inline void SensorBase::setOPtr(const StateBlockPtr _o_ptr)
+inline void SensorBase::setO(const StateBlockPtr _o_ptr)
{
setStateBlockPtrStatic(1, _o_ptr);
}
-inline void SensorBase::setIntrinsicPtr(const StateBlockPtr _intr_ptr)
+inline void SensorBase::setIntrinsic(const StateBlockPtr _intr_ptr)
{
setStateBlockPtrStatic(2, _intr_ptr);
}
-inline void SensorBase::setHardwarePtr(const HardwareBasePtr _hw_ptr)
+inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
{
hardware_ptr_ = _hw_ptr;
}
diff --git a/include/base/sensor/sensor_camera.h b/include/base/sensor/sensor_camera.h
index c7db047c3c28808c9dd40374bdf93adcdd894a2f..c5b8f4423661a0b4dc2e5dc00cd17bd3392455cb 100644
--- a/include/base/sensor/sensor_camera.h
+++ b/include/base/sensor/sensor_camera.h
@@ -69,7 +69,7 @@ class SensorCamera : public SensorBase
inline bool SensorCamera::useRawImages()
{
- getIntrinsicPtr()->setState(pinhole_model_raw_);
+ getIntrinsic()->setState(pinhole_model_raw_);
K_ = setIntrinsicMatrix(pinhole_model_raw_);
using_raw_ = true;
@@ -78,7 +78,7 @@ inline bool SensorCamera::useRawImages()
inline bool SensorCamera::useRectifiedImages()
{
- getIntrinsicPtr()->setState(pinhole_model_rectified_);
+ getIntrinsic()->setState(pinhole_model_rectified_);
K_ = setIntrinsicMatrix(pinhole_model_rectified_);
using_raw_ = false;
diff --git a/include/base/solver/solver_manager.h b/include/base/solver/solver_manager.h
index 4801fd57f08f9a546b4255f51f9e11e4ca1bd354..f64b3571970400704e9ea680e291b705362cdebc 100644
--- a/include/base/solver/solver_manager.h
+++ b/include/base/solver/solver_manager.h
@@ -4,7 +4,7 @@
//wolf includes
#include "base/wolf.h"
#include "base/state_block.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
namespace wolf {
@@ -53,7 +53,7 @@ public:
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks) = 0;
- virtual void computeCovariances(const StateBlockList& st_list) = 0;
+ virtual void computeCovariances(const StateBlockPtrList& st_list) = 0;
virtual bool hasConverged() = 0;
@@ -65,7 +65,7 @@ public:
virtual void update();
- ProblemPtr getProblemPtr();
+ ProblemPtr getProblem();
protected:
@@ -76,9 +76,9 @@ protected:
virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
- virtual void addConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+ virtual void addFactor(const FactorBasePtr& fac_ptr) = 0;
- virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr) = 0;
+ virtual void removeFactor(const FactorBasePtr& fac_ptr) = 0;
virtual void addStateBlock(const StateBlockPtr& state_ptr) = 0;
diff --git a/include/base/solver_suitesparse/ccolamd_ordering.h b/include/base/solver_suitesparse/ccolamd_ordering.h
index bae2eff4d4e15611126c3b9806aeff2664740a14..b20b857bdf543f65ec61d23a06a3a6ca9de567cb 100644
--- a/include/base/solver_suitesparse/ccolamd_ordering.h
+++ b/include/base/solver_suitesparse/ccolamd_ordering.h
@@ -44,9 +44,9 @@ class CCOLAMDOrdering
IndexVector p(n + 1), A(Alen);
for (Index i = 0; i <= n; i++)
- p(i) = mat.outerIndexPtr()[i];
+ p(i) = mat.outerIndex()[i];
for (Index i = 0; i < nnz; i++)
- A(i) = mat.innerIndexPtr()[i];
+ A(i) = mat.innerIndex()[i];
// std::cout << "p = " << std::endl << p.transpose() << std::endl;
// std::cout << "A = " << std::endl << A.head(nnz).transpose() << std::endl;
diff --git a/include/base/solver_suitesparse/cost_function_sparse.h b/include/base/solver_suitesparse/cost_function_sparse.h
index 713e29bd3308ac58d4d9c48c267fef2e18817614..bdcde3834cc43b950fd968a1c559ee778365e0b0 100644
--- a/include/base/solver_suitesparse/cost_function_sparse.h
+++ b/include/base/solver_suitesparse/cost_function_sparse.h
@@ -11,7 +11,7 @@
namespace wolf
{
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -23,7 +23,7 @@ template <class ConstraintT,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
-class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
+class CostFunctionSparse : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -37,8 +37,8 @@ class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
BLOCK_9_SIZE>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparse<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -49,7 +49,7 @@ class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- BLOCK_9_SIZE>(_constraint_ptr)
+ BLOCK_9_SIZE>(_factor_ptr)
{
}
@@ -58,34 +58,34 @@ class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
{
// if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE > 0)
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ , this->jets_9_ ,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE > 0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE > 0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE > 0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE > 0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE > 0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE > 0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE > 0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE > 0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->residuals_jet_);
// else if (BLOCK_0_SIZE > 0 && BLOCK_1_SIZE ==0 && BLOCK_2_SIZE ==0 && BLOCK_3_SIZE ==0 && BLOCK_4_SIZE ==0 &&
// BLOCK_5_SIZE ==0 && BLOCK_6_SIZE ==0 && BLOCK_7_SIZE ==0 && BLOCK_8_SIZE ==0 && BLOCK_9_SIZE == 0)
-// (*this->constraint_ptr_)(this->jets_0_, this->residuals_jet_);
+// (*this->factor_ptr_)(this->jets_0_, this->residuals_jet_);
// else
// assert("Wrong combination of zero sized blocks");
@@ -93,7 +93,7 @@ class CostFunctionSparse : CostFunctionSparseBase<ConstraintT,
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -104,7 +104,7 @@ template <class ConstraintT,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -115,7 +115,7 @@ class CostFunctionSparse<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -129,8 +129,8 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -141,18 +141,18 @@ class CostFunctionSparse<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_, this->jets_8_ ,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -162,7 +162,7 @@ template <class ConstraintT,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE,
unsigned int BLOCK_7_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -173,7 +173,7 @@ class CostFunctionSparse<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -187,8 +187,8 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -199,18 +199,18 @@ class CostFunctionSparse<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_, this->jets_7_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -219,7 +219,7 @@ template <class ConstraintT,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE,
unsigned int BLOCK_6_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -229,7 +229,7 @@ class CostFunctionSparse<ConstraintT,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -242,8 +242,8 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -253,18 +253,18 @@ class CostFunctionSparse<ConstraintT,
BLOCK_5_SIZE,
BLOCK_6_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_, this->jets_6_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -272,7 +272,7 @@ template <class ConstraintT,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE,
unsigned int BLOCK_5_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -281,7 +281,7 @@ class CostFunctionSparse<ConstraintT,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -293,8 +293,8 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -303,25 +303,25 @@ class CostFunctionSparse<ConstraintT,
BLOCK_4_SIZE,
BLOCK_5_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_, this->jets_5_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE,
unsigned int BLOCK_4_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -329,7 +329,7 @@ class CostFunctionSparse<ConstraintT,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -340,8 +340,8 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -349,31 +349,31 @@ class CostFunctionSparse<ConstraintT,
BLOCK_3_SIZE,
BLOCK_4_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_, this->jets_4_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE,
unsigned int BLOCK_3_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -383,37 +383,37 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
BLOCK_3_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_, this->jets_3_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
unsigned int BLOCK_2_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -422,34 +422,34 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
BLOCK_2_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_, this->jets_2_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -457,50 +457,50 @@ class CostFunctionSparse<ConstraintT,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_, this->jets_1_,this->residuals_jet_);
}
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE>
-class CostFunctionSparse<ConstraintT,
+class CostFunctionSparse<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
- 0> : CostFunctionSparseBase<ConstraintT,
+ 0> : CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
0>
{
public:
- CostFunctionSparse(ConstraintT* _constraint_ptr) :
- CostFunctionSparseBase<ConstraintT,
+ CostFunctionSparse(FactorT* _factor_ptr) :
+ CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
0,
- 0>(_constraint_ptr)
+ 0>(_factor_ptr)
{
}
void callFunctor()
{
- (*this->constraint_ptr_)(this->jets_0_,this->residuals_jet_);
+ (*this->factor_ptr_)(this->jets_0_,this->residuals_jet_);
}
};
diff --git a/include/base/solver_suitesparse/cost_function_sparse_base.h b/include/base/solver_suitesparse/cost_function_sparse_base.h
index d10fbab5886f344a0eb9c73262daba7c533b6393..9aeff876c9134363b1c95055f4fbef9b1d87a740 100644
--- a/include/base/solver_suitesparse/cost_function_sparse_base.h
+++ b/include/base/solver_suitesparse/cost_function_sparse_base.h
@@ -18,7 +18,7 @@
namespace wolf
{
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE = 0,
@@ -43,7 +43,7 @@ class CostFunctionSparseBase : CostFunctionBase
BLOCK_8_SIZE +
BLOCK_9_SIZE> WolfJet;
protected:
- ConstraintT* constraint_ptr_;
+ FactorT* factor_ptr_;
WolfJet jets_0_[BLOCK_0_SIZE];
WolfJet jets_1_[BLOCK_1_SIZE];
WolfJet jets_2_[BLOCK_2_SIZE];
@@ -58,12 +58,12 @@ class CostFunctionSparseBase : CostFunctionBase
public:
- /** \brief Constructor with constraint pointer
+ /** \brief Constructor with factor pointer
*
- * Constructor with constraint pointer
+ * Constructor with factor pointer
*
*/
- CostFunctionSparseBase(ConstraintT* _constraint_ptr);
+ CostFunctionSparseBase(FactorT* _factor_ptr);
/** \brief Default destructor
*
@@ -72,18 +72,18 @@ class CostFunctionSparseBase : CostFunctionBase
*/
virtual ~CostFunctionSparseBase();
- /** \brief Evaluate residuals and jacobians of the constraint in the current x
+ /** \brief Evaluate residuals and jacobians of the factor in the current x
*
- * Evaluate residuals and jacobians of the constraint in the current x
+ * Evaluate residuals and jacobians of the factor in the current x
*
*/
virtual void evaluateResidualJacobians();
protected:
- /** \brief Calls the functor of the constraint evaluating jets
+ /** \brief Calls the functor of the factor evaluating jets
*
- * Calls the functor of the constraint evaluating jets
+ * Calls the functor of the factor evaluating jets
*
*/
virtual void callFunctor() = 0;
@@ -103,7 +103,7 @@ class CostFunctionSparseBase : CostFunctionBase
void evaluateX();
};
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -115,7 +115,7 @@ template <class ConstraintT,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
-CostFunctionSparseBase<ConstraintT,
+CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -126,14 +126,14 @@ CostFunctionSparseBase<ConstraintT,
BLOCK_6_SIZE,
BLOCK_7_SIZE,
BLOCK_8_SIZE,
- BLOCK_9_SIZE>::CostFunctionSparseBase(ConstraintT* _constraint_ptr) :
+ BLOCK_9_SIZE>::CostFunctionSparseBase(FactorT* _factor_ptr) :
CostFunctionBase(MEASUREMENT_SIZE, BLOCK_0_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE, BLOCK_3_SIZE, BLOCK_4_SIZE, BLOCK_5_SIZE, BLOCK_6_SIZE, BLOCK_7_SIZE, BLOCK_8_SIZE,BLOCK_9_SIZE),
- constraint_ptr_(_constraint_ptr)
+ factor_ptr_(_factor_ptr)
{
initializeJets();
}
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -145,7 +145,7 @@ template <class ConstraintT,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
-CostFunctionSparseBase<ConstraintT,
+CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -161,7 +161,7 @@ CostFunctionSparseBase<ConstraintT,
}
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -173,7 +173,7 @@ template <class ConstraintT,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
-void CostFunctionSparseBase<ConstraintT,
+void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -205,7 +205,7 @@ void CostFunctionSparseBase<ConstraintT,
residual_(i) = residuals_jet_[i].a;
}
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -217,7 +217,7 @@ const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<ConstraintT,
+ void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -263,7 +263,7 @@ const unsigned int MEASUREMENT_SIZE,
jets_9_[i] = WolfJet(0, last_jet_idx++);
}
-template <class ConstraintT,
+template <class FactorT,
const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_0_SIZE,
unsigned int BLOCK_1_SIZE,
@@ -275,7 +275,7 @@ const unsigned int MEASUREMENT_SIZE,
unsigned int BLOCK_7_SIZE,
unsigned int BLOCK_8_SIZE,
unsigned int BLOCK_9_SIZE>
- void CostFunctionSparseBase<ConstraintT,
+ void CostFunctionSparseBase<FactorT,
MEASUREMENT_SIZE,
BLOCK_0_SIZE,
BLOCK_1_SIZE,
@@ -290,34 +290,34 @@ const unsigned int MEASUREMENT_SIZE,
{
// JET 0
for (int i = 0; i < BLOCK_0_SIZE; i++)
- jets_0_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(0)+i);
+ jets_0_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(0)+i);
// JET 1
for (int i = 0; i < BLOCK_1_SIZE; i++)
- jets_1_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(1)+i);
+ jets_1_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(1)+i);
// JET 2
for (int i = 0; i < BLOCK_2_SIZE; i++)
- jets_2_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(2)+i);
+ jets_2_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(2)+i);
// JET 3
for (int i = 0; i < BLOCK_3_SIZE; i++)
- jets_3_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(3)+i);
+ jets_3_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(3)+i);
// JET 4
for (int i = 0; i < BLOCK_4_SIZE; i++)
- jets_4_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(4)+i);
+ jets_4_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(4)+i);
// JET 5
for (int i = 0; i < BLOCK_5_SIZE; i++)
- jets_5_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(5)+i);
+ jets_5_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(5)+i);
// JET 6
for (int i = 0; i < BLOCK_6_SIZE; i++)
- jets_6_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(6)+i);
+ jets_6_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(6)+i);
// JET 7
for (int i = 0; i < BLOCK_7_SIZE; i++)
- jets_7_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(7)+i);
+ jets_7_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(7)+i);
// JET 8
for (int i = 0; i < BLOCK_8_SIZE; i++)
- jets_8_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(8)+i);
+ jets_8_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(8)+i);
// JET 9
for (int i = 0; i < BLOCK_9_SIZE; i++)
- jets_9_[i].a = *(constraint_ptr_->getStateScalarPtrVector().at(9)+i);
+ jets_9_[i].a = *(factor_ptr_->getStateScalarPtrVector().at(9)+i);
}
} // wolf namespace
diff --git a/include/base/solver_suitesparse/qr_solver.h b/include/base/solver_suitesparse/qr_solver.h
index 6d032e8b2aadfbeac183e0e2db23a00d934db711..154b78639d03c71368b65bbaf65ee778421ba313 100644
--- a/include/base/solver_suitesparse/qr_solver.h
+++ b/include/base/solver_suitesparse/qr_solver.h
@@ -14,10 +14,10 @@
//Wolf includes
#include "base/state_block.h"
-#include "../constraint_sparse.h"
-#include "base/constraint/constraint_odom_2D.h"
-#include "base/constraint/constraint_corner_2D.h"
-#include "base/constraint/constraint_container.h"
+#include "../factor_sparse.h"
+#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_corner_2D.h"
+#include "base/factor/factor_container.h"
#include "base/solver_suitesparse/sparse_utils.h"
// wolf solver
@@ -28,7 +28,7 @@
#include <eigen3/Eigen/OrderingMethods>
#include <eigen3/Eigen/SparseQR>
#include <Eigen/StdVector>
-#include "base/constraint/constraint_pose_2D.h"
+#include "base/factor/factor_pose_2D.h"
namespace wolf
{
@@ -41,7 +41,7 @@ class SolverQR
Eigen::SparseMatrix<double> A_, R_;
Eigen::VectorXd b_, x_incr_;
std::vector<StateBlockPtr> nodes_;
- std::vector<ConstraintBasePtr> constraints_;
+ std::vector<FactorBasePtr> factors_;
std::vector<CostFunctionBasePtr> cost_functions_;
// ordering
@@ -51,8 +51,8 @@ class SolverQR
Eigen::CCOLAMDOrdering<int> orderer_;
Eigen::VectorXi node_ordering_restrictions_;
Eigen::ArrayXi node_locations_;
- std::vector<unsigned int> constraint_locations_;
- unsigned int n_new_constraints_;
+ std::vector<unsigned int> factor_locations_;
+ unsigned int n_new_factors_;
// time
clock_t t_ordering_, t_solving_, t_managing_;
@@ -60,11 +60,11 @@ class SolverQR
public:
SolverQR(ProblemPtr problem_ptr_) :
- problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_constraints_(
+ problem_ptr_(problem_ptr_), A_(0, 0), R_(0, 0), A_nodes_(0, 0), acc_node_permutation_(0), n_new_factors_(
0), time_ordering_(0), time_solving_(0), time_managing_(0)
{
node_locations_.resize(0);
- constraint_locations_.resize(0);
+ factor_locations_.resize(0);
}
virtual ~SolverQR()
@@ -99,25 +99,25 @@ class SolverQR
}
problem_ptr_->getStateBlockNotificationList().pop_front();
}
- // UPDATE CONSTRAINTS
- while (!problem_ptr_->getConstraintNotificationList().empty())
+ // UPDATE FACTORS
+ while (!problem_ptr_->getFactorNotificationList().empty())
{
- switch (problem_ptr_->getConstraintNotificationList().front().notification_)
+ switch (problem_ptr_->getFactorNotificationList().front().notification_)
{
case ADD:
{
- addConstraint(problem_ptr_->getConstraintNotificationList().front().constraint_ptr_);
+ addFactor(problem_ptr_->getFactorNotificationList().front().factor_ptr_);
break;
}
case REMOVE:
{
- // TODO: removeConstraint(problem_ptr_->getConstraintNotificationList().front().id_);
+ // TODO: removeFactor(problem_ptr_->getFactorNotificationList().front().id_);
break;
}
default:
- throw std::runtime_error("SolverQR::update: Constraint notification must be ADD or REMOVE.");
+ throw std::runtime_error("SolverQR::update: Factor notification must be ADD or REMOVE.");
}
- problem_ptr_->getConstraintNotificationList().pop_front();
+ problem_ptr_->getFactorNotificationList().pop_front();
}
}
@@ -125,13 +125,13 @@ class SolverQR
{
t_managing_ = clock();
- std::cout << "adding state unit " << _state_ptr->getPtr() << std::endl;
+ std::cout << "adding state unit " << _state_ptr->get() << std::endl;
if (!_state_ptr->isFixed())
{
nodes_.push_back(_state_ptr);
- id_2_idx_[_state_ptr->getPtr()] = nodes_.size() - 1;
+ id_2_idx_[_state_ptr->get()] = nodes_.size() - 1;
- std::cout << "idx " << id_2_idx_[_state_ptr->getPtr()] << std::endl;
+ std::cout << "idx " << id_2_idx_[_state_ptr->get()] << std::endl;
// Resize accumulated permutations
augmentPermutation(acc_node_permutation_, nNodes());
@@ -152,17 +152,17 @@ class SolverQR
//TODO
}
- void addConstraint(ConstraintBasePtr _constraint_ptr)
+ void addFactor(FactorBasePtr _factor_ptr)
{
- std::cout << "adding constraint " << _constraint_ptr->id() << std::endl;
+ std::cout << "adding factor " << _factor_ptr->id() << std::endl;
t_managing_ = clock();
- constraints_.push_back(_constraint_ptr);
- cost_functions_.push_back(createCostFunction(_constraint_ptr));
+ factors_.push_back(_factor_ptr);
+ cost_functions_.push_back(createCostFunction(_factor_ptr));
- unsigned int meas_dim = _constraint_ptr->getSize();
+ unsigned int meas_dim = _factor_ptr->getSize();
- std::vector<Eigen::MatrixXs> jacobians(_constraint_ptr->getStateBlockPtrVector().size());
+ std::vector<Eigen::MatrixXs> jacobians(_factor_ptr->getStateBlockPtrVector().size());
Eigen::VectorXs error(meas_dim);
cost_functions_.back()->evaluateResidualJacobians();
@@ -170,17 +170,17 @@ class SolverQR
cost_functions_.back()->getJacobians(jacobians);
std::vector<unsigned int> idxs;
- for (unsigned int i = 0; i < _constraint_ptr->getStateBlockPtrVector().size(); i++)
- if (!_constraint_ptr->getStateBlockPtrVector().at(i)->isFixed())
- idxs.push_back(id_2_idx_[_constraint_ptr->getStateBlockPtrVector().at(i)->getPtr()]);
+ for (unsigned int i = 0; i < _factor_ptr->getStateBlockPtrVector().size(); i++)
+ if (!_factor_ptr->getStateBlockPtrVector().at(i)->isFixed())
+ idxs.push_back(id_2_idx_[_factor_ptr->getStateBlockPtrVector().at(i)->get()]);
- n_new_constraints_++;
- constraint_locations_.push_back(A_.rows());
+ n_new_factors_++;
+ factor_locations_.push_back(A_.rows());
// Resize problem
A_.conservativeResize(A_.rows() + meas_dim, A_.cols());
b_.conservativeResize(b_.size() + meas_dim);
- A_nodes_.conservativeResize(constraints_.size(), nNodes());
+ A_nodes_.conservativeResize(factors_.size(), nNodes());
// ADD MEASUREMENTS
for (unsigned int j = 0; j < idxs.size(); j++)
@@ -208,7 +208,7 @@ class SolverQR
// full problem ordering
if (_first_ordered_idx == -1)
{
- // ordering ordering constraints
+ // ordering ordering factors
node_ordering_restrictions_.resize(nNodes());
node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
@@ -241,7 +241,7 @@ class SolverQR
//std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
Eigen::SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
- // _partial_ordering ordering_ constraints
+ // _partial_ordering ordering_ factors
node_ordering_restrictions_.resize(ordered_nodes);
node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
@@ -278,16 +278,16 @@ class SolverQR
{
unsigned int first_ordered_node = nNodes();
unsigned int first_ordered_idx;
- for (unsigned int i = 0; i < n_new_constraints_; i++)
+ for (unsigned int i = 0; i < n_new_factors_; i++)
{
- ConstraintBasePtr ct_ptr = constraints_.at(constraints_.size() - 1 - i);
- std::cout << "constraint: " << i << " id: " << constraints_.at(constraints_.size() - 1 - i)->id()
+ FactorBasePtr ct_ptr = factors_.at(factors_.size() - 1 - i);
+ std::cout << "factor: " << i << " id: " << factors_.at(factors_.size() - 1 - i)->id()
<< std::endl;
for (unsigned int j = 0; j < ct_ptr->getStateBlockPtrVector().size(); j++)
{
if (!ct_ptr->getStateBlockPtrVector().at(j)->isFixed())
{
- unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->getPtr()];
+ unsigned int idx = id_2_idx_[ct_ptr->getStateBlockPtrVector().at(j)->get()];
//std::cout << "estimated idx " << idx << std::endl;
//std::cout << "node_order(idx) " << node_order(idx) << std::endl;
//std::cout << "first_ordered_node " << first_ordered_node << std::endl;
@@ -309,7 +309,7 @@ class SolverQR
bool solve(const unsigned int mode)
{
- if (n_new_constraints_ == 0)
+ if (n_new_factors_ == 0)
return 1;
std::cout << "solving mode " << mode << std::endl;
@@ -377,10 +377,10 @@ class SolverQR
// finding measurements block
Eigen::SparseMatrix<int> measurements_to_initial = A_nodes_.col(nodeOrder(first_ordered_idx));
//std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- //std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
+ //std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
unsigned int first_ordered_measurement =
- measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
- unsigned int ordered_measurements = A_.rows() - constraint_locations_.at(first_ordered_measurement);
+ measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
+ unsigned int ordered_measurements = A_.rows() - factor_locations_.at(first_ordered_measurement);
unsigned int ordered_variables = A_.cols() - nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
unsigned int unordered_variables = nodeLocation(first_ordered_idx); //nodes_.at(first_ordered_idx).location;
@@ -423,14 +423,14 @@ class SolverQR
// UPDATE X VALUE
for (unsigned int i = 0; i < nodes_.size(); i++)
{
- Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->getPtr(), nodes_.at(i)->getSize());
+ Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
b_.setZero();
time_solving_ += ((double)clock() - t_solving_) / CLOCKS_PER_SEC;
- n_new_constraints_ = 0;
+ n_new_factors_ = 0;
return 1;
}
@@ -528,37 +528,37 @@ class SolverQR
return nodes_.size();
}
- CostFunctionBasePtr createCostFunction(ConstraintBasePtr _corrPtr)
+ CostFunctionBasePtr createCostFunction(FactorBasePtr _fac_ptr)
{
- //std::cout << "adding ctr " << _corrPtr->id() << std::endl;
- //_corrPtr->print();
+ //std::cout << "adding fac " << _fac_ptr->id() << std::endl;
+ //_fac_ptr->print();
- switch (_corrPtr->getTypeId())
+ switch (_fac_ptr->getTypeId())
{
- case CTR_GPS_FIX_2D:
+ case FAC_GPS_FIX_2D:
{
- ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
- return (CostFunctionBasePtr)(new CostFunctionSparse<ConstraintGPS2D, specific_ptr->residualSize,
+ FactorGPS2D* specific_ptr = (FactorGPS2D*)(_fac_ptr);
+ return (CostFunctionBasePtr)(new CostFunctionSparse<FactorGPS2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr));
break;
}
- case CTR_ODOM_2D:
+ case FAC_ODOM_2D:
{
- ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
- return (CostFunctionBasePtr)new CostFunctionSparse<ConstraintOdom2D, specific_ptr->residualSize,
+ FactorOdom2D* specific_ptr = (FactorOdom2D*)(_fac_ptr);
+ return (CostFunctionBasePtr)new CostFunctionSparse<FactorOdom2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D:
+ case FAC_CORNER_2D:
{
- ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
- return (CostFunctionBasePtr)new CostFunctionSparse<ConstraintCorner2D, specific_ptr->residualSize,
+ FactorCorner2D* specific_ptr = (FactorCorner2D*)(_fac_ptr);
+ return (CostFunctionBasePtr)new CostFunctionSparse<FactorCorner2D, specific_ptr->residualSize,
specific_ptr->block0Size, specific_ptr->block1Size, specific_ptr->block2Size,
specific_ptr->block3Size, specific_ptr->block4Size, specific_ptr->block5Size,
specific_ptr->block6Size, specific_ptr->block7Size, specific_ptr->block8Size,
@@ -566,7 +566,7 @@ class SolverQR
break;
}
default:
- std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()"
+ std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()"
<< std::endl;
return nullptr;
diff --git a/include/base/solver_suitesparse/solver_QR.h b/include/base/solver_suitesparse/solver_QR.h
index 9451ceec95d774f6abd4e2686f9670a3f6bfce81..e625515d2fcba147bb6cda32e1697bf893010551 100644
--- a/include/base/solver_suitesparse/solver_QR.h
+++ b/include/base/solver_suitesparse/solver_QR.h
@@ -24,7 +24,7 @@ class SolverQR
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix;
CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_constraints;
+ VectorXi nodes_ordering_factors;
private:
};
diff --git a/include/base/solver_suitesparse/solver_manager.h b/include/base/solver_suitesparse/solver_manager.h
index 479cc4509156514312f434cda2d0dd0b3a4d3758..09c0abe7f8a83cde800e76968da2a5a990e0007c 100644
--- a/include/base/solver_suitesparse/solver_manager.h
+++ b/include/base/solver_suitesparse/solver_manager.h
@@ -2,16 +2,16 @@
#define CERES_MANAGER_H_
//wolf includes
-#include "base/constraint/constraint_GPS_2D.h"
+#include "base/factor/factor_GPS_2D.h"
#include "base/wolf.h"
#include "base/state_block.h"
#include "../state_point.h"
#include "../state_complex_angle.h"
#include "../state_theta.h"
-#include "../constraint_sparse.h"
-#include "../constraint_odom_2D_theta.h"
-#include "../constraint_odom_2D_complex_angle.h"
-#include "../constraint_corner_2D_theta.h"
+#include "../factor_sparse.h"
+#include "../factor_odom_2D_theta.h"
+#include "../factor_odom_2D_complex_angle.h"
+#include "../factor_corner_2D_theta.h"
/** \brief solver manager for WOLF
*
@@ -32,9 +32,9 @@ class SolverManager
void update(const WolfProblemPtr _problem_ptr);
- void addConstraint(ConstraintBasePtr _corr_ptr);
+ void addFactor(FactorBasePtr _fac_ptr);
- void removeConstraint(const unsigned int& _corr_idx);
+ void removeFactor(const unsigned int& _fac_idx);
void addStateUnit(StateBlockPtr _st_ptr);
@@ -42,7 +42,7 @@ class SolverManager
void updateStateUnitStatus(StateBlockPtr _st_ptr);
- ceres::CostFunction* createCostFunction(ConstraintBasePtr _corrPtr);
+ ceres::CostFunction* createCostFunction(FactorBasePtr _fac_ptr);
};
#endif
diff --git a/include/base/state_block.h b/include/base/state_block.h
index 06962ced0ebb901d5c00649275007faaf8e3b844..447e8a0e9b3534f8048f3aa55131bdaed617e0d7 100644
--- a/include/base/state_block.h
+++ b/include/base/state_block.h
@@ -108,11 +108,11 @@ public:
/** \brief Returns the state local parametrization ptr
**/
- LocalParametrizationBasePtr getLocalParametrizationPtr() const;
+ LocalParametrizationBasePtr getLocalParametrization() const;
/** \brief Sets a local parametrization
**/
- void setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param);
+ void setLocalParametrization(LocalParametrizationBasePtr _local_param);
/** \brief Removes the state_block local parametrization
**/
@@ -230,7 +230,7 @@ inline bool StateBlock::hasLocalParametrization() const
return (local_param_ptr_ != nullptr);
}
-inline LocalParametrizationBasePtr StateBlock::getLocalParametrizationPtr() const
+inline LocalParametrizationBasePtr StateBlock::getLocalParametrization() const
{
return local_param_ptr_;
}
@@ -242,7 +242,7 @@ inline void StateBlock::removeLocalParametrization()
local_param_updated_.store(true);
}
-inline void StateBlock::setLocalParametrizationPtr(LocalParametrizationBasePtr _local_param)
+inline void StateBlock::setLocalParametrization(LocalParametrizationBasePtr _local_param)
{
assert(_local_param != nullptr && "setting a null local parametrization");
local_param_ptr_ = _local_param;
diff --git a/include/base/track_matrix.h b/include/base/track_matrix.h
index 5aa6159c4118a03f822317b915af42e52929c354..4fc5af1d030175c4d6763fb608ad569906ebd2c2 100644
--- a/include/base/track_matrix.h
+++ b/include/base/track_matrix.h
@@ -59,7 +59,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
* The storage is implemented as two maps of maps, so each addition and removal of single features is accomplished in logarithmic time:
*
* Tracks are identified with the track ID in f->trackId()
- * Snapshots are identified with the Capture pointer in f->getCapturePtr()
+ * Snapshots are identified with the Capture pointer in f->getCapture()
*
* these fields of FeatureBase are initialized each time a feature is added to the track matrix:
*
@@ -68,7 +68,7 @@ typedef map<size_t, pair<FeatureBasePtr, FeatureBasePtr> > TrackMatches; // mat
* so e.g. given a feature f,
*
* getTrack (f->trackId()) ; // returns all the track where feature f is.
- * getSnapshot(f->getCapturePtr()) ; // returns all the features in the same capture of f.
+ * getSnapshot(f->getCapture()) ; // returns all the features in the same capture of f.
*
*/
diff --git a/include/base/trajectory_base.h b/include/base/trajectory_base.h
index 4e8d261d337af082e399f2a20188a122ef44bf6c..6ceddf9e6a8742133b01ee9277369a0a3bcaa5be 100644
--- a/include/base/trajectory_base.h
+++ b/include/base/trajectory_base.h
@@ -36,37 +36,37 @@ class TrajectoryBase : public NodeBase, public std::enable_shared_from_this<Traj
// Frames
FrameBasePtr addFrame(FrameBasePtr _frame_ptr);
- FrameBaseList& getFrameList();
- FrameBasePtr getLastFramePtr();
- FrameBasePtr getLastKeyFramePtr();
- FrameBasePtr findLastKeyFramePtr();
+ FrameBasePtrList& getFrameList();
+ FrameBasePtr getLastFrame();
+ FrameBasePtr getLastKeyFrame();
+ FrameBasePtr findLastKeyFrame();
FrameBasePtr closestKeyFrameToTimeStamp(const TimeStamp& _ts);
- void setLastKeyFramePtr(FrameBasePtr _key_frame_ptr);
+ void setLastKeyFrame(FrameBasePtr _key_frame_ptr);
void sortFrame(FrameBasePtr _frm_ptr);
void moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place);
FrameBaseIter computeFrameOrder(FrameBasePtr _frame_ptr);
- // constraints
- void getConstraintList(ConstraintBaseList & _ctr_list);
+ // factors
+ void getFactorList(FactorBasePtrList & _fac_list);
};
-inline FrameBaseList& TrajectoryBase::getFrameList()
+inline FrameBasePtrList& TrajectoryBase::getFrameList()
{
return frame_list_;
}
-inline FrameBasePtr TrajectoryBase::getLastFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastFrame()
{
return frame_list_.back();
}
-inline FrameBasePtr TrajectoryBase::getLastKeyFramePtr()
+inline FrameBasePtr TrajectoryBase::getLastKeyFrame()
{
return last_key_frame_ptr_;
}
-inline void TrajectoryBase::setLastKeyFramePtr(FrameBasePtr _key_frame_ptr)
+inline void TrajectoryBase::setLastKeyFrame(FrameBasePtr _key_frame_ptr)
{
last_key_frame_ptr_ = _key_frame_ptr;
}
diff --git a/include/base/wolf.h b/include/base/wolf.h
index d642f730a22538e9bfa33c6d6d12544a5c82e381..74b7a0bbb3f0f161c7279f1f873a55b1af478935 100644
--- a/include/base/wolf.h
+++ b/include/base/wolf.h
@@ -216,9 +216,9 @@ struct MatrixSizeCheck
#define WOLF_LIST_TYPEDEFS(ClassName) \
class ClassName; \
- typedef std::list<ClassName##Ptr> ClassName##List; \
- typedef ClassName##List::iterator ClassName##Iter; \
- typedef ClassName##List::reverse_iterator ClassName##RevIter;
+ typedef std::list<ClassName##Ptr> ClassName##PtrList; \
+ typedef ClassName##PtrList::iterator ClassName##Iter; \
+ typedef ClassName##PtrList::reverse_iterator ClassName##RevIter;
#define WOLF_STRUCT_PTR_TYPEDEFS(StructName) \
struct StructName; \
@@ -266,9 +266,9 @@ WOLF_LIST_TYPEDEFS(CaptureBase);
WOLF_PTR_TYPEDEFS(FeatureBase);
WOLF_LIST_TYPEDEFS(FeatureBase);
-// - Constraint
-WOLF_PTR_TYPEDEFS(ConstraintBase);
-WOLF_LIST_TYPEDEFS(ConstraintBase);
+// - Factor
+WOLF_PTR_TYPEDEFS(FactorBase);
+WOLF_LIST_TYPEDEFS(FactorBase);
// Map
WOLF_PTR_TYPEDEFS(MapBase);
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 6743a3b8a8ec5ab7129a0ebf004505df48f19b21..78c8303e47195a85b6e6f3c3b84fed68a7959f02 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -191,25 +191,25 @@ capture/capture_GPS_fix.h
capture/capture_IMU.h
capture/capture_odom_2D.h
capture/capture_odom_3D.h
-constraint/constraint_block_absolute.h
-constraint/constraint_container.h
-constraint/constraint_corner_2D.h
-constraint/constraint_AHP.h
-constraint/constraint_epipolar.h
-constraint/constraint_IMU.h
-constraint/constraint_fix_bias.h
-constraint/constraint_GPS_2D.h
-constraint/constraint_GPS_pseudorange_3D.h
-constraint/constraint_GPS_pseudorange_2D.h
-constraint/constraint_odom_2D.h
-constraint/constraint_odom_2D_analytic.h
-constraint/constraint_odom_3D.h
-constraint/constraint_point_2D.h
-constraint/constraint_point_to_line_2D.h
-constraint/constraint_pose_2D.h
-constraint/constraint_pose_3D.h
-constraint/constraint_quaternion_absolute.h
-constraint/constraint_relative_2D_analytic.h
+factor/factor_block_absolute.h
+factor/factor_container.h
+factor/factor_corner_2D.h
+factor/factor_AHP.h
+factor/factor_epipolar.h
+factor/factor_IMU.h
+factor/factor_fix_bias.h
+factor/factor_GPS_2D.h
+factor/factor_GPS_pseudorange_3D.h
+factor/factor_GPS_pseudorange_2D.h
+factor/factor_odom_2D.h
+factor/factor_odom_2D_analytic.h
+factor/factor_odom_3D.h
+factor/factor_point_2D.h
+factor/factor_point_to_line_2D.h
+factor/factor_pose_2D.h
+factor/factor_pose_3D.h
+factor/factor_quaternion_absolute.h
+factor/factor_relative_2D_analytic.h
temp/diff_drive_tools.h
temp/diff_drive_tools.hpp
feature/feature_corner_2D.h
@@ -247,28 +247,28 @@ capture/capture_velocity.h
capture/capture_wheel_joint_position.h
)
SET(HDRS_CONSTRAINT
-constraint/constraint_autodiff_trifocal.h
-constraint/constraint_autodiff_distance_3D.h
-constraint/constraint_AHP.h
-constraint/constraint_block_absolute.h
-constraint/constraint_container.h
-constraint/constraint_corner_2D.h
-constraint/constraint_diff_drive.h
-constraint/constraint_epipolar.h
-constraint/constraint_IMU.h
-constraint/constraint_fix_bias.h
-constraint/constraint_GPS_2D.h
-constraint/constraint_GPS_pseudorange_3D.h
-constraint/constraint_GPS_pseudorange_2D.h
-constraint/constraint_odom_2D.h
-constraint/constraint_odom_2D_analytic.h
-constraint/constraint_odom_3D.h
-constraint/constraint_point_2D.h
-constraint/constraint_point_to_line_2D.h
-constraint/constraint_pose_2D.h
-constraint/constraint_pose_3D.h
-constraint/constraint_quaternion_absolute.h
-constraint/constraint_relative_2D_analytic.h
+factor/factor_autodiff_trifocal.h
+factor/factor_autodiff_distance_3D.h
+factor/factor_AHP.h
+factor/factor_block_absolute.h
+factor/factor_container.h
+factor/factor_corner_2D.h
+factor/factor_diff_drive.h
+factor/factor_epipolar.h
+factor/factor_IMU.h
+factor/factor_fix_bias.h
+factor/factor_GPS_2D.h
+factor/factor_GPS_pseudorange_3D.h
+factor/factor_GPS_pseudorange_2D.h
+factor/factor_odom_2D.h
+factor/factor_odom_2D_analytic.h
+factor/factor_odom_3D.h
+factor/factor_point_2D.h
+factor/factor_point_to_line_2D.h
+factor/factor_pose_2D.h
+factor/factor_pose_3D.h
+factor/factor_quaternion_absolute.h
+factor/factor_relative_2D_analytic.h
)
SET(HDRS_FEATURE
feature/feature_corner_2D.h
@@ -321,9 +321,9 @@ core/capture_base.h
core/capture_buffer.h
core/capture_pose.h
core/capture_void.h
-core/constraint_analytic.h
-core/constraint_autodiff.h
-core/constraint_base.h
+core/factor_analytic.h
+core/factor_autodiff.h
+core/factor_base.h
core/factory.h
core/feature_base.h
core/feature_match.h
@@ -364,8 +364,8 @@ SET(SRCS_CORE
core/capture_base.cpp
core/capture_pose.cpp
core/capture_void.cpp
-core/constraint_analytic.cpp
-core/constraint_base.cpp
+core/factor_analytic.cpp
+core/factor_base.cpp
core/feature_base.cpp
core/feature_pose.cpp
core/frame_base.cpp
@@ -535,8 +535,8 @@ ENDIF(vision_utils_FOUND)
# Add the capture sub-directory
# ADD_SUBDIRECTORY(captures)
-# Add the constraints sub-directory
-# ADD_SUBDIRECTORY(constraints)
+# Add the factors sub-directory
+# ADD_SUBDIRECTORY(factors)
# Add the features sub-directory
# ADD_SUBDIRECTORY(features)
@@ -663,7 +663,7 @@ INSTALL(FILES ${HDRS_CORE}
INSTALL(FILES ${HDRS_CAPTURE}
DESTINATION include/iri-algorithms/wolf/capture)
INSTALL(FILES ${HDRS_CONSTRAINT}
- DESTINATION include/iri-algorithms/wolf/constraint)
+ DESTINATION include/iri-algorithms/wolf/factor)
INSTALL(FILES ${HDRS_FEATURE}
DESTINATION include/iri-algorithms/wolf/feature)
INSTALL(FILES ${HDRS_SENSOR}
diff --git a/src/association/association_node.cpp b/src/association/association_node.cpp
index f1705db6e1ff175890c3642d3c0bd1e7cd175b77..1321dac069e0e86c2bfb9c2df8060cc514d49797 100644
--- a/src/association/association_node.cpp
+++ b/src/association/association_node.cpp
@@ -54,7 +54,7 @@ double AssociationNode::getTreeProb() const
return tree_prob_;
}
-AssociationNode* AssociationNode::upNodePtr() const
+AssociationNode* AssociationNode::upNode() const
{
//return &(*up_node_ptr_);
return up_node_ptr_;
diff --git a/src/association/association_tree.cpp b/src/association/association_tree.cpp
index 1337e4babb19b03ce0965a0658de3c719871e4a6..b425f8e5db4a5392d303d87b06c7b65ae2c8c822 100644
--- a/src/association/association_tree.cpp
+++ b/src/association/association_tree.cpp
@@ -113,7 +113,7 @@ void AssociationTree::solve(std::map<unsigned int, unsigned int> & _pairs, std::
_associated_mask.at(anPtr->getDetectionIndex()) = true;
_pairs[anPtr->getDetectionIndex()] = anPtr->getTargetIndex();
}
- anPtr = anPtr->upNodePtr();
+ anPtr = anPtr->upNode();
}
}
@@ -159,7 +159,7 @@ void AssociationTree::solve(std::vector<std::pair<unsigned int, unsigned int> >
_associated_mask.at(anPtr->getDetectionIndex()) = true;
_pairs.push_back( std::pair<unsigned int, unsigned int>(anPtr->getDetectionIndex(), anPtr->getTargetIndex()) );
}
- anPtr = anPtr->upNodePtr();
+ anPtr = anPtr->upNode();
}
}
diff --git a/src/capture/capture_GPS_fix.cpp b/src/capture/capture_GPS_fix.cpp
index 052bf8f63487d7691f4fc38db3a234785df3fcac..ccf9b8631bd0759448ec6c20da7b00b9f534087d 100644
--- a/src/capture/capture_GPS_fix.cpp
+++ b/src/capture/capture_GPS_fix.cpp
@@ -28,7 +28,7 @@ void CaptureGPSFix::process()
addFeature(std::make_shared<FeatureGPSFix>(data_,data_covariance_));
// ADD CONSTRAINT
- getFeatureList().front()->addConstraint(std::make_shared <ConstraintGPS2D>(getFeatureList().front()));
+ getFeatureList().front()->addFactor(std::make_shared <FactorGPS2D>(getFeatureList().front()));
}
} //namespace wolf
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 051b93c9ff33929900026a534e2948120480d30d..1d2aa58dbd1b0a5b1767def1eb55d8978f729ac4 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -48,7 +48,7 @@ CaptureBase::CaptureBase(const std::string& _type,
WOLF_ERROR("Provided dynamic sensor intrinsics but the sensor intrinsics are static");
}
- getSensorPtr()->setHasCapture();
+ getSensor()->setHasCapture();
}
else if (_p_ptr || _o_ptr || _intr_ptr)
{
@@ -99,56 +99,56 @@ FeatureBasePtr CaptureBase::addFeature(FeatureBasePtr _ft_ptr)
{
//std::cout << "Adding feature" << std::endl;
feature_list_.push_back(_ft_ptr);
- _ft_ptr->setCapturePtr(shared_from_this());
+ _ft_ptr->setCapture(shared_from_this());
_ft_ptr->setProblem(getProblem());
return _ft_ptr;
}
-void CaptureBase::addFeatureList(FeatureBaseList& _new_ft_list)
+void CaptureBase::addFeatureList(FeatureBasePtrList& _new_ft_list)
{
for (FeatureBasePtr feature_ptr : _new_ft_list)
{
- feature_ptr->setCapturePtr(shared_from_this());
+ feature_ptr->setCapture(shared_from_this());
if (getProblem() != nullptr)
feature_ptr->setProblem(getProblem());
}
feature_list_.splice(feature_list_.end(), _new_ft_list);
}
-void CaptureBase::getConstraintList(ConstraintBaseList& _ctr_list)
+void CaptureBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto f_ptr : getFeatureList())
- f_ptr->getConstraintList(_ctr_list);
+ f_ptr->getFactorList(_fac_list);
}
-ConstraintBasePtr CaptureBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+FactorBasePtr CaptureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setCaptureOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setCaptureOther(shared_from_this());
+ return _fac_ptr;
}
-StateBlockPtr CaptureBase::getStateBlockPtr(unsigned int _i) const
+StateBlockPtr CaptureBase::getStateBlock(unsigned int _i) const
{
- if (getSensorPtr())
+ if (getSensor())
{
if (_i < 2) // _i == 0 is position, 1 is orientation, 2 and onwards are intrinsics
- if (getSensorPtr()->extrinsicsInCaptures())
+ if (getSensor()->extrinsicsInCaptures())
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
else
- return getSensorPtr()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStateBlockPtrStatic(_i);
else // 2 and onwards are intrinsics
- if (getSensorPtr()->intrinsicsInCaptures())
+ if (getSensor()->intrinsicsInCaptures())
{
assert(_i < state_block_vec_.size() && "Requested a state block pointer out of the vector range!");
return state_block_vec_[_i];
}
else
- return getSensorPtr()->getStateBlockPtrStatic(_i);
+ return getSensor()->getStateBlockPtrStatic(_i);
}
else // No sensor associated: assume sensor params are here
{
@@ -168,7 +168,7 @@ void CaptureBase::removeStateBlocks()
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
@@ -177,7 +177,7 @@ void CaptureBase::fix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -188,7 +188,7 @@ void CaptureBase::unfix()
{
for (unsigned int i = 0; i<getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -199,7 +199,7 @@ void CaptureBase::fixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -210,7 +210,7 @@ void CaptureBase::unfixExtrinsics()
{
for (unsigned int i = 0; i < 2; i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -221,7 +221,7 @@ void CaptureBase::fixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->fix();
}
@@ -232,7 +232,7 @@ void CaptureBase::unfixIntrinsics()
{
for (unsigned int i = 2; i < getStateBlockVec().size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
sbp->unfix();
}
@@ -254,7 +254,7 @@ SizeEigen CaptureBase::computeCalibSize() const
SizeEigen sz = 0;
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
sz += sb->getSize();
}
@@ -267,7 +267,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
{
calib.segment(index, sb->getSize()) = sb->getState();
@@ -284,7 +284,7 @@ void CaptureBase::setCalibration(const VectorXs& _calib)
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
- auto sb = getStateBlockPtr(i);
+ auto sb = getStateBlock(i);
if (sb && !sb->isFixed())
{
sb->setState(_calib.segment(index, sb->getSize()));
diff --git a/src/capture/capture_laser_2D.cpp b/src/capture/capture_laser_2D.cpp
index 0c3c6fe5834ced132b19037e2d8e9dff9a9e04fd..a2cdd817ebdfd3e73eb0de6d2e4e0183d157ce0a 100644
--- a/src/capture/capture_laser_2D.cpp
+++ b/src/capture/capture_laser_2D.cpp
@@ -3,7 +3,7 @@
namespace wolf {
CaptureLaser2D::CaptureLaser2D(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const std::vector<float>& _ranges) :
- CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensorPtr())), scan_(_ranges)
+ CaptureBase("LASER 2D", _ts, _sensor_ptr), laser_ptr_(std::static_pointer_cast<SensorLaser2D>(getSensor())), scan_(_ranges)
{
//
}
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 96aab020b3ed0c240fb7e05fe81dd40fba12f76d..4f5cf88a4670649f8e61df9423353851797d424b 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -15,17 +15,17 @@ CapturePose::~CapturePose()
//
}
-void CapturePose::emplaceFeatureAndConstraint()
+void CapturePose::emplaceFeatureAndFactor()
{
// Emplace feature
FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
addFeature(feature_pose);
- // Emplace constraint
+ // Emplace factor
if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- feature_pose->addConstraint(std::make_shared<ConstraintPose2D>(feature_pose));
+ feature_pose->addFactor(std::make_shared<FactorPose2D>(feature_pose));
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- feature_pose->addConstraint(std::make_shared<ConstraintPose3D>(feature_pose));
+ feature_pose->addFactor(std::make_shared<FactorPose3D>(feature_pose));
else
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
}
diff --git a/src/capture/capture_wheel_joint_position.cpp b/src/capture/capture_wheel_joint_position.cpp
index 9290da89ae42095e4a2578d80bfedfdf13592c73..7b11d9c8de9e6189be3c9b6a4bef5864b392e9f6 100644
--- a/src/capture/capture_wheel_joint_position.cpp
+++ b/src/capture/capture_wheel_joint_position.cpp
@@ -14,7 +14,7 @@ CaptureWheelJointPosition::CaptureWheelJointPosition(const TimeStamp& _ts,
//
const IntrinsicsDiffDrive intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
+ *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
setDataCovariance(computeWheelJointPositionCov(getPositions(),
intrinsics.left_resolution_,
diff --git a/src/ceres_wrapper/ceres_manager.cpp b/src/ceres_wrapper/ceres_manager.cpp
index e7e1455675969eb3203f5573bd468b6018ea6fa4..829393980aa4093dfd86438ece963a58eba8a85c 100644
--- a/src/ceres_wrapper/ceres_manager.cpp
+++ b/src/ceres_wrapper/ceres_manager.cpp
@@ -36,8 +36,8 @@ CeresManager::CeresManager(const ProblemPtr& _wolf_problem,
CeresManager::~CeresManager()
{
- while (!ctr_2_residual_idx_.empty())
- removeConstraint(ctr_2_residual_idx_.begin()->first);
+ while (!fac_2_residual_idx_.empty())
+ removeFactor(fac_2_residual_idx_.begin()->first);
}
std::string CeresManager::solveImpl(const ReportVerbosity report_level)
@@ -80,14 +80,14 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// first create a vector containing all state blocks
std::vector<StateBlockPtr> all_state_blocks, landmark_state_blocks;
//frame state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKey())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
all_state_blocks.push_back(sb);
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
{
landmark_state_blocks = l_ptr->getUsedStateBlockVec();
all_state_blocks.insert(all_state_blocks.end(), landmark_state_blocks.begin(), landmark_state_blocks.end());
@@ -105,7 +105,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
case CovarianceBlocksToBeComputed::ALL_MARGINALS:
{
// first create a vector containing all state blocks
- for(auto fr_ptr : wolf_problem_->getTrajectoryPtr()->getFrameList())
+ for(auto fr_ptr : wolf_problem_->getTrajectory()->getFrameList())
if (fr_ptr->isKey())
for (auto sb : fr_ptr->getStateBlockVec())
if (sb)
@@ -119,7 +119,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
}
// landmark state blocks
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
for (auto sb : l_ptr->getUsedStateBlockVec())
for(auto sb2 : l_ptr->getUsedStateBlockVec())
{
@@ -135,31 +135,31 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
// state_block_pairs.emplace_back(all_state_blocks[2*i],all_state_blocks[2*i+1]);
// state_block_pairs.emplace_back(all_state_blocks[2*i+1],all_state_blocks[2*i+1]);
//
- // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i]->getPtr());
- // double_pairs.emplace_back(all_state_blocks[2*i]->getPtr(),all_state_blocks[2*i+1]->getPtr());
- // double_pairs.emplace_back(all_state_blocks[2*i+1]->getPtr(),all_state_blocks[2*i+1]->getPtr());
+ // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i]->get());
+ // double_pairs.emplace_back(all_state_blocks[2*i]->get(),all_state_blocks[2*i+1]->get());
+ // double_pairs.emplace_back(all_state_blocks[2*i+1]->get(),all_state_blocks[2*i+1]->get());
// }
break;
}
case CovarianceBlocksToBeComputed::ROBOT_LANDMARKS:
{
//robot-robot
- auto last_key_frame = wolf_problem_->getLastKeyFramePtr();
+ auto last_key_frame = wolf_problem_->getLastKeyFrame();
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getPPtr());
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), last_key_frame->getOPtr());
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), last_key_frame->getOPtr());
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getP());
+ state_block_pairs.emplace_back(last_key_frame->getP(), last_key_frame->getO());
+ state_block_pairs.emplace_back(last_key_frame->getO(), last_key_frame->getO());
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getPPtr()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
- getAssociatedMemBlockPtr(last_key_frame->getOPtr()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
+ getAssociatedMemBlockPtr(last_key_frame->getP()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
+ getAssociatedMemBlockPtr(last_key_frame->getO()));
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
+ getAssociatedMemBlockPtr(last_key_frame->getO()));
// landmarks
std::vector<StateBlockPtr> landmark_state_blocks;
- for(auto l_ptr : wolf_problem_->getMapPtr()->getLandmarkList())
+ for(auto l_ptr : wolf_problem_->getMap()->getLandmarkList())
{
// load state blocks vector
landmark_state_blocks = l_ptr->getUsedStateBlockVec();
@@ -167,11 +167,11 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
for (auto state_it = landmark_state_blocks.begin(); state_it != landmark_state_blocks.end(); state_it++)
{
// robot - landmark
- state_block_pairs.emplace_back(last_key_frame->getPPtr(), *state_it);
- state_block_pairs.emplace_back(last_key_frame->getOPtr(), *state_it);
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getPPtr()),
+ state_block_pairs.emplace_back(last_key_frame->getP(), *state_it);
+ state_block_pairs.emplace_back(last_key_frame->getO(), *state_it);
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getP()),
getAssociatedMemBlockPtr((*state_it)));
- double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getOPtr()),
+ double_pairs.emplace_back(getAssociatedMemBlockPtr(last_key_frame->getO()),
getAssociatedMemBlockPtr((*state_it)));
// landmark marginal
@@ -204,7 +204,7 @@ void CeresManager::computeCovariances(const CovarianceBlocksToBeComputed _blocks
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::computeCovariances(const StateBlockList& st_list)
+void CeresManager::computeCovariances(const StateBlockPtrList& st_list)
{
//std::cout << "CeresManager: computing covariances..." << std::endl;
@@ -243,40 +243,40 @@ void CeresManager::computeCovariances(const StateBlockList& st_list)
std::cout << "WARNING: Couldn't compute covariances!" << std::endl;
}
-void CeresManager::addConstraint(const ConstraintBasePtr& ctr_ptr)
+void CeresManager::addFactor(const FactorBasePtr& fac_ptr)
{
- assert(ctr_2_costfunction_.find(ctr_ptr) == ctr_2_costfunction_.end() && "adding a constraint that is already in the ctr_2_costfunction_ map");
+ assert(fac_2_costfunction_.find(fac_ptr) == fac_2_costfunction_.end() && "adding a factor that is already in the fac_2_costfunction_ map");
- auto cost_func_ptr = createCostFunction(ctr_ptr);
- ctr_2_costfunction_[ctr_ptr] = cost_func_ptr;
+ auto cost_func_ptr = createCostFunction(fac_ptr);
+ fac_2_costfunction_[fac_ptr] = cost_func_ptr;
std::vector<Scalar*> res_block_mem;
- res_block_mem.reserve(ctr_ptr->getStateBlockPtrVector().size());
- for (const StateBlockPtr& st : ctr_ptr->getStateBlockPtrVector())
+ res_block_mem.reserve(fac_ptr->getStateBlockPtrVector().size());
+ for (const StateBlockPtr& st : fac_ptr->getStateBlockPtrVector())
{
res_block_mem.emplace_back( getAssociatedMemBlockPtr(st) );
}
- auto loss_func_ptr = (ctr_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
+ auto loss_func_ptr = (fac_ptr->getApplyLossFunction()) ? new ceres::CauchyLoss(0.5) : nullptr;
- assert(ctr_2_residual_idx_.find(ctr_ptr) == ctr_2_residual_idx_.end() && "adding a constraint that is already in the ctr_2_residual_idx_ map");
+ assert(fac_2_residual_idx_.find(fac_ptr) == fac_2_residual_idx_.end() && "adding a factor that is already in the fac_2_residual_idx_ map");
- ctr_2_residual_idx_[ctr_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
+ fac_2_residual_idx_[fac_ptr] = ceres_problem_->AddResidualBlock(cost_func_ptr.get(),
loss_func_ptr,
res_block_mem);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
-void CeresManager::removeConstraint(const ConstraintBasePtr& _ctr_ptr)
+void CeresManager::removeFactor(const FactorBasePtr& _fac_ptr)
{
- assert(ctr_2_residual_idx_.find(_ctr_ptr) != ctr_2_residual_idx_.end() && "removing a constraint that is not in the ctr_2_residual map");
+ assert(fac_2_residual_idx_.find(_fac_ptr) != fac_2_residual_idx_.end() && "removing a factor that is not in the fac_2_residual map");
- ceres_problem_->RemoveResidualBlock(ctr_2_residual_idx_[_ctr_ptr]);
- ctr_2_residual_idx_.erase(_ctr_ptr);
- ctr_2_costfunction_.erase(_ctr_ptr);
+ ceres_problem_->RemoveResidualBlock(fac_2_residual_idx_[_fac_ptr]);
+ fac_2_residual_idx_.erase(_fac_ptr);
+ fac_2_costfunction_.erase(_fac_ptr);
- assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == ctr_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
+ assert((unsigned int)(ceres_problem_->NumResidualBlocks()) == fac_2_residual_idx_.size() && "ceres residuals different from wrapper residuals");
}
void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
@@ -287,7 +287,7 @@ void CeresManager::addStateBlock(const StateBlockPtr& state_ptr)
state_blocks_local_param_.find(state_ptr)==state_blocks_local_param_.end())
{
auto p = state_blocks_local_param_.emplace(state_ptr,
- std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrizationPtr()));
+ std::make_shared<LocalParametrizationWrapper>(state_ptr->getLocalParametrization()));
local_parametrization_ptr = p.first->second.get();
}
@@ -322,23 +322,23 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
/* in ceres the easiest way to update (add or remove) a local parameterization
* of a state block (parameter block in ceres) is remove & add:
* - the state block: The associated memory block (that identified the parameter_block) is and MUST be the same
- * - all involved constraints (residual_blocks in ceres)
+ * - all involved factors (residual_blocks in ceres)
*/
- // get all involved constraints
- ConstraintBaseList involved_constraints;
- for (auto pair : ctr_2_costfunction_)
+ // get all involved factors
+ FactorBasePtrList involved_factors;
+ for (auto pair : fac_2_costfunction_)
for (const StateBlockPtr& st : pair.first->getStateBlockPtrVector())
if (st == state_ptr)
{
// store
- involved_constraints.push_back(pair.first);
+ involved_factors.push_back(pair.first);
break;
}
- // Remove all involved constraints (it does not remove any parameter block)
- for (auto ctr : involved_constraints)
- removeConstraint(ctr);
+ // Remove all involved factors (it does not remove any parameter block)
+ for (auto fac : involved_factors)
+ removeFactor(fac);
// Remove state block (it removes all involved residual blocks but they just were removed)
removeStateBlock(state_ptr);
@@ -346,9 +346,9 @@ void CeresManager::updateStateBlockLocalParametrization(const StateBlockPtr& sta
// Add state block
addStateBlock(state_ptr);
- // Add all involved constraints
- for (auto ctr : involved_constraints)
- addConstraint(ctr);
+ // Add all involved factors
+ for (auto fac : involved_factors)
+ addFactor(fac);
}
bool CeresManager::hasConverged()
@@ -371,17 +371,17 @@ Scalar CeresManager::finalCost()
return Scalar(summary_.final_cost);
}
-ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr& _ctr_ptr)
+ceres::CostFunctionPtr CeresManager::createCostFunction(const FactorBasePtr& _fac_ptr)
{
- assert(_ctr_ptr != nullptr);
+ assert(_fac_ptr != nullptr);
// analitic & autodiff jacobian
- if (_ctr_ptr->getJacobianMethod() == JAC_ANALYTIC || _ctr_ptr->getJacobianMethod() == JAC_AUTO)
- return std::make_shared<CostFunctionWrapper>(_ctr_ptr);
+ if (_fac_ptr->getJacobianMethod() == JAC_ANALYTIC || _fac_ptr->getJacobianMethod() == JAC_AUTO)
+ return std::make_shared<CostFunctionWrapper>(_fac_ptr);
// numeric jacobian
- else if (_ctr_ptr->getJacobianMethod() == JAC_NUMERIC)
- return createNumericDiffCostFunction(_ctr_ptr);
+ else if (_fac_ptr->getJacobianMethod() == JAC_NUMERIC)
+ return createNumericDiffCostFunction(_fac_ptr);
else
throw std::invalid_argument( "Wrong Jacobian Method!" );
@@ -390,8 +390,8 @@ ceres::CostFunctionPtr CeresManager::createCostFunction(const ConstraintBasePtr&
void CeresManager::check()
{
// Check numbers
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_costfunction_.size());
- assert(ceres_problem_->NumResidualBlocks() == ctr_2_residual_idx_.size());
+ assert(ceres_problem_->NumResidualBlocks() == fac_2_costfunction_.size());
+ assert(ceres_problem_->NumResidualBlocks() == fac_2_residual_idx_.size());
assert(ceres_problem_->NumParameterBlocks() == state_blocks_.size());
// Check parameter blocks
@@ -399,20 +399,20 @@ void CeresManager::check()
assert(ceres_problem_->HasParameterBlock(state_block_pair.second.data()));
// Check residual blocks
- for (auto&& ctr_res_pair : ctr_2_residual_idx_)
+ for (auto&& fac_res_pair : fac_2_residual_idx_)
{
// costfunction - residual
- assert(ctr_2_costfunction_.find(ctr_res_pair.first) != ctr_2_costfunction_.end());
- assert(ctr_2_costfunction_[ctr_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second));
+ assert(fac_2_costfunction_.find(fac_res_pair.first) != fac_2_costfunction_.end());
+ assert(fac_2_costfunction_[fac_res_pair.first].get() == ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second));
- // constraint - residual
- assert(ctr_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(ctr_res_pair.second))->getConstraintPtr());
+ // factor - residual
+ assert(fac_res_pair.first == static_cast<const CostFunctionWrapper*>(ceres_problem_->GetCostFunctionForResidualBlock(fac_res_pair.second))->getFactor());
// parameter blocks - state blocks
std::vector<Scalar*> param_blocks;
- ceres_problem_->GetParameterBlocksForResidualBlock(ctr_res_pair.second, ¶m_blocks);
+ ceres_problem_->GetParameterBlocksForResidualBlock(fac_res_pair.second, ¶m_blocks);
auto i = 0;
- for (const StateBlockPtr& st : ctr_res_pair.first->getStateBlockPtrVector())
+ for (const StateBlockPtr& st : fac_res_pair.first->getStateBlockPtrVector())
{
assert(getAssociatedMemBlockPtr(st) == param_blocks[i]);
i++;
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 9c63afd0315f90410aa60a073120efa36bc84d25..f2e40fac0122c6eb1599cbe27a62659f439a89f0 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -24,7 +24,7 @@ QRManager::QRManager(ProblemPtr _wolf_problem, const unsigned int& _N_batch) :
QRManager::~QRManager()
{
sb_2_col_.clear();
- ctr_2_row_.clear();
+ fac_2_row_.clear();
}
std::string QRManager::solve(const unsigned int& _report_level)
@@ -57,7 +57,7 @@ void QRManager::computeCovariances(CovarianceBlocksToBeComputed _blocks)
// TODO
}
-void QRManager::computeCovariances(const StateBlockList& _sb_list)
+void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
{
//std::cout << "computing covariances.." << std::endl;
update();
@@ -106,10 +106,10 @@ bool QRManager::computeDecomposition()
}
unsigned int meas_size = 0;
- for (auto ctr_pair : ctr_2_row_)
+ for (auto fac_pair : fac_2_row_)
{
- ctr_2_row_[ctr_pair.first] = meas_size;
- meas_size += ctr_pair.first->getSize();
+ fac_2_row_[fac_pair.first] = meas_size;
+ meas_size += fac_pair.first->getSize();
}
// resize and setZero A, b
@@ -119,9 +119,9 @@ bool QRManager::computeDecomposition()
if (any_state_block_removed_ || new_state_blocks_ >= N_batch_)
{
- // relinearize all constraints
- for (auto ctr_pair : ctr_2_row_)
- relinearizeConstraint(ctr_pair.first);
+ // relinearize all factors
+ for (auto fac_pair : fac_2_row_)
+ relinearizeFactor(fac_pair.first);
any_state_block_removed_ = false;
new_state_blocks_ = 0;
@@ -138,29 +138,29 @@ bool QRManager::computeDecomposition()
return true;
}
-void QRManager::addConstraint(ConstraintBasePtr _ctr_ptr)
+void QRManager::addFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "add constraint " << _ctr_ptr->id() << std::endl;
- assert(ctr_2_row_.find(_ctr_ptr) == ctr_2_row_.end() && "adding existing constraint");
- ctr_2_row_[_ctr_ptr] = A_.rows();
- A_.conservativeResize(A_.rows() + _ctr_ptr->getSize(), A_.cols());
- b_.conservativeResize(b_.size() + _ctr_ptr->getSize());
+ //std::cout << "add factor " << _fac_ptr->id() << std::endl;
+ assert(fac_2_row_.find(_fac_ptr) == fac_2_row_.end() && "adding existing factor");
+ fac_2_row_[_fac_ptr] = A_.rows();
+ A_.conservativeResize(A_.rows() + _fac_ptr->getSize(), A_.cols());
+ b_.conservativeResize(b_.size() + _fac_ptr->getSize());
- assert(A_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad A number of rows");
- assert(b_.rows() >= ctr_2_row_[_ctr_ptr] + _ctr_ptr->getSize() - 1 && "bad b number of rows");
+ assert(A_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad A number of rows");
+ assert(b_.rows() >= fac_2_row_[_fac_ptr] + _fac_ptr->getSize() - 1 && "bad b number of rows");
- relinearizeConstraint(_ctr_ptr);
+ relinearizeFactor(_fac_ptr);
pending_changes_ = true;
}
-void QRManager::removeConstraint(ConstraintBasePtr _ctr_ptr)
+void QRManager::removeFactor(FactorBasePtr _fac_ptr)
{
- //std::cout << "remove constraint " << _ctr_ptr->id() << std::endl;
- assert(ctr_2_row_.find(_ctr_ptr) != ctr_2_row_.end() && "removing unknown constraint");
- eraseBlockRow(A_, ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize());
- b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()).setZero();
- ctr_2_row_.erase(_ctr_ptr);
+ //std::cout << "remove factor " << _fac_ptr->id() << std::endl;
+ assert(fac_2_row_.find(_fac_ptr) != fac_2_row_.end() && "removing unknown factor");
+ eraseBlockRow(A_, fac_2_row_[_fac_ptr], _fac_ptr->getSize());
+ b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()).setZero();
+ fac_2_row_.erase(_fac_ptr);
pending_changes_ = true;
}
@@ -202,30 +202,30 @@ void QRManager::updateStateBlockStatus(StateBlockPtr _st_ptr)
addStateBlock(_st_ptr);
}
-void QRManager::relinearizeConstraint(ConstraintBasePtr _ctr_ptr)
+void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
{
- // evaluate constraint
- std::vector<const Scalar*> ctr_states_ptr;
- for (auto sb : _ctr_ptr->getStateBlockPtrVector())
- ctr_states_ptr.push_back(sb->getPtr());
- Eigen::VectorXs residual(_ctr_ptr->getSize());
+ // evaluate factor
+ std::vector<const Scalar*> fac_states_ptr;
+ for (auto sb : _fac_ptr->getStateBlockPtrVector())
+ fac_states_ptr.push_back(sb->get());
+ Eigen::VectorXs residual(_fac_ptr->getSize());
std::vector<Eigen::MatrixXs> jacobians;
- _ctr_ptr->evaluate(ctr_states_ptr,residual,jacobians);
+ _fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
// Fill jacobians
- Eigen::SparseMatrixs A_block_row(_ctr_ptr->getSize(), A_.cols());
+ Eigen::SparseMatrixs A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
- if (!_ctr_ptr->getStateBlockPtrVector()[i]->isFixed())
+ if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
- assert(sb_2_col_.find(_ctr_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "constraint involving a state block not added");
- assert(A_.cols() >= sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
+ assert(sb_2_col_.find(_fac_ptr->getStateBlockPtrVector()[i]) != sb_2_col_.end() && "factor involving a state block not added");
+ assert(A_.cols() >= sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]] + jacobians[i].cols() - 1 && "bad A number of cols");
// insert since A_block_row has just been created so it's empty for sure
- insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_ctr_ptr->getStateBlockPtrVector()[i]]);
+ insertSparseBlock(jacobians[i], A_block_row, 0, sb_2_col_[_fac_ptr->getStateBlockPtrVector()[i]]);
}
- assignBlockRow(A_, A_block_row, ctr_2_row_[_ctr_ptr]);
+ assignBlockRow(A_, A_block_row, fac_2_row_[_fac_ptr]);
// Fill residual
- b_.segment(ctr_2_row_[_ctr_ptr], _ctr_ptr->getSize()) = residual;
+ b_.segment(fac_2_row_[_fac_ptr], _fac_ptr->getSize()) = residual;
}
} /* namespace wolf */
diff --git a/src/ceres_wrapper/solver_manager.cpp b/src/ceres_wrapper/solver_manager.cpp
index cdaae51c3c9a23dd5f227c5e938a9910036af873..06ae2d113c1a4c4a988fa6a684be614c90ed4d9a 100644
--- a/src/ceres_wrapper/solver_manager.cpp
+++ b/src/ceres_wrapper/solver_manager.cpp
@@ -18,18 +18,18 @@ void SolverManager::update()
{
//std::cout << "SolverManager: updating... " << std::endl;
//std::cout << wolf_problem_->getStateBlockNotificationList().size() << " state block notifications" << std::endl;
- //std::cout << wolf_problem_->getConstraintNotificationList().size() << " constraint notifications" << std::endl;
+ //std::cout << wolf_problem_->getFactorNotificationList().size() << " factor notifications" << std::endl;
// REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getConstraintNotificationList().begin();
- while ( ctr_notification_it != wolf_problem_->getConstraintNotificationList().end() )
- if (ctr_notification_it->notification_ == REMOVE)
+ auto fac_notification_it = wolf_problem_->getFactorNotificationList().begin();
+ while ( fac_notification_it != wolf_problem_->getFactorNotificationList().end() )
+ if (fac_notification_it->notification_ == REMOVE)
{
- removeConstraint(ctr_notification_it->constraint_ptr_);
- ctr_notification_it = wolf_problem_->getConstraintNotificationList().erase(ctr_notification_it);
+ removeFactor(fac_notification_it->factor_ptr_);
+ fac_notification_it = wolf_problem_->getFactorNotificationList().erase(fac_notification_it);
}
else
- ctr_notification_it++;
+ fac_notification_it++;
// REMOVE STATE BLOCKS
auto state_notification_it = wolf_problem_->getStateBlockNotificationList().begin();
@@ -65,26 +65,26 @@ void SolverManager::update()
wolf_problem_->getStateBlockNotificationList().pop_front();
}
// ADD CONSTRAINTS
- while (!wolf_problem_->getConstraintNotificationList().empty())
+ while (!wolf_problem_->getFactorNotificationList().empty())
{
- switch (wolf_problem_->getConstraintNotificationList().front().notification_)
+ switch (wolf_problem_->getFactorNotificationList().front().notification_)
{
case ADD:
{
- addConstraint(wolf_problem_->getConstraintNotificationList().front().constraint_ptr_);
+ addFactor(wolf_problem_->getFactorNotificationList().front().factor_ptr_);
break;
}
default:
- throw std::runtime_error("SolverManager::update: Constraint notification must be ADD or REMOVE.");
+ throw std::runtime_error("SolverManager::update: Factor notification must be ADD or REMOVE.");
}
- wolf_problem_->getConstraintNotificationList().pop_front();
+ wolf_problem_->getFactorNotificationList().pop_front();
}
- assert(wolf_problem_->getConstraintNotificationList().empty() && "wolf problem's constraints notification list not empty after update");
+ assert(wolf_problem_->getFactorNotificationList().empty() && "wolf problem's factors notification list not empty after update");
assert(wolf_problem_->getStateBlockNotificationList().empty() && "wolf problem's state_blocks notification list not empty after update");
}
-ProblemPtr SolverManager::getProblemPtr()
+ProblemPtr SolverManager::getProblem()
{
return wolf_problem_;
}
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 9126139db6e82fcd819dc4cc2a77f1536c383f42..836caa3d1c337a37eb8680871139251a371353a2 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -100,9 +100,9 @@ ENDIF(Suitesparse_FOUND)
ADD_EXECUTABLE(test_sparsification test_sparsification.cpp)
TARGET_LINK_LIBRARIES(test_sparsification ${PROJECT_NAME})
-# Comparing analytic and autodiff odometry constraints
-#ADD_EXECUTABLE(test_analytic_odom_constraint test_analytic_odom_constraint.cpp)
-#TARGET_LINK_LIBRARIES(test_analytic_odom_constraint ${PROJECT_NAME})
+# Comparing analytic and autodiff odometry factors
+#ADD_EXECUTABLE(test_analytic_odom_factor test_analytic_odom_factor.cpp)
+#TARGET_LINK_LIBRARIES(test_analytic_odom_factor ${PROJECT_NAME})
# Vision
IF(vision_utils_FOUND)
@@ -126,9 +126,9 @@ IF(vision_utils_FOUND)
ADD_EXECUTABLE(test_projection_points test_projection_points.cpp)
TARGET_LINK_LIBRARIES(test_projection_points ${PROJECT_NAME})
- # Constraint test
- ADD_EXECUTABLE(test_constraint_AHP test_constraint_AHP.cpp)
- TARGET_LINK_LIBRARIES(test_constraint_AHP ${PROJECT_NAME})
+ # Factor test
+ ADD_EXECUTABLE(test_factor_AHP test_factor_AHP.cpp)
+ TARGET_LINK_LIBRARIES(test_factor_AHP ${PROJECT_NAME})
# ORB tracker test
ADD_EXECUTABLE(test_tracker_ORB test_tracker_ORB.cpp)
@@ -168,7 +168,7 @@ TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
#ADD_EXECUTABLE(test_imuDock_autoKFs test_imuDock_autoKFs.cpp)
#TARGET_LINK_LIBRARIES(test_imuDock_autoKFs ${PROJECT_NAME})
-# ConstraintIMU - factors test
+# FactorIMU - factors test
#ADD_EXECUTABLE(test_imu_constrained0 test_imu_constrained0.cpp)
#TARGET_LINK_LIBRARIES(test_imu_constrained0 ${PROJECT_NAME})
@@ -176,9 +176,9 @@ TARGET_LINK_LIBRARIES(test_processor_diff_drive ${PROJECT_NAME})
#ADD_EXECUTABLE(test_imuPlateform_Offline test_imuPlateform_Offline.cpp)
#TARGET_LINK_LIBRARIES(test_imuPlateform_Offline ${PROJECT_NAME})
-# IMU - constraintIMU
-#ADD_EXECUTABLE(test_constraint_imu test_constraint_imu.cpp)
-#TARGET_LINK_LIBRARIES(test_constraint_imu ${PROJECT_NAME})
+# IMU - factorIMU
+#ADD_EXECUTABLE(test_factor_imu test_factor_imu.cpp)
+#TARGET_LINK_LIBRARIES(test_factor_imu ${PROJECT_NAME})
# IF (laser_scan_utils_FOUND)
# ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp)
diff --git a/src/examples/solver/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp
index ebba25db70ed2f76db699d8aeb4e361e26aa50a2..2fae213bcb4493647a2e35b963af6c89bdb34dfc 100644
--- a/src/examples/solver/test_ccolamd.cpp
+++ b/src/examples/solver/test_ccolamd.cpp
@@ -45,7 +45,7 @@ int main(int argc, char *argv[])
CholmodSupernodalLLT < SparseMatrix<double, ColMajor, SizeEigen> > solver;
PermutationMatrix<Dynamic, Dynamic, SizeEigen> perm(size);
CCOLAMDOrdering<SizeEigen> ordering;
- Matrix<SizeEigen, Dynamic, 1> ordering_constraints = Matrix<SizeEigen, Dynamic, 1>::Ones(size);
+ Matrix<SizeEigen, Dynamic, 1> ordering_factors = Matrix<SizeEigen, Dynamic, 1>::Ones(size);
VectorXd b(size), bordered(size), xordered(size), x(size);
clock_t t1, t2, t3;
double time1, time2, time3;
@@ -82,17 +82,17 @@ int main(int argc, char *argv[])
std::cout << "x = " << x.transpose() << std::endl;
// SOLVING AFTER REORDERING ------------------------------------
- // ordering constraints
- ordering_constraints(size-1) = 2;
- ordering_constraints(0) = 2;
+ // ordering factors
+ ordering_factors(size-1) = 2;
+ ordering_factors(0) = 2;
// ordering
t2 = clock();
A.makeCompressed();
std::cout << "Reordering using CCOLAMD:" << std::endl;
- std::cout << "ordering_constraints = " << std::endl << ordering_constraints.transpose() << std::endl << std::endl;
- ordering(A, perm, ordering_constraints.data());
+ std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
+ ordering(A, perm, ordering_factors.data());
std::cout << "perm = " << std::endl << perm.indices().transpose() << std::endl << std::endl;
bordered = perm * b;
diff --git a/src/examples/solver/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp
index b3d4d646baf22fa72babfe2cd09cae7bbcbf4623..83cc7af8407282484ba5aa26dc66eec59a30bb11 100644
--- a/src/examples/solver/test_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_ccolamd_blocks.cpp
@@ -70,8 +70,8 @@ int main(int argc, char *argv[])
CholmodSupernodalLLT < SparseMatrix<double> > solver, solver2, solver3;
PermutationMatrix<Dynamic, Dynamic, int> perm(size), perm_blocks(size * dim);
CCOLAMDOrdering<int> ordering;
- VectorXi block_ordering_constraints = VectorXi::Ones(size);
- VectorXi ordering_constraints = VectorXi::Ones(size*dim);
+ VectorXi block_ordering_factors = VectorXi::Ones(size);
+ VectorXi ordering_factors = VectorXi::Ones(size*dim);
VectorXd b(size * dim), b_ordered(size * dim), b_block_ordered(size * dim), x_block_ordered(size * dim), x_ordered(size * dim), x(size * dim);
clock_t t1, t2, t3;
double time1, time2, time3;
@@ -114,17 +114,17 @@ int main(int argc, char *argv[])
time1 = ((double) clock() - t1) / CLOCKS_PER_SEC;
// SOLVING AFTER REORDERING ------------------------------------
- // ordering constraints
- ordering_constraints.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
- ordering_constraints.segment(0, dim) = VectorXi::Constant(dim,2);
+ // ordering factors
+ ordering_factors.segment(dim * (size-1), dim) = VectorXi::Constant(dim,2);
+ ordering_factors.segment(0, dim) = VectorXi::Constant(dim,2);
// variable ordering
t2 = clock();
H.makeCompressed();
std::cout << "Reordering using CCOLAMD:" << std::endl;
- std::cout << "ordering_constraints = " << std::endl << ordering_constraints.transpose() << std::endl << std::endl;
- ordering(H, perm, ordering_constraints.data());
+ std::cout << "ordering_factors = " << std::endl << ordering_factors.transpose() << std::endl << std::endl;
+ ordering(H, perm, ordering_factors.data());
b_ordered = perm * b;
H_ordered = H.twistedBy(perm);
@@ -141,17 +141,17 @@ int main(int argc, char *argv[])
time2 = ((double) clock() - t2) / CLOCKS_PER_SEC;
// SOLVING AFTER BLOCK REORDERING ------------------------------------
- // ordering constraints
- block_ordering_constraints(size-1) = 2;
- block_ordering_constraints(0) = 2;
+ // ordering factors
+ block_ordering_factors(size-1) = 2;
+ block_ordering_factors(0) = 2;
// block ordering
t3 = clock();
FactorMatrix.makeCompressed();
std::cout << "Reordering using Block CCOLAMD:" << std::endl;
- std::cout << "block_ordering_constraints = " << std::endl << block_ordering_constraints.transpose() << std::endl << std::endl;
- ordering(FactorMatrix, perm_blocks, block_ordering_constraints.data());
+ std::cout << "block_ordering_factors = " << std::endl << block_ordering_factors.transpose() << std::endl << std::endl;
+ ordering(FactorMatrix, perm_blocks, block_ordering_factors.data());
// variable ordering
permutation_2_block_permutation(perm_blocks, perm , dim, size);
diff --git a/src/examples/solver/test_iQR.cpp b/src/examples/solver/test_iQR.cpp
index 868e84440bc1dce0931527e780f9e2a4d8234dfe..b027c874d6a2e6c4910d5dc45114055d87aa2d54 100644
--- a/src/examples/solver/test_iQR.cpp
+++ b/src/examples/solver/test_iQR.cpp
@@ -129,7 +129,7 @@ int main(int argc, char *argv[])
PermutationMatrix<Dynamic, Dynamic, int> acc_permutation_nodes_matrix(0);
CCOLAMDOrdering<int> ordering, partial_ordering;
- VectorXi nodes_ordering_constraints;
+ VectorXi nodes_ordering_factors;
// results variables
clock_t t_ordering, t_solving_ordered_full, t_solving_unordered, t_solving_ordered_partial, t4;
@@ -222,13 +222,13 @@ int main(int argc, char *argv[])
std::cout << "ordering partial problem: " << min_ordered_node << " to "<< n_nodes - 1 << std::endl;
SparseMatrix<int> sub_A_nodes_ordered = A_nodes_ordered.rightCols(ordered_nodes);
- // partial ordering constraints
- VectorXi nodes_partial_ordering_constraints = sub_A_nodes_ordered.bottomRows(1).transpose();
+ // partial ordering factors
+ VectorXi nodes_partial_ordering_factors = sub_A_nodes_ordered.bottomRows(1).transpose();
// computing nodes partial ordering
A_nodes_ordered.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_nodes_matrix(ordered_nodes);
- partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_constraints.data());
+ partial_ordering(sub_A_nodes_ordered, partial_permutation_nodes_matrix, nodes_partial_ordering_factors.data());
// node ordering to variable ordering
PermutationMatrix<Dynamic, Dynamic, int> partial_permutation_matrix(A_ordered.cols());
@@ -258,8 +258,8 @@ int main(int argc, char *argv[])
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_ordered.col(min_ordered_node);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
-// std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
- int initial_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+// std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ int initial_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
SparseMatrix<double> A_ordered_partial = A_ordered.bottomRightCorner((n_nodes - initial_measurement) * dim, ordered_nodes * dim);
solver_ordered_partial.compute(A_ordered_partial);
diff --git a/src/examples/solver/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp
index 70624a0d65a7d074f2f1f8355a0a8cc5348ebf3e..2fdc1f9f22ca75801b6dc7155dea9bb515d9ccd0 100644
--- a/src/examples/solver/test_iQR_wolf.cpp
+++ b/src/examples/solver/test_iQR_wolf.cpp
@@ -193,7 +193,7 @@ class SolverQR
time_managing_ += ((double) clock() - t_managing_) / CLOCKS_PER_SEC;
}
- void addConstraint(const measurement& _meas)
+ void addFactor(const measurement& _meas)
{
t_managing_ = clock();
@@ -242,7 +242,7 @@ class SolverQR
// full problem ordering
if (_first_ordered_node == 0)
{
- // ordering ordering constraints
+ // ordering ordering factors
node_ordering_restrictions_.resize(n_nodes_);
node_ordering_restrictions_ = A_nodes_.bottomRows(1).transpose();
@@ -274,7 +274,7 @@ class SolverQR
//std::cout << "ordering partial_ordering problem: " << _first_ordered_node << " to "<< n_nodes_ - 1 << std::endl;
SparseMatrix<int> sub_A_nodes_ = A_nodes_.rightCols(ordered_nodes);
- // _partial_ordering ordering_ constraints
+ // _partial_ordering ordering_ factors
node_ordering_restrictions_.resize(ordered_nodes);
node_ordering_restrictions_ = sub_A_nodes_.bottomRows(1).transpose();
@@ -343,8 +343,8 @@ class SolverQR
// finding measurements block
SparseMatrix<int> measurements_to_initial = A_nodes_.col(first_ordered_node_);
// std::cout << "measurements_to_initial " << measurements_to_initial << std::endl;
- // std::cout << "measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] " << measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]] << std::endl;
- int first_ordered_measurement = measurements_to_initial.innerIndexPtr()[measurements_to_initial.outerIndexPtr()[0]];
+ // std::cout << "measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] " << measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]] << std::endl;
+ int first_ordered_measurement = measurements_to_initial.innerIndex()[measurements_to_initial.outerIndex()[0]];
int ordered_measurements = A_.rows() - measurements_.at(first_ordered_measurement).location;
int ordered_variables = A_.cols() - nodes_.at(first_ordered_node_).location;
int unordered_variables = nodes_.at(first_ordered_node_).location;
@@ -531,9 +531,9 @@ int main(int argc, char *argv[])
}
// ADD MEASUREMENTS
- solver_unordered.addConstraint(measurements.at(i));
- solver_ordered.addConstraint(measurements.at(i));
- solver_ordered_partial.addConstraint(measurements.at(i));
+ solver_unordered.addFactor(measurements.at(i));
+ solver_ordered.addFactor(measurements.at(i));
+ solver_ordered_partial.addFactor(measurements.at(i));
// PRINT PROBLEM
solver_unordered.printProblem();
diff --git a/src/examples/solver/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp
index d3fb71cadd8f50bc0eda0ea36decf7af2872d0be..93b39fe6572185da38ef7fccb8cb48b6302bab1c 100644
--- a/src/examples/solver/test_iQR_wolf2.cpp
+++ b/src/examples/solver/test_iQR_wolf2.cpp
@@ -18,7 +18,7 @@
//Wolf includes
#include "base/state_block.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/sensor/sensor_laser_2D.h"
#include "wolf_manager.h"
@@ -181,11 +181,11 @@ int main(int argc, char *argv[])
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblemPtr(), ceres_options);
+ CeresManager* ceres_manager = new CeresManager(wolf_manager_ceres->getProblem(), ceres_options);
std::ofstream log_file, landmark_file; //output file
// Own solver
- SolverQR solver_(wolf_manager_QR->getProblemPtr());
+ SolverQR solver_(wolf_manager_QR->getProblem());
std::cout << "STARTING INCREMENTAL QR TEST" << std::endl << std::endl;
std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
@@ -254,7 +254,7 @@ int main(int argc, char *argv[])
// UPDATING SOLVER ---------------------------
//std::cout << "UPDATING..." << std::endl;
- // update state units and constraints in ceres
+ // update state units and factors in ceres
solver_.update();
// PRINT PROBLEM
@@ -289,9 +289,9 @@ int main(int argc, char *argv[])
// draw landmarks
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ Scalar* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -324,7 +324,7 @@ int main(int argc, char *argv[])
usleep(100000 - 1e6 * dt);
// std::cout << "\nTree after step..." << std::endl;
-// wolf_manager->getProblemPtr()->print();
+// wolf_manager->getProblem()->print();
}
// DISPLAY RESULTS ============================================================================================
@@ -339,14 +339,14 @@ int main(int argc, char *argv[])
std::cout << " loop time: " << mean_times(6) << std::endl;
// std::cout << "\nTree before deleting..." << std::endl;
-// wolf_manager->getProblemPtr()->print();
+// wolf_manager->getProblem()->print();
// Draw Final result -------------------------
std::cout << "Drawing final results..." << std::endl;
std::vector<double> landmark_vector;
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+ Scalar* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,23 +364,23 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXs state_poses(wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().size() * 3);
- for (auto frame_it = wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblemPtr()->getTrajectoryPtr()->getFrameList().end(); frame_it++)
+ Eigen::VectorXs state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
+ for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
{
if (complex_angle)
- state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr() + 1));
+ state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), atan2(*(*frame_it)->getO()->get(), *((*frame_it)->getO()->get() + 1));
else
- state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+ state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
i += 3;
}
// Landmarks
std::cout << "Landmarks..." << std::endl;
i = 0;
- Eigen::VectorXs landmarks(wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().size() * 2);
- for (auto landmark_it = wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+ Eigen::VectorXs landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
+ for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+ Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
landmarks.segment(i, 2) = landmark;
i += 2;
}
diff --git a/src/examples/solver/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
index 792250a81b7e23e3465d865ec6ed69c041b3d35f..9283f8411954e0aea8783d067a419e85a09db932 100644
--- a/src/examples/solver/test_incremental_ccolamd_blocks.cpp
+++ b/src/examples/solver/test_incremental_ccolamd_blocks.cpp
@@ -91,8 +91,8 @@ int main(int argc, char *argv[])
acc_permutation_factors(0) = 0;
CCOLAMDOrdering<int> ordering;
- VectorXi factor_ordering_constraints(1);
- VectorXi ordering_constraints(1);
+ VectorXi factor_ordering_factors(1);
+ VectorXi ordering_factors(1);
// results variables
clock_t t1, t2, t3;
@@ -166,16 +166,16 @@ int main(int argc, char *argv[])
b_ordered = acc_permutation_matrix * b;
H_ordered = H.twistedBy(acc_permutation_matrix);
- // ordering constraints
- ordering_constraints.resize(dim*(i+1));
- ordering_constraints = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1)));
+ // ordering factors
+ ordering_factors.resize(dim*(i+1));
+ ordering_factors = ((H_ordered.rightCols(3) * MatrixXd::Ones(3,1)).array() == 0).select(VectorXi::Zero(dim*(i+1)),VectorXi::Ones(dim*(i+1)));
// variable ordering
t2 = clock();
H_ordered.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> permutation_matrix(dim*(i+1));
- ordering(H_ordered, permutation_matrix, ordering_constraints.data());
+ ordering(H_ordered, permutation_matrix, ordering_factors.data());
// applying ordering
acc_permutation_matrix = permutation_matrix * acc_permutation_matrix;
@@ -209,16 +209,16 @@ int main(int argc, char *argv[])
acc_permutation_factors_matrix.indices() = acc_permutation_factors;
factors_ordered = factors.twistedBy(acc_permutation_factors_matrix);
- // ordering constraints
- factor_ordering_constraints.resize(i);
- factor_ordering_constraints = factors_ordered.rightCols(1);
+ // ordering factors
+ factor_ordering_factors.resize(i);
+ factor_ordering_factors = factors_ordered.rightCols(1);
// block ordering
t3 = clock();
factors_ordered.makeCompressed();
PermutationMatrix<Dynamic, Dynamic, int> permutation_factors_matrix(i+1);
- ordering(factors_ordered, permutation_factors_matrix, factor_ordering_constraints.data());
+ ordering(factors_ordered, permutation_factors_matrix, factor_ordering_factors.data());
// applying ordering
permutation_2_block_permutation(permutation_factors_matrix, permutation_matrix , dim, i+1);
diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp
index d8d91b6381b718f79591bf21f96a1df08ab0f66f..10824d51e65440e25dd4a2e5b93b810d5f0a2372 100644
--- a/src/examples/test_2_lasers_offline.cpp
+++ b/src/examples/test_2_lasers_offline.cpp
@@ -263,18 +263,18 @@ int main(int argc, char** argv)
// Vehicle poses
int i = 0;
Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectoryPtr()->getFrameList()))
+ for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
- state_poses.segment(i, 3) << frame->getPPtr()->getVector(), frame->getOPtr()->getVector();
+ state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
i += 3;
}
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMapPtr()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMapPtr()->getLandmarkList()))
+ Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
- landmarks.segment(i, 2) = landmark->getPPtr()->getVector();
+ landmarks.segment(i, 2) = landmark->getP()->getVector();
i += 2;
}
@@ -310,9 +310,9 @@ int main(int argc, char** argv)
std::getchar();
std::cout << "Problem:" << std::endl;
- std::cout << "Frames: " << problem.getTrajectoryPtr()->getFrameList().size() << std::endl;
- std::cout << "Landmarks: " << problem.getMapPtr()->getLandmarkList()->size() << std::endl;
- std::cout << "Sensors: " << problem.getHardwarePtr()->getSensorList()->size() << std::endl;
+ std::cout << "Frames: " << problem.getTrajectory()->getFrameList().size() << std::endl;
+ std::cout << "Landmarks: " << problem.getMap()->getLandmarkList()->size() << std::endl;
+ std::cout << "Sensors: " << problem.getHardware()->getSensorList()->size() << std::endl;
std::cout << " ========= END ===========" << std::endl << std::endl;
diff --git a/src/examples/test_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_factor.cpp
similarity index 89%
rename from src/examples/test_analytic_odom_constraint.cpp
rename to src/examples/test_analytic_odom_factor.cpp
index 854d4543f742cdfef89f7b7e0fe963f5af76844b..344c284e2d4d898106f3fd6d6f3a706cec56abda 100644
--- a/src/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_factor.cpp
@@ -126,8 +126,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_autodiff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_analytic = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_autodiff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_autodiff);
- wolf_problem_analytic->getTrajectoryPtr()->addFrame(vertex_frame_ptr_analytic);
+ wolf_problem_autodiff->getTrajectory()->addFrame(vertex_frame_ptr_autodiff);
+ wolf_problem_analytic->getTrajectory()->addFrame(vertex_frame_ptr_analytic);
// store
index_2_frame_ptr_autodiff[vertex_index] = vertex_frame_ptr_autodiff;
index_2_frame_ptr_analytic[vertex_index] = vertex_frame_ptr_analytic;
@@ -233,7 +233,7 @@ int main(int argc, char** argv)
edge_information(2,1) = atof(bNum.c_str());
bNum.clear();
- // add capture, feature and constraint to problem
+ // add capture, feature and factor to problem
FeatureBasePtr feature_ptr_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
@@ -242,11 +242,11 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_autodiff = index_2_frame_ptr_autodiff[edge_new];
frame_new_ptr_autodiff->addCapture(capture_ptr_autodiff);
capture_ptr_autodiff->addFeature(feature_ptr_autodiff);
- ConstraintOdom2D* constraint_ptr_autodiff = new ConstraintOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
- feature_ptr_autodiff->addConstraint(constraint_ptr_autodiff);
- //std::cout << "Added autodiff edge! " << constraint_ptr_autodiff->id() << " from vertex " << constraint_ptr_autodiff->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_autodiff->getFrameOtherPtr()->id() << std::endl;
+ FactorOdom2D* factor_ptr_autodiff = new FactorOdom2D(feature_ptr_autodiff, frame_old_ptr_autodiff);
+ feature_ptr_autodiff->addFactor(factor_ptr_autodiff);
+ //std::cout << "Added autodiff edge! " << factor_ptr_autodiff->id() << " from vertex " << factor_ptr_autodiff->getCapture()->getFrame()->id() << " to " << factor_ptr_autodiff->getFrameOther()->id() << std::endl;
- // add capture, feature and constraint to problem
+ // add capture, feature and factor to problem
FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
@@ -255,12 +255,12 @@ int main(int argc, char** argv)
FrameBasePtr frame_new_ptr_analytic = index_2_frame_ptr_analytic[edge_new];
frame_new_ptr_analytic->addCapture(capture_ptr_analytic);
capture_ptr_analytic->addFeature(feature_ptr_analytic);
- ConstraintOdom2DAnalytic* constraint_ptr_analytic = new ConstraintOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
- feature_ptr_analytic->addConstraint(constraint_ptr_analytic);
- //std::cout << "Added analytic edge! " << constraint_ptr_analytic->id() << " from vertex " << constraint_ptr_analytic->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_analytic->getFrameOtherPtr()->id() << std::endl;
- //std::cout << "vector " << constraint_ptr_analytic->getMeasurement().transpose() << std::endl;
+ FactorOdom2DAnalytic* factor_ptr_analytic = new FactorOdom2DAnalytic(feature_ptr_analytic, frame_old_ptr_analytic);
+ feature_ptr_analytic->addFactor(factor_ptr_analytic);
+ //std::cout << "Added analytic edge! " << factor_ptr_analytic->id() << " from vertex " << factor_ptr_analytic->getCapture()->getFrame()->id() << " to " << factor_ptr_analytic->getFrameOther()->id() << std::endl;
+ //std::cout << "vector " << factor_ptr_analytic->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << constraint_ptr_analytic->getMeasurementCovariance() << std::endl;
+ //std::cout << "covariance " << std::endl << factor_ptr_analytic->getMeasurementCovariance() << std::endl;
}
}
else
@@ -272,14 +272,14 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
first_frame_analytic->addCapture(initial_covariance_analytic);
- initial_covariance_autodiff->emplaceFeatureAndConstraint();
- initial_covariance_analytic->emplaceFeatureAndConstraint();
+ initial_covariance_autodiff->emplaceFeatureAndFactor();
+ initial_covariance_analytic->emplaceFeatureAndFactor();
// SOLVING PROBLEMS
std::cout << "solving..." << std::endl;
diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
index a605635467398570350ae7424c5cab116ebcbf40..a4c75ccde0b6a186ae6923ac98ff8dd741a249a5 100644
--- a/src/examples/test_autodiff.cpp
+++ b/src/examples/test_autodiff.cpp
@@ -142,8 +142,8 @@ int main(int argc, char** argv)
// ceres_options.minimizer_progress_to_stdout = false;
// ceres_options.line_search_direction_type = ceres::LBFGS;
// ceres_options.max_num_iterations = 100;
- CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblemPtr(), false);
- CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblemPtr(), true);
+ CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblem(), false);
+ CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblem(), true);
std::ofstream log_file, landmark_file; //output file
//std::cout << "START TRAJECTORY..." << std::endl;
@@ -218,7 +218,7 @@ int main(int argc, char** argv)
// UPDATING CERES ---------------------------
std::cout << "UPDATING CERES..." << std::endl;
t1 = clock();
- // update state units and constraints in ceres
+ // update state units and factors in ceres
ceres_manager_ceres->update();
ceres_manager_wolf->update();
mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
@@ -247,23 +247,23 @@ int main(int argc, char** argv)
ceres_manager_ceres->computeCovariances(ALL_MARGINALS);
ceres_manager_wolf->computeCovariances(ALL_MARGINALS);
Eigen::MatrixXs marginal_ceres(3,3), marginal_wolf(3,3);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
marginal_ceres, 0, 0);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_ceres, 0, 2);
- wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
- wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_ceres->getProblem()->getCovarianceBlock(wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
+ wolf_manager_ceres->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_ceres, 2, 2);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
marginal_wolf, 0, 0);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getP(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_wolf, 0, 2);
- wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
- wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_wolf->getProblem()->getCovarianceBlock(wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
+ wolf_manager_wolf->getProblem()->getTrajectory()->getLastFrame()->getO(),
marginal_wolf, 2, 2);
std::cout << "CERES AUTO DIFF covariance:" << std::endl;
std::cout << marginal_ceres << std::endl;
@@ -287,9 +287,9 @@ int main(int argc, char** argv)
// // draw landmarks
// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+// Scalar* position_ptr = (*landmark_it)->getP()->get();
// landmark_vector.push_back(*position_ptr); //x
// landmark_vector.push_back(*(position_ptr + 1)); //y
// landmark_vector.push_back(0.2); //z
@@ -340,9 +340,9 @@ int main(int argc, char** argv)
// // Draw Final result -------------------------
// std::vector<double> landmark_vector;
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Scalar* position_ptr = (*landmark_it)->getPPtr()->getPtr();
+// Scalar* position_ptr = (*landmark_it)->getP()->get();
// landmark_vector.push_back(*position_ptr); //x
// landmark_vector.push_back(*(position_ptr + 1)); //y
// landmark_vector.push_back(0.2); //z
@@ -359,18 +359,18 @@ int main(int argc, char** argv)
// Vehicle poses
// int i = 0;
// Eigen::VectorXs state_poses(n_execution * 3);
-// for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameList().begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameList().end(); frame_it++)
+// for (auto frame_it = wolf_manager->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
// {
-// state_poses.segment(i, 3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr() + 1), *(*frame_it)->getOPtr()->getPtr();
+// state_poses.segment(i, 3) << *(*frame_it)->getP()->get(), *((*frame_it)->getP()->get() + 1), *(*frame_it)->getO()->get();
// i += 3;
// }
//
// // Landmarks
// i = 0;
-// Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().size() * 2);
-// for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkList().end(); landmark_it++)
+// Eigen::VectorXs landmarks(wolf_manager->getProblem()->getMap()->getLandmarkList().size() * 2);
+// for (auto landmark_it = wolf_manager->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
// {
-// Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr());
+// Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
// landmarks.segment(i, 2) = landmark;
// i += 2;
// }
diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp
index 8978383534cc5c7cf9b3fea30c13eaf9a3ad381c..d4afdc7d00f1805782648bf826159d5ea955d288 100644
--- a/src/examples/test_ceres_2_lasers.cpp
+++ b/src/examples/test_ceres_2_lasers.cpp
@@ -126,7 +126,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBaseList& feature_list, int laser, const LandmarkBaseList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
{
// detected corners
//std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -334,8 +334,8 @@ int main(int argc, char** argv)
//std::cout << "RENDERING..." << std::endl;
t1 = clock();
if (step % 3 == 0)
- robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBaseList({}) : *laser_1_processor->getLastPtr()->getFeatureList(), 1, *problem.getMapPtr()->getLandmarkList(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLastPtr() == nullptr ? FeatureBaseList({}) : *laser_2_processor->getLastPtr()->getFeatureListPtr(), 2, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
+ robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
+ //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// TIME MANAGEMENT ---------------------------
@@ -361,24 +361,24 @@ int main(int argc, char** argv)
// std::cout << "\nTree before deleting..." << std::endl;
// Draw Final result -------------------------
- robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBaseList({}) : *laser_1_processor->getLastPtr()->getFeatureList(), 1, *problem.getMapPtr()->getLandmarkList(), problem.getCurrentState());
+ robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
- for (auto frame : *(problem.getTrajectoryPtr()->getFrameList()))
+ for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
- state_poses.segment(i, 3) << frame->getPPtr()->getVector(), frame->getOPtr()->getVector();
+ state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
i += 3;
}
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMapPtr()->getLandmarkList()->size() * 2);
- for (auto landmark : *(problem.getMapPtr()->getLandmarkList()))
+ Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
- landmarks.segment(i, 2) = landmark->getPPtr()->getVector();
+ landmarks.segment(i, 2) = landmark->getP()->getVector();
i += 2;
}
diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp
index 70928589ae5cdd3ab79ee9c520266d01ee6f853d..9e9ca5390a97a42ec61f7ad2a76081fa40e72b5c 100644
--- a/src/examples/test_ceres_2_lasers_polylines.cpp
+++ b/src/examples/test_ceres_2_lasers_polylines.cpp
@@ -126,7 +126,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBaseList& feature_list, int laser, const LandmarkBaseList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
{
// detected corners
std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -341,8 +341,8 @@ int main(int argc, char** argv)
std::cout << "RENDERING..." << std::endl;
t1 = clock();
// if (step % 3 == 0)
-// robot.render(laser_1_processor->getLastPtr() == nullptr ? FeatureBaseList({}) : *laser_1_processor->getLastPtr()->getFeatureListPtr(), 1, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
- //robot.render(laser_2_processor->getLastPtr() == nullptr ? FeatureBaseList({}) : *laser_2_processor->getLastPtr()->getFeatureListPtr(), 2, *problem.getMapPtr()->getLandmarkListPtr(), problem.getCurrentState());
+// robot.render(laser_1_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_1_processor->getLast()->getFeatureList(), 1, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
+ //robot.render(laser_2_processor->getLast() == nullptr ? FeatureBasePtrList({}) : *laser_2_processor->getLast()->getFeatureList(), 2, *problem.getMap()->getLandmarkList(), problem.getCurrentState());
mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
// TIME MANAGEMENT ---------------------------
diff --git a/src/examples/test_diff_drive.cpp b/src/examples/test_diff_drive.cpp
index 302204da34cc60589aa5ec39b8d43a8df2dc1c5c..306c7dfdc0fae360e7ec0d68a763cebf278d4182 100644
--- a/src/examples/test_diff_drive.cpp
+++ b/src/examples/test_diff_drive.cpp
@@ -223,7 +223,7 @@ int main(int argc, char** argv)
t.set(stamp_secs);
auto processor_diff_drive_ptr =
- std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotionPtr());
+ std::static_pointer_cast<ProcessorDiffDrive>(wolf_problem_ptr_->getProcessorMotion());
processor_diff_drive_ptr->setTimeTolerance(period_secs/2); // overwrite time tolerance based on new evidence
// Set the origin
@@ -270,19 +270,19 @@ int main(int argc, char** argv)
"-----------------------------------------");
WOLF_INFO("Integrated delta: " , /* std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose());
+ wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose());
WOLF_INFO("Integrated state: " , /*std::fixed , std::setprecision(3),*/
- wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().transpose());
+ wolf_problem_ptr_->getProcessorMotion()->getCurrentState().transpose());
WOLF_INFO("Integrated std : " , /*std::fixed , std::setprecision(3),*/
- (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().
+ (wolf_problem_ptr_->getProcessorMotion()->getMotion().
delta_integr_cov_.diagonal().transpose()).array().sqrt());
// Print statistics
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
- double N = (double)wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ double N = (double)wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
WOLF_INFO("t0 : " , t0.get() , " secs");
WOLF_INFO("tf : " , tf.get() , " secs");
diff --git a/src/examples/test_constraint_AHP.cpp b/src/examples/test_factor_AHP.cpp
similarity index 77%
rename from src/examples/test_constraint_AHP.cpp
rename to src/examples/test_factor_AHP.cpp
index 7dc1eabe72c1085e4aa1192058837c861a753419..b3938285e114b46926da4d5b18255e10e4130be6 100644
--- a/src/examples/test_constraint_AHP.cpp
+++ b/src/examples/test_factor_AHP.cpp
@@ -1,6 +1,6 @@
#include "base/pinhole_tools.h"
#include "base/landmark/landmark_AHP.h"
-#include "base/constraint/constraint_AHP.h"
+#include "base/factor/factor_AHP.h"
#include "base/state_block.h"
#include "base/state_quaternion.h"
#include "base/sensor/sensor_camera.h"
@@ -10,7 +10,7 @@ int main()
{
using namespace wolf;
- std::cout << std::endl << "==================== constraint AHP test ======================" << std::endl;
+ std::cout << std::endl << "==================== factor AHP test ======================" << std::endl;
TimeStamp t = 1;
@@ -60,7 +60,7 @@ int main()
FrameBasePtr last_frame = std::make_shared<FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
std::cout << "Last frame" << std::endl;
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
// Capture
CaptureImagePtr image_ptr;
@@ -78,7 +78,7 @@ int main()
image_ptr->addFeature(feat_point_image_ptr);
FrameBasePtr anchor_frame = std::make_shared< FrameBase>(t,std::make_shared<StateBlock>(frame_val.head(3)), std::make_shared<StateQuaternion>(frame_val.tail(4)));
- //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectoryPtr()->getLastFramePtr();
+ //FrameBasePtr anchor_frame = wolf_problem_ptr_->getTrajectory()->getLastFrame();
// create the landmark
Eigen::Vector2s point2D;
@@ -89,12 +89,12 @@ int main()
Scalar distance = 2; // arbitrary value
Eigen::Vector4s vec_homogeneous;
- Eigen::VectorXs correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getCorrectionVector();
+ Eigen::VectorXs correction_vec = (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector();
std::cout << "correction vector: " << correction_vec << std::endl;
- std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getDistortionVector() << std::endl;
- point2D = pinhole::depixellizePoint(image_ptr->getSensorPtr()->getIntrinsicPtr()->getState(),point2D);
+ std::cout << "distortion vector: " << (std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector() << std::endl;
+ point2D = pinhole::depixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D);
std::cout << "point2D depixellized: " << point2D.transpose() << std::endl;
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getCorrectionVector(),point2D);
+ point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getCorrectionVector(),point2D);
std::cout << "point2D undistorted: " << point2D.transpose() << std::endl;
Eigen::Vector3s point3D;
@@ -107,35 +107,35 @@ int main()
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
std::cout << "vec_homogeneous: " << vec_homogeneous.transpose() << std::endl;
- LandmarkAHPPtr landmark = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, image_ptr->getSensorPtr(), feat_point_image_ptr->getDescriptor());
+ LandmarkAHPPtr landmark = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, image_ptr->getSensor(), feat_point_image_ptr->getDescriptor());
std::cout << "Landmark AHP created" << std::endl;
- // Create the constraint
- ConstraintAHPPtr constraint_ptr = std::make_shared<ConstraintAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
+ // Create the factor
+ FactorAHPPtr factor_ptr = std::make_shared<FactorAHP>(feat_point_image_ptr, std::static_pointer_cast<LandmarkAHP>(landmark), processor_ptr);
- feat_point_image_ptr->addConstraint(constraint_ptr);
- std::cout << "Constraint AHP created" << std::endl;
+ feat_point_image_ptr->addFactor(factor_ptr);
+ std::cout << "Factor AHP created" << std::endl;
Eigen::Vector2s point2D_proj = point3D.head<2>()/point3D(2);
std::cout << "point2D projected: " << point2D_proj.transpose() << std::endl;
Eigen::Vector2s point2D_dist;
- point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensorPtr()))->getDistortionVector(),point2D_proj);
+ point2D_dist = pinhole::distortPoint((std::static_pointer_cast<SensorCamera>(image_ptr->getSensor()))->getDistortionVector(),point2D_proj);
std::cout << "point2D distorted: " << point2D_dist.transpose() << std::endl;
Eigen::Vector2s point2D_pix;
- point2D_pix = pinhole::pixellizePoint(image_ptr->getSensorPtr()->getIntrinsicPtr()->getState(),point2D_dist);
+ point2D_pix = pinhole::pixellizePoint(image_ptr->getSensor()->getIntrinsic()->getState(),point2D_dist);
std::cout << "point2D pixellized: " << point2D_pix.transpose() << std::endl;
Eigen::Vector2s expectation;
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getState();
- Eigen::Vector4s current_frame_o = last_frame->getOPtr()->getState();
- Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getPPtr()->getState();
- Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getOPtr()->getState();
- Eigen::Vector4s landmark_ = landmark->getPPtr()->getState();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getState();
+ Eigen::Vector4s current_frame_o = last_frame->getO()->getState();
+ Eigen::Vector3s anchor_frame_p = landmark->getAnchorFrame()->getP()->getState();
+ Eigen::Vector4s anchor_frame_o = landmark->getAnchorFrame()->getO()->getState();
+ Eigen::Vector4s landmark_ = landmark->getP()->getState();
- constraint_ptr ->expectation(current_frame_p.data(), current_frame_o.data(),
+ factor_ptr ->expectation(current_frame_p.data(), current_frame_o.data(),
anchor_frame_p.data(), anchor_frame_o.data(),
landmark_.data(), expectation.data());
// current_frame p; current_frame o; anchor_frame p; anchor_frame o; homogeneous_vector landmark, residual
diff --git a/src/examples/test_constraint_imu.cpp b/src/examples/test_factor_imu.cpp
similarity index 54%
rename from src/examples/test_constraint_imu.cpp
rename to src/examples/test_factor_imu.cpp
index 77464a9e4365484f836f5a5ae8a7f3d161ceef91..5dde0e16a573a9b8e4e5fb90d6b65e905dcb18e6 100644
--- a/src/examples/test_constraint_imu.cpp
+++ b/src/examples/test_factor_imu.cpp
@@ -5,7 +5,7 @@
#include "base/capture/capture_pose.h"
#include "base/wolf.h"
#include "base/problem.h"
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
#include "base/state_block.h"
#include "base/state_quaternion.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -19,7 +19,7 @@ int main(int argc, char** argv)
using std::make_shared;
using std::static_pointer_cast;
- std::cout << std::endl << "==================== test_constraint_imu ======================" << std::endl;
+ std::cout << std::endl << "==================== test_factor_imu ======================" << std::endl;
bool c0(false), c1(false);// c2(true), c3(true), c4(true);
// Wolf problem
@@ -46,15 +46,15 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t); //this also creates a keyframe at origin
- wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front()->fix(); //fix the keyframe at origin
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t); //this also creates a keyframe at origin
+ wolf_problem_ptr_->getTrajectory()->getFrameList().front()->fix(); //fix the keyframe at origin
TimeStamp ts(0);
Eigen::VectorXs origin_state = x0;
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity()) );
- imu_ptr->setFramePtr(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back());
+ imu_ptr->setFrame(wolf_problem_ptr_->getTrajectory()->getFrameList().back());
// set variables
using namespace std;
@@ -76,46 +76,46 @@ int main(int argc, char** argv)
if(c0){
/// ******************************************************************************************** ///
- /// constraint creation
+ /// factor creation
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
- //create a constraintIMU
- ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, last_frame);
- feat_imu->addConstraint(constraint_imu);
- last_frame->addConstrainedBy(constraint_imu);
+ //create a factorIMU
+ FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
+ feat_imu->addFactor(factor_imu);
+ last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
- constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
+ factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, current_frame_p, current_frame_o, current_frame_v, expect);
std::cout << "expectation : " << expect.transpose() << std::endl;
- constraint_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
+ factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
std::cout << "residuals : " << residu.transpose() << std::endl;
//reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(last_frame);
- imu_ptr->setFramePtr(last_frame);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
+ imu_ptr->setFrame(last_frame);
}
/// ******************************************************************************************** ///
@@ -137,46 +137,46 @@ int main(int argc, char** argv)
if(c1){
/// ******************************************************************************************** ///
- /// constraint creation
+ /// factor creation
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
- //create a constraintIMU
- ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, last_frame);
- feat_imu->addConstraint(constraint_imu);
- last_frame->addConstrainedBy(constraint_imu);
+ //create a factorIMU
+ FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
+ feat_imu->addFactor(factor_imu);
+ last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
- constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
+ factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
std::cout << "expectation : " << expect.transpose() << std::endl;
- constraint_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
+ factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
std::cout << "residuals : " << residu.transpose() << std::endl;
//reset origin of motion to new frame
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(last_frame);
- imu_ptr->setFramePtr(last_frame);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(last_frame);
+ imu_ptr->setFrame(last_frame);
}
mpu_clock = 0.004046;
@@ -195,41 +195,41 @@ int main(int argc, char** argv)
imu_ptr->setTimeStamp(t);
sensor_ptr->process(imu_ptr);
- //create the constraint
+ //create the factor
//create FrameIMU
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
FrameIMUPtr last_frame = std::make_shared<FrameIMU>(KEY_FRAME, ts, state_vec);
- wolf_problem_ptr_->getTrajectoryPtr()->addFrame(last_frame);
+ wolf_problem_ptr_->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
- delta_preint = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
+ delta_preint_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
+ delta_preint = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
std::shared_ptr<FeatureIMU> feat_imu = std::make_shared<FeatureIMU>(delta_preint, delta_preint_cov);
- feat_imu->setCapturePtr(imu_ptr);
+ feat_imu->setCapture(imu_ptr);
- //create a constraintIMU
- ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, last_frame);
- feat_imu->addConstraint(constraint_imu);
- last_frame->addConstrainedBy(constraint_imu);
+ //create a factorIMU
+ FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, last_frame);
+ feat_imu->addFactor(factor_imu);
+ last_frame->addConstrainedBy(factor_imu);
- FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFramePtr()));
+ FrameIMUPtr ref_frame_ptr(std::static_pointer_cast<FrameIMU>(imu_ptr->getFrame()));
Eigen::Matrix<wolf::Scalar, 10, 1> expect;
- Eigen::Vector3s ref_frame_p = ref_frame_ptr->getPPtr()->getVector();
- Eigen::Quaternions ref_frame_o(ref_frame_ptr->getOPtr()->getVector().data());
- Eigen::Vector3s ref_frame_v = ref_frame_ptr->getVPtr()->getVector();
- Eigen::Vector3s current_frame_p = last_frame->getPPtr()->getVector();
- Eigen::Quaternions current_frame_o(last_frame->getOPtr()->getVector().data());
- Eigen::Vector3s current_frame_v = last_frame->getVPtr()->getVector();
- Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBiasPtr()->getVector()), gyro_bias(ref_frame_ptr->getGyroBiasPtr()->getVector());
+ Eigen::Vector3s ref_frame_p = ref_frame_ptr->getP()->getVector();
+ Eigen::Quaternions ref_frame_o(ref_frame_ptr->getO()->getVector().data());
+ Eigen::Vector3s ref_frame_v = ref_frame_ptr->getV()->getVector();
+ Eigen::Vector3s current_frame_p = last_frame->getP()->getVector();
+ Eigen::Quaternions current_frame_o(last_frame->getO()->getVector().data());
+ Eigen::Vector3s current_frame_v = last_frame->getV()->getVector();
+ Eigen::Vector3s acc_bias(ref_frame_ptr->getAccBias()->getVector()), gyro_bias(ref_frame_ptr->getGyroBias()->getVector());
Eigen::Matrix<wolf::Scalar, 9, 1> residu;
residu << 0,0,0, 0,0,0, 0,0,0;
- constraint_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
+ factor_imu->expectation(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v, expect);
std::cout << "expectation : " << expect.transpose() << std::endl;
- constraint_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
+ factor_imu->getResiduals(ref_frame_p, ref_frame_o, ref_frame_v, acc_bias, gyro_bias, current_frame_p, current_frame_o, current_frame_v,residu);
std::cout << "residuals : " << residu.transpose() << std::endl;
if(wolf_problem_ptr_->check(1)){
@@ -239,18 +239,18 @@ int main(int argc, char** argv)
///having a look at covariances
Eigen::MatrixXs predelta_cov;
predelta_cov.resize(9,9);
- predelta_cov = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ predelta_cov = wolf_problem_ptr_->getProcessorMotion()->getCurrentDeltaPreintCov();
//std::cout << "predelta_cov : \n" << predelta_cov << std::endl;
///Optimization
// PRIOR
- //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front();
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front());
+ //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectory()->getFrameList().front();
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(wolf_problem_ptr_->getTrajectory()->getFrameList().front());
//SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0);
//CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01);
//first_frame->addCapture(initial_covariance);
//initial_covariance->process();
- //std::cout << "initial covariance: constraint " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl;
+ //std::cout << "initial covariance: factor " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl;
// COMPUTE COVARIANCES
std::cout << "computing covariances..." << std::endl;
@@ -261,9 +261,9 @@ int main(int argc, char** argv)
// SOLVING PROBLEMS
ceres::Solver::Summary summary_diff;
std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
summary_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_wolf_state = wolf_problem_ptr_->getTrajectory()->getFrameList().back()->getState();
std::cout << " prev_wolf_state : " << prev_wolf_state.transpose() << "\n post_wolf_state : " << post_wolf_state.transpose() << std::endl;
//std::cout << summary_wolf_diff.BriefReport() << std::endl;
std::cout << "solved!" << std::endl;
diff --git a/src/examples/test_constraint_odom_3D.cpp b/src/examples/test_factor_odom_3D.cpp
similarity index 56%
rename from src/examples/test_constraint_odom_3D.cpp
rename to src/examples/test_factor_odom_3D.cpp
index 5cdbfccf519adadafd23f9f7210b1143290b04ed..77fa4c80276b07ba875fefd5b4e53756e4424e79 100644
--- a/src/examples/test_constraint_odom_3D.cpp
+++ b/src/examples/test_factor_odom_3D.cpp
@@ -1,11 +1,11 @@
/*
- * test_constraint_odom_3D.cpp
+ * test_factor_odom_3D.cpp
*
* Created on: Oct 7, 2016
* Author: jsola
*/
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
namespace wolf
{
diff --git a/src/examples/test_faramotics_simulation.cpp b/src/examples/test_faramotics_simulation.cpp
index 1a3feedafe7777a48a60923e62e71a8a33b3939f..621f651866200d68a0b3061cb699c6f56d896954 100644
--- a/src/examples/test_faramotics_simulation.cpp
+++ b/src/examples/test_faramotics_simulation.cpp
@@ -115,7 +115,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBaseList& feature_list, int laser, const LandmarkBaseList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
{
// detected corners
//std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -135,7 +135,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getPPtr()->getPtr();
+ Scalar* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
diff --git a/src/examples/test_image.cpp b/src/examples/test_image.cpp
index feb794e1f9263bb3732bcfadb41d61fae0b5be8c..b2b61f5207d29021982729d795978ba73a42bed4 100644
--- a/src/examples/test_image.cpp
+++ b/src/examples/test_image.cpp
@@ -64,7 +64,7 @@ int main(int argc, char** argv)
// std::make_shared<StateBlock>(Eigen::Vector3s::Zero()),
// std::make_shared<StateBlock>(k,false),img_width,img_height);
//
-// wolf_problem_->getHardwarePtr()->addSensor(camera_ptr);
+// wolf_problem_->getHardware()->addSensor(camera_ptr);
//
// // PROCESSOR
// ProcessorParamsImage tracker_params;
@@ -167,9 +167,9 @@ int main(int argc, char** argv)
// std::cout << summary << std::endl;
//
// std::cout << "Last key frame pose: "
-// << wolf_problem_->getLastKeyFramePtr()->getPPtr()->getState().transpose() << std::endl;
+// << wolf_problem_->getLastKeyFrame()->getP()->getState().transpose() << std::endl;
// std::cout << "Last key frame orientation: "
-// << wolf_problem_->getLastKeyFramePtr()->getOPtr()->getState().transpose() << std::endl;
+// << wolf_problem_->getLastKeyFrame()->getO()->getState().transpose() << std::endl;
//
// cv::waitKey(0);
// }
diff --git a/src/examples/test_imuDock.cpp b/src/examples/test_imuDock.cpp
index bcaa85d9c56b2586473609e1ec388fe6f6169f9f..dff0efd51fe4a71d8d9dc22b6630f4b50fa62946 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -13,13 +13,13 @@
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
-//Constraints headers
-#include "base/constraint/constraint_fix_bias.h"
+//Factors headers
+#include "base/factor/factor_fix_bias.h"
//std
#include <iostream>
#include <fstream>
-#include "base/constraint/constraint_pose_3D.h"
+#include "base/factor/factor_pose_3D.h"
#define OUTPUT_RESULTS
//#define ADD_KF3
@@ -28,13 +28,13 @@
In this test, we use the experimental conditions needed for Humanoids 2017.
IMU data are acquired using the docking station.
- Constraints are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
+ Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
invar : P1, V1, V2
var : Q1,B1,P2,Q2,B2
- constraints : Odometry constraint between KeyFrames
- IMU constraint
- FixBias constraint --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D constraint
+ factors : Odometry factor between KeyFrames
+ IMU factor
+ FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
+ Fix3D factor
What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
@@ -159,8 +159,8 @@ int main(int argc, char** argv)
mot_ptr->setData(data_odom);
sensorOdom->process(mot_ptr);
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_middle));
- FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(t_middle));
+ FrameIMUPtr KF3 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
#else
//now impose final odometry using last timestamp of imu
data_odom << 0,-0.06,0, 0,0,0;
@@ -168,13 +168,13 @@ int main(int argc, char** argv)
mot_ptr->setData(data_odom);
sensorOdom->process(mot_ptr);
- FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr KF2 = std::static_pointer_cast<FrameIMU>(problem->getTrajectory()->closestKeyFrameToTimeStamp(ts));
#endif
//#################################################### OPTIMIZATION PART
- // ___Create needed constraints___
+ // ___Create needed factors___
- //Add Fix3D constraint on first KeyFrame (with large covariance except for yaw)
+ //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
Eigen::MatrixXs featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXs::Identity(6,6);
featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
@@ -183,42 +183,42 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
- //create a FeatureBase to constraint biases
+ //create a FeatureBase to factor biases
FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- ConstraintFixBiasPtr ctr_fixBias = std::static_pointer_cast<ConstraintFixBias>(featureFixBias->addConstraint(std::make_shared<ConstraintFixBias>(featureFixBias)));
+ FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
// ___Fix/Unfix stateblocks___
- KF1->getPPtr()->fix();
- KF1->getOPtr()->unfix();
- KF1->getVPtr()->fix();
- KF1->getAccBiasPtr()->unfix();
- KF1->getGyroBiasPtr()->unfix();
+ KF1->getP()->fix();
+ KF1->getO()->unfix();
+ KF1->getV()->fix();
+ KF1->getAccBias()->unfix();
+ KF1->getGyroBias()->unfix();
#ifdef ADD_KF3
- KF2->getPPtr()->fix();
- KF2->getOPtr()->unfix();
- KF2->getVPtr()->fix();
- KF2->getAccBiasPtr()->unfix();
- KF2->getGyroBiasPtr()->unfix();
-
- KF3->getPPtr()->unfix();
- KF3->getOPtr()->unfix();
- KF3->getVPtr()->fix();
- KF3->getAccBiasPtr()->unfix();
- KF3->getGyroBiasPtr()->unfix();
+ KF2->getP()->fix();
+ KF2->getO()->unfix();
+ KF2->getV()->fix();
+ KF2->getAccBias()->unfix();
+ KF2->getGyroBias()->unfix();
+
+ KF3->getP()->unfix();
+ KF3->getO()->unfix();
+ KF3->getV()->fix();
+ KF3->getAccBias()->unfix();
+ KF3->getGyroBias()->unfix();
#else
- KF2->getPPtr()->unfix();
- KF2->getOPtr()->unfix();
- KF2->getVPtr()->fix();
- KF2->getAccBiasPtr()->unfix();
- KF2->getGyroBiasPtr()->unfix();
+ KF2->getP()->unfix();
+ KF2->getO()->unfix();
+ KF2->getV()->fix();
+ KF2->getAccBias()->unfix();
+ KF2->getGyroBias()->unfix();
#endif
#ifdef OUTPUT_RESULTS
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index d0665c7463d3bb087c967641f0e326fe54eaa2e6..4415b685cecace3b3aa348d6ca4adfd748219acd 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -13,13 +13,13 @@
#include "base/sensor/sensor_odom_3D.h"
#include "base/processor/processor_odom_3D.h"
-//Constraints headers
-#include "base/constraint/constraint_fix_bias.h"
+//Factors headers
+#include "base/factor/factor_fix_bias.h"
//std
#include <iostream>
#include <fstream>
-#include "base/constraint/constraint_pose_3D.h"
+#include "base/factor/factor_pose_3D.h"
#define OUTPUT_RESULTS
//#define AUTO_KFS
@@ -28,15 +28,15 @@
In this test, we use the experimental conditions needed for Humanoids 2017.
IMU data are acquired using the docking station.
- Constraints are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
+ Factors are (supposing to KeyFrames, stateblocks or first Keyframe are noted *1 and second Keyframes's are *2) :
invar : P1, V1, V2
var : Q1,B1,P2,Q2,B2
All Keyframes coming after KF2 are constrained just like KF2
- constraints : Odometry constraint between KeyFrames
- IMU constraint
- FixBias constraint --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
- Fix3D constraint
+ factors : Odometry factor between KeyFrames
+ IMU factor
+ FixBias factor --> makes the problem observable (adding a big covariance on all part but a smaller one on yaw)
+ Fix3D factor
What we expect : Estimate biases (this will strongly depend on the actual trajectory of the IMU)
Estimate the position and orienttion of the IMU (check with the standard deviation using covariance matrix)
@@ -183,13 +183,13 @@ int main(int argc, char** argv)
odom_data_input.close();
//A KeyFrame should have been created (depending on time_span in processors). get all frames
- FrameBaseList trajectory = problem->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList trajectory = problem->getTrajectory()->getFrameList();
//#################################################### OPTIMIZATION PART
- // ___Create needed constraints___
+ // ___Create needed factors___
- //Add Fix3D constraint on first KeyFrame (with large covariance except for yaw)
+ //Add Fix3D factor on first KeyFrame (with large covariance except for yaw)
Eigen::MatrixXs featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXs::Identity(6,6);
featureFix_cov.topLeftCorner(3,3) *= 1e-8; // position variances (it's fixed anyway)
@@ -198,16 +198,16 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(featureFix->addFactor(std::make_shared<FactorPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
featureFixBias_cov.topLeftCorner(3,3) *= sensorIMU->getAbInitialStdev() * sensorIMU->getAbInitialStdev();
featureFixBias_cov.bottomRightCorner(3,3) *= sensorIMU->getWbInitialStdev() * sensorIMU->getWbInitialStdev();
CaptureBasePtr cap_fixbias = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.tail(6), featureFixBias_cov, 6, 6, nullptr));
- //create a FeatureBase to constraint biases
+ //create a FeatureBase to factor biases
FeatureBasePtr featureFixBias = cap_fixbias->addFeature(std::make_shared<FeatureBase>("FIX BIAS", problem_origin.tail(6), featureFixBias_cov));
- ConstraintFixBiasPtr ctr_fixBias = std::static_pointer_cast<ConstraintFixBias>(featureFixBias->addConstraint(std::make_shared<ConstraintFixBias>(featureFixBias)));
+ FactorFixBiasPtr fac_fixBias = std::static_pointer_cast<FactorFixBias>(featureFixBias->addFactor(std::make_shared<FactorFixBias>(featureFixBias)));
// ___Fix/Unfix stateblocks___
// fix all Keyframes here
@@ -218,17 +218,17 @@ int main(int argc, char** argv)
frame_imu = std::static_pointer_cast<FrameIMU>(frame);
if(frame_imu->isKey())
{
- frame_imu->getPPtr()->fix();
- frame_imu->getOPtr()->unfix();
- frame_imu->getVPtr()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
- frame_imu->getVPtr()->fix();
- frame_imu->getAccBiasPtr()->unfix();
- frame_imu->getGyroBiasPtr()->unfix();
+ frame_imu->getP()->fix();
+ frame_imu->getO()->unfix();
+ frame_imu->getV()->setState((Eigen::Vector3s()<<0,0,0).finished()); //fix all velocties to 0 ()
+ frame_imu->getV()->fix();
+ frame_imu->getAccBias()->unfix();
+ frame_imu->getGyroBias()->unfix();
}
}
//KF1 (origin) needs to be also fixed in position
- KF1->getPPtr()->fix();
+ KF1->getP()->fix();
#ifdef OUTPUT_RESULTS
// ___OUTPUT___
diff --git a/src/examples/test_imuPlateform_Offline.cpp b/src/examples/test_imuPlateform_Offline.cpp
index e1f9d23fb11994758d55f4cd145fb92c1e2d103d..e7ed6a9157f104629a2d6f41d4277e4450763672 100644
--- a/src/examples/test_imuPlateform_Offline.cpp
+++ b/src/examples/test_imuPlateform_Offline.cpp
@@ -5,7 +5,7 @@
#include "base/wolf.h"
#include "base/problem.h"
#include "base/sensor/sensor_odom_3D.h"
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
#include "base/state_block.h"
#include "base/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
@@ -112,8 +112,8 @@ int main(int argc, char** argv)
processor_ptr_odom3D->setOrigin(origin_KF);
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -147,9 +147,9 @@ int main(int argc, char** argv)
}
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
//Finally, process the only one odom input
mot_ptr->setTimeStamp(ts);
@@ -157,7 +157,7 @@ int main(int argc, char** argv)
sen_odom3D->process(mot_ptr);
clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
//closing file
imu_data_input.close();
@@ -170,20 +170,20 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x_origin.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
/*TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();*/
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();*/
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
@@ -194,9 +194,9 @@ int main(int argc, char** argv)
std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -212,7 +212,7 @@ int main(int argc, char** argv)
Eigen::MatrixXs covX(16,16);
Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
- wolf::FrameBaseList frame_list = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList();
+ wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
{
if(frm_ptr->isKey())
@@ -224,11 +224,11 @@ int main(int argc, char** argv)
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getVPtr(), frmIMU_ptr->getVPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBiasPtr(), frmIMU_ptr->getAccBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBiasPtr(), frmIMU_ptr->getGyroBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
covX.block(13,13,3,3) = cov3;
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
diff --git a/src/examples/test_imu_constrained0.cpp b/src/examples/test_imu_constrained0.cpp
index d20f108d8288c6d2381d7cc1619281171104c3d7..f134ccc124603660a1d687fe5fa3357427677415 100644
--- a/src/examples/test_imu_constrained0.cpp
+++ b/src/examples/test_imu_constrained0.cpp
@@ -5,7 +5,7 @@
#include "base/wolf.h"
#include "base/problem.h"
#include "base/sensor/sensor_odom_3D.h"
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
#include "base/state_block.h"
#include "base/state_quaternion.h"
#include "base/processor/processor_odom_3D.h"
@@ -137,8 +137,8 @@ int main(int argc, char** argv)
expected_final_state[4] >> expected_final_state[5] >> expected_final_state[7] >> expected_final_state[8] >> expected_final_state[9];
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
//===================================================== PROCESS DATA
// PROCESS DATA
@@ -206,7 +206,7 @@ int main(int argc, char** argv)
}
clock_t end = clock();
- FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts));
+ FrameIMUPtr last_KF = std::static_pointer_cast<FrameIMU>(wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts));
//closing file
imu_data_input.close();
@@ -225,20 +225,20 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x_origin.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
std::cout << "If you want meaningful CPU metrics, remove all couts in the loop / remove DEBUG_RESULTS definition variable, and compile in RELEASE mode!" << std::endl;
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
@@ -249,17 +249,17 @@ int main(int argc, char** argv)
std::cout << "integr/s : " << (N-1)/elapsed_secs << " ips" << std::endl;
//fix parts of the problem if needed
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
std::cout << "\t\t\t ______solving______" << std::endl;
std::string report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::FULL); //0: nothing, 1: BriefReport, 2: FullReport
std::cout << report << std::endl;
- last_KF->getAccBiasPtr()->fix();
- last_KF->getGyroBiasPtr()->fix();
+ last_KF->getAccBias()->fix();
+ last_KF->getGyroBias()->fix();
std::cout << "\t\t\t solving after fixBias" << std::endl;
report = ceres_manager_wolf_diff->solve(SolverManager::ReportVerbosity::BRIEF); //0: nothing, 1: BriefReport, 2: FullReport
@@ -275,7 +275,7 @@ int main(int argc, char** argv)
Eigen::MatrixXs covX(16,16);
Eigen::MatrixXs cov3(Eigen::Matrix3s::Zero());
- wolf::FrameBaseList frame_list = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList();
+ wolf::FrameBasePtrList frame_list = wolf_problem_ptr_->getTrajectory()->getFrameList();
for(FrameBasePtr frm_ptr : frame_list)
{
if(frm_ptr->isKey())
@@ -287,11 +287,11 @@ int main(int argc, char** argv)
//get data from covariance blocks
wolf_problem_ptr_->getFrameCovariance(frmIMU_ptr, covX);
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getVPtr(), frmIMU_ptr->getVPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getV(), frmIMU_ptr->getV(), cov3);
covX.block(7,7,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBiasPtr(), frmIMU_ptr->getAccBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getAccBias(), frmIMU_ptr->getAccBias(), cov3);
covX.block(10,10,3,3) = cov3;
- wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBiasPtr(), frmIMU_ptr->getGyroBiasPtr(), cov3);
+ wolf_problem_ptr_->getCovarianceBlock(frmIMU_ptr->getGyroBias(), frmIMU_ptr->getGyroBias(), cov3);
covX.block(13,13,3,3) = cov3;
for(int i = 0; i<16; i++)
cov_stdev(i) = ( covX(i,i)? 2*sqrt(covX(i,i)):0); //if diagonal value is 0 then store 0 else store 2*sqrt(diag_value)
@@ -307,7 +307,7 @@ int main(int argc, char** argv)
}
}
- //trials to print all constraintIMUs' residuals
+ //trials to print all factorIMUs' residuals
Eigen::Matrix<wolf::Scalar,15,1> IMU_residuals;
Eigen::Vector3s p1(Eigen::Vector3s::Zero());
Eigen::Vector4s q1_vec(Eigen::Vector4s::Zero());
@@ -326,26 +326,26 @@ int main(int argc, char** argv)
{
if(frm_ptr->isKey())
{
- ConstraintBaseList ctr_list = frm_ptr->getConstrainedByList();
- for(ConstraintBasePtr ctr_ptr : ctr_list)
+ FactorBasePtrList fac_list = frm_ptr->getConstrainedByList();
+ for(FactorBasePtr fac_ptr : fac_list)
{
- if(ctr_ptr->getTypeId() == CTR_IMU)
+ if(fac_ptr->getTypeId() == FAC_IMU)
{
- //Eigen::VectorXs prev_KF_state(ctr_ptr->getFrameOtherPtr()->getState());
- //Eigen::VectorXs curr_KF_state(ctr_ptr->getFeaturePtr()->getFramePtr()->getState());
- p1 = ctr_ptr->getFrameOtherPtr()->getPPtr()->getState();
- q1_vec = ctr_ptr->getFrameOtherPtr()->getOPtr()->getState();
- v1 = ctr_ptr->getFrameOtherPtr()->getVPtr()->getState();
- ab1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOtherPtr())->getAccBiasPtr()->getState();
- wb1 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFrameOtherPtr())->getGyroBiasPtr()->getState();
-
- p2 = ctr_ptr->getFeaturePtr()->getFramePtr()->getPPtr()->getState();
- q2_vec = ctr_ptr->getFeaturePtr()->getFramePtr()->getOPtr()->getState();
- v2 = ctr_ptr->getFeaturePtr()->getFramePtr()->getVPtr()->getState();
- ab2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeaturePtr()->getFramePtr())->getAccBiasPtr()->getState();
- wb2 = std::static_pointer_cast<FrameIMU>(ctr_ptr->getFeaturePtr()->getFramePtr())->getGyroBiasPtr()->getState();
-
- std::static_pointer_cast<ConstraintIMU>(ctr_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
+ //Eigen::VectorXs prev_KF_state(fac_ptr->getFrameOther()->getState());
+ //Eigen::VectorXs curr_KF_state(fac_ptr->getFeature()->getFrame()->getState());
+ p1 = fac_ptr->getFrameOther()->getP()->getState();
+ q1_vec = fac_ptr->getFrameOther()->getO()->getState();
+ v1 = fac_ptr->getFrameOther()->getV()->getState();
+ ab1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getAccBias()->getState();
+ wb1 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFrameOther())->getGyroBias()->getState();
+
+ p2 = fac_ptr->getFeature()->getFrame()->getP()->getState();
+ q2_vec = fac_ptr->getFeature()->getFrame()->getO()->getState();
+ v2 = fac_ptr->getFeature()->getFrame()->getV()->getState();
+ ab2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getAccBias()->getState();
+ wb2 = std::static_pointer_cast<FrameIMU>(fac_ptr->getFeature()->getFrame())->getGyroBias()->getState();
+
+ std::static_pointer_cast<FactorIMU>(fac_ptr)->residual(p1, q1, v1, ab1, wb1, p2, q2, v2, ab2, wb2, IMU_residuals);
std::cout << "IMU residuals : " << IMU_residuals.transpose() << std::endl;
}
}
diff --git a/src/examples/test_kf_callback.cpp b/src/examples/test_kf_callback.cpp
index 8f049c8d62e8dc807dbf81c3aad3326dbc6a1090..05bd165977ac79ac6b6b4f61d6a72cb83d6e2eff 100644
--- a/src/examples/test_kf_callback.cpp
+++ b/src/examples/test_kf_callback.cpp
@@ -35,8 +35,8 @@ int main()
sen_ftr->addProcessor(prc_ftr);
prc_ftr->setTimeTolerance(0.1);
- cout << "Motion sensor : " << problem->getProcessorMotionPtr()->getSensorPtr()->getName() << endl;
- cout << "Motion processor : " << problem->getProcessorMotionPtr()->getName() << endl;
+ cout << "Motion sensor : " << problem->getProcessorMotion()->getSensor()->getName() << endl;
+ cout << "Motion processor : " << problem->getProcessorMotion()->getName() << endl;
TimeStamp t(0);
cout << "=======================\n>> TIME: " << t.get() << endl;
diff --git a/src/examples/test_map_yaml.cpp b/src/examples/test_map_yaml.cpp
index 7990919ca49bfee5be8950726b008490d523ed29..7dedb1200e1fcfa14483a848bf5b3032aac3ccd5 100644
--- a/src/examples/test_map_yaml.cpp
+++ b/src/examples/test_map_yaml.cpp
@@ -25,20 +25,20 @@ void print(MapBase& _map)
if (lmk_ptr->getType() == "POLYLINE 2D")
{
LandmarkPolyline2DPtr poly_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- std::cout << "\npos: " << poly_ptr->getPPtr()->getState().transpose() << " -- fixed: " << poly_ptr->getPPtr()->isFixed();
- std::cout << "\nori: " << poly_ptr->getOPtr()->getState().transpose() << " -- fixed: " << poly_ptr->getOPtr()->isFixed();
+ std::cout << "\npos: " << poly_ptr->getP()->getState().transpose() << " -- fixed: " << poly_ptr->getP()->isFixed();
+ std::cout << "\nori: " << poly_ptr->getO()->getState().transpose() << " -- fixed: " << poly_ptr->getO()->isFixed();
std::cout << "\nn points: " << poly_ptr->getNPoints();
std::cout << "\nFirst idx: " << poly_ptr->getFirstId();
std::cout << "\nFirst def: " << poly_ptr->isFirstDefined();
std::cout << "\nLast def: " << poly_ptr->isLastDefined();
for (int idx = poly_ptr->getFirstId(); idx <= poly_ptr->getLastId(); idx++)
- std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlockPtr(idx)->getState().transpose();
+ std::cout << "\n point: " << idx << ": " << poly_ptr->getPointStateBlock(idx)->getState().transpose();
break;
}
else if (lmk_ptr->getType() == "AHP")
{
LandmarkAHPPtr ahp_ptr = std::static_pointer_cast<LandmarkAHP>(lmk_ptr);
- std::cout << "\npos: " << ahp_ptr->getPPtr()->getState().transpose() << " -- fixed: " << ahp_ptr->getPPtr()->isFixed();
+ std::cout << "\npos: " << ahp_ptr->getP()->getState().transpose() << " -- fixed: " << ahp_ptr->getP()->isFixed();
std::cout << "\ndescript: " << ahp_ptr->getCvDescriptor().t();
break;
}
@@ -58,7 +58,7 @@ int main()
v << 1, 2, 3, 4;
cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
LandmarkAHP lmk_1(v, nullptr, nullptr, d);
- std::cout << "Pos 1 = " << lmk_1.getPPtr()->getState().transpose() << std::endl;
+ std::cout << "Pos 1 = " << lmk_1.getP()->getState().transpose() << std::endl;
std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
YAML::Node n = lmk_1.saveToYaml();
@@ -66,7 +66,7 @@ int main()
std::cout << "Des n = " << n["descriptor"].as<VectorXs>().transpose() << std::endl;
LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
- std::cout << "Pos 2 = " << lmk_2.getPPtr()->getState().transpose() << std::endl;
+ std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
std::cout << "Des 2 = " << lmk_2.getCvDescriptor().t() << std::endl;
std::string filename;
@@ -81,21 +81,21 @@ int main()
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
- print(*(problem->getMapPtr()));
+ print(*(problem->getMap()));
filename = wolf_config + "/map_polyline_example_write.yaml";
std::cout << "Writing map to file: " << filename << std::endl;
std::string thisfile = __FILE__;
- problem->getMapPtr()->save(filename, "Example generated by test file " + thisfile);
+ problem->getMap()->save(filename, "Example generated by test file " + thisfile);
std::cout << "Clearing map... " << std::endl;
- problem->getMapPtr()->getLandmarkList().clear();
+ problem->getMap()->getLandmarkList().clear();
std::cout << "Re-reading map from file: " << filename << std::endl;
problem->loadMap(filename);
std::cout << "Printing map..." << std::endl;
- print(*(problem->getMapPtr()));
+ print(*(problem->getMap()));
return 0;
}
diff --git a/src/examples/test_mpu.cpp b/src/examples/test_mpu.cpp
index 08db29a812d7f6b2d1ce89605e059ff56b0abd03..0e397cac34729accf6ecd24da3829f011044800a 100644
--- a/src/examples/test_mpu.cpp
+++ b/src/examples/test_mpu.cpp
@@ -105,7 +105,7 @@ int main(int argc, char** argv)
ProblemPtr wolf_problem_ptr_ = Problem::create("PQVBB 3D");
Eigen::VectorXs IMU_extrinsics(7);
IMU_extrinsics << 0,0,0, 0,0,0,1; // IMU pose in the robot
- SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBasePtr());
+ SensorBasePtr sensor_ptr = wolf_problem_ptr_->installSensor("IMU", "Main IMU", IMU_extrinsics, IntrinsicsBase());
wolf_problem_ptr_->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
// Time and data variables
@@ -118,7 +118,7 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0, 1,0,0,0, 0,0,.001, 0,0,.002; // Try some non-zero biases
- wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr( std::make_shared<CaptureIMU>(t, sensor_ptr, data_) );
@@ -165,10 +165,10 @@ int main(int argc, char** argv)
Eigen::VectorXs x_debug;
TimeStamp ts;
- delta_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_;
- delta_integr_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
- x_debug = wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState();
- ts = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ delta_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_;
+ delta_integr_debug = wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
+ x_debug = wolf_problem_ptr_->getProcessorMotion()->getCurrentState();
+ ts = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
if(debug_results)
debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
@@ -188,11 +188,11 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x0.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << wolf_problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << wolf_problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (wolf_problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (wolf_problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
@@ -204,9 +204,9 @@ int main(int argc, char** argv)
#endif
TimeStamp t0, tf;
- t0 = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = wolf_problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = wolf_problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
diff --git a/src/examples/test_processor_imu.cpp b/src/examples/test_processor_imu.cpp
index f9b57a323450fb99fb84f192f4d20c4a1bbe0085..7dc68ebd7d36b75ee7dc15299495ede4bc6f8fe2 100644
--- a/src/examples/test_processor_imu.cpp
+++ b/src/examples/test_processor_imu.cpp
@@ -93,7 +93,7 @@ int main(int argc, char** argv)
// Set the origin
Eigen::VectorXs x0(16);
x0 << 0,0,0, 0,0,0,1, 0,0,0, 0,0,0, 0,0,0; // Try some non-zero biases
- problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
CaptureIMUPtr imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
@@ -132,18 +132,18 @@ int main(int argc, char** argv)
<< data.transpose() << std::endl;
std::cout << "Current delta: " << std::fixed << std::setprecision(3) << std::setw(8) << std::right
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_.transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
- Eigen::VectorXs x = problem_ptr_->getProcessorMotionPtr()->getCurrentState();
+ Eigen::VectorXs x = problem_ptr_->getProcessorMotion()->getCurrentState();
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x.head(10).transpose() << std::endl;
std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
- << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+ << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
std::cout << std::endl;
@@ -155,10 +155,10 @@ int main(int argc, char** argv)
Eigen::VectorXs x_debug;
TimeStamp ts;
- delta_debug = problem_ptr_->getProcessorMotionPtr()->getMotion().delta_;
- delta_integr_debug = problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_;
- x_debug = problem_ptr_->getProcessorMotionPtr()->getCurrentState();
- ts = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
+ delta_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_;
+ delta_integr_debug = problem_ptr_->getProcessorMotion()->getMotion().delta_integr_;
+ x_debug = problem_ptr_->getProcessorMotion()->getCurrentState();
+ ts = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
if(debug_results)
debug_results << ts.get() << "\t" << delta_debug(0) << "\t" << delta_debug(1) << "\t" << delta_debug(2) << "\t" << delta_debug(3) << "\t" << delta_debug(4) << "\t"
@@ -178,11 +178,11 @@ int main(int argc, char** argv)
std::cout << "Initial state: " << std::fixed << std::setprecision(3) << std::setw(8)
<< x0.head(16).transpose() << std::endl;
std::cout << "Integrated delta: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_.transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getMotion().delta_integr_.transpose() << std::endl;
std::cout << "Integrated state: " << std::fixed << std::setprecision(3) << std::setw(8)
- << problem_ptr_->getProcessorMotionPtr()->getCurrentState().head(16).transpose() << std::endl;
+ << problem_ptr_->getProcessorMotion()->getCurrentState().head(16).transpose() << std::endl;
// std::cout << "Integrated std : " << std::fixed << std::setprecision(3) << std::setw(8)
-// << (problem_ptr_->getProcessorMotionPtr()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
+// << (problem_ptr_->getProcessorMotion()->getMotion().delta_integr_cov_.diagonal().transpose()).array().sqrt() << std::endl;
// Print statistics
std::cout << "\nStatistics -----------------------------------------------------------------------------------" << std::endl;
@@ -194,9 +194,9 @@ int main(int argc, char** argv)
#endif
TimeStamp t0, tf;
- t0 = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().front().ts_;
- tf = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- int N = problem_ptr_->getProcessorMotionPtr()->getBuffer().get().size();
+ t0 = problem_ptr_->getProcessorMotion()->getBuffer().get().front().ts_;
+ tf = problem_ptr_->getProcessorMotion()->getBuffer().get().back().ts_;
+ int N = problem_ptr_->getProcessorMotion()->getBuffer().get().size();
std::cout << "t0 : " << t0.get() << " s" << std::endl;
std::cout << "tf : " << tf.get() << " s" << std::endl;
std::cout << "duration : " << tf-t0 << " s" << std::endl;
diff --git a/src/examples/test_processor_imu_jacobians.cpp b/src/examples/test_processor_imu_jacobians.cpp
index e876a21d97080d8c9b20bf0ef3e1691124f3018a..6cf92cef243dde6230b39220224c1a2a87b18724 100644
--- a/src/examples/test_processor_imu_jacobians.cpp
+++ b/src/examples/test_processor_imu_jacobians.cpp
@@ -49,7 +49,7 @@ int main(int argc, char** argv)
Eigen::VectorXs x0(16);
x0 << 0,1,0, 0,0,0,1, 1,0,0, 0,0,.000, 0,0,.000; // P Q V B B
- //wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(x0, t);
+ //wolf_problem_ptr_->getProcessorMotion()->setOrigin(x0, t);
//CaptureIMU* imu_ptr;
diff --git a/src/examples/test_processor_odom_3D.cpp b/src/examples/test_processor_odom_3D.cpp
index c9321b51e9da6693783ea1a220d4a158623d37d3..ffeb5ce92272894a19ec5c7161ee354412a7072a 100644
--- a/src/examples/test_processor_odom_3D.cpp
+++ b/src/examples/test_processor_odom_3D.cpp
@@ -85,7 +85,7 @@ int main (int argc, char** argv)
}
problem->print(1,0,1,0);
-// for (auto frm : problem->getTrajectoryPtr()->getFrameList())
+// for (auto frm : problem->getTrajectory()->getFrameList())
// {
// frm->setState(problem->zeroState());
// }
diff --git a/src/examples/test_processor_tracker_landmark.cpp b/src/examples/test_processor_tracker_landmark.cpp
index 15ed24d723b3c0ceee70cc4bd40ef388c910ac5b..81900c7ef6077fe947f09c9f59a42d094c268a40 100644
--- a/src/examples/test_processor_tracker_landmark.cpp
+++ b/src/examples/test_processor_tracker_landmark.cpp
@@ -20,8 +20,8 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
{
using namespace wolf;
std::cout << "\n-----------\nWolf tree begin" << std::endl;
- std::cout << "Hrd: " << wolf_problem_ptr_->getHardwarePtr()->getProblem() << std::endl;
- for (SensorBasePtr sen : wolf_problem_ptr_->getHardwarePtr()->getSensorList())
+ std::cout << "Hrd: " << wolf_problem_ptr_->getHardware()->getProblem() << std::endl;
+ for (SensorBasePtr sen : wolf_problem_ptr_->getHardware()->getSensorList())
{
std::cout << "\tSen: " << sen->getProblem() << std::endl;
for (ProcessorBasePtr prc : sen->getProcessorList())
@@ -29,8 +29,8 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
std::cout << "\t\tPrc: " << prc->getProblem() << std::endl;
}
}
- std::cout << "Trj: " << wolf_problem_ptr_->getTrajectoryPtr()->getProblem() << std::endl;
- for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectoryPtr()->getFrameList())
+ std::cout << "Trj: " << wolf_problem_ptr_->getTrajectory()->getProblem() << std::endl;
+ for (FrameBasePtr frm : wolf_problem_ptr_->getTrajectory()->getFrameList())
{
std::cout << "\tFrm: " << frm->getProblem() << std::endl;
for (CaptureBasePtr cap : frm->getCaptureList())
@@ -39,15 +39,15 @@ void print_problem_pointers(wolf::ProblemPtr wolf_problem_ptr_)
for (FeatureBasePtr ftr : cap->getFeatureList())
{
std::cout << "\t\t\tFtr: " << ftr->getProblem() << std::endl;
- for (ConstraintBasePtr ctr : ftr->getConstraintList())
+ for (FactorBasePtr ctr : ftr->getFactorList())
{
std::cout << "\t\t\t\tCtr: " << ctr->getProblem() << std::endl;
}
}
}
}
- std::cout << "Map: " << wolf_problem_ptr_->getMapPtr()->getProblem() << std::endl;
- for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMapPtr()->getLandmarkList())
+ std::cout << "Map: " << wolf_problem_ptr_->getMap()->getProblem() << std::endl;
+ for (LandmarkBasePtr lmk : wolf_problem_ptr_->getMap()->getLandmarkList())
{
std::cout << "\tLmk: " << lmk->getProblem() << std::endl;
}
diff --git a/src/examples/test_processor_tracker_landmark_image.cpp b/src/examples/test_processor_tracker_landmark_image.cpp
index f5b084e13fb1e336cbc3d80fc9a532d22e5ba043..d8b22c18d234ff658b22179060a6008ef663e117 100644
--- a/src/examples/test_processor_tracker_landmark_image.cpp
+++ b/src/examples/test_processor_tracker_landmark_image.cpp
@@ -28,14 +28,14 @@ using Eigen::Vector7s;
using namespace wolf;
void cleanupMap(const ProblemPtr& _problem, const TimeStamp& _t, Scalar _dt_max,
- SizeEigen _min_constraints)
+ SizeEigen _min_factors)
{
std::list<LandmarkBasePtr> lmks_to_remove;
- for (auto lmk : _problem->getMapPtr()->getLandmarkList())
+ for (auto lmk : _problem->getMap()->getLandmarkList())
{
TimeStamp t0 = std::static_pointer_cast<LandmarkAHP>(lmk)->getAnchorFrame()->getTimeStamp();
if (_t - t0 > _dt_max)
- if (lmk->getConstrainedByList().size() <= _min_constraints)
+ if (lmk->getConstrainedByList().size() <= _min_factors)
lmks_to_remove.push_back(lmk);
}
@@ -99,7 +99,7 @@ int main(int argc, char** argv)
// Origin Key Frame is fixed
TimeStamp t = 0;
FrameBasePtr origin_frame = problem->emplaceFrame(KEY_FRAME, (Vector7s()<<1,0,0,0,0,0,1).finished(), t);
- problem->getProcessorMotionPtr()->setOrigin(origin_frame);
+ problem->getProcessorMotion()->setOrigin(origin_frame);
origin_frame->fix();
std::cout << "t: " << 0 << " \t\t\t x = ( " << problem->getCurrentState().transpose() << ")" << std::endl;
@@ -161,7 +161,7 @@ int main(int argc, char** argv)
cap_odo->setTimeStamp(t);
// previous state
- FrameBasePtr prev_key_fr_ptr = problem->getLastKeyFramePtr();
+ FrameBasePtr prev_key_fr_ptr = problem->getLastKeyFrame();
// Eigen::Vector7s x_prev = problem->getCurrentState();
Eigen::Vector7s x_prev = prev_key_fr_ptr->getState();
@@ -169,11 +169,11 @@ int main(int argc, char** argv)
FrameBasePtr prev_prev_key_fr_ptr = nullptr;
Vector7s x_prev_prev;
Vector7s dx;
- for (auto f_it = problem->getTrajectoryPtr()->getFrameList().rbegin(); f_it != problem->getTrajectoryPtr()->getFrameList().rend(); f_it++)
+ for (auto f_it = problem->getTrajectory()->getFrameList().rbegin(); f_it != problem->getTrajectory()->getFrameList().rend(); f_it++)
if ((*f_it) == prev_key_fr_ptr)
{
f_it++;
- if (f_it != problem->getTrajectoryPtr()->getFrameList().rend())
+ if (f_it != problem->getTrajectory()->getFrameList().rend())
{
prev_prev_key_fr_ptr = (*f_it);
}
@@ -227,9 +227,9 @@ int main(int argc, char** argv)
// Solve -----------------------------------------------
// solve only when new KFs are added
- if (problem->getTrajectoryPtr()->getFrameList().size() > number_of_KFs)
+ if (problem->getTrajectory()->getFrameList().size() > number_of_KFs)
{
- number_of_KFs = problem->getTrajectoryPtr()->getFrameList().size();
+ number_of_KFs = problem->getTrajectory()->getFrameList().size();
std::string summary = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);// 0: nothing, 1: BriefReport, 2: FullReport
std::cout << summary << std::endl;
}
diff --git a/src/examples/test_sh_ptr.cpp b/src/examples/test_sh_ptr.cpp
index 294555ed1d5e7a06119463e933bf116f5b65ae14..3a10903c85ee72967dbe8ca23dd1584dbda494b8 100644
--- a/src/examples/test_sh_ptr.cpp
+++ b/src/examples/test_sh_ptr.cpp
@@ -35,7 +35,7 @@ class T; // trajectory
class F; // frame
class C; // capture
class f; // feature
-class c; // constraint
+class c; // factor
class M; // map
class L; // landmark
@@ -156,7 +156,7 @@ class F : public enable_shared_from_this<F>
weak_ptr<T> T_ptr_;
list<shared_ptr<C>> C_list_;
- list<shared_ptr<c>> c_by_list; // list of constraints to this frame
+ list<shared_ptr<c>> c_by_list; // list of factors to this frame
static int id_count_;
bool is_removing;
@@ -228,7 +228,7 @@ class f : public enable_shared_from_this<f>
weak_ptr<C> C_ptr_;
list<shared_ptr<c>> c_list_;
- list<shared_ptr<c>> c_by_list; // list of constraints to this feature
+ list<shared_ptr<c>> c_by_list; // list of factors to this feature
static int id_count_;
bool is_deleting;
@@ -323,7 +323,7 @@ class L : public enable_shared_from_this<L>
weak_ptr<P> P_ptr_;
weak_ptr<M> M_ptr_;
- list<shared_ptr<c>> c_by_list; // list of constraints to this landmark
+ list<shared_ptr<c>> c_by_list; // list of factors to this landmark
static int id_count_;
bool is_deleting;
@@ -549,7 +549,7 @@ using namespace wolf;
void print_cF(const shared_ptr<P>& Pp)
{
- cout << "Frame constraints" << endl;
+ cout << "Frame factors" << endl;
for (auto Fp : Pp->getT()->getFlist())
{
cout << "F" << Fp->id << " @ " << Fp.get() << endl;
@@ -563,7 +563,7 @@ void print_cF(const shared_ptr<P>& Pp)
void print_cf(const shared_ptr<P>& Pp)
{
- cout << "Feature constraints" << endl;
+ cout << "Feature factors" << endl;
for (auto Fp : Pp->getT()->getFlist())
{
for (auto Cp : Fp->getClist())
@@ -583,7 +583,7 @@ void print_cf(const shared_ptr<P>& Pp)
void print_cL(const shared_ptr<P>& Pp)
{
- cout << "Landmark constraints" << endl;
+ cout << "Landmark factors" << endl;
for (auto Lp : Pp->getM()->getLlist())
{
cout << "L" << Lp->id << " @ " << Lp.get() << endl;
@@ -597,7 +597,7 @@ void print_cL(const shared_ptr<P>& Pp)
void print_c(const shared_ptr<P>& Pp)
{
- cout << "All constraints" << endl;
+ cout << "All factors" << endl;
for (auto Fp : Pp->getT()->getFlist())
{
for (auto Cp : Fp->getClist())
@@ -623,7 +623,7 @@ void print_c(const shared_ptr<P>& Pp)
<< " -> L" << cp->getLother()->id << " @ " << cp->getLother() << endl;
break;
default:
- cout << "Bad constraint" << endl;
+ cout << "Bad factor" << endl;
break;
}
}
@@ -668,7 +668,7 @@ shared_ptr<P> buildProblem(int N)
for (int fi = 0; fi < 1; fi++)
{
shared_ptr<f> fp = Cp->add_f(make_shared<f>());
- if (Ci || !Fi) // landmark constraint
+ if (Ci || !Fi) // landmark factor
{
shared_ptr<c> cp = fp->add_c(make_shared<c>(*Lit));
(*Lit)->add_c_by(cp);
@@ -676,7 +676,7 @@ shared_ptr<P> buildProblem(int N)
if (Lit == Pp->getM()->getLlist().end())
Lit = Pp->getM()->getLlist().begin();
}
- else // motion constraint
+ else // motion factor
{
shared_ptr<F> Fp = Fvec.at(Fi-1).lock();
if (Fp)
@@ -703,17 +703,17 @@ int c::id_count_ = 0;
int L::id_count_ = 0;
// tests
-void removeConstraints(const shared_ptr<P>& Pp)
+void removeFactors(const shared_ptr<P>& Pp)
{
- cout << "Removing constraint type L ----------" << endl;
+ cout << "Removing factor type L ----------" << endl;
Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing constraint type L ----------" << endl;
+ cout << "Removing factor type L ----------" << endl;
Pp->getT()->getFlist().front()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing constraint type F ----------" << endl;
+ cout << "Removing factor type F ----------" << endl;
Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
- cout << "Removing constraint type L ----------" << endl;
+ cout << "Removing factor type L ----------" << endl;
Pp->getT()->getFlist().back()->getClist().back()->getflist().front()->getclist().front()->remove();
- cout << "Removing constraint type F ----------" << endl;
+ cout << "Removing factor type F ----------" << endl;
Pp->getT()->getFlist().back()->getClist().front()->getflist().front()->getclist().front()->remove();
}
@@ -754,7 +754,7 @@ int main()
shared_ptr<P> Pp = buildProblem(N);
cout << "Wolf tree created ----------------------------" << endl;
- cout << "\nShowing constraints --------------------------" << endl;
+ cout << "\nShowing factors --------------------------" << endl;
cout<<endl;
print_cF(Pp);
cout<<endl;
@@ -768,8 +768,8 @@ int main()
// Several tests. Uncomment the desired test.
// Run only one test at a time, otherwise you'll get segfaults!
-// cout << "\nRemoving constraints -------------------------" << endl;
-// removeConstraints(Pp);
+// cout << "\nRemoving factors -------------------------" << endl;
+// removeFactors(Pp);
// cout << "\nRemoving landmarks ---------------------------" << endl;
// removeLandmarks(Pp);
diff --git a/src/examples/test_simple_AHP.cpp b/src/examples/test_simple_AHP.cpp
index 818250e17f27518cd834bd154eb5ec01d7ebef1e..142b9980846a38c123e01a61c4df2513f724729e 100644
--- a/src/examples/test_simple_AHP.cpp
+++ b/src/examples/test_simple_AHP.cpp
@@ -12,7 +12,7 @@
#include "base/rotations.h"
#include "base/capture/capture_image.h"
#include "base/landmark/landmark_AHP.h"
-#include "base/constraint/constraint_AHP.h"
+#include "base/factor/factor_AHP.h"
#include "base/ceres_wrapper/ceres_manager.h"
// Vision utils
@@ -88,7 +88,7 @@ int main(int argc, char** argv)
* 8. crear captures
* 9. crear features amb les mesures de 4 i 5
* 10. crear lmk2 des de kf3
- * 11. crear constraint des del kf4 a lmk2, amb ancora al kf3
+ * 11. crear factor des del kf4 a lmk2, amb ancora al kf3
* 12. solve
* 13. lmk1 == lmk2 ?
*/
@@ -96,7 +96,7 @@ int main(int argc, char** argv)
// ============================================================================================================
/* 1 */
ProblemPtr problem = Problem::create("PO 3D");
- // One anchor frame to define the lmk, and a copy to make a constraint
+ // One anchor frame to define the lmk, and a copy to make a factor
FrameBasePtr kf_1 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
FrameBasePtr kf_2 = problem->emplaceFrame(KEY_FRAME,(Vector7s()<<0,0,0,0,0,0,1).finished(), TimeStamp(0));
// and two other frames to observe the lmk
@@ -158,24 +158,24 @@ int main(int argc, char** argv)
lmk_1->fix();
std::cout << "Landmark 1: " << lmk_1->point().transpose() << std::endl;
- // Constraints------------------
- ConstraintAHPPtr ctr_0 = ConstraintAHP::create(feat_0, lmk_1, nullptr);
- feat_0->addConstraint(ctr_0);
- ConstraintAHPPtr ctr_1 = ConstraintAHP::create(feat_1, lmk_1, nullptr);
- feat_1->addConstraint(ctr_1);
- ConstraintAHPPtr ctr_2 = ConstraintAHP::create(feat_2, lmk_1, nullptr);
- feat_2->addConstraint(ctr_2);
+ // Factors------------------
+ FactorAHPPtr fac_0 = FactorAHP::create(feat_0, lmk_1, nullptr);
+ feat_0->addFactor(fac_0);
+ FactorAHPPtr fac_1 = FactorAHP::create(feat_1, lmk_1, nullptr);
+ feat_1->addFactor(fac_1);
+ FactorAHPPtr fac_2 = FactorAHP::create(feat_2, lmk_1, nullptr);
+ feat_2->addFactor(fac_2);
// Projections----------------------------
- Eigen::VectorXs pix_0 = ctr_0->expectation();
+ Eigen::VectorXs pix_0 = fac_0->expectation();
kp_0 = cv::KeyPoint(pix_0(0), pix_0(1), 0);
feat_0->setKeypoint(kp_0);
- Eigen::VectorXs pix_1 = ctr_1->expectation();
+ Eigen::VectorXs pix_1 = fac_1->expectation();
kp_1 = cv::KeyPoint(pix_1(0), pix_1(1), 0);
feat_1->setKeypoint(kp_1);
- Eigen::VectorXs pix_2 = ctr_2->expectation();
+ Eigen::VectorXs pix_2 = fac_2->expectation();
kp_2 = cv::KeyPoint(pix_2(0), pix_2(1), 0);
feat_2->setKeypoint(kp_2);
@@ -208,14 +208,14 @@ int main(int argc, char** argv)
problem->addLandmark(lmk_2);
std::cout << "Landmark 2: " << lmk_2->point().transpose() << std::endl;
- // New constraints from kf3 and kf4
- ConstraintAHPPtr ctr_3 = ConstraintAHP::create(feat_3, lmk_2, nullptr);
- feat_3->addConstraint(ctr_3);
- ConstraintAHPPtr ctr_4 = ConstraintAHP::create(feat_4, lmk_2, nullptr);
- feat_4->addConstraint(ctr_4);
+ // New factors from kf3 and kf4
+ FactorAHPPtr fac_3 = FactorAHP::create(feat_3, lmk_2, nullptr);
+ feat_3->addFactor(fac_3);
+ FactorAHPPtr fac_4 = FactorAHP::create(feat_4, lmk_2, nullptr);
+ feat_4->addFactor(fac_4);
- Eigen::Vector2s pix_3 = ctr_3->expectation();
- Eigen::Vector2s pix_4 = ctr_4->expectation();
+ Eigen::Vector2s pix_3 = fac_3->expectation();
+ Eigen::Vector2s pix_4 = fac_4->expectation();
std::cout << "pix 3: " << pix_3.transpose() << std::endl;
std::cout << "pix 4: " << pix_4.transpose() << std::endl;
diff --git a/src/examples/test_sort_keyframes.cpp b/src/examples/test_sort_keyframes.cpp
index 967d37006e16bacb3af02c088c6397f2efabea57..e046765f0bd18cb289a4137f452c581440ab08c8 100644
--- a/src/examples/test_sort_keyframes.cpp
+++ b/src/examples/test_sort_keyframes.cpp
@@ -23,7 +23,7 @@ using namespace wolf;
void printFrames(ProblemPtr _problem_ptr)
{
std::cout << "TRAJECTORY:" << std::endl;
- for (auto frm : _problem_ptr->getTrajectoryPtr()->getFrameList())
+ for (auto frm : _problem_ptr->getTrajectory()->getFrameList())
std::cout << "\t " << (frm->isKey() ? "KEY FRAME: " : "FRAME: ") << frm->id() << " - TS: " << frm->getTimeStamp().getSeconds() << "." << frm->getTimeStamp().getNanoSeconds() << std::endl;
}
diff --git a/src/examples/test_sparsification.cpp b/src/examples/test_sparsification.cpp
index 41b3b8eae524684c795439a5d7021cf8fdf589f8..7b9e85c085a04dc401097db87be3dcc8ea25e17d 100644
--- a/src/examples/test_sparsification.cpp
+++ b/src/examples/test_sparsification.cpp
@@ -16,7 +16,7 @@
//Wolf includes
#include "base/capture/capture_void.h"
#include "base/feature/feature_odom_2D.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
@@ -252,7 +252,7 @@ int main(int argc, char** argv)
if (edge_from == last_frame_ptr->id())
frame_from_ptr = last_frame_ptr;
else
- for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
if ((*frm_rit)->id() == edge_from)
{
frame_from_ptr = *frm_rit;
@@ -261,7 +261,7 @@ int main(int argc, char** argv)
if (edge_to == last_frame_ptr->id())
frame_to_ptr = last_frame_ptr;
else
- for (auto frm_rit = bl_problem_ptr->getTrajectoryPtr()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectoryPtr()->getFrameList().rend(); frm_rit++)
+ for (auto frm_rit = bl_problem_ptr->getTrajectory()->getFrameList().rbegin(); frm_rit != bl_problem_ptr->getTrajectory()->getFrameList().rend(); frm_rit++)
if ((*frm_rit)->id() == edge_to)
{
frame_to_ptr = *frm_rit;
@@ -285,9 +285,9 @@ int main(int argc, char** argv)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINT
- ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, frame_to_ptr);
- feature_ptr->addConstraint(constraint_ptr);
- frame_to_ptr->addConstrainedBy(constraint_ptr);
+ FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, frame_to_ptr);
+ feature_ptr->addFactor(factor_ptr);
+ frame_to_ptr->addConstrainedBy(factor_ptr);
// SOLVE
// solution
diff --git a/src/examples/test_state_quaternion.cpp b/src/examples/test_state_quaternion.cpp
index 4d2aa60cc3f0f677c831f469195432e061f6fe30..5885553ed2fbbed5b6769d1f361c84cb9195509f 100644
--- a/src/examples/test_state_quaternion.cpp
+++ b/src/examples/test_state_quaternion.cpp
@@ -24,9 +24,9 @@ int main (void)
FrameBase pqv(t,pp,op,vp);
- std::cout << "P local param: " << pqv.getPPtr()->getLocalParametrizationPtr() << std::endl;
- std::cout << "Q local param: " << pqv.getOPtr()->getLocalParametrizationPtr() << std::endl;
- std::cout << "V local param: " << pqv.getVPtr()->getLocalParametrizationPtr() << std::endl;
+ std::cout << "P local param: " << pqv.getP()->getLocalParametrization() << std::endl;
+ std::cout << "Q local param: " << pqv.getO()->getLocalParametrization() << std::endl;
+ std::cout << "V local param: " << pqv.getV()->getLocalParametrization() << std::endl;
// delete pp;
// delete op;
diff --git a/src/examples/test_virtual_hierarchy.cpp b/src/examples/test_virtual_hierarchy.cpp
index d83183b840a7f5cdb07c85727babda3cf453406e..fb1b248ce8bae0371579391351daec340fa2c8c5 100644
--- a/src/examples/test_virtual_hierarchy.cpp
+++ b/src/examples/test_virtual_hierarchy.cpp
@@ -43,7 +43,7 @@ class ChildOf : virtual public N
protected:
ChildOf(Parent* _up_ptr) : up_ptr_(_up_ptr) { }
virtual ~ChildOf() { }
- Parent* upPtr() { return up_ptr_; }
+ Parent* up() { return up_ptr_; }
};
/**
diff --git a/src/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
index a8de6a82b12647cb6e2165bbc9400d2da91b56cd..1fa76e585e51796e19a0597aac1397ebf19cd954 100644
--- a/src/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -114,8 +114,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_ceres_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_wolf_diff = new FrameBase(TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_ceres_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_ceres_diff);
- wolf_problem_wolf_diff->getTrajectoryPtr()->addFrame(vertex_frame_ptr_wolf_diff);
+ wolf_problem_ceres_diff->getTrajectory()->addFrame(vertex_frame_ptr_ceres_diff);
+ wolf_problem_wolf_diff->getTrajectory()->addFrame(vertex_frame_ptr_wolf_diff);
// store
index_2_frame_ptr_ceres_diff[vertex_index] = vertex_frame_ptr_ceres_diff;
index_2_frame_ptr_wolf_diff[vertex_index] = vertex_frame_ptr_wolf_diff;
@@ -222,7 +222,7 @@ int main(int argc, char** argv)
edge_information(2,1) = atof(bNum.c_str());
bNum.clear();
- // add capture, feature and constraint to problem
+ // add capture, feature and factor to problem
FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_ceres_diff = new CaptureVoid(TimeStamp(0), sensor);
@@ -239,14 +239,14 @@ int main(int argc, char** argv)
frame_new_ptr_wolf_diff->addCapture(capture_ptr_wolf_diff);
capture_ptr_ceres_diff->addFeature(feature_ptr_ceres_diff);
capture_ptr_wolf_diff->addFeature(feature_ptr_wolf_diff);
- ConstraintOdom2D* constraint_ptr_ceres_diff = new ConstraintOdom2D(feature_ptr_ceres_diff, frame_old_ptr_ceres_diff);
- ConstraintOdom2D* constraint_ptr_wolf_diff = new ConstraintOdom2D(feature_ptr_wolf_diff, frame_old_ptr_wolf_diff);
- feature_ptr_ceres_diff->addConstraint(constraint_ptr_ceres_diff);
- feature_ptr_wolf_diff->addConstraint(constraint_ptr_wolf_diff);
- //std::cout << "Added edge! " << constraint_ptr_wolf_diff->id() << " from vertex " << constraint_ptr_wolf_diff->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_wolf_diff->getFrameToPtr()->id() << std::endl;
- //std::cout << "vector " << constraint_ptr_wolf_diff->getMeasurement().transpose() << std::endl;
+ FactorOdom2D* factor_ptr_ceres_diff = new FactorOdom2D(feature_ptr_ceres_diff, frame_old_ptr_ceres_diff);
+ FactorOdom2D* factor_ptr_wolf_diff = new FactorOdom2D(feature_ptr_wolf_diff, frame_old_ptr_wolf_diff);
+ feature_ptr_ceres_diff->addFactor(factor_ptr_ceres_diff);
+ feature_ptr_wolf_diff->addFactor(factor_ptr_wolf_diff);
+ //std::cout << "Added edge! " << factor_ptr_wolf_diff->id() << " from vertex " << factor_ptr_wolf_diff->getCapture()->getFrame()->id() << " to " << factor_ptr_wolf_diff->getFrameTo()->id() << std::endl;
+ //std::cout << "vector " << factor_ptr_wolf_diff->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << constraint_ptr_wolf_diff->getMeasurementCovariance() << std::endl;
+ //std::cout << "covariance " << std::endl << factor_ptr_wolf_diff->getMeasurementCovariance() << std::endl;
}
}
else
@@ -258,15 +258,15 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_ceres_diff = wolf_problem_ceres_diff->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_wolf_diff = wolf_problem_wolf_diff->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_ceres_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_ceres_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_wolf_diff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_wolf_diff->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_ceres_diff->addCapture(initial_covariance_ceres_diff);
first_frame_wolf_diff->addCapture(initial_covariance_wolf_diff);
initial_covariance_ceres_diff->process();
initial_covariance_wolf_diff->process();
- //std::cout << "initial covariance: constraint " << initial_covariance_wolf_diff->getFeatureList().front()->getConstraintFromList().front()->id() << std::endl << initial_covariance_wolf_diff->getFeatureList().front()->getMeasurementCovariance() << std::endl;
+ //std::cout << "initial covariance: factor " << initial_covariance_wolf_diff->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_wolf_diff->getFeatureList().front()->getMeasurementCovariance() << std::endl;
// COMPUTE COVARIANCES
std::cout << "computing covariances..." << std::endl;
@@ -280,13 +280,13 @@ int main(int argc, char** argv)
// SOLVING PROBLEMS
std::cout << "solving..." << std::endl;
- Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
summary_ceres_diff = ceres_manager_ceres_diff->solve();
- Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_ceres_state = wolf_problem_ceres_diff->getTrajectory()->getFrameList().back()->getState();
//std::cout << summary_ceres_diff.BriefReport() << std::endl;
- Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs prev_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
summary_wolf_diff = ceres_manager_wolf_diff->solve();
- Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectoryPtr()->getFrameList().back()->getState();
+ Eigen::VectorXs post_wolf_state = wolf_problem_wolf_diff->getTrajectory()->getFrameList().back()->getState();
//std::cout << summary_wolf_diff.BriefReport() << std::endl;
std::cout << "solved!" << std::endl;
diff --git a/src/examples/test_wolf_factories.cpp b/src/examples/test_wolf_factories.cpp
index 3ada8333437f02034267943e68a4ab2c8bf57517..b35d0dc1b335d88759ecdcd3b4c277a309b58636 100644
--- a/src/examples/test_wolf_factories.cpp
+++ b/src/examples/test_wolf_factories.cpp
@@ -78,7 +78,7 @@ int main(void)
problem->installSensor("CAMERA", "rear camera", pq_3d, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
// print available sensors
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
{
cout << "Sensor " << setw(2) << left << sen->id()
<< " | type " << setw(8) << sen->getType()
@@ -95,12 +95,12 @@ int main(void)
// problem->createProcessor("GPS", "GPS pseudoranges", "GPS raw");
// print installed processors
- for (auto sen : problem->getHardwarePtr()->getSensorList())
+ for (auto sen : problem->getHardware()->getSensorList())
for (auto prc : sen->getProcessorList())
cout << "Processor " << setw(2) << left << prc->id()
<< " | type : " << setw(8) << prc->getType()
<< " | name: " << setw(17) << prc->getName()
- << " | bound to sensor " << setw(2) << prc->getSensorPtr()->id() << ": " << prc->getSensorPtr()->getName() << endl;
+ << " | bound to sensor " << setw(2) << prc->getSensor()->id() << ": " << prc->getSensor()->getName() << endl;
return 0;
}
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index cbfe23b787c559b5896c1cd178fdd5b260aaa79b..1113af8b7f7e9bd40033ed60e182cc2f485eee99 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -132,8 +132,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
- wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
+ wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
+ wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
@@ -242,7 +242,7 @@ int main(int argc, char** argv)
edge_information(2,1) = atof(bNum.c_str());
bNum.clear();
- // add capture, feature and constraint to problem
+ // add capture, feature and factor to problem
FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
@@ -259,22 +259,22 @@ int main(int argc, char** argv)
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
capture_ptr_prun->addFeature(feature_ptr_prun);
- ConstraintOdom2D* constraint_ptr_full = new ConstraintOdom2D(feature_ptr_full, frame_old_ptr_full);
- ConstraintOdom2D* constraint_ptr_prun = new ConstraintOdom2D(feature_ptr_prun, frame_old_ptr_prun);
- feature_ptr_full->addConstraint(constraint_ptr_full);
- feature_ptr_prun->addConstraint(constraint_ptr_prun);
- //std::cout << "Added edge! " << constraint_ptr_prun->id() << " from vertex " << constraint_ptr_prun->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_prun->getFrameToPtr()->id() << std::endl;
- //std::cout << "vector " << constraint_ptr_prun->getMeasurement().transpose() << std::endl;
+ FactorOdom2D* factor_ptr_full = new FactorOdom2D(feature_ptr_full, frame_old_ptr_full);
+ FactorOdom2D* factor_ptr_prun = new FactorOdom2D(feature_ptr_prun, frame_old_ptr_prun);
+ feature_ptr_full->addFactor(factor_ptr_full);
+ feature_ptr_prun->addFactor(factor_ptr_prun);
+ //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameTo()->id() << std::endl;
+ //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl;
+ //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
- Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
- Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
- Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+ Scalar xi = *(frame_old_ptr_prun->getP()->get());
+ Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
+ Scalar thi = *(frame_old_ptr_prun->getO()->get());
Scalar si = sin(thi);
Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
- Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+ Scalar xj = *(frame_new_ptr_prun->getP()->get());
+ Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
Eigen::MatrixXs Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -301,22 +301,22 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
first_frame_prun->addCapture(initial_covariance_prun);
initial_covariance_full->process();
initial_covariance_prun->process();
- //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureList().front()->getConstraintFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
+ //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
Lambda = Lambda + DeltaLambda;
// COMPUTE COVARIANCES
- ConstraintBaseList constraints;
- wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints);
+ FactorBasePtrList factors;
+ wolf_problem_prun->getTrajectory()->getFactorList(factors);
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
@@ -333,40 +333,40 @@ int main(int argc, char** argv)
t1 = clock();
- for (auto c_it=constraints.begin(); c_it!=constraints.end(); c_it++)
+ for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
{
- if ((*c_it)->getCategory() != CTR_FRAME) continue;
+ if ((*c_it)->getCategory() != FAC_FRAME) continue;
// ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3) << std::endl;
+// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
// jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
// ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
//jacobian
- xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
- yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
- thi = *(*c_it)->getFrameOtherPtr()->getOPtr()->getPtr();
+ xi = *(*c_it)->getFrameOther()->getP()->get();
+ yi = *((*c_it)->getFrameOther()->getP()->get()+1);
+ thi = *(*c_it)->getFrameOther()->getO()->get();
si = sin(thi);
ci = cos(thi);
- xj = *(*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr();
- yj = *((*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr()+1);
+ xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
+ yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -378,7 +378,7 @@ int main(int argc, char** argv)
//std::cout << "Jj" << std::endl << Jj << std::endl;
// Measurement covariance
- Sigma_z = (*c_it)->getFeaturePtr()->getMeasurementCovariance();
+ Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
//std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
//std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
@@ -388,7 +388,7 @@ int main(int argc, char** argv)
//std::cout << "IG = " << IG << std::endl;
if (IG < 2)
- (*c_it)->setStatus(CTR_INACTIVE);
+ (*c_it)->setStatus(FAC_INACTIVE);
}
double t_ig = ((double) clock() - t1) / CLOCKS_PER_SEC;
std::cout << "manual sigma computation " << t_sigma_manual << "s" << std::endl;
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 13b3dd86d1602f3c3b9fdf9764bcde1f001cdd12..f3ed29007190ada48bba1758a1ac4e57843ff423 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -13,7 +13,7 @@
//Wolf includes
#include "wolf_manager.h"
#include "base/capture/capture_void.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/ceres_wrapper/ceres_manager.h"
// EIGEN
@@ -82,7 +82,7 @@ int main(int argc, char** argv)
Eigen::SparseMatrix<Scalar> Lambda(0,0);
// prunning
- ConstraintBaseList ordered_ctr_ptr;
+ FactorBasePtrList ordered_fac_ptr;
std::list<Scalar> ordered_ig;
// Ceres wrapper
@@ -150,8 +150,8 @@ int main(int argc, char** argv)
// add frame to problem
FrameBasePtr vertex_frame_ptr_full = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
FrameBasePtr vertex_frame_ptr_prun = new FrameBase(KEY_FRAME, TimeStamp(0), std::make_shared<StateBlock>(vertex_pose.head(2)), std::make_shared<StateBlock>(vertex_pose.tail(1)));
- wolf_problem_full->getTrajectoryPtr()->addFrame(vertex_frame_ptr_full);
- wolf_problem_prun->getTrajectoryPtr()->addFrame(vertex_frame_ptr_prun);
+ wolf_problem_full->getTrajectory()->addFrame(vertex_frame_ptr_full);
+ wolf_problem_prun->getTrajectory()->addFrame(vertex_frame_ptr_prun);
// store
index_2_frame_ptr_full[vertex_index] = vertex_frame_ptr_full;
index_2_frame_ptr_prun[vertex_index] = vertex_frame_ptr_prun;
@@ -261,7 +261,7 @@ int main(int argc, char** argv)
bNum.clear();
//std::cout << "Adding edge... " << std::endl;
- // add capture, feature and constraint to problem
+ // add capture, feature and factor to problem
FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
@@ -278,23 +278,23 @@ int main(int argc, char** argv)
frame_new_ptr_prun->addCapture(capture_ptr_prun);
capture_ptr_full->addFeature(feature_ptr_full);
capture_ptr_prun->addFeature(feature_ptr_prun);
- ConstraintOdom2DAnalytic* constraint_ptr_full = new ConstraintOdom2DAnalytic(feature_ptr_full, frame_old_ptr_full);
- ConstraintOdom2DAnalytic* constraint_ptr_prun = new ConstraintOdom2DAnalytic(feature_ptr_prun, frame_old_ptr_prun);
- feature_ptr_full->addConstraint(constraint_ptr_full);
- feature_ptr_prun->addConstraint(constraint_ptr_prun);
- //std::cout << "Added edge! " << constraint_ptr_prun->id() << " from vertex " << constraint_ptr_prun->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_prun->getFrameOtherPtr()->id() << std::endl;
- //std::cout << "vector " << constraint_ptr_prun->getMeasurement().transpose() << std::endl;
+ FactorOdom2DAnalytic* factor_ptr_full = new FactorOdom2DAnalytic(feature_ptr_full, frame_old_ptr_full);
+ FactorOdom2DAnalytic* factor_ptr_prun = new FactorOdom2DAnalytic(feature_ptr_prun, frame_old_ptr_prun);
+ feature_ptr_full->addFactor(factor_ptr_full);
+ feature_ptr_prun->addFactor(factor_ptr_prun);
+ //std::cout << "Added edge! " << factor_ptr_prun->id() << " from vertex " << factor_ptr_prun->getCapture()->getFrame()->id() << " to " << factor_ptr_prun->getFrameOther()->id() << std::endl;
+ //std::cout << "vector " << factor_ptr_prun->getMeasurement().transpose() << std::endl;
//std::cout << "information " << std::endl << edge_information << std::endl;
- //std::cout << "covariance " << std::endl << constraint_ptr_prun->getMeasurementCovariance() << std::endl;
+ //std::cout << "covariance " << std::endl << factor_ptr_prun->getMeasurementCovariance() << std::endl;
t1 = clock();
- Scalar xi = *(frame_old_ptr_prun->getPPtr()->getPtr());
- Scalar yi = *(frame_old_ptr_prun->getPPtr()->getPtr()+1);
- Scalar thi = *(frame_old_ptr_prun->getOPtr()->getPtr());
+ Scalar xi = *(frame_old_ptr_prun->getP()->get());
+ Scalar yi = *(frame_old_ptr_prun->getP()->get()+1);
+ Scalar thi = *(frame_old_ptr_prun->getO()->get());
Scalar si = sin(thi);
Scalar ci = cos(thi);
- Scalar xj = *(frame_new_ptr_prun->getPPtr()->getPtr());
- Scalar yj = *(frame_new_ptr_prun->getPPtr()->getPtr()+1);
+ Scalar xj = *(frame_new_ptr_prun->getP()->get());
+ Scalar yj = *(frame_new_ptr_prun->getP()->get()+1);
Eigen::MatrixXs Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -322,15 +322,15 @@ int main(int argc, char** argv)
printf("\nError opening file\n");
// PRIOR
- FrameBasePtr first_frame_full = wolf_problem_full->getTrajectoryPtr()->getFrameList().front();
- FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectoryPtr()->getFrameList().front();
+ FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
+ FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
first_frame_prun->addCapture(initial_covariance_prun);
initial_covariance_full->process();
initial_covariance_prun->process();
- //std::cout << "initial covariance: constraint " << initial_covariance_prun->getFeatureList().front()->getConstraintFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
+ //std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
t1 = clock();
Eigen::SparseMatrix<Scalar> DeltaLambda(Lambda.rows(), Lambda.cols());
insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
@@ -338,8 +338,8 @@ int main(int argc, char** argv)
t_sigma_manual += ((double) clock() - t1) / CLOCKS_PER_SEC;
// COMPUTE COVARIANCES
- ConstraintBaseList constraints;
- wolf_problem_prun->getTrajectoryPtr()->getConstraintList(constraints);
+ FactorBasePtrList factors;
+ wolf_problem_prun->getTrajectory()->getFactorList(factors);
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<Scalar>> chol(Lambda); // performs a Cholesky factorization of A
@@ -356,40 +356,40 @@ int main(int argc, char** argv)
t1 = clock();
- for (auto c_it=constraints.begin(); c_it!=constraints.end(); c_it++)
+ for (auto c_it=factors.begin(); c_it!=factors.end(); c_it++)
{
- if ((*c_it)->getCategory() != CTR_FRAME) continue;
+ if ((*c_it)->getCategory() != FAC_FRAME) continue;
// Measurement covariance
- Sigma_z = (*c_it)->getFeaturePtr()->getMeasurementCovariance();
+ Sigma_z = (*c_it)->getFeature()->getMeasurementCovariance();
//std::cout << "Sigma_z" << std::endl << Sigma_z << std::endl;
//std::cout << "Sigma_z.determinant() = " << Sigma_z.determinant() << std::endl;
// NEW WAY
// State covariance
//11
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_11);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_11);
//12
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_12);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_12);
//13
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_13);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_13);
//14
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_14);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_14);
//22
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_22);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_22);
//23
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_23);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_23);
//24
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_24);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_24);
//33
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_33);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_33);
//34
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_34);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_34);
//44
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_44);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_44);
// std::cout << "Sigma_11" << std::endl << Sigma_11 << std::endl;
// std::cout << "Sigma_12" << std::endl << Sigma_12 << std::endl;
@@ -403,7 +403,7 @@ int main(int argc, char** argv)
// std::cout << "Sigma_44" << std::endl << Sigma_44 << std::endl;
// jacobians
- ((ConstraintAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
+ ((FactorAnalytic*)(*c_it))->evaluatePureJacobians(jacobians);
Eigen::MatrixXs& J1 = jacobians[0];
Eigen::MatrixXs& J2 = jacobians[1];
Eigen::MatrixXs& J3 = jacobians[2];
@@ -452,35 +452,35 @@ int main(int argc, char** argv)
// OLD WAY
// ii (old)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getPPtr(), Sigma_ii, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getFrameOtherPtr()->getOPtr(), Sigma_ii, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getP(), Sigma_ii, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getFrameOther()->getO(), Sigma_ii, 2, 2);
// std::cout << "Sigma_ii" << std::endl << Sigma_ii << std::endl;
-// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3) << std::endl;
+// std::cout << "Sigma(i,i)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3) << std::endl;
// jj (new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_jj, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_jj, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_jj, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getCapture()->getFrame()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_jj, 2, 2);
// std::cout << "Sigma_jj" << std::endl << Sigma_jj << std::endl;
-// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(j,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
// ij (old-new)
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 0, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getPPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 0, 2);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getPPtr(), Sigma_ij, 2, 0);
- wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOtherPtr()->getOPtr(), (*c_it)->getCapturePtr()->getFramePtr()->getOPtr(), Sigma_ij, 2, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 0, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getP(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 0, 2);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getP(), Sigma_ij, 2, 0);
+ wolf_problem_prun->getCovarianceBlock((*c_it)->getFrameOther()->getO(), (*c_it)->getCapture()->getFrame()->getO(), Sigma_ij, 2, 2);
// std::cout << "Sigma_ij" << std::endl << Sigma_ij << std::endl;
-// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameToPtr()]*3, frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()]*3) << std::endl;
+// std::cout << "Sigma(i,j)" << std::endl << Sigma.block<3,3>(frame_ptr_2_index_prun[(*c_it)->getFrameTo()]*3, frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()]*3) << std::endl;
//jacobian
- xi = *(*c_it)->getFrameOtherPtr()->getPPtr()->getPtr();
- yi = *((*c_it)->getFrameOtherPtr()->getPPtr()->getPtr()+1);
- thi = *(*c_it)->getFrameOtherPtr()->getOPtr()->getPtr();
+ xi = *(*c_it)->getFrameOther()->getP()->get();
+ yi = *((*c_it)->getFrameOther()->getP()->get()+1);
+ thi = *(*c_it)->getFrameOther()->getO()->get();
si = sin(thi);
ci = cos(thi);
- xj = *(*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr();
- yj = *((*c_it)->getCapturePtr()->getFramePtr()->getPPtr()->getPtr()+1);
+ xj = *(*c_it)->getCapture()->getFrame()->getP()->get();
+ yj = *((*c_it)->getCapture()->getFrame()->getP()->get()+1);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
@@ -507,31 +507,31 @@ int main(int argc, char** argv)
if (IG < 2 && IG > 0 && !std::isnan(IG))
{
// Store as a candidate to be prunned, ordered by information gain
- auto ordered_ctr_it = ordered_ctr_ptr.begin();
- for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_ctr_it++ )
+ auto ordered_fac_it = ordered_fac_ptr.begin();
+ for (auto ordered_ig_it = ordered_ig.begin(); ordered_ig_it != ordered_ig.end(); ordered_ig_it++, ordered_fac_it++ )
if (IG < (*ordered_ig_it))
{
ordered_ig.insert(ordered_ig_it, IG);
- ordered_ctr_ptr.insert(ordered_ctr_it, (*c_it));
+ ordered_fac_ptr.insert(ordered_fac_it, (*c_it));
break;
}
ordered_ig.insert(ordered_ig.end(), IG);
- ordered_ctr_ptr.insert(ordered_ctr_ptr.end(), (*c_it));
+ ordered_fac_ptr.insert(ordered_fac_ptr.end(), (*c_it));
}
}
// PRUNNING
- std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectoryPtr()->getFrameList().size(), false);
- for (auto c_it = ordered_ctr_ptr.begin(); c_it != ordered_ctr_ptr.end(); c_it++ )
+ std::vector<bool> any_inactive_in_frame(wolf_problem_prun->getTrajectory()->getFrameList().size(), false);
+ for (auto c_it = ordered_fac_ptr.begin(); c_it != ordered_fac_ptr.end(); c_it++ )
{
- // Check heuristic: constraint do not share node with any inactive constraint
- unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapturePtr()->getFramePtr()];
- unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOtherPtr()];
+ // Check heuristic: factor do not share node with any inactive factor
+ unsigned int& index_frame = frame_ptr_2_index_prun[(*c_it)->getCapture()->getFrame()];
+ unsigned int& index_frame_other = frame_ptr_2_index_prun[(*c_it)->getFrameOther()];
if (!any_inactive_in_frame[index_frame] && !any_inactive_in_frame[index_frame_other])
{
std::cout << "setting inactive" << (*c_it)->id() << std::endl;
- (*c_it)->setStatus(CTR_INACTIVE);
+ (*c_it)->setStatus(FAC_INACTIVE);
std::cout << "set!" << std::endl;
any_inactive_in_frame[index_frame] = true;
any_inactive_in_frame[index_frame_other] = true;
diff --git a/src/constraint/CMakeLists.txt b/src/factor/CMakeLists.txt
similarity index 77%
rename from src/constraint/CMakeLists.txt
rename to src/factor/CMakeLists.txt
index 8d9661d2c7d598c4ae26ea543ac1f3b573db5bbf..9c575b51f3e266d23e243f6713356516d70901ab 100644
--- a/src/constraint/CMakeLists.txt
+++ b/src/factor/CMakeLists.txt
@@ -11,9 +11,9 @@ INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR})
#=========================================
SET(HDRS_CONSTRAINT ${HDRS_CONSTRAINT}
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_distance_3D.h
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_autodiff_trifocal.h
- ${CMAKE_CURRENT_SOURCE_DIR}/constraint_diff_drive.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_distance_3D.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/factor_autodiff_trifocal.h
+ ${CMAKE_CURRENT_SOURCE_DIR}/factor_diff_drive.h
)
SET(SRCS_CONSTRAINT ${SRCS_CONSTRAINT}
diff --git a/src/constraint/constraint_analytic.cpp b/src/factor/factor_analytic.cpp
similarity index 82%
rename from src/constraint/constraint_analytic.cpp
rename to src/factor/factor_analytic.cpp
index 9cfb538b1647487968e1e35e73db4af4603bc857..c86bec4aa9b35d7e7879f5a6c7f53c9181e78570 100644
--- a/src/constraint/constraint_analytic.cpp
+++ b/src/factor/factor_analytic.cpp
@@ -1,13 +1,13 @@
-#include "base/constraint/constraint_analytic.h"
+#include "base/factor/factor_analytic.h"
#include "base/state_block.h"
namespace wolf {
-ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
- bool _apply_loss_function, ConstraintStatus _status,
+FactorAnalytic::FactorAnalytic(const std::string& _tp,
+ bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _apply_loss_function, _status),
+ FactorBase(_tp, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
@@ -24,16 +24,16 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
resizeVectors();
}
-ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
+FactorAnalytic::FactorAnalytic(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status,
+ bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase(_tp, _frame_other_ptr, _capture_other_ptr, _feature_other_ptr, _landmark_other_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr}),
state_block_sizes_vector_({_state0Ptr ? (unsigned int) _state0Ptr->getSize() : 0,
@@ -50,33 +50,33 @@ ConstraintAnalytic::ConstraintAnalytic(const std::string& _tp,
resizeVectors();
}
/*
-ConstraintAnalytic::ConstraintAnalytic(ConstraintType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status,
+FactorAnalytic::FactorAnalytic(FactorType _tp, const LandmarkBasePtr& _landmark_ptr, const ProcessorBasePtr& _processor_ptr,
+ bool _apply_loss_function, FactorStatus _status,
StateBlockPtr _state0Ptr, StateBlockPtr _state1Ptr, StateBlockPtr _state2Ptr, StateBlockPtr _state3Ptr, StateBlockPtr _state4Ptr,
StateBlockPtr _state5Ptr, StateBlockPtr _state6Ptr, StateBlockPtr _state7Ptr, StateBlockPtr _state8Ptr, StateBlockPtr _state9Ptr ) :
- ConstraintBase( _tp, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
+ FactorBase( _tp, nullptr, nullptr, _landmark_ptr, _processor_ptr, _apply_loss_function, _status),
state_ptr_vector_({_state0Ptr, _state1Ptr, _state2Ptr, _state3Ptr, _state4Ptr,
_state5Ptr, _state6Ptr, _state7Ptr, _state8Ptr, _state9Ptr})
{
resizeVectors();
}
*/
-std::vector<StateBlockPtr> ConstraintAnalytic::getStateBlockPtrVector() const
+std::vector<StateBlockPtr> FactorAnalytic::getStateBlockPtrVector() const
{
return state_ptr_vector_;
}
-std::vector<unsigned int> ConstraintAnalytic::getStateSizes() const
+std::vector<unsigned int> FactorAnalytic::getStateSizes() const
{
return state_block_sizes_vector_;
}
-JacobianMethod ConstraintAnalytic::getJacobianMethod() const
+JacobianMethod FactorAnalytic::getJacobianMethod() const
{
return JAC_ANALYTIC;
}
-void ConstraintAnalytic::resizeVectors()
+void FactorAnalytic::resizeVectors()
{
assert(state_ptr_vector_[0] != nullptr && "at least one not null state block pointer required");
diff --git a/src/constraint/constraint_base.cpp b/src/factor/factor_base.cpp
similarity index 59%
rename from src/constraint/constraint_base.cpp
rename to src/factor/factor_base.cpp
index f521cf214ecd8de80205a7dfbe8572433eca353c..e3dc7b1ab767657d613dc99c9f41925cf6a290da 100644
--- a/src/constraint/constraint_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -1,17 +1,17 @@
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/frame_base.h"
#include "base/landmark/landmark_base.h"
namespace wolf {
-unsigned int ConstraintBase::constraint_id_count_ = 0;
+unsigned int FactorBase::factor_id_count_ = 0;
-ConstraintBase::ConstraintBase(const std::string& _tp,
+FactorBase::FactorBase(const std::string& _tp,
bool _apply_loss_function,
- ConstraintStatus _status) :
+ FactorStatus _status) :
NodeBase("CONSTRAINT", _tp),
feature_ptr_(), // nullptr
- constraint_id_(++constraint_id_count_),
+ factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_ptr_(), // nullptr
@@ -22,16 +22,16 @@ ConstraintBase::ConstraintBase(const std::string& _tp,
// std::cout << "constructed +c" << id() << std::endl;
}
-ConstraintBase::ConstraintBase(const std::string& _tp,
+FactorBase::FactorBase(const std::string& _tp,
const FrameBasePtr& _frame_other_ptr,
const CaptureBasePtr& _capture_other_ptr,
const FeatureBasePtr& _feature_other_ptr,
const LandmarkBasePtr& _landmark_other_ptr,
const ProcessorBasePtr& _processor_ptr,
- bool _apply_loss_function, ConstraintStatus _status) :
+ bool _apply_loss_function, FactorStatus _status) :
NodeBase("CONSTRAINT", _tp),
feature_ptr_(),
- constraint_id_(++constraint_id_count_),
+ factor_id_(++factor_id_count_),
status_(_status),
apply_loss_function_(_apply_loss_function),
frame_other_ptr_(_frame_other_ptr),
@@ -43,22 +43,22 @@ ConstraintBase::ConstraintBase(const std::string& _tp,
// std::cout << "constructed +c" << id() << std::endl;
}
-void ConstraintBase::remove()
+void FactorBase::remove()
{
if (!is_removing_)
{
is_removing_ = true;
- ConstraintBasePtr this_c = shared_from_this(); // keep this alive while removing it
+ FactorBasePtr this_c = shared_from_this(); // keep this alive while removing it
FeatureBasePtr f = feature_ptr_.lock();
if (f)
{
- f->getConstraintList().remove(shared_from_this()); // remove from upstream
- if (f->getConstraintList().empty() && f->getConstrainedByList().empty())
+ f->getFactorList().remove(shared_from_this()); // remove from upstream
+ if (f->getFactorList().empty() && f->getConstrainedByList().empty())
f->remove(); // remove upstream
}
- // add constraint to be removed from solver
+ // add factor to be removed from solver
if (getProblem() != nullptr)
- getProblem()->removeConstraint(shared_from_this());
+ getProblem()->removeFactor(shared_from_this());
// remove other: {Frame, Capture, Feature, Landmark}
FrameBasePtr frm_o = frame_other_ptr_.lock();
@@ -81,7 +81,7 @@ void ConstraintBase::remove()
if (ftr_o)
{
ftr_o->getConstrainedByList().remove(shared_from_this());
- if (ftr_o->getConstrainedByList().empty() && ftr_o->getConstraintList().empty())
+ if (ftr_o->getConstrainedByList().empty() && ftr_o->getFactorList().empty())
ftr_o->remove();
}
@@ -97,51 +97,51 @@ void ConstraintBase::remove()
}
}
-const Eigen::VectorXs& ConstraintBase::getMeasurement() const
+const Eigen::VectorXs& FactorBase::getMeasurement() const
{
- return getFeaturePtr()->getMeasurement();
+ return getFeature()->getMeasurement();
}
-const Eigen::MatrixXs& ConstraintBase::getMeasurementCovariance() const
+const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
{
- return getFeaturePtr()->getMeasurementCovariance();
+ return getFeature()->getMeasurementCovariance();
}
-const Eigen::MatrixXs& ConstraintBase::getMeasurementSquareRootInformationUpper() const
+const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
{
- return getFeaturePtr()->getMeasurementSquareRootInformationUpper();
+ return getFeature()->getMeasurementSquareRootInformationUpper();
}
-CaptureBasePtr ConstraintBase::getCapturePtr() const
+CaptureBasePtr FactorBase::getCapture() const
{
- return getFeaturePtr()->getCapturePtr();
+ return getFeature()->getCapture();
}
-void ConstraintBase::setStatus(ConstraintStatus _status)
+void FactorBase::setStatus(FactorStatus _status)
{
if (getProblem() == nullptr)
- std::cout << "constraint not linked with 'top', only status changed" << std::endl;
+ std::cout << "factor not linked with 'top', only status changed" << std::endl;
else if (_status != status_)
{
- if (_status == CTR_ACTIVE)
- getProblem()->addConstraint(shared_from_this());
- else if (_status == CTR_INACTIVE)
- getProblem()->removeConstraint(shared_from_this());
+ if (_status == FAC_ACTIVE)
+ getProblem()->addFactor(shared_from_this());
+ else if (_status == FAC_INACTIVE)
+ getProblem()->removeFactor(shared_from_this());
}
status_ = _status;
}
-void ConstraintBase::setApplyLossFunction(const bool _apply)
+void FactorBase::setApplyLossFunction(const bool _apply)
{
if (apply_loss_function_ != _apply)
{
if (getProblem() == nullptr)
- std::cout << "constraint not linked with Problem, apply loss function change not notified" << std::endl;
+ std::cout << "factor not linked with Problem, apply loss function change not notified" << std::endl;
else
{
- ConstraintBasePtr this_c = shared_from_this();
- getProblem()->removeConstraint(this_c);
- getProblem()->addConstraint(this_c);
+ FactorBasePtr this_c = shared_from_this();
+ getProblem()->removeFactor(this_c);
+ getProblem()->addFactor(this_c);
}
}
}
diff --git a/src/feature/feature_IMU.cpp b/src/feature/feature_IMU.cpp
index 0f811cff599f6bbc62e28dd16b0cc6a82648b02b..b35baf2d0a7dc9191e0a17886aea0a6134b060df 100644
--- a/src/feature/feature_IMU.cpp
+++ b/src/feature/feature_IMU.cpp
@@ -13,7 +13,7 @@ FeatureIMU::FeatureIMU(const Eigen::VectorXs& _delta_preintegrated,
jacobian_bias_(_dD_db_jacobians)
{
if (_cap_imu_ptr)
- this->setCapturePtr(_cap_imu_ptr);
+ this->setCapture(_cap_imu_ptr);
}
FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
@@ -22,7 +22,7 @@ FeatureIMU::FeatureIMU(CaptureMotionPtr _cap_imu_ptr):
gyro_bias_preint_(_cap_imu_ptr->getCalibrationPreint().tail<3>()),
jacobian_bias_(_cap_imu_ptr->getJacobianCalib())
{
- this->setCapturePtr(_cap_imu_ptr);
+ this->setCapture(_cap_imu_ptr);
}
FeatureIMU::~FeatureIMU()
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 314457be284f5fc16bfba370c41c47f8ff963ffa..b47c956b9f39e574f8a8c019788669bc024efbe7 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -1,5 +1,5 @@
#include "base/feature/feature_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/capture/capture_base.h"
namespace wolf {
@@ -40,9 +40,9 @@ void FeatureBase::remove()
}
// remove downstream
- while (!constraint_list_.empty())
+ while (!factor_list_.empty())
{
- constraint_list_.front()->remove(); // remove downstream
+ factor_list_.front()->remove(); // remove downstream
}
while (!constrained_by_list_.empty())
{
@@ -51,42 +51,42 @@ void FeatureBase::remove()
}
}
-ConstraintBasePtr FeatureBase::addConstraint(ConstraintBasePtr _co_ptr)
+FactorBasePtr FeatureBase::addFactor(FactorBasePtr _co_ptr)
{
- constraint_list_.push_back(_co_ptr);
- _co_ptr->setFeaturePtr(shared_from_this());
+ factor_list_.push_back(_co_ptr);
+ _co_ptr->setFeature(shared_from_this());
_co_ptr->setProblem(getProblem());
- // add constraint to be added in solver
+ // add factor to be added in solver
if (getProblem() != nullptr)
{
- if (_co_ptr->getStatus() == CTR_ACTIVE)
- getProblem()->addConstraint(_co_ptr);
+ if (_co_ptr->getStatus() == FAC_ACTIVE)
+ getProblem()->addFactor(_co_ptr);
}
else
WOLF_TRACE("WARNING: ADDING CONSTRAINT ", _co_ptr->id(), " TO FEATURE ", this->id(), " NOT CONNECTED WITH PROBLEM.");
return _co_ptr;
}
-FrameBasePtr FeatureBase::getFramePtr() const
+FrameBasePtr FeatureBase::getFrame() const
{
- return capture_ptr_.lock()->getFramePtr();
+ return capture_ptr_.lock()->getFrame();
}
-ConstraintBasePtr FeatureBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+FactorBasePtr FeatureBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setFeatureOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setFeatureOther(shared_from_this());
+ return _fac_ptr;
}
-ConstraintBaseList& FeatureBase::getConstraintList()
+FactorBasePtrList& FeatureBase::getFactorList()
{
- return constraint_list_;
+ return factor_list_;
}
-void FeatureBase::getConstraintList(ConstraintBaseList & _ctr_list)
+void FeatureBase::getFactorList(FactorBasePtrList & _fac_list)
{
- _ctr_list.insert(_ctr_list.end(), constraint_list_.begin(), constraint_list_.end());
+ _fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
@@ -117,7 +117,7 @@ void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
void FeatureBase::setProblem(ProblemPtr _problem)
{
NodeBase::setProblem(_problem);
- for (auto ctr : constraint_list_)
+ for (auto ctr : factor_list_)
ctr->setProblem(_problem);
}
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
index 4f37086f9f5b8a79178a24f0253ac512f5afd53b..472072e54eca2369c43b6e8cbb2e8c09d66f86e2 100644
--- a/src/feature/feature_odom_2D.cpp
+++ b/src/feature/feature_odom_2D.cpp
@@ -13,7 +13,7 @@ FeatureOdom2D::~FeatureOdom2D()
//
}
-void FeatureOdom2D::findConstraints()
+void FeatureOdom2D::findFactors()
{
}
diff --git a/src/frame_base.cpp b/src/frame_base.cpp
index 5c1b7fee23ab834725340ac91f5df77c9db4b051..ccd51dc7f71cf6e696ed68c5e77a50f314b5bf45 100644
--- a/src/frame_base.cpp
+++ b/src/frame_base.cpp
@@ -1,6 +1,6 @@
#include "base/frame_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/trajectory_base.h"
#include "base/capture/capture_base.h"
#include "base/state_block.h"
@@ -71,8 +71,8 @@ void FrameBase::remove()
if ( isKey() )
removeStateBlocks();
- if (getTrajectoryPtr()->getLastKeyFramePtr()->id() == this_F->id())
- getTrajectoryPtr()->setLastKeyFramePtr(getTrajectoryPtr()->findLastKeyFramePtr());
+ if (getTrajectory()->getLastKeyFrame()->id() == this_F->id())
+ getTrajectory()->setLastKeyFrame(getTrajectory()->findLastKeyFrame());
// std::cout << "Removed F" << id() << std::endl;
}
@@ -81,8 +81,8 @@ void FrameBase::remove()
void FrameBase::setTimeStamp(const TimeStamp& _ts)
{
time_stamp_ = _ts;
- if (isKey() && getTrajectoryPtr() != nullptr)
- getTrajectoryPtr()->sortFrame(shared_from_this());
+ if (isKey() && getTrajectory() != nullptr)
+ getTrajectory()->sortFrame(shared_from_this());
}
void FrameBase::registerNewStateBlocks()
@@ -99,14 +99,14 @@ void FrameBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- StateBlockPtr sbp = getStateBlockPtr(i);
+ StateBlockPtr sbp = getStateBlock(i);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
@@ -118,10 +118,10 @@ void FrameBase::setKey()
type_ = KEY_FRAME;
registerNewStateBlocks();
- if (getTrajectoryPtr()->getLastKeyFramePtr() == nullptr || getTrajectoryPtr()->getLastKeyFramePtr()->getTimeStamp() < time_stamp_)
- getTrajectoryPtr()->setLastKeyFramePtr(shared_from_this());
+ if (getTrajectory()->getLastKeyFrame() == nullptr || getTrajectory()->getLastKeyFrame()->getTimeStamp() < time_stamp_)
+ getTrajectory()->setLastKeyFrame(shared_from_this());
- getTrajectoryPtr()->sortFrame(shared_from_this());
+ getTrajectory()->sortFrame(shared_from_this());
// WOLF_DEBUG("Set KF", this->id());
}
@@ -229,18 +229,18 @@ Eigen::MatrixXs FrameBase::getCovariance() const
FrameBasePtr FrameBase::getPreviousFrame() const
{
//std::cout << "finding previous frame of " << this->frame_id_ << std::endl;
- if (getTrajectoryPtr() == nullptr)
+ if (getTrajectory() == nullptr)
//std::cout << "This Frame is not linked to any trajectory" << std::endl;
- assert(getTrajectoryPtr() != nullptr && "This Frame is not linked to any trajectory");
+ assert(getTrajectory() != nullptr && "This Frame is not linked to any trajectory");
//look for the position of this node in the upper list (frame list of trajectory)
- for (auto f_it = getTrajectoryPtr()->getFrameList().rbegin(); f_it != getTrajectoryPtr()->getFrameList().rend(); f_it++ )
+ for (auto f_it = getTrajectory()->getFrameList().rbegin(); f_it != getTrajectory()->getFrameList().rend(); f_it++ )
{
if ( this->frame_id_ == (*f_it)->id() )
{
f_it++;
- if (f_it != getTrajectoryPtr()->getFrameList().rend())
+ if (f_it != getTrajectory()->getFrameList().rend())
{
//std::cout << "previous frame found!" << std::endl;
return *f_it;
@@ -259,11 +259,11 @@ FrameBasePtr FrameBase::getPreviousFrame() const
FrameBasePtr FrameBase::getNextFrame() const
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
- auto f_it = getTrajectoryPtr()->getFrameList().rbegin();
+ auto f_it = getTrajectory()->getFrameList().rbegin();
f_it++; //starting from second last frame
//look for the position of this node in the frame list of trajectory
- while (f_it != getTrajectoryPtr()->getFrameList().rend())
+ while (f_it != getTrajectory()->getFrameList().rend())
{
if ( this->frame_id_ == (*f_it)->id())
{
@@ -279,7 +279,7 @@ FrameBasePtr FrameBase::getNextFrame() const
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
capture_list_.push_back(_capt_ptr);
- _capt_ptr->setFramePtr(shared_from_this());
+ _capt_ptr->setFrame(shared_from_this());
_capt_ptr->setProblem(getProblem());
_capt_ptr->registerNewStateBlocks();
return _capt_ptr;
@@ -288,7 +288,7 @@ CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr && capture_ptr->getType() == type)
+ if (capture_ptr->getSensor() == _sensor_ptr && capture_ptr->getType() == type)
return capture_ptr;
return nullptr;
}
@@ -296,16 +296,16 @@ CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const st
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr)
{
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr)
+ if (capture_ptr->getSensor() == _sensor_ptr)
return capture_ptr;
return nullptr;
}
-CaptureBaseList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
+CaptureBasePtrList FrameBase::getCapturesOf(const SensorBasePtr _sensor_ptr)
{
- CaptureBaseList captures;
+ CaptureBasePtrList captures;
for (CaptureBasePtr capture_ptr : getCaptureList())
- if (capture_ptr->getSensorPtr() == _sensor_ptr)
+ if (capture_ptr->getSensor() == _sensor_ptr)
captures.push_back(capture_ptr);
return captures;
}
@@ -316,33 +316,33 @@ void FrameBase::unlinkCapture(CaptureBasePtr _cap_ptr)
capture_list_.remove(_cap_ptr);
}
-ConstraintBasePtr FrameBase::getConstraintOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, const std::string& type)
{
- for (const ConstraintBasePtr& constaint_ptr : getConstrainedByList())
+ for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
if (constaint_ptr->getProcessor() == _processor_ptr && constaint_ptr->getType() == type)
return constaint_ptr;
return nullptr;
}
-ConstraintBasePtr FrameBase::getConstraintOf(const ProcessorBasePtr _processor_ptr)
+FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr)
{
- for (const ConstraintBasePtr& constaint_ptr : getConstrainedByList())
+ for (const FactorBasePtr& constaint_ptr : getConstrainedByList())
if (constaint_ptr->getProcessor() == _processor_ptr)
return constaint_ptr;
return nullptr;
}
-void FrameBase::getConstraintList(ConstraintBaseList& _ctr_list)
+void FrameBase::getFactorList(FactorBasePtrList& _fac_list)
{
for (auto c_ptr : getCaptureList())
- c_ptr->getConstraintList(_ctr_list);
+ c_ptr->getFactorList(_fac_list);
}
-ConstraintBasePtr FrameBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+FactorBasePtr FrameBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setFrameOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setFrameOther(shared_from_this());
+ return _fac_ptr;
}
FrameBasePtr FrameBase::create_PO_2D(const FrameType & _tp,
diff --git a/src/hardware_base.cpp b/src/hardware_base.cpp
index 3349005129e275a5dd930cd0927d408149994680..945b412c87cbab6be87a8d7d5536d2850996a14f 100644
--- a/src/hardware_base.cpp
+++ b/src/hardware_base.cpp
@@ -18,7 +18,7 @@ SensorBasePtr HardwareBase::addSensor(SensorBasePtr _sensor_ptr)
{
sensor_list_.push_back(_sensor_ptr);
_sensor_ptr->setProblem(getProblem());
- _sensor_ptr->setHardwarePtr(shared_from_this());
+ _sensor_ptr->setHardware(shared_from_this());
_sensor_ptr->registerNewStateBlocks();
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index 17a22bac36a1aa27f3b83f3589670abf1c12d85f..1daa37470b0c1ce87d47e4d174344fd09e471d9d 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -37,7 +37,7 @@ YAML::Node LandmarkAHP::saveToYaml() const
Eigen::Vector3s LandmarkAHP::getPointInAnchorSensor() const
{
- Eigen::Map<const Eigen::Vector4s> hmg_point(getPPtr()->getState().data());
+ Eigen::Map<const Eigen::Vector4s> hmg_point(getP()->getState().data());
return hmg_point.head<3>()/hmg_point(3);
}
@@ -45,12 +45,12 @@ Eigen::Vector3s LandmarkAHP::point() const
{
using namespace Eigen;
Transform<Scalar,3,Affine> T_w_r
- = Translation<Scalar,3>(getAnchorFrame()->getPPtr()->getState())
- * Quaternions(getAnchorFrame()->getOPtr()->getState().data());
+ = Translation<Scalar,3>(getAnchorFrame()->getP()->getState())
+ * Quaternions(getAnchorFrame()->getO()->getState().data());
Transform<Scalar,3,Affine> T_r_c
- = Translation<Scalar,3>(getAnchorSensor()->getPPtr()->getState())
- * Quaternions(getAnchorSensor()->getOPtr()->getState().data());
- Vector4s point_hmg_c = getPPtr()->getState();
+ = Translation<Scalar,3>(getAnchorSensor()->getP()->getState())
+ * Quaternions(getAnchorSensor()->getO()->getState().data());
+ Vector4s point_hmg_c = getP()->getState();
Vector4s point_hmg = T_w_r * T_r_c * point_hmg_c;
return point_hmg.head<3>()/point_hmg(3);
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 1ae594cb348c66819b3efffdbcf1acf0878e8fa2..d1fb63043ac989955ab8954c9cc9a19754184bec 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -1,6 +1,6 @@
#include "base/landmark/landmark_base.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/map_base.h"
#include "base/state_block.h"
#include "base/yaml/yaml_conversion.h"
@@ -108,14 +108,14 @@ void LandmarkBase::removeStateBlocks()
{
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
- auto sbp = getStateBlockPtr(i);
+ auto sbp = getStateBlock(i);
if (sbp != nullptr)
{
if (getProblem() != nullptr)
{
getProblem()->removeStateBlock(sbp);
}
- setStateBlockPtr(i, nullptr);
+ setStateBlock(i, nullptr);
}
}
}
@@ -157,24 +157,24 @@ YAML::Node LandmarkBase::saveToYaml() const
YAML::Node node;
node["id"] = landmark_id_;
node["type"] = node_type_;
- if (getPPtr() != nullptr)
+ if (getP() != nullptr)
{
- node["position"] = getPPtr()->getState();
- node["position fixed"] = getPPtr()->isFixed();
+ node["position"] = getP()->getState();
+ node["position fixed"] = getP()->isFixed();
}
- if (getOPtr() != nullptr)
+ if (getO() != nullptr)
{
- node["orientation"] = getOPtr()->getState();
- node["orientation fixed"] = getOPtr()->isFixed();
+ node["orientation"] = getO()->getState();
+ node["orientation fixed"] = getO()->isFixed();
}
return node;
}
-ConstraintBasePtr LandmarkBase::addConstrainedBy(ConstraintBasePtr _ctr_ptr)
+FactorBasePtr LandmarkBase::addConstrainedBy(FactorBasePtr _fac_ptr)
{
- constrained_by_list_.push_back(_ctr_ptr);
- _ctr_ptr->setLandmarkOtherPtr(shared_from_this());
- return _ctr_ptr;
+ constrained_by_list_.push_back(_fac_ptr);
+ _fac_ptr->setLandmarkOther(shared_from_this());
+ return _fac_ptr;
}
} // namespace wolf
diff --git a/src/landmark/landmark_container.cpp b/src/landmark/landmark_container.cpp
index 6cef90a382ded760d9ad637803f7bff8205f58b2..3a424869ba5a8775bda6025b7a83aeadda89009a 100644
--- a/src/landmark/landmark_container.cpp
+++ b/src/landmark/landmark_container.cpp
@@ -31,8 +31,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// Computing center
WOLF_DEBUG( "Container constructor: Computing center pose... " );
- Eigen::Vector2s container_position(getPPtr()->getState());
- Eigen::Vector1s container_orientation(getOPtr()->getState());
+ Eigen::Vector2s container_position(getP()->getState());
+ Eigen::Vector1s container_orientation(getO()->getState());
WOLF_DEBUG( "Container constructor: _corner_1_idx ", _corner_1_idx,
"_corner_2_idx ", _corner_2_idx );
@@ -83,8 +83,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
WOLF_DEBUG( "_corner_2_pose... ", _corner_2_pose.transpose() );
WOLF_DEBUG( "Container center pose... ", container_position.transpose(), " ", container_orientation.transpose() );
- getPPtr()->setState(container_position);
- getOPtr()->setState(container_orientation);
+ getP()->setState(container_position);
+ getO()->setState(container_orientation);
}
//LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, LandmarkCorner2D* _corner_A_ptr, LandmarkCorner2D* _corner_B_ptr, LandmarkCorner2D* _corner_C_ptr, LandmarkCorner2D* _corner_D_ptr, const Scalar& _witdh, const Scalar& _length) :
@@ -103,8 +103,8 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
//
// // Computing center
// //std::cout << "Container constructor: Computing center position... " << std::endl;
-// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->getPtr());
-// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->getPtr(), _o_ptr->getSize());
+// Eigen::Map<Eigen::Vector2s> container_position(_p_ptr->get());
+// Eigen::Map<Eigen::VectorXs> container_orientation(_o_ptr->get(), _o_ptr->getSize());
//
// container_position = Eigen::Vector2s::Zero();
//
@@ -112,53 +112,53 @@ LandmarkContainer::LandmarkContainer(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr,
// // A & B
// if (_corner_A_ptr != nullptr && _corner_B_ptr != nullptr)
// {
-// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s AB = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
// Eigen::Vector2s perpendicularAB;
// perpendicularAB << -AB(1)/AB.norm(), AB(0)/AB.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AB / 2 + perpendicularAB * _witdh / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AB / 2 + perpendicularAB * _witdh / 2;
// container_orientation(0) = atan2(AB(1),AB(0));
// }
// // C & D
// else if (_corner_C_ptr != nullptr && _corner_D_ptr != nullptr)
// {
-// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s CD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get());
// Eigen::Vector2s perpendicularCD;
// perpendicularCD << -CD(1)/CD.norm(), CD(0)/CD.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) + CD / 2 + perpendicularCD * _witdh / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) + CD / 2 + perpendicularCD * _witdh / 2;
// container_orientation(0) = atan2(-CD(1),-CD(0));
// }
// // Short side detected
// // B & C
// else if (_corner_B_ptr != nullptr && _corner_C_ptr != nullptr)
// {
-// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s BC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
// Eigen::Vector2s perpendicularBC;
// perpendicularBC << -BC(1)/BC.norm(), BC(0)/BC.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BC / 2 + perpendicularBC * _length / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BC / 2 + perpendicularBC * _length / 2;
// container_orientation(0) = atan2(BC(1),BC(0));
// }
// // D & A
// else if (_corner_D_ptr != nullptr && _corner_A_ptr != nullptr)
// {
-// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr());
+// Eigen::Vector2s DA = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get());
// Eigen::Vector2s perpendicularDA;
// perpendicularDA << -DA(1)/DA.norm(), DA(0)/DA.norm();
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) + DA / 2 + perpendicularDA * _length / 2;
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) + DA / 2 + perpendicularDA * _length / 2;
// container_orientation(0) = atan2(-DA(1),-DA(0));
// }
// // Diagonal detected
// // A & C
// else if (_corner_A_ptr != nullptr && _corner_C_ptr != nullptr)
// {
-// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getPPtr()->getPtr()) + AC / 2;
+// Eigen::Vector2s AC = Eigen::Map<Eigen::Vector2s>(_corner_C_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get());
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_A_ptr->getP()->get()) + AC / 2;
// container_orientation(0) = atan2(AC(1),AC(0)) - atan2(_witdh,_length);
// }
// // B & D
// else if (_corner_B_ptr != nullptr && _corner_D_ptr != nullptr)
// {
-// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getPPtr()->getPtr()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr());
-// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getPPtr()->getPtr()) + BD / 2;
+// Eigen::Vector2s BD = Eigen::Map<Eigen::Vector2s>(_corner_D_ptr->getP()->get()) - Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get());
+// container_position = Eigen::Map<Eigen::Vector2s>(_corner_B_ptr->getP()->get()) + BD / 2;
// container_orientation(0) = atan2(BD(1),BD(0)) + atan2(_witdh,_length);
// }
//}
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 3e1fcf45c828bd7295882983f5ead485f79047f4..d7c5c7d76a3e3b3237805b0b55218f9c258cbeaf 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -8,8 +8,8 @@
#include "base/feature/feature_polyline_2D.h"
#include "base/landmark/landmark_polyline_2D.h"
#include "base/local_parametrization_polyline_extreme.h"
-#include "base/constraint/constraint_point_2D.h"
-#include "base/constraint/constraint_point_to_line_2D.h"
+#include "base/factor/factor_point_2D.h"
+#include "base/factor/factor_point_to_line_2D.h"
#include "base/state_block.h"
#include "base/factory.h"
#include "base/yaml/yaml_conversion.h"
@@ -26,9 +26,9 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
point_state_ptr_vector_.push_back(std::make_shared<StateBlock>(_points.col(i).head<2>()));
if (!first_defined_)
- point_state_ptr_vector_.front()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
+ point_state_ptr_vector_.front()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[1]));
if (!last_defined_)
- point_state_ptr_vector_.back()->setLocalParametrizationPtr(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
+ point_state_ptr_vector_.back()->setLocalParametrization(std::make_shared<LocalParametrizationPolylineExtreme>(point_state_ptr_vector_[point_state_ptr_vector_.size() - 2]));
assert(point_state_ptr_vector_.front()->hasLocalParametrization() ? !first_defined_ : first_defined_);
assert(point_state_ptr_vector_.back()->hasLocalParametrization() ? !last_defined_ : last_defined_);
@@ -67,7 +67,7 @@ const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
return point_state_ptr_vector_[_i-first_id_]->getState();
}
-StateBlockPtr LandmarkPolyline2D::getPointStateBlockPtr(int _i)
+StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
{
assert(_i-first_id_ >= 0 && _i-first_id_ <= (int)(point_state_ptr_vector_.size()) && "out of range!");
return point_state_ptr_vector_[_i-first_id_];
@@ -165,7 +165,7 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
assert((_back ? !last_defined_: !first_defined_) && "defining an already defined extreme");
assert(state->hasLocalParametrization() && "not defined extreme without local parameterization");
- //std::cout << "Defining extreme --> Removing and adding state blocks and constraints" << std::endl;
+ //std::cout << "Defining extreme --> Removing and adding state blocks and factors" << std::endl;
// remove and add state block without local parameterization
if (getProblem() != nullptr)
@@ -176,13 +176,13 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
if (getProblem() != nullptr)
getProblem()->addStateBlock(state);
- // remove and add all constraints to the point
- for (auto ctr_ptr : getConstrainedByList())
- for (auto st_ptr : ctr_ptr->getStateBlockPtrVector())
+ // remove and add all factors to the point
+ for (auto fac_ptr : getConstrainedByList())
+ for (auto st_ptr : fac_ptr->getStateBlockPtrVector())
if (st_ptr == state && getProblem() != nullptr)
{
- getProblem()->removeConstraint(ctr_ptr);
- getProblem()->addConstraint(ctr_ptr);
+ getProblem()->removeFactor(fac_ptr);
+ getProblem()->addFactor(fac_ptr);
}
// update boolean
@@ -230,74 +230,74 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
assert(_remain_id < getLastId() || last_defined_);
// take a defined extreme as remaining
- StateBlockPtr remove_state = getPointStateBlockPtr(_remove_id);
+ StateBlockPtr remove_state = getPointStateBlock(_remove_id);
std::cout << "state block to remove " << remove_state->getState().transpose() << std::endl;
- // Change constraints from remove_state to remain_state
- ConstraintBaseList old_constraints_list = getConstrainedByList();
- std::cout << "changing constraints: " << old_constraints_list.size() << std::endl;
- ConstraintBasePtr new_ctr_ptr = nullptr;
- for (auto ctr_ptr : old_constraints_list)
+ // Change factors from remove_state to remain_state
+ FactorBasePtrList old_factors_list = getConstrainedByList();
+ std::cout << "changing factors: " << old_factors_list.size() << std::endl;
+ FactorBasePtr new_fac_ptr = nullptr;
+ for (auto fac_ptr : old_factors_list)
{
- ConstraintPoint2DPtr ctr_point_ptr;
- ConstraintPointToLine2DPtr ctr_point_line_ptr;
- if ( (ctr_point_ptr = std::dynamic_pointer_cast<ConstraintPoint2D>(ctr_ptr)))
-// if (ctr_ptr->getTypeId() == CTR_POINT_2D)
+ FactorPoint2DPtr fac_point_ptr;
+ FactorPointToLine2DPtr fac_point_line_ptr;
+ if ( (fac_point_ptr = std::dynamic_pointer_cast<FactorPoint2D>(fac_ptr)))
+// if (fac_ptr->getTypeId() == FAC_POINT_2D)
{
-// ConstraintPoint2DPtr ctr_point_ptr = std::static_pointer_cast<ConstraintPoint2D>(ctr_ptr);
+// FactorPoint2DPtr fac_point_ptr = std::static_pointer_cast<FactorPoint2D>(fac_ptr);
- // If landmark point constrained -> new constraint
- if (ctr_point_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<ConstraintPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ // If landmark point constrained -> new factor
+ if (fac_point_ptr->getLandmarkPointId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPoint2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_ptr->getProcessor(),
- ctr_point_ptr->getFeaturePointId(),
+ fac_point_ptr->getProcessor(),
+ fac_point_ptr->getFeaturePointId(),
_remain_id,
- ctr_point_ptr->getApplyLossFunction(),
- ctr_point_ptr->getStatus());
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_ptr->getStatus());
}
- else if ((ctr_point_line_ptr = std::dynamic_pointer_cast<ConstraintPointToLine2D>(ctr_ptr)))
-// else if (ctr_ptr->getTypeId() == CTR_POINT_TO_LINE_2D)
+ else if ((fac_point_line_ptr = std::dynamic_pointer_cast<FactorPointToLine2D>(fac_ptr)))
+// else if (fac_ptr->getTypeId() == FAC_POINT_TO_LINE_2D)
{
-// ConstraintPointToLine2DPtr ctr_point_line_ptr = std::static_pointer_cast<ConstraintPointToLine2D>(ctr_ptr);
+// FactorPointToLine2DPtr fac_point_line_ptr = std::static_pointer_cast<FactorPointToLine2D>(fac_ptr);
- // If landmark point constrained -> new constraint
- if (ctr_point_line_ptr->getLandmarkPointId() == _remove_id)
- new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ // If landmark point constrained -> new factor
+ if (fac_point_line_ptr->getLandmarkPointId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_line_ptr->getProcessor(),
- ctr_point_line_ptr->getFeaturePointId(),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
_remain_id,
- ctr_point_line_ptr->getLandmarkPointAuxId(),
- ctr_point_ptr->getApplyLossFunction(),
- ctr_point_line_ptr->getStatus());
- // If landmark point is aux point -> new constraint
- else if (ctr_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
- new_ctr_ptr = std::make_shared<ConstraintPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(ctr_ptr->getFeaturePtr()),
+ fac_point_line_ptr->getLandmarkPointAuxId(),
+ fac_point_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
+ // If landmark point is aux point -> new factor
+ else if (fac_point_line_ptr->getLandmarkPointAuxId() == _remove_id)
+ new_fac_ptr = std::make_shared<FactorPointToLine2D>(std::static_pointer_cast<FeaturePolyline2D>(fac_ptr->getFeature()),
std::static_pointer_cast<LandmarkPolyline2D>(shared_from_this()),
- ctr_point_line_ptr->getProcessor(),
- ctr_point_line_ptr->getFeaturePointId(),
- ctr_point_line_ptr->getLandmarkPointId(),
+ fac_point_line_ptr->getProcessor(),
+ fac_point_line_ptr->getFeaturePointId(),
+ fac_point_line_ptr->getLandmarkPointId(),
_remain_id,
- ctr_point_line_ptr->getApplyLossFunction(),
- ctr_point_line_ptr->getStatus());
+ fac_point_line_ptr->getApplyLossFunction(),
+ fac_point_line_ptr->getStatus());
}
else
- throw std::runtime_error ("polyline constraint of unknown type");
+ throw std::runtime_error ("polyline factor of unknown type");
- // If new constraint
- if (new_ctr_ptr != nullptr)
+ // If new factor
+ if (new_fac_ptr != nullptr)
{
- std::cout << "created new constraint: " << new_ctr_ptr->id() << std::endl;
- std::cout << "deleting constraint: " << ctr_ptr->id() << std::endl;
+ std::cout << "created new factor: " << new_fac_ptr->id() << std::endl;
+ std::cout << "deleting factor: " << fac_ptr->id() << std::endl;
- // add new constraint
- ctr_ptr->getFeaturePtr()->addConstraint(new_ctr_ptr);
+ // add new factor
+ fac_ptr->getFeature()->addFactor(new_fac_ptr);
- // remove constraint
- ctr_ptr->remove();
+ // remove factor
+ fac_ptr->remove();
- new_ctr_ptr = nullptr;
+ new_fac_ptr = nullptr;
}
}
diff --git a/src/map_base.cpp b/src/map_base.cpp
index 5e8008a6a36783a463de9ecf5ac1497d5fa749c0..d1ad03121691b64052bfab01372010f68de863f5 100644
--- a/src/map_base.cpp
+++ b/src/map_base.cpp
@@ -30,18 +30,18 @@ MapBase::~MapBase()
LandmarkBasePtr MapBase::addLandmark(LandmarkBasePtr _landmark_ptr)
{
landmark_list_.push_back(_landmark_ptr);
- _landmark_ptr->setMapPtr(shared_from_this());
+ _landmark_ptr->setMap(shared_from_this());
_landmark_ptr->setProblem(getProblem());
_landmark_ptr->registerNewStateBlocks();
return _landmark_ptr;
}
-void MapBase::addLandmarkList(LandmarkBaseList& _landmark_list)
+void MapBase::addLandmarkList(LandmarkBasePtrList& _landmark_list)
{
- LandmarkBaseList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
+ LandmarkBasePtrList lmk_list_copy = _landmark_list; //since _landmark_list will be empty after addDownNodeList()
for (LandmarkBasePtr landmark_ptr : lmk_list_copy)
{
- landmark_ptr->setMapPtr(shared_from_this());
+ landmark_ptr->setMap(shared_from_this());
landmark_ptr->setProblem(getProblem());
landmark_ptr->registerNewStateBlocks();
}
diff --git a/src/problem.cpp b/src/problem.cpp
index d96a1d29065a18815bd99eb65245856e89cde66d..b9cd0a1e6bd63bfb40a3eb67bda35e1b5c6096b2 100644
--- a/src/problem.cpp
+++ b/src/problem.cpp
@@ -7,7 +7,7 @@
#include "base/processor/processor_motion.h"
#include "base/processor/processor_tracker.h"
#include "base/capture/capture_pose.h"
-#include "base/constraint/constraint_base.h"
+#include "base/factor/factor_base.h"
#include "base/sensor/sensor_factory.h"
#include "base/processor/processor_factory.h"
#include "base/state_block.h"
@@ -79,7 +79,7 @@ Problem::~Problem()
void Problem::addSensor(SensorBasePtr _sen_ptr)
{
- getHardwarePtr()->addSensor(_sen_ptr);
+ getHardware()->addSensor(_sen_ptr);
}
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
@@ -127,7 +127,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
// setting the origin in all processor motion if origin already setted
if (prc_ptr->isMotion() && prior_is_set_)
- (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFramePtr());
+ (std::static_pointer_cast<ProcessorMotion>(prc_ptr))->setOrigin(getLastKeyFrame());
// setting the main processor motion
if (prc_ptr->isMotion() && processor_motion_ptr_ == nullptr)
@@ -141,7 +141,7 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
const std::string& _corresponding_sensor_name, //
const std::string& _params_filename)
{
- SensorBasePtr sen_ptr = getSensorPtr(_corresponding_sensor_name);
+ SensorBasePtr sen_ptr = getSensor(_corresponding_sensor_name);
if (sen_ptr == nullptr)
throw std::runtime_error("Sensor not found. Cannot bind Processor.");
if (_params_filename == "")
@@ -154,16 +154,16 @@ ProcessorBasePtr Problem::installProcessor(const std::string& _prc_type, //
}
}
-SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
+SensorBasePtr Problem::getSensor(const std::string& _sensor_name)
{
- auto sen_it = std::find_if(getHardwarePtr()->getSensorList().begin(),
- getHardwarePtr()->getSensorList().end(),
+ auto sen_it = std::find_if(getHardware()->getSensorList().begin(),
+ getHardware()->getSensorList().end(),
[&](SensorBasePtr sb)
{
return sb->getName() == _sensor_name;
}); // lambda function for the find_if
- if (sen_it == getHardwarePtr()->getSensorList().end())
+ if (sen_it == getHardware()->getSensorList().end())
return nullptr;
return (*sen_it);
@@ -171,7 +171,7 @@ SensorBasePtr Problem::getSensorPtr(const std::string& _sensor_name)
ProcessorMotionPtr Problem::setProcessorMotion(const std::string& _processor_name)
{
- for (auto sen : getHardwarePtr()->getSensorList()) // loop all sensors
+ for (auto sen : getHardware()->getSensorList()) // loop all sensors
{
auto prc_it = std::find_if(sen->getProcessorList().begin(), // find processor by its name
sen->getProcessorList().end(),
@@ -253,8 +253,8 @@ void Problem::getCurrentState(Eigen::VectorXs& state)
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(state);
- else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
- trajectory_ptr_->getLastKeyFramePtr()->getState(state);
+ else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
+ trajectory_ptr_->getLastKeyFrame()->getState(state);
else
state = zeroState();
}
@@ -268,10 +268,10 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& state, TimeStamp& ts)
processor_motion_ptr_->getCurrentState(state);
processor_motion_ptr_->getCurrentTimeStamp(ts);
}
- else if (trajectory_ptr_->getLastKeyFramePtr() != nullptr)
+ else if (trajectory_ptr_->getLastKeyFrame() != nullptr)
{
- trajectory_ptr_->getLastKeyFramePtr()->getTimeStamp(ts);
- trajectory_ptr_->getLastKeyFramePtr()->getState(state);
+ trajectory_ptr_->getLastKeyFrame()->getTimeStamp(ts);
+ trajectory_ptr_->getLastKeyFrame()->getState(state);
}
else
state = zeroState();
@@ -356,13 +356,13 @@ void Problem::keyFrameCallback(FrameBasePtr _keyframe_ptr, ProcessorBasePtr _pro
LandmarkBasePtr Problem::addLandmark(LandmarkBasePtr _lmk_ptr)
{
- getMapPtr()->addLandmark(_lmk_ptr);
+ getMap()->addLandmark(_lmk_ptr);
return _lmk_ptr;
}
-void Problem::addLandmarkList(LandmarkBaseList& _lmk_list)
+void Problem::addLandmarkList(LandmarkBasePtrList& _lmk_list)
{
- getMapPtr()->addLandmarkList(_lmk_list);
+ getMap()->addLandmarkList(_lmk_list);
}
StateBlockPtr Problem::addStateBlock(StateBlockPtr _state_ptr)
@@ -422,31 +422,31 @@ void Problem::removeStateBlock(StateBlockPtr _state_ptr)
state_block_notification_map_[_state_ptr] = REMOVE;
}
-ConstraintBasePtr Problem::addConstraint(ConstraintBasePtr _constraint_ptr)
+FactorBasePtr Problem::addFactor(FactorBasePtr _factor_ptr)
{
- //std::cout << "Problem::addConstraintPtr " << _constraint_ptr->id() << std::endl;
+ //std::cout << "Problem::addFactorPtr " << _factor_ptr->id() << std::endl;
// Add ADD notification
// Check if there is already a notification for this state block
- auto notification_it = constraint_notification_map_.find(_constraint_ptr);
- if (notification_it != constraint_notification_map_.end() && notification_it->second == ADD)
+ auto notification_it = factor_notification_map_.find(_factor_ptr);
+ if (notification_it != factor_notification_map_.end() && notification_it->second == ADD)
{
- WOLF_WARN("There is already an ADD notification of this constraint");
+ WOLF_WARN("There is already an ADD notification of this factor");
}
// Add ADD notification (override in case of REMOVE)
else
- constraint_notification_map_[_constraint_ptr] = ADD;
+ factor_notification_map_[_factor_ptr] = ADD;
- return _constraint_ptr;
+ return _factor_ptr;
}
-void Problem::removeConstraint(ConstraintBasePtr _constraint_ptr)
+void Problem::removeFactor(FactorBasePtr _factor_ptr)
{
- //std::cout << "Problem::removeConstraintPtr " << _constraint_ptr->id() << std::endl;
+ //std::cout << "Problem::removeFactorPtr " << _factor_ptr->id() << std::endl;
// Check if there is already a notification for this state block
- auto notification_it = constraint_notification_map_.find(_constraint_ptr);
- if (notification_it != constraint_notification_map_.end())
+ auto notification_it = factor_notification_map_.find(_factor_ptr);
+ if (notification_it != factor_notification_map_.end())
{
if (notification_it->second == REMOVE)
{
@@ -454,11 +454,11 @@ void Problem::removeConstraint(ConstraintBasePtr _constraint_ptr)
}
// Remove ADD notification
else
- constraint_notification_map_.erase(notification_it);
+ factor_notification_map_.erase(notification_it);
}
// Add REMOVE notification
else
- constraint_notification_map_[_constraint_ptr] = REMOVE;
+ factor_notification_map_[_factor_ptr] = REMOVE;
}
void Problem::clearCovariance()
@@ -598,7 +598,7 @@ Eigen::MatrixXs Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr)
Eigen::MatrixXs Problem::getLastKeyFrameCovariance()
{
- FrameBasePtr frm = getLastKeyFramePtr();
+ FrameBasePtr frm = getLastKeyFrame();
return getFrameCovariance(frm);
}
@@ -641,32 +641,32 @@ Eigen::MatrixXs Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_pt
return covariance;
}
-MapBasePtr Problem::getMapPtr()
+MapBasePtr Problem::getMap()
{
return map_ptr_;
}
-TrajectoryBasePtr Problem::getTrajectoryPtr()
+TrajectoryBasePtr Problem::getTrajectory()
{
return trajectory_ptr_;
}
-HardwareBasePtr Problem::getHardwarePtr()
+HardwareBasePtr Problem::getHardware()
{
return hardware_ptr_;
}
-FrameBasePtr Problem::getLastFramePtr()
+FrameBasePtr Problem::getLastFrame()
{
- return trajectory_ptr_->getLastFramePtr();
+ return trajectory_ptr_->getLastFrame();
}
-FrameBasePtr Problem::getLastKeyFramePtr()
+FrameBasePtr Problem::getLastKeyFrame()
{
- return trajectory_ptr_->getLastKeyFramePtr();
+ return trajectory_ptr_->getLastKeyFrame();
}
-StateBlockList& Problem::getStateBlockList()
+StateBlockPtrList& Problem::getStateBlockPtrList()
{
return state_block_list_;
}
@@ -688,8 +688,8 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
origin_keyframe->addCapture(init_capture);
- // emplace feature and constraint
- init_capture->emplaceFeatureAndConstraint();
+ // emplace feature and factor
+ init_capture->emplaceFeatureAndFactor();
// Notify all processors about the prior KF
for (auto sensor : hardware_ptr_->getSensorList())
@@ -714,12 +714,12 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen:
void Problem::loadMap(const std::string& _filename_dot_yaml)
{
- getMapPtr()->load(_filename_dot_yaml);
+ getMap()->load(_filename_dot_yaml);
}
void Problem::saveMap(const std::string& _filename_dot_yaml, const std::string& _map_name)
{
- getMapPtr()->save(_filename_dot_yaml, _map_name);
+ getMap()->save(_filename_dot_yaml, _map_name);
}
void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
@@ -729,11 +729,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
cout << endl;
cout << "P: wolf tree status ---------------------" << endl;
- cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardwarePtr()->getSensorList().size()) + "S") : "") << endl;
+ cout << "Hardware" << ((depth < 1) ? (" -- " + std::to_string(getHardware()->getSensorList().size()) + "S") : "") << endl;
if (depth >= 1)
{
// Sensors =======================================================================================
- for (auto S : getHardwarePtr()->getSensorList())
+ for (auto S : getHardware()->getSensorList())
{
cout << " S" << S->id() << " " << S->getType();
if (!metric && !state_blocks) cout << (S->isExtrinsicDynamic() ? " [Dyn," : " [Sta,") << (S->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
@@ -746,7 +746,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
if (i==0) cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
if (i==2) cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << " = [";
- auto sb = S->getStateBlockPtr(i);
+ auto sb = S->getStateBlock(i);
if (sb)
{
cout << (sb->isFixed() ? " Fix( " : " Est( ") << sb->getState().transpose() << " )";
@@ -758,13 +758,13 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
else if (metric)
{
cout << " Extr " << (S->isExtrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( ";
- if (S->getPPtr())
- cout << S->getPPtr()->getState().transpose();
- if (S->getOPtr())
- cout << " " << S->getOPtr()->getState().transpose();
+ if (S->getP())
+ cout << S->getP()->getState().transpose();
+ if (S->getO())
+ cout << " " << S->getO()->getState().transpose();
cout << " )";
- if (S->getIntrinsicPtr())
- cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsicPtr()->getState().transpose() << " )";
+ if (S->getIntrinsic())
+ cout << " Intr " << (S->isIntrinsicDynamic() ? "[Dyn]" : "[Sta]") << "= ( " << S->getIntrinsic()->getState().transpose() << " )";
cout << endl;
}
else if (state_blocks)
@@ -784,14 +784,14 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
std::cout << " pm" << p->id() << " " << p->getType() << endl;
ProcessorMotionPtr pm = std::static_pointer_cast<ProcessorMotion>(p);
- if (pm->getOriginPtr())
- cout << " o: C" << pm->getOriginPtr()->id() << " - " << (pm->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pm->getOriginPtr()->getFramePtr()->id() << endl;
- if (pm->getLastPtr())
- cout << " l: C" << pm->getLastPtr()->id() << " - " << (pm->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pm->getLastPtr()->getFramePtr()->id() << endl;
- if (pm->getIncomingPtr())
- cout << " i: C" << pm->getIncomingPtr()->id() << endl;
+ if (pm->getOrigin())
+ cout << " o: C" << pm->getOrigin()->id() << " - " << (pm->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ << pm->getOrigin()->getFrame()->id() << endl;
+ if (pm->getLast())
+ cout << " l: C" << pm->getLast()->id() << " - " << (pm->getLast()->getFrame()->isKey() ? " KF" : " F")
+ << pm->getLast()->getFrame()->id() << endl;
+ if (pm->getIncoming())
+ cout << " i: C" << pm->getIncoming()->id() << endl;
}
else
{
@@ -802,29 +802,29 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// ProcessorTrackerFeatureTrifocalPtr ptt = std::dynamic_pointer_cast<ProcessorTrackerFeatureTrifocal>(pt);
// if (ptt)
// {
-// if (ptt->getPrevOriginPtr())
-// cout << " p: C" << ptt->getPrevOriginPtr()->id() << " - " << (ptt->getPrevOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
-// << ptt->getPrevOriginPtr()->getFramePtr()->id() << endl;
+// if (ptt->getPrevOrigin())
+// cout << " p: C" << ptt->getPrevOrigin()->id() << " - " << (ptt->getPrevOrigin()->getFrame()->isKey() ? " KF" : " F")
+// << ptt->getPrevOrigin()->getFrame()->id() << endl;
// }
- if (pt->getOriginPtr())
- cout << " o: C" << pt->getOriginPtr()->id() << " - " << (pt->getOriginPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pt->getOriginPtr()->getFramePtr()->id() << endl;
- if (pt->getLastPtr())
- cout << " l: C" << pt->getLastPtr()->id() << " - " << (pt->getLastPtr()->getFramePtr()->isKey() ? " KF" : " F")
- << pt->getLastPtr()->getFramePtr()->id() << endl;
- if (pt->getIncomingPtr())
- cout << " i: C" << pt->getIncomingPtr()->id() << endl;
+ if (pt->getOrigin())
+ cout << " o: C" << pt->getOrigin()->id() << " - " << (pt->getOrigin()->getFrame()->isKey() ? " KF" : " F")
+ << pt->getOrigin()->getFrame()->id() << endl;
+ if (pt->getLast())
+ cout << " l: C" << pt->getLast()->id() << " - " << (pt->getLast()->getFrame()->isKey() ? " KF" : " F")
+ << pt->getLast()->getFrame()->id() << endl;
+ if (pt->getIncoming())
+ cout << " i: C" << pt->getIncoming()->id() << endl;
}
}
} // for p
}
} // for S
}
- cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectoryPtr()->getFrameList().size()) + "F") : "") << endl;
+ cout << "Trajectory" << ((depth < 1) ? (" -- " + std::to_string(getTrajectory()->getFrameList().size()) + "F") : "") << endl;
if (depth >= 1)
{
// Frames =======================================================================================
- for (auto F : getTrajectoryPtr()->getFrameList())
+ for (auto F : getTrajectory()->getFrameList())
{
cout << (F->isKey() ? " KF" : " F") << F->id() << ((depth < 2) ? " -- " + std::to_string(F->getCaptureList().size()) + "C " : "");
if (constr_by)
@@ -856,19 +856,19 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
{
cout << " C" << (C->isMotion() ? "M" : "") << C->id() << " " << C->getType();
- if(C->getSensorPtr() != nullptr)
+ if(C->getSensor() != nullptr)
{
- cout << " -> S" << C->getSensorPtr()->id();
- cout << (C->getSensorPtr()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
- cout << (C->getSensorPtr()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
+ cout << " -> S" << C->getSensor()->id();
+ cout << (C->getSensor()->isExtrinsicDynamic() ? " [Dyn, ": " [Sta, ");
+ cout << (C->getSensor()->isIntrinsicDynamic() ? "Dyn]" : "Sta]");
}
else
cout << " -> S-";
if (C->isMotion())
{
auto CM = std::static_pointer_cast<CaptureMotion>(C);
- if (CM->getOriginFramePtr())
- cout << " -> OF" << CM->getOriginFramePtr()->id();
+ if (CM->getOriginFrame())
+ cout << " -> OF" << CM->getOriginFrame()->id();
}
cout << ((depth < 3) ? " -- " + std::to_string(C->getFeatureList().size()) + "f" : "");
@@ -913,7 +913,7 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
// Features =======================================================================================
for (auto f : C->getFeatureList())
{
- cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getConstraintList().size()) + "c " : "");
+ cout << " f" << f->id() << " trk" << f->trackId() << " " << f->getType() << ((depth < 4) ? " -- " + std::to_string(f->getFactorList().size()) + "c " : "");
if (constr_by)
{
cout << " <--\t";
@@ -926,20 +926,20 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
<< " )" << endl;
if (depth >= 4)
{
- // Constraints =======================================================================================
- for (auto c : f->getConstraintList())
+ // Factors =======================================================================================
+ for (auto c : f->getFactorList())
{
cout << " c" << c->id() << " " << c->getType() << " -->";
- if (c->getFrameOtherPtr() == nullptr && c->getCaptureOtherPtr() == nullptr && c->getFeatureOtherPtr() == nullptr && c->getLandmarkOtherPtr() == nullptr)
+ if (c->getFrameOther() == nullptr && c->getCaptureOther() == nullptr && c->getFeatureOther() == nullptr && c->getLandmarkOther() == nullptr)
cout << " A";
- if (c->getFrameOtherPtr() != nullptr)
- cout << " F" << c->getFrameOtherPtr()->id();
- if (c->getCaptureOtherPtr() != nullptr)
- cout << " C" << c->getCaptureOtherPtr()->id();
- if (c->getFeatureOtherPtr() != nullptr)
- cout << " f" << c->getFeatureOtherPtr()->id();
- if (c->getLandmarkOtherPtr() != nullptr)
- cout << " L" << c->getLandmarkOtherPtr()->id();
+ if (c->getFrameOther() != nullptr)
+ cout << " F" << c->getFrameOther()->id();
+ if (c->getCaptureOther() != nullptr)
+ cout << " C" << c->getCaptureOther()->id();
+ if (c->getFeatureOther() != nullptr)
+ cout << " f" << c->getFeatureOther()->id();
+ if (c->getLandmarkOther() != nullptr)
+ cout << " L" << c->getLandmarkOther()->id();
cout << endl;
} // for c
}
@@ -949,11 +949,11 @@ void Problem::print(int depth, bool constr_by, bool metric, bool state_blocks)
}
} // for F
}
- cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMapPtr()->getLandmarkList().size()) + "L") : "") << endl;
+ cout << "Map" << ((depth < 1) ? (" -- " + std::to_string(getMap()->getLandmarkList().size()) + "L") : "") << endl;
if (depth >= 1)
{
// Landmarks =======================================================================================
- for (auto L : getMapPtr()->getLandmarkList())
+ for (auto L : getMap()->getLandmarkList())
{
cout << " L" << L->id() << " " << L->getType();
if (constr_by)
@@ -1020,13 +1020,13 @@ bool Problem::check(int verbose_level)
{
cout << " S" << S->id() << " @ " << S.get() << endl;
cout << " -> P @ " << S->getProblem().get() << endl;
- cout << " -> H @ " << S->getHardwarePtr().get() << endl;
+ cout << " -> H @ " << S->getHardware().get() << endl;
for (auto sb : S->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1035,20 +1035,20 @@ bool Problem::check(int verbose_level)
}
// check problem and hardware pointers
is_consistent = is_consistent && (S->getProblem().get() == P_raw);
- is_consistent = is_consistent && (S->getHardwarePtr() == H);
+ is_consistent = is_consistent && (S->getHardware() == H);
// Processors =======================================================================================
for (auto p : S->getProcessorList())
{
if (verbose_level > 0)
{
- cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensorPtr()->id() << endl;
+ cout << " p" << p->id() << " @ " << p.get() << " -> S" << p->getSensor()->id() << endl;
cout << " -> P @ " << p->getProblem().get() << endl;
- cout << " -> S" << p->getSensorPtr()->id() << " @ " << p->getSensorPtr().get() << endl;
+ cout << " -> S" << p->getSensor()->id() << " @ " << p->getSensor().get() << endl;
}
// check problem and sensor pointers
is_consistent = is_consistent && (p->getProblem().get() == P_raw);
- is_consistent = is_consistent && (p->getSensorPtr() == S);
+ is_consistent = is_consistent && (p->getSensor() == S);
}
}
// ------------------------
@@ -1069,13 +1069,13 @@ bool Problem::check(int verbose_level)
{
cout << (F->isKey() ? " KF" : " F") << F->id() << " @ " << F.get() << endl;
cout << " -> P @ " << F->getProblem().get() << endl;
- cout << " -> T @ " << F->getTrajectoryPtr().get() << endl;
+ cout << " -> T @ " << F->getTrajectory().get() << endl;
for (auto sb : F->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1084,15 +1084,15 @@ bool Problem::check(int verbose_level)
}
// check problem and trajectory pointers
is_consistent = is_consistent && (F->getProblem().get() == P_raw);
- is_consistent = is_consistent && (F->getTrajectoryPtr() == T);
+ is_consistent = is_consistent && (F->getTrajectory() == T);
for (auto cby : F->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> F" << cby->getFrameOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> F" << cby->getFrameOther()->id() << endl;
}
// check constrained_by pointer to this frame
- is_consistent = is_consistent && (cby->getFrameOtherPtr() == F);
+ is_consistent = is_consistent && (cby->getFrameOther() == F);
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose_level > 0)
@@ -1100,7 +1100,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1115,17 +1115,17 @@ bool Problem::check(int verbose_level)
if (verbose_level > 0)
{
cout << " C" << C->id() << " @ " << C.get() << " -> S";
- if (C->getSensorPtr()) cout << C->getSensorPtr()->id();
+ if (C->getSensor()) cout << C->getSensor()->id();
else cout << "-";
cout << endl;
cout << " -> P @ " << C->getProblem().get() << endl;
- cout << " -> F" << C->getFramePtr()->id() << " @ " << C->getFramePtr().get() << endl;
+ cout << " -> F" << C->getFrame()->id() << " @ " << C->getFrame().get() << endl;
for (auto sb : C->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1134,7 +1134,7 @@ bool Problem::check(int verbose_level)
}
// check problem and frame pointers
is_consistent = is_consistent && (C->getProblem().get() == P_raw);
- is_consistent = is_consistent && (C->getFramePtr() == F);
+ is_consistent = is_consistent && (C->getFrame() == F);
// Features =======================================================================================
for (auto f : C->getFeatureList())
@@ -1143,33 +1143,33 @@ bool Problem::check(int verbose_level)
{
cout << " f" << f->id() << " @ " << f.get() << endl;
cout << " -> P @ " << f->getProblem().get() << endl;
- cout << " -> C" << f->getCapturePtr()->id() << " @ " << f->getCapturePtr().get()
+ cout << " -> C" << f->getCapture()->id() << " @ " << f->getCapture().get()
<< endl;
}
// check problem and capture pointers
is_consistent = is_consistent && (f->getProblem().get() == P_raw);
- is_consistent = is_consistent && (f->getCapturePtr() == C);
+ is_consistent = is_consistent && (f->getCapture() == C);
for (auto cby : f->getConstrainedByList())
{
if (verbose_level > 0)
{
- cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> f" << cby->getFeatureOther()->id() << endl;
}
// check constrained_by pointer to this feature
- is_consistent = is_consistent && (cby->getFeatureOtherPtr() == f);
+ is_consistent = is_consistent && (cby->getFeatureOther() == f);
}
- // Constraints =======================================================================================
- for (auto c : f->getConstraintList())
+ // Factors =======================================================================================
+ for (auto c : f->getFactorList())
{
if (verbose_level > 0)
cout << " c" << c->id() << " @ " << c.get();
- auto Fo = c->getFrameOtherPtr();
- auto Co = c->getCaptureOtherPtr();
- auto fo = c->getFeatureOtherPtr();
- auto Lo = c->getLandmarkOtherPtr();
+ auto Fo = c->getFrameOther();
+ auto Co = c->getCaptureOther();
+ auto fo = c->getFeatureOther();
+ auto Lo = c->getLandmarkOther();
if ( !Fo && !Co && !fo && !Lo ) // case ABSOLUTE:
{
@@ -1251,14 +1251,14 @@ bool Problem::check(int verbose_level)
if (verbose_level > 0)
{
cout << " -> P @ " << c->getProblem().get() << endl;
- cout << " -> f" << c->getFeaturePtr()->id() << " @ " << c->getFeaturePtr().get() << endl;
+ cout << " -> f" << c->getFeature()->id() << " @ " << c->getFeature().get() << endl;
}
// check problem and feature pointers
is_consistent = is_consistent && (c->getProblem().get() == P_raw);
- is_consistent = is_consistent && (c->getFeaturePtr() == f);
+ is_consistent = is_consistent && (c->getFeature() == f);
// find state block pointers in all constrained nodes
- SensorBasePtr S = c->getFeaturePtr()->getCapturePtr()->getSensorPtr(); // get own sensor to check sb
+ SensorBasePtr S = c->getFeature()->getCapture()->getSensor(); // get own sensor to check sb
for (auto sb : c->getStateBlockPtrVector())
{
bool found = false;
@@ -1267,7 +1267,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1289,13 +1289,13 @@ bool Problem::check(int verbose_level)
if (fo)
{
// find in constrained feature's Frame
- FrameBasePtr foF = fo->getFramePtr();
+ FrameBasePtr foF = fo->getFrame();
found = found || (std::find(foF->getStateBlockVec().begin(), foF->getStateBlockVec().end(), sb) != foF->getStateBlockVec().end());
// find in constrained feature's Capture
- CaptureBasePtr foC = fo->getCapturePtr();
+ CaptureBasePtr foC = fo->getCapture();
found = found || (std::find(foC->getStateBlockVec().begin(), foC->getStateBlockVec().end(), sb) != foC->getStateBlockVec().end());
// find in constrained feature's Sensor
- SensorBasePtr foS = fo->getCapturePtr()->getSensorPtr();
+ SensorBasePtr foS = fo->getCapture()->getSensor();
found = found || (std::find(foS->getStateBlockVec().begin(), foS->getStateBlockVec().end(), sb) != foS->getStateBlockVec().end());
}
// find in constrained landmark
@@ -1332,13 +1332,13 @@ bool Problem::check(int verbose_level)
{
cout << " L" << L->id() << " @ " << L.get() << endl;
cout << " -> P @ " << L->getProblem().get() << endl;
- cout << " -> M @ " << L->getMapPtr().get() << endl;
+ cout << " -> M @ " << L->getMap().get() << endl;
for (auto sb : L->getStateBlockVec())
{
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
@@ -1347,13 +1347,13 @@ bool Problem::check(int verbose_level)
}
// check problem and map pointers
is_consistent = is_consistent && (L->getProblem().get() == P_raw);
- is_consistent = is_consistent && (L->getMapPtr() == M);
+ is_consistent = is_consistent && (L->getMap() == M);
for (auto cby : L->getConstrainedByList())
{
if (verbose_level > 0)
- cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOtherPtr()->id() << endl;
+ cout << " <- c" << cby->id() << " -> L" << cby->getLandmarkOther()->id() << endl;
// check constrained_by pointer to this landmark
- is_consistent = is_consistent && (cby->getLandmarkOtherPtr() && L->id() == cby->getLandmarkOtherPtr()->id());
+ is_consistent = is_consistent && (cby->getLandmarkOther() && L->id() == cby->getLandmarkOther()->id());
for (auto sb : cby->getStateBlockPtrVector())
{
if (verbose_level > 0)
@@ -1361,7 +1361,7 @@ bool Problem::check(int verbose_level)
cout << " sb @ " << sb.get();
if (sb)
{
- auto lp = sb->getLocalParametrizationPtr();
+ auto lp = sb->getLocalParametrization();
if (lp)
cout << " (lp @ " << lp.get() << ")";
}
diff --git a/src/processor/processor_GPS.cpp b/src/processor/processor_GPS.cpp
index 2440699a6a9cb922fdceb9640dc882a1eb1f9f44..8409f78c2572489083d36a6c9deb31c45ed53ee1 100644
--- a/src/processor/processor_GPS.cpp
+++ b/src/processor/processor_GPS.cpp
@@ -2,7 +2,7 @@
// Created by ptirindelli on 16/12/15.
//
-#include "base/constraint/constraint_GPS_pseudorange_2D.h"
+#include "base/factor/factor_GPS_pseudorange_2D.h"
#include "base/feature/feature_GPS_pseudorange.h"
#include "base/processor/processor_GPS.h"
@@ -40,13 +40,13 @@ void ProcessorGPS::process(CaptureBasePtr _capture_ptr)
capture_gps_ptr_->addFeature(std::make_shared<FeatureGPSPseudorange>(sat_pos, pr, gps_covariance_));
}
//std::cout << "gps features extracted" << std::endl;
- //std::cout << "Establishing constraints to gps features..." << std::endl;
+ //std::cout << "Establishing factors to gps features..." << std::endl;
for (auto i_it = capture_gps_ptr_->getFeatureList().begin();
i_it != capture_gps_ptr_->getFeatureList().end(); i_it++)
{
- capture_gps_ptr_->getFeatureList().front()->addConstraint(std::make_shared<ConstraintGPSPseudorange2D>((*i_it), shared_from_this()));
+ capture_gps_ptr_->getFeatureList().front()->addFactor(std::make_shared<FactorGPSPseudorange2D>((*i_it), shared_from_this()));
}
- //std::cout << "Constraints established" << std::endl;
+ //std::cout << "Factors established" << std::endl;
}
bool ProcessorGPS::voteForKeyFrame()
diff --git a/src/processor/processor_IMU.cpp b/src/processor/processor_IMU.cpp
index 568b61a784bde288cd046d11c81b644c7efb52bd..3f1f5b2445ea42bda8b863beddaf52210bbf4c4c 100644
--- a/src/processor/processor_IMU.cpp
+++ b/src/processor/processor_IMU.cpp
@@ -1,6 +1,6 @@
// wolf
#include "base/processor/processor_IMU.h"
-#include "base/constraint/constraint_IMU.h"
+#include "base/factor/factor_IMU.h"
#include "base/IMU_tools.h"
namespace wolf {
@@ -210,18 +210,18 @@ FeatureBasePtr ProcessorIMU::createFeature(CaptureMotionPtr _capture_motion)
return key_feature_ptr;
}
-ConstraintBasePtr ProcessorIMU::emplaceConstraint(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorIMU::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
CaptureIMUPtr cap_imu = std::static_pointer_cast<CaptureIMU>(_capture_origin);
FeatureIMUPtr ftr_imu = std::static_pointer_cast<FeatureIMU>(_feature_motion);
- ConstraintIMUPtr ctr_imu = std::make_shared<ConstraintIMU>(ftr_imu, cap_imu, shared_from_this());
+ FactorIMUPtr fac_imu = std::make_shared<FactorIMU>(ftr_imu, cap_imu, shared_from_this());
// link ot wolf tree
- _feature_motion->addConstraint(ctr_imu);
- _capture_origin->addConstrainedBy(ctr_imu);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_imu);
+ _feature_motion->addFactor(fac_imu);
+ _capture_origin->addConstrainedBy(fac_imu);
+ _capture_origin->getFrame()->addConstrainedBy(fac_imu);
- return ctr_imu;
+ return fac_imu;
}
void ProcessorIMU::computeCurrentDelta(const Eigen::VectorXs& _data,
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 60569422e15f307d04dc9f6f8c350eea313643af..82fa41c137c0211bb78a699125d287118edc1775 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -62,7 +62,7 @@ void ProcessorBase::remove()
if (isMotion())
{
ProblemPtr P = getProblem();
- if(P && P->getProcessorMotionPtr()->id() == this->id())
+ if(P && P->getProcessorMotion()->id() == this->id())
P->clearProcessorMotion();
}
diff --git a/src/processor/processor_capture_holder.cpp b/src/processor/processor_capture_holder.cpp
index 58e3550ab2421abc6c35c07e4d5059fb94df2011..2958739d7e6d0c45e8db4d2df31c5c1db2ae5ae0 100644
--- a/src/processor/processor_capture_holder.cpp
+++ b/src/processor/processor_capture_holder.cpp
@@ -26,7 +26,7 @@ void ProcessorCaptureHolder::process(CaptureBasePtr _capture_ptr)
void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
const Scalar& _time_tolerance)
{
- assert(_keyframe_ptr->getTrajectoryPtr() != nullptr
+ assert(_keyframe_ptr->getTrajectory() != nullptr
&& "ProcessorMotion::keyFrameCallback: key frame must be in the trajectory.");
// get keyframe's time stamp
@@ -48,7 +48,7 @@ void ProcessorCaptureHolder::keyFrameCallback(FrameBasePtr _keyframe_ptr,
// add motion capture to keyframe
if (std::abs(new_ts - existing_capture->getTimeStamp()) < _time_tolerance)
{
- auto frame_ptr = existing_capture->getFramePtr();
+ auto frame_ptr = existing_capture->getFrame();
if (frame_ptr != _keyframe_ptr)
{
@@ -92,11 +92,11 @@ CaptureBasePtr ProcessorCaptureHolder::findCaptureContainingTimeStamp(const Time
// // go to the previous motion capture
// if (capture_ptr == last_ptr_)
// capture_ptr = origin_ptr_;
-// else if (capture_ptr->getOriginFramePtr() == nullptr)
+// else if (capture_ptr->getOriginFrame() == nullptr)
// return nullptr;
// else
// {
-// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+// CaptureBasePtr capture_base_ptr = capture_ptr->getOriginFrame()->getCaptureOf(getSensor());
// if (capture_base_ptr == nullptr)
// return nullptr;
// else
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index c1b8bbf5680318090e46320d9e413765e01c9b5e..4e307962e94a3fc2e205cf7cd1c80f480d2871c4 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -6,7 +6,7 @@
#include "base/capture/capture_velocity.h"
#include "base/rotations.h"
-#include "base/constraint/constraint_odom_2D.h"
+#include "base/factor/factor_odom_2D.h"
#include "base/feature/feature_diff_drive.h"
namespace wolf
@@ -34,7 +34,7 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
/// Retrieve sensor intrinsics
const IntrinsicsDiffDrive& intrinsics =
- *(std::static_pointer_cast<SensorDiffDrive>(getSensorPtr())->getIntrinsics());
+ *(std::static_pointer_cast<SensorDiffDrive>(getSensor())->getIntrinsics());
VelocityComand<Scalar> vel;
Eigen::MatrixXs J_vel_data;
@@ -209,17 +209,17 @@ CaptureMotionPtr ProcessorDiffDrive::createCapture(const TimeStamp& _ts,
_frame_origin, nullptr, nullptr, i_ptr);
}
-ConstraintBasePtr ProcessorDiffDrive::emplaceConstraint(FeatureBasePtr _feature,
+FactorBasePtr ProcessorDiffDrive::emplaceFactor(FeatureBasePtr _feature,
CaptureBasePtr _capture_origin)
{
- ConstraintOdom2DPtr ctr_odom =
- std::make_shared<ConstraintOdom2D>(_feature, _capture_origin->getFramePtr(),
+ FactorOdom2DPtr fac_odom =
+ std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
- _feature->addConstraint(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
+ _feature->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
- return ctr_odom;
+ return fac_odom;
}
FeatureBasePtr ProcessorDiffDrive::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
index 4901170c2e99579a6fbe3618a261f4917bcd472a..769cf871be36a1969a0e2c4883b044f9cabdbda3 100644
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ b/src/processor/processor_frame_nearest_neighbor_filter.cpp
@@ -58,11 +58,11 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
const ProblemPtr problem_ptr = getProblem();
const std::string frame_structure =
- problem_ptr->getTrajectoryPtr()->getFrameStructure();
+ problem_ptr->getTrajectory()->getFrameStructure();
// get the list of all frames
- const FrameBaseList& frame_list = problem_ptr->
- getTrajectoryPtr()->
+ const FrameBasePtrList& frame_list = problem_ptr->
+ getTrajectory()->
getFrameList();
bool found_possible_candidate = false;
@@ -72,17 +72,17 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
// check for LC just if frame is key frame
// Assert that the evaluated KF has a capture of the
// same sensor as this processor
- if (key_it->isKey() && key_it->getCaptureOf(getSensorPtr()/*, "LASER 2D"*/) != nullptr)
+ if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
{
// Check if the two frames currently evaluated are already
// constrained one-another.
- const ConstraintBaseList& ctr_list = key_it->getConstrainedByList();
+ const FactorBasePtrList& fac_list = key_it->getConstrainedByList();
bool are_constrained = false;
- for (const ConstraintBasePtr& crt : ctr_list)
+ for (const FactorBasePtr& crt : fac_list)
{
// Are the two frames constrained one-another ?
- if (crt->getFrameOtherPtr() == problem_ptr->getLastKeyFramePtr())
+ if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
{
// By this very processor ?
if (crt->getProcessor() == shared_from_this())
@@ -127,7 +127,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
Eigen::Vector5s frame_covariance, current_covariance;
if (!computeEllipse2D(key_it,
frame_covariance)) continue;
- if (!computeEllipse2D(getProblem()->getLastKeyFramePtr(),
+ if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
current_covariance)) continue;
found_possible_candidate = ellipse2DIntersect(frame_covariance,
current_covariance);
@@ -160,7 +160,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
Eigen::Vector10s frame_covariance, current_covariance;
if (!computeEllipsoid3D(key_it,
frame_covariance)) continue;
- if (!computeEllipsoid3D(getProblem()->getLastKeyFramePtr(),
+ if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
frame_covariance)) continue;
found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
current_covariance);
@@ -170,7 +170,7 @@ bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /
case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
{
found_possible_candidate = insideMahalanisDistance(key_it,
- problem_ptr->getLastKeyFramePtr());
+ problem_ptr->getLastKeyFrame());
break;
}
@@ -210,7 +210,7 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& f
}
// get position of frame AKA mean [x, y]
- const Eigen::VectorXs frame_position = frame_ptr->getPPtr()->getState();
+ const Eigen::VectorXs frame_position = frame_ptr->getP()->getState();
ellipse(0) = frame_position(0);
ellipse(1) = frame_position(1);
@@ -239,7 +239,7 @@ bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr&
// Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
// get position of frame AKA mean [x, y, z]
- const Eigen::VectorXs frame_position = frame_pointer->getPPtr()->getState();
+ const Eigen::VectorXs frame_position = frame_pointer->getP()->getState();
ellipsoid(0) = frame_position(0);
ellipsoid(1) = frame_position(1);
ellipsoid(2) = frame_position(2);
@@ -447,7 +447,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
Eigen::VectorXs traj_pose, query_pose;
// get state and covariance matrix for both frames
- if (trajectory->getPPtr()->getState().size() == 2)
+ if (trajectory->getP()->getState().size() == 2)
{
traj_pose.resize(3);
query_pose.resize(3);
@@ -458,11 +458,11 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
query_pose.resize(7);
}
- traj_pose << trajectory->getPPtr()->getState(),
- trajectory->getOPtr()->getState();
+ traj_pose << trajectory->getP()->getState(),
+ trajectory->getO()->getState();
- query_pose << query->getPPtr()->getState(),
- query->getOPtr()->getState();
+ query_pose << query->getP()->getState(),
+ query->getO()->getState();
const Eigen::MatrixXs traj_covariance = getProblem()->getFrameCovariance(trajectory);
const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query);
@@ -482,7 +482,7 @@ Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBaseP
//##############################################################################
bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
{
- FrameBasePtr keyframe = getProblem()->getLastKeyFramePtr();
+ FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
return false;
else
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 10a1759fddc497a31614080ca07ee265ed881a1b..ef5cbb1cd61cfe586cfcda1b573ddfc7056142ae 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -63,7 +63,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
}
// Update the existing capture
- _capture_source->setOriginFramePtr(_keyframe_target);
+ _capture_source->setOriginFrame(_keyframe_target);
// re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer(_capture_source);
@@ -102,11 +102,11 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
auto existing_capture = findCaptureContainingTimeStamp(ts_from_callback);
// Find the frame acting as the capture's origin
- auto keyframe_origin = existing_capture->getOriginFramePtr();
+ auto keyframe_origin = existing_capture->getOriginFrame();
// emplace a new motion capture to the new keyframe
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensorPtr(),
+ getSensor(),
ts_from_callback,
Eigen::VectorXs::Zero(data_size_),
existing_capture->getDataCovariance(),
@@ -121,10 +121,10 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// create motion feature and add it to the capture
auto new_feature = emplaceFeature(capture_for_keyframe_callback);
- // create motion constraint and add it to the feature, and constrain to the other capture (origin)
- emplaceConstraint(new_feature, keyframe_origin->getCaptureOf(getSensorPtr()) );
+ // create motion factor and add it to the feature, and constrain to the other capture (origin)
+ emplaceFactor(new_feature, keyframe_origin->getCaptureOf(getSensor()) );
- // modify existing feature and constraint (if they exist in the existing capture)
+ // modify existing feature and factor (if they exist in the existing capture)
if (!existing_capture->getFeatureList().empty())
{
auto existing_feature = existing_capture->getFeatureList().back(); // there is only one feature!
@@ -133,11 +133,11 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
existing_feature->setMeasurement (existing_capture->getBuffer().get().back().delta_integr_);
existing_feature->setMeasurementCovariance(existing_capture->getBuffer().get().back().delta_integr_cov_);
- // Modify existing constraint --------
+ // Modify existing factor --------
// Instead of modifying, we remove one ctr, and create a new one.
- auto ctr_to_remove = existing_feature->getConstraintList().back(); // there is only one constraint!
- auto new_ctr = emplaceConstraint(existing_feature, capture_for_keyframe_callback);
- ctr_to_remove ->remove(); // remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
+ auto fac_to_remove = existing_feature->getFactorList().back(); // there is only one factor!
+ auto new_ctr = emplaceFactor(existing_feature, capture_for_keyframe_callback);
+ fac_to_remove ->remove(); // remove old factor now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
break;
}
@@ -149,12 +149,12 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
// Find the frame acting as the capture's origin
- auto keyframe_origin = last_ptr_->getOriginFramePtr();
+ auto keyframe_origin = last_ptr_->getOriginFrame();
// emplace a new motion capture to the new keyframe
VectorXs calib = last_ptr_->getCalibration();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
- getSensorPtr(),
+ getSensor(),
ts_from_callback,
Eigen::VectorXs::Zero(data_size_),
last_ptr_->getDataCovariance(),
@@ -169,8 +169,8 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// create motion feature and add it to the capture
auto feature_for_keyframe_callback = emplaceFeature(capture_for_keyframe_callback);
- // create motion constraint and add it to the feature, and constrain to the other capture (origin)
- emplaceConstraint(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensorPtr()) );
+ // create motion factor and add it to the feature, and constrain to the other capture (origin)
+ emplaceFactor(feature_for_keyframe_callback, keyframe_origin->getCaptureOf(getSensor()) );
// reset processor origin
origin_ptr_ = capture_for_keyframe_callback;
@@ -190,20 +190,20 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
// Update state and time stamps
last_ptr_->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFramePtr()->setTimeStamp(getCurrentTimeStamp());
- last_ptr_->getFramePtr()->setState(getCurrentState());
+ last_ptr_->getFrame()->setTimeStamp(getCurrentTimeStamp());
+ last_ptr_->getFrame()->setState(getCurrentState());
if (voteForKeyFrame() && permittedKeyFrame())
{
// Set the frame of last_ptr as key
- auto key_frame_ptr = last_ptr_->getFramePtr();
+ auto key_frame_ptr = last_ptr_->getFrame();
key_frame_ptr->setKey();
// create motion feature and add it to the key_capture
auto key_feature_ptr = emplaceFeature(last_ptr_);
- // create motion constraint and link it to parent feature and other frame (which is origin's frame)
- auto ctr_ptr = emplaceConstraint(key_feature_ptr, origin_ptr_);
+ // create motion factor and link it to parent feature and other frame (which is origin's frame)
+ auto fac_ptr = emplaceFactor(key_feature_ptr, origin_ptr_);
// create a new frame
auto new_frame_ptr = getProblem()->emplaceFrame(NON_KEY_FRAME,
@@ -211,7 +211,7 @@ void ProcessorMotion::process(CaptureBasePtr _incoming_ptr)
getCurrentTimeStamp());
// create a new capture
auto new_capture_ptr = emplaceCapture(new_frame_ptr,
- getSensorPtr(),
+ getSensor(),
key_frame_ptr->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
@@ -260,8 +260,8 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
if (capture_motion) // We found a CaptureMotion whose buffer contains the time stamp
{
// Get origin state and calibration
- VectorXs state_0 = capture_motion->getOriginFramePtr()->getState();
- CaptureBasePtr cap_orig = capture_motion->getOriginFramePtr()->getCaptureOf(getSensorPtr());
+ VectorXs state_0 = capture_motion->getOriginFrame()->getState();
+ CaptureBasePtr cap_orig = capture_motion->getOriginFrame()->getCaptureOf(getSensor());
VectorXs calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
@@ -273,7 +273,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x)
// Compose on top of origin state using the buffered time stamp, not the query time stamp
// TODO Interpolate the delta to produce a state at the query time stamp _ts
- Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOriginPtr()->getTimeStamp();
+ Scalar dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_; // = _ts - capture_motion->getOrigin()->getTimeStamp();
statePlusDelta(state_0, delta, dt, _x);
}
else
@@ -297,19 +297,19 @@ FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const
void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
{
assert(_origin_frame && "ProcessorMotion::setOrigin: Provided frame pointer is nullptr.");
- assert(_origin_frame->getTrajectoryPtr() != nullptr
+ assert(_origin_frame->getTrajectory() != nullptr
&& "ProcessorMotion::setOrigin: origin frame must be in the trajectory.");
assert(_origin_frame->isKey() && "ProcessorMotion::setOrigin: origin frame must be KEY FRAME.");
// -------- ORIGIN ---------
// emplace (empty) origin Capture
origin_ptr_ = emplaceCapture(_origin_frame,
- getSensorPtr(),
+ getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensorPtr()->getCalibration(),
- getSensorPtr()->getCalibration(),
+ getSensor()->getCalibration(),
+ getSensor()->getCalibration(),
nullptr);
// ---------- LAST ----------
@@ -319,12 +319,12 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
_origin_frame->getTimeStamp());
// emplace (emtpy) last Capture
last_ptr_ = emplaceCapture(new_frame_ptr,
- getSensorPtr(),
+ getSensor(),
_origin_frame->getTimeStamp(),
Eigen::VectorXs::Zero(data_size_),
Eigen::MatrixXs::Zero(data_size_, data_size_),
- getSensorPtr()->getCalibration(),
- getSensorPtr()->getCalibration(),
+ getSensor()->getCalibration(),
+ getSensor()->getCalibration(),
_origin_frame);
// clear and reset buffer
@@ -394,7 +394,7 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
{
// start with empty motion
- _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFramePtr()->getTimeStamp()));
+ _capture_ptr->getBuffer().get().push_front(motionZero(_capture_ptr->getOriginFrame()->getTimeStamp()));
VectorXs calib = _capture_ptr->getCalibrationPreint();
@@ -534,12 +534,12 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
FrameBasePtr frame = nullptr;
CaptureBasePtr capture = nullptr;
CaptureMotionPtr capture_motion = nullptr;
- for (auto frame_rev_iter = getProblem()->getTrajectoryPtr()->getFrameList().rbegin();
- frame_rev_iter != getProblem()->getTrajectoryPtr()->getFrameList().rend();
+ for (auto frame_rev_iter = getProblem()->getTrajectory()->getFrameList().rbegin();
+ frame_rev_iter != getProblem()->getTrajectory()->getFrameList().rend();
++frame_rev_iter)
{
frame = *frame_rev_iter;
- capture = frame->getCaptureOf(getSensorPtr());
+ capture = frame->getCaptureOf(getSensor());
if (capture != nullptr)
{
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index 791103d47ad9594ff8afa0374309a0d08fb3c890..a474ed1f3a3bd6d015624d4e3cb5165de005dd16 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -136,8 +136,8 @@ bool ProcessorOdom2D::voteForKeyFrame()
return true;
}
// Time criterion
- WOLF_TRACE("orig t: ", origin_ptr_->getFramePtr()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get());
- if (getBuffer().get().back().ts_.get() - origin_ptr_->getFramePtr()->getTimeStamp().get() > params_odom_2D_->max_time_span)
+ WOLF_TRACE("orig t: ", origin_ptr_->getFrame()->getTimeStamp().get(), " current t: ", getBuffer().get().back().ts_.get() , " dt: ", getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get());
+ if (getBuffer().get().back().ts_.get() - origin_ptr_->getFrame()->getTimeStamp().get() > params_odom_2D_->max_time_span)
{
return true;
}
@@ -152,13 +152,13 @@ CaptureMotionPtr ProcessorOdom2D::createCapture(const TimeStamp& _ts, const Sens
return capture_odom;
}
-ConstraintBasePtr ProcessorOdom2D::emplaceConstraint(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBasePtr _capture_origin)
{
- ConstraintOdom2DPtr ctr_odom = std::make_shared<ConstraintOdom2D>(_feature, _capture_origin->getFramePtr(),
+ FactorOdom2DPtr fac_odom = std::make_shared<FactorOdom2D>(_feature, _capture_origin->getFrame(),
shared_from_this());
- _feature->addConstraint(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
- return ctr_odom;
+ _feature->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+ return fac_odom;
}
FeatureBasePtr ProcessorOdom2D::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 716d6831ad3a8175041e5273d7cba8b6b4e2be6f..7415459711b62482df8a0de5d32bb56a496180d2 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -403,13 +403,13 @@ CaptureMotionPtr ProcessorOdom3D::createCapture(const TimeStamp& _ts, const Sens
return capture_odom;
}
-ConstraintBasePtr ProcessorOdom3D::emplaceConstraint(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
+FactorBasePtr ProcessorOdom3D::emplaceFactor(FeatureBasePtr _feature_motion, CaptureBasePtr _capture_origin)
{
- ConstraintOdom3DPtr ctr_odom = std::make_shared<ConstraintOdom3D>(_feature_motion, _capture_origin->getFramePtr(),
+ FactorOdom3DPtr fac_odom = std::make_shared<FactorOdom3D>(_feature_motion, _capture_origin->getFrame(),
shared_from_this());
- _feature_motion->addConstraint(ctr_odom);
- _capture_origin->getFramePtr()->addConstrainedBy(ctr_odom);
- return ctr_odom;
+ _feature_motion->addFactor(fac_odom);
+ _capture_origin->getFrame()->addConstrainedBy(fac_odom);
+ return fac_odom;
}
FeatureBasePtr ProcessorOdom3D::createFeature(CaptureMotionPtr _capture_motion)
diff --git a/src/processor/processor_tracker.cpp b/src/processor/processor_tracker.cpp
index f8138e1fc7e1b0d152bf0576ee6fcb454e75ca0b..e502ba296a1a4fee6a355839c9d7a6d52871a54a 100644
--- a/src/processor/processor_tracker.cpp
+++ b/src/processor/processor_tracker.cpp
@@ -110,8 +110,8 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
processKnown();
processNew(params_tracker_->max_new_features);
- // Establish constraints
- establishConstraints();
+ // Establish factors
+ establishFactors();
// Update pointers
resetDerived();
@@ -131,7 +131,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
processKnown();
// Capture last_ is added to the new keyframe
- FrameBasePtr last_old_frame = last_ptr_->getFramePtr();
+ FrameBasePtr last_old_frame = last_ptr_->getFrame();
last_old_frame->unlinkCapture(last_ptr_);
last_old_frame->remove();
pack->key_frame->addCapture(last_ptr_);
@@ -140,11 +140,11 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
frm->addCapture(incoming_ptr_);
- // Detect new Features, initialize Landmarks, create Constraints, ...
+ // Detect new Features, initialize Landmarks, create Factors, ...
processNew(params_tracker_->max_new_features);
- // Establish constraints
- establishConstraints();
+ // Establish factors
+ establishFactors();
// Update pointers
resetDerived();
@@ -170,7 +170,7 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We create a KF
// set KF on last
- last_ptr_->getFramePtr()->setKey();
+ last_ptr_->getFrame()->setKey();
// make F; append incoming to new F
FrameBasePtr frm = getProblem()->emplaceFrame(NON_KEY_FRAME, incoming_ptr_->getTimeStamp());
@@ -180,16 +180,16 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
processNew(params_tracker_->max_new_features);
// // Set key
- // last_ptr_->getFramePtr()->setKey();
+ // last_ptr_->getFrame()->setKey();
//
// Set state to the keyframe
- last_ptr_->getFramePtr()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
+ last_ptr_->getFrame()->setState(getProblem()->getState(last_ptr_->getTimeStamp()));
- // Establish constraints
- establishConstraints();
+ // Establish factors
+ establishFactors();
- // Call the new keyframe callback in order to let the other processors to establish their constraints
- getProblem()->keyFrameCallback(last_ptr_->getFramePtr(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
+ // Call the new keyframe callback in order to let the other processors to establish their factors
+ getProblem()->keyFrameCallback(last_ptr_->getFrame(), std::static_pointer_cast<ProcessorBase>(shared_from_this()), params_tracker_->time_tolerance);
// Update pointers
resetDerived();
@@ -203,9 +203,9 @@ void ProcessorTracker::process(CaptureBasePtr const _incoming_ptr)
// We do not create a KF
// Advance this
- last_ptr_->getFramePtr()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
+ last_ptr_->getFrame()->addCapture(incoming_ptr_); // Add incoming Capture to the tracker's last Frame
last_ptr_->remove();
- incoming_ptr_->getFramePtr()->setTimeStamp(incoming_ptr_->getTimeStamp());
+ incoming_ptr_->getFrame()->setTimeStamp(incoming_ptr_->getTimeStamp());
// Update pointers
advanceDerived();
@@ -257,7 +257,7 @@ void ProcessorTracker::computeProcessingStep()
if (kf_pack_buffer_.selectPack(last_ptr_, params_tracker_->time_tolerance))
{
- if (last_ptr_->getFramePtr()->isKey())
+ if (last_ptr_->getFrame()->isKey())
{
WOLF_WARN("||*||");
WOLF_INFO(" ... It seems you missed something!");
diff --git a/src/processor/processor_tracker_feature.cpp b/src/processor/processor_tracker_feature.cpp
index 905bd828845ce7d1a988dc770e6b5571a4efb736..0ee10e6f75b5fa6e843421eaba7728edc7b2f4ea 100644
--- a/src/processor/processor_tracker_feature.cpp
+++ b/src/processor/processor_tracker_feature.cpp
@@ -139,7 +139,7 @@ void ProcessorTrackerFeature::resetDerived()
}
}
-void ProcessorTrackerFeature::establishConstraints()
+void ProcessorTrackerFeature::establishFactors()
{
TrackMatches matches_origin_last = track_matrix_.matches(origin_ptr_, last_ptr_);
@@ -148,11 +148,11 @@ void ProcessorTrackerFeature::establishConstraints()
FeatureBasePtr feature_in_origin = pair_trkid_pair.second.first;
FeatureBasePtr feature_in_last = pair_trkid_pair.second.second;
- auto ctr_ptr = createConstraint(feature_in_last, feature_in_origin);
- feature_in_last ->addConstraint(ctr_ptr);
- feature_in_origin->addConstrainedBy(ctr_ptr);
+ auto fac_ptr = createFactor(feature_in_last, feature_in_origin);
+ feature_in_last ->addFactor(fac_ptr);
+ feature_in_origin->addConstrainedBy(fac_ptr);
- WOLF_DEBUG( "Constraint: track: " , feature_in_last->trackId(),
+ WOLF_DEBUG( "Factor: track: " , feature_in_last->trackId(),
" origin: " , feature_in_origin->id() ,
" from last: " , feature_in_last->id() );
}
diff --git a/src/processor/processor_tracker_feature_corner.cpp b/src/processor/processor_tracker_feature_corner.cpp
index 0a8e7c7c232fd465f07c3704bbf63f7d615a20cc..4414d8caf499e187e28961b54f8a65e80455716a 100644
--- a/src/processor/processor_tracker_feature_corner.cpp
+++ b/src/processor/processor_tracker_feature_corner.cpp
@@ -46,11 +46,11 @@ void ProcessorTrackerFeatureCorner::preProcess()
R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_.head<2>() = getSensorPtr()->getPPtr()->getState();
- t_robot_sensor_(2) = getSensorPtr()->getOPtr()->getState()(0);
+ t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
+ t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -70,8 +70,8 @@ void ProcessorTrackerFeatureCorner::advanceDerived()
corners_last_ = std::move(corners_incoming_);
}
-unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& _features_last_in,
- FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBasePtrList& _features_last_in,
+ FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences)
{
std::cout << "tracking " << _features_last_in.size() << " features..." << std::endl;
@@ -107,21 +107,21 @@ bool ProcessorTrackerFeatureCorner::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_corner_->n_corners_th;
}
-unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
// in corners_last_ remain all not tracked corners
_features_incoming_out = std::move(corners_last_);
return _features_incoming_out.size();
}
-ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr _feature_ptr,
+FactorBasePtr ProcessorTrackerFeatureCorner::createFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
// Getting landmark ptr
LandmarkCorner2DPtr landmark_ptr = nullptr;
- for (auto constraint : _feature_other_ptr->getConstraintList())
- if (constraint->getLandmarkOtherPtr() != nullptr && constraint->getLandmarkOtherPtr()->getType() == "CORNER 2D")
- landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(constraint->getLandmarkOtherPtr());
+ for (auto factor : _feature_other_ptr->getFactorList())
+ if (factor->getLandmarkOther() != nullptr && factor->getLandmarkOther()->getType() == "CORNER 2D")
+ landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(factor->getLandmarkOther());
if (landmark_ptr == nullptr)
{
@@ -131,25 +131,25 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
std::make_shared<StateBlock>(feature_global_pose.tail(1)),
_feature_ptr->getMeasurement()(3));
- // Add landmark constraint to the other feature
- _feature_other_ptr->addConstraint(std::make_shared<ConstraintCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
+ // Add landmark factor to the other feature
+ _feature_other_ptr->addFactor(std::make_shared<FactorCorner2D>(_feature_other_ptr, landmark_ptr, shared_from_this()));
}
-// std::cout << "creating constraint: last feature " << _feature_ptr->getMeasurement()
+// std::cout << "creating factor: last feature " << _feature_ptr->getMeasurement()
// << " with origin feature " << _feature_other_ptr->getMeasurement() << std::endl
// << " corresponding to landmark " << landmark_ptr->id() << std::endl;
- return std::make_shared<ConstraintCorner2D>(_feature_ptr, landmark_ptr, shared_from_this());
+ return std::make_shared<FactorCorner2D>(_feature_ptr, landmark_ptr, shared_from_this());
}
void ProcessorTrackerFeatureCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBaseList& _corner_list)
+ FeatureBasePtrList& _corner_list)
{
// TODO: sort corners by bearing
std::list<laserscanutils::CornerPoint> corners;
std::cout << "Extracting corners..." << std::endl;
corner_finder_.findCorners(_capture_laser_ptr->getScan(),
- (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(),
+ (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(),
line_finder_,
corners);
diff --git a/src/processor/processor_tracker_feature_dummy.cpp b/src/processor/processor_tracker_feature_dummy.cpp
index 6ce3d4fe18fbabc83d63582a9ac2090c63ce53be..060b54e83a3aba8e7b483c2e2678b88b7e468678 100644
--- a/src/processor/processor_tracker_feature_dummy.cpp
+++ b/src/processor/processor_tracker_feature_dummy.cpp
@@ -13,8 +13,8 @@ namespace wolf
unsigned int ProcessorTrackerFeatureDummy::count_ = 0;
-unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& _features_last_in,
- FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBasePtrList& _features_last_in,
+ FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_correspondences)
{
WOLF_INFO("tracking " , _features_last_in.size() , " features...");
@@ -49,7 +49,7 @@ bool ProcessorTrackerFeatureDummy::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < params_tracker_feature_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
WOLF_INFO("Detecting " , _max_features , " new features..." );
@@ -70,13 +70,13 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const unsigned int&
return _features_incoming_out.size();
}
-ConstraintBasePtr ProcessorTrackerFeatureDummy::createConstraint(FeatureBasePtr _feature_ptr,
+FactorBasePtr ProcessorTrackerFeatureDummy::createFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- WOLF_INFO( "creating constraint: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
+ WOLF_INFO( "creating factor: track " , _feature_other_ptr->trackId() , " last feature " , _feature_ptr->id()
, " with origin feature " , _feature_other_ptr->id() );
- auto ctr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
+ auto ctr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr, shared_from_this());
return ctr;
}
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index 486cd623c75ca91b63ef63abca62d3b0bf552be0..3f6bec8fe828580d8b04e5b0c390a8441cc24a9e 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -135,7 +135,7 @@ void ProcessorTrackerFeatureImage::postProcess()
{
}
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
FeatureMatchMap& _feature_matches)
{
KeyPointVector candidate_keypoints;
@@ -244,7 +244,7 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
}
}
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -306,12 +306,12 @@ Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _
return normalized_score;
}
-ConstraintBasePtr ProcessorTrackerFeatureImage::createConstraint(FeatureBasePtr _feature_ptr,
+FactorBasePtr ProcessorTrackerFeatureImage::createFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- ConstraintEpipolarPtr const_epipolar_ptr = std::make_shared<ConstraintEpipolar>(_feature_ptr, _feature_other_ptr,
+ FactorEpipolarPtr const_epipolar_ptr = std::make_shared<FactorEpipolar>(_feature_ptr, _feature_other_ptr,
shared_from_this());
- // _feature_ptr->addConstraint(const_epipolar_ptr);
+ // _feature_ptr->addFactor(const_epipolar_ptr);
// _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
return const_epipolar_ptr;
}
@@ -324,7 +324,7 @@ unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi
return _new_keypoints.size();
}
-void ProcessorTrackerFeatureImage::resetVisualizationFlag(FeatureBaseList& _feature_list_last)
+void ProcessorTrackerFeatureImage::resetVisualizationFlag(FeatureBasePtrList& _feature_list_last)
{
for (auto feature_base_last_ptr : _feature_list_last)
{
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index 30018991042f601481d15289a0bf008aa9dc6501..980c652af00ecb490250ac0baabb375ddf5ba98e 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -4,7 +4,7 @@
#include "base/sensor/sensor_camera.h"
#include "base/feature/feature_point_image.h"
-#include "base/constraint/constraint_autodiff_trifocal.h"
+#include "base/factor/factor_autodiff_trifocal.h"
#include "base/capture/capture_image.h"
// vision_utils
@@ -134,7 +134,7 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
}
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned int& _max_new_features, FeatureBasePtrList& _features_incoming_out)
{
// // DEBUG =====================================
// clock_t debug_tStart;
@@ -202,7 +202,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const unsigned i
return _features_incoming_out.size();
}
-unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_matches)
+unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches)
{
// // DEBUG =====================================
// clock_t debug_tStart;
@@ -312,7 +312,7 @@ void ProcessorTrackerFeatureTrifocal::resetDerived()
void ProcessorTrackerFeatureTrifocal::preProcess()
{
- WOLF_TRACE("-------- Image ", getIncomingPtr()->id(), " -- t = ", getIncomingPtr()->getTimeStamp(), " s ----------");
+ WOLF_TRACE("-------- Image ", getIncoming()->id(), " -- t = ", getIncoming()->getTimeStamp(), " s ----------");
if (!initialized_)
{
@@ -384,29 +384,29 @@ void ProcessorTrackerFeatureTrifocal::postProcess()
cv::waitKey(1);
}
-ConstraintBasePtr ProcessorTrackerFeatureTrifocal::createConstraint(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+FactorBasePtr ProcessorTrackerFeatureTrifocal::createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
// NOTE: This function cannot be implemented because the API lacks an input to feature_prev_origin.
- // Therefore, we implement establishConstraints() instead and do all the job there.
+ // Therefore, we implement establishFactors() instead and do all the job there.
// This function remains empty, and with a warning or even an error issued in case someone calls it.
- std::cout << "033[1;33m [WARN]:033[0m ProcessorTrackerFeatureTrifocal::createConstraint is empty." << std::endl;
- ConstraintBasePtr return_var{}; //TODO: fill this variable
+ std::cout << "033[1;33m [WARN]:033[0m ProcessorTrackerFeatureTrifocal::createFactor is empty." << std::endl;
+ FactorBasePtr return_var{}; //TODO: fill this variable
return return_var;
}
-void ProcessorTrackerFeatureTrifocal::establishConstraints()
+void ProcessorTrackerFeatureTrifocal::establishFactors()
{
if (initialized_)
{
// get tracks between prev, origin and last
TrackMatches matches = track_matrix_.matches(prev_origin_ptr_, last_ptr_); // it's guaranteed by construction that the track also includes origin
- for (auto pair_trkid_match : matches) // OMG this will add potentially a loooot of constraints! TODO see a smarter way of adding constraints
- { // Currently reduced by creating constraints for large tracks
+ for (auto pair_trkid_match : matches) // OMG this will add potentially a loooot of factors! TODO see a smarter way of adding factors
+ { // Currently reduced by creating factors for large tracks
// get track ID
SizeStd trk_id = pair_trkid_match.first;
- if (track_matrix_.trackSize(trk_id)>params_tracker_feature_trifocal_->min_track_length_for_constraint)
+ if (track_matrix_.trackSize(trk_id)>params_tracker_feature_trifocal_->min_track_length_for_factor)
{
// get the three features for this track
// FeatureBasePtr ftr_prev = track_matrix_.feature(trk_id, prev_origin_ptr_); // left here for ref, but implemented in a quicker way below
@@ -415,11 +415,11 @@ void ProcessorTrackerFeatureTrifocal::establishConstraints()
FeatureBasePtr ftr_orig = track_matrix_.feature(trk_id, origin_ptr_); // because it's a tracker, this feature in the middle of prev and last exists for sure!
FeatureBasePtr ftr_last = pair_trkid_match.second.second;
- // make constraint
- ConstraintAutodiffTrifocalPtr ctr = std::make_shared<ConstraintAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, CTR_ACTIVE);
+ // make factor
+ FactorAutodiffTrifocalPtr ctr = std::make_shared<FactorAutodiffTrifocal>(ftr_prev, ftr_orig, ftr_last, shared_from_this(), false, FAC_ACTIVE);
// link to wolf tree
- ftr_last->addConstraint(ctr);
+ ftr_last->addFactor(ctr);
ftr_orig->addConstrainedBy(ctr);
ftr_prev->addConstrainedBy(ctr);
}
diff --git a/src/processor/processor_tracker_landmark.cpp b/src/processor/processor_tracker_landmark.cpp
index 873a45e2ed10ee90271d0b0c1d0ca7f50b5be6b2..d19610391edb98ee2565c2f477970a11c0088075 100644
--- a/src/processor/processor_tracker_landmark.cpp
+++ b/src/processor/processor_tracker_landmark.cpp
@@ -65,7 +65,7 @@ unsigned int ProcessorTrackerLandmark::processNew(const unsigned int& _max_featu
/* Rationale: A keyFrame will be created using the last Capture.
* First, we work on this Capture to detect new Features,
* eventually create Landmarks with them,
- * and in such case create the new Constraints feature-landmark.
+ * and in such case create the new Factors feature-landmark.
* When done, we need to track these new Features to the incoming Capture.
* At the end, all new Features are appended to the lists of known Features in
* the last and incoming Captures.
@@ -116,8 +116,8 @@ void ProcessorTrackerLandmark::createNewLandmarks()
unsigned int ProcessorTrackerLandmark::processKnown()
{
// Find landmarks in incoming_ptr_
- FeatureBaseList known_features_list_incoming;
- unsigned int n = findLandmarks(getProblem()->getMapPtr()->getLandmarkList(),
+ FeatureBasePtrList known_features_list_incoming;
+ unsigned int n = findLandmarks(getProblem()->getMap()->getLandmarkList(),
known_features_list_incoming, matches_landmark_from_incoming_);
// Append found incoming features
incoming_ptr_->addFeatureList(known_features_list_incoming);
@@ -125,18 +125,18 @@ unsigned int ProcessorTrackerLandmark::processKnown()
return n;
}
-void ProcessorTrackerLandmark::establishConstraints()
+void ProcessorTrackerLandmark::establishFactors()
{
// Loop all features in last_ptr_
for (auto last_feature : last_ptr_->getFeatureList())
{
auto lmk = matches_landmark_from_last_[last_feature]->landmark_ptr_;
- ConstraintBasePtr ctr_ptr = createConstraint(last_feature,
+ FactorBasePtr fac_ptr = createFactor(last_feature,
lmk);
- if (ctr_ptr != nullptr) // constraint links
+ if (fac_ptr != nullptr) // factor links
{
- last_feature->addConstraint(ctr_ptr);
- lmk->addConstrainedBy(ctr_ptr);
+ last_feature->addFactor(fac_ptr);
+ lmk->addConstrainedBy(fac_ptr);
}
}
}
diff --git a/src/processor/processor_tracker_landmark_corner.cpp b/src/processor/processor_tracker_landmark_corner.cpp
index 522034204bb5dd4f4b3140d3f55db773faa070af..aeba30f8e908a72a57aad5b5f617edfa249b8982 100644
--- a/src/processor/processor_tracker_landmark_corner.cpp
+++ b/src/processor/processor_tracker_landmark_corner.cpp
@@ -17,11 +17,11 @@ void ProcessorTrackerLandmarkCorner::preProcess()
R_world_robot.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_world_robot_(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_.head<2>() = getSensorPtr()->getPPtr()->getState();
- t_robot_sensor_(2) = getSensorPtr()->getOPtr()->getState()(0);
+ t_robot_sensor_.head<2>() = getSensor()->getP()->getState();
+ t_robot_sensor_(2) = getSensor()->getO()->getState()(0);
R_robot_sensor_.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(t_robot_sensor_(2)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -40,14 +40,14 @@ void ProcessorTrackerLandmarkCorner::preProcess()
//std::cout << "PreProcess: incoming new features: " << corners_incoming_.size() << std::endl;
}
-unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseList& _landmarks_corner_searched,
- FeatureBaseList& _features_corner_found,
+unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBasePtrList& _landmarks_corner_searched,
+ FeatureBasePtrList& _features_corner_found,
LandmarkMatchMap& _feature_landmark_correspondences)
{
//std::cout << "ProcessorTrackerLandmarkCorner::findLandmarks: " << _landmarks_corner_searched.size() << " features: " << corners_incoming_.size() << std::endl;
// NAIVE FIRST NEAREST NEIGHBOR MATCHING
- LandmarkBaseList not_matched_landmarks = _landmarks_corner_searched;
+ LandmarkBasePtrList not_matched_landmarks = _landmarks_corner_searched;
Scalar dm2;
// COMPUTING ALL EXPECTED FEATURES
@@ -206,7 +206,7 @@ bool ProcessorTrackerLandmarkCorner::voteForKeyFrame()
// option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
for (auto new_feat : new_features_last_)
{
- if (last_ptr_->getFramePtr()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getFramePtr()->id() > loop_frames_th_)
+ if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_feat]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > loop_frames_th_)
{
std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
return true;
@@ -223,12 +223,12 @@ bool ProcessorTrackerLandmarkCorner::voteForKeyFrame()
}
void ProcessorTrackerLandmarkCorner::extractCorners(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBaseList& _corner_list)
+ FeatureBasePtrList& _corner_list)
{
// TODO: sort corners by bearing
std::list<laserscanutils::CornerPoint> corners;
- corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), line_finder_, corners);
+ corner_finder_.findCorners(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), line_finder_, corners);
Eigen::Vector4s measurement;
for (auto corner : corners)
@@ -282,38 +282,38 @@ void ProcessorTrackerLandmarkCorner::expectedFeature(LandmarkBasePtr _landmark_p
Eigen::Matrix3s& expected_feature_cov_)
{
//std::cout << "ProcessorTrackerLandmarkCorner::expectedFeature: " << std::endl;
- //std::cout << "Landmark global pose: " << _landmark_ptr->getPPtr()->getVector().transpose() << " " << _landmark_ptr->getOPtr()->getVector() << std::endl;
+ //std::cout << "Landmark global pose: " << _landmark_ptr->getP()->getVector().transpose() << " " << _landmark_ptr->getO()->getVector() << std::endl;
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
// ------------ expected feature measurement
- expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getPPtr()->getState() - t_world_sensor_.head<2>());
- expected_feature_(2) = _landmark_ptr->getOPtr()->getState()(0) - t_world_sensor_(2);
+ expected_feature_.head<2>() = R_sensor_world_.topLeftCorner<2,2>() * (_landmark_ptr->getP()->getState() - t_world_sensor_.head<2>());
+ expected_feature_(2) = _landmark_ptr->getO()->getState()(0) - t_world_sensor_(2);
expected_feature_(3) = (std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr))->getAperture();
//std::cout << "Expected feature pose: " << expected_feature_.head<3>().transpose() << std::endl;
// ------------ expected feature covariance
Eigen::MatrixXs Sigma = Eigen::MatrixXs::Zero(6, 6);
// closer keyframe with computed covariance
- FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFramePtr() : nullptr);
+ FrameBasePtr key_frame_ptr = (origin_ptr_ != nullptr ? origin_ptr_->getFrame() : nullptr);
// If all covariance blocks are stored wolfproblem (filling upper diagonal only)
if (key_frame_ptr != nullptr &&
// Sigma_rr
- getProblem()->getCovarianceBlock(key_frame_ptr->getPPtr(), key_frame_ptr->getPPtr(), Sigma, 3, 3) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getPPtr(), key_frame_ptr->getOPtr(), Sigma, 3, 5) &&
- getProblem()->getCovarianceBlock(key_frame_ptr->getOPtr(), key_frame_ptr->getOPtr(), Sigma, 5, 5) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getP(), Sigma, 3, 3) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getP(), key_frame_ptr->getO(), Sigma, 3, 5) &&
+ getProblem()->getCovarianceBlock(key_frame_ptr->getO(), key_frame_ptr->getO(), Sigma, 5, 5) &&
// Sigma_ll
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getPPtr(), Sigma, 0, 0) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), _landmark_ptr->getOPtr(), Sigma, 0, 2) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), _landmark_ptr->getOPtr(), Sigma, 2, 2) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getP(), Sigma, 0, 0) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), _landmark_ptr->getO(), Sigma, 0, 2) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), _landmark_ptr->getO(), Sigma, 2, 2) &&
// Sigma_lr
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), key_frame_ptr->getPPtr(), Sigma, 0, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getPPtr(), key_frame_ptr->getOPtr(), Sigma, 0, 5) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), key_frame_ptr->getPPtr(), Sigma, 2, 3) &&
- getProblem()->getCovarianceBlock(_landmark_ptr->getOPtr(), key_frame_ptr->getOPtr(), Sigma, 2, 5) )
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getP(), Sigma, 0, 3) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getP(), key_frame_ptr->getO(), Sigma, 0, 5) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getP(), Sigma, 2, 3) &&
+ getProblem()->getCovarianceBlock(_landmark_ptr->getO(), key_frame_ptr->getO(), Sigma, 2, 5) )
{
// Jacobian
- Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getPPtr()->getState();
+ Eigen::Vector2s p_robot_landmark = t_world_robot_.head<2>() - _landmark_ptr->getP()->getState();
Eigen::Matrix<Scalar, 3, 6> Jlr = Eigen::Matrix<Scalar, 3, 6>::Zero();
Jlr.block<3, 3>(0, 3) = -R_world_sensor_.transpose();
Jlr.block<3, 3>(0, 3) = R_world_sensor_.transpose();
@@ -375,19 +375,19 @@ LandmarkBasePtr ProcessorTrackerLandmarkCorner::createLandmark(FeatureBasePtr _f
_feature_ptr->getMeasurement()(3));
}
-unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkCorner::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
// already computed since each scan is computed in preprocess()
_features_incoming_out = std::move(corners_last_);
return _features_incoming_out.size();
}
-ConstraintBasePtr ProcessorTrackerLandmarkCorner::createConstraint(FeatureBasePtr _feature_ptr,
+FactorBasePtr ProcessorTrackerLandmarkCorner::createFactor(FeatureBasePtr _feature_ptr,
LandmarkBasePtr _landmark_ptr)
{
assert(_feature_ptr != nullptr && _landmark_ptr != nullptr
- && "ProcessorTrackerLandmarkCorner::createConstraint: feature and landmark pointers can not be nullptr!");
- return std::make_shared<ConstraintCorner2D>(_feature_ptr,
+ && "ProcessorTrackerLandmarkCorner::createFactor: feature and landmark pointers can not be nullptr!");
+ return std::make_shared<FactorCorner2D>(_feature_ptr,
std::static_pointer_cast<LandmarkCorner2D>((_landmark_ptr)),
shared_from_this());
}
diff --git a/src/processor/processor_tracker_landmark_dummy.cpp b/src/processor/processor_tracker_landmark_dummy.cpp
index 06c10b37f1f8b9607b773c2c808e101c39dfb412..b2f92d72dd8bdc8d377905977185da6ae776688f 100644
--- a/src/processor/processor_tracker_landmark_dummy.cpp
+++ b/src/processor/processor_tracker_landmark_dummy.cpp
@@ -7,7 +7,7 @@
#include "base/processor/processor_tracker_landmark_dummy.h"
#include "base/landmark/landmark_corner_2D.h"
-#include "base/constraint/constraint_corner_2D.h"
+#include "base/factor/factor_corner_2D.h"
namespace wolf
{
@@ -26,8 +26,8 @@ ProcessorTrackerLandmarkDummy::~ProcessorTrackerLandmarkDummy()
//
}
-unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBaseList& _landmarks_in,
- FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
std::cout << "\tProcessorTrackerLandmarkDummy::findLandmarks" << std::endl;
@@ -63,7 +63,7 @@ bool ProcessorTrackerLandmarkDummy::voteForKeyFrame()
return incoming_ptr_->getFeatureList().size() < 4;
}
-unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
std::cout << "\tProcessorTrackerLandmarkDummy::detectNewFeatures" << std::endl;
@@ -84,12 +84,12 @@ LandmarkBasePtr ProcessorTrackerLandmarkDummy::createLandmark(FeatureBasePtr _fe
return std::make_shared<LandmarkCorner2D>(std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1), _feature_ptr->getMeasurement(0));
}
-ConstraintBasePtr ProcessorTrackerLandmarkDummy::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkDummy::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- std::cout << "\tProcessorTrackerLandmarkDummy::createConstraint" << std::endl;
+ std::cout << "\tProcessorTrackerLandmarkDummy::createFactor" << std::endl;
std::cout << "\t\tfeature " << _feature_ptr->getMeasurement() << std::endl;
std::cout << "\t\tlandmark "<< _landmark_ptr->getDescriptor() << std::endl;
- return std::make_shared<ConstraintCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
+ return std::make_shared<FactorCorner2D>(_feature_ptr, std::static_pointer_cast<LandmarkCorner2D>(_landmark_ptr), shared_from_this());
}
} //namespace wolf
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index b055f1353a95302d17f11824744e6c0df482a7db..a88ec47c932f3c1c7f9098a532285c20940dbeda 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -1,7 +1,7 @@
#include "base/processor/processor_tracker_landmark_image.h"
#include "base/capture/capture_image.h"
-#include "base/constraint/constraint_AHP.h"
+#include "base/factor/factor_AHP.h"
#include "base/feature/feature_base.h"
#include "base/feature/feature_point_image.h"
#include "base/frame_base.h"
@@ -144,8 +144,8 @@ void ProcessorTrackerLandmarkImage::postProcess()
feat_lmk_found_.clear();
}
-unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList& _landmarks_in,
- FeatureBaseList& _features_incoming_out,
+unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ FeatureBasePtrList& _features_incoming_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
KeyPointVector candidate_keypoints;
@@ -159,13 +159,13 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBaseList
// project landmark into incoming capture
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_in_ptr);
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensorPtr());
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensor());
Eigen::Vector4s point3D_hmg;
Eigen::Vector2s pixel;
landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
- pixel = pinhole::projectPoint(camera->getIntrinsicPtr()->getState(),
+ pixel = pinhole::projectPoint(camera->getIntrinsic()->getState(),
camera->getDistortionVector(),
point3D_hmg.head<3>());
@@ -225,7 +225,7 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBaseList& _features_incoming_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -283,7 +283,7 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- FrameBasePtr anchor_frame = getLastPtr()->getFramePtr();
+ FrameBasePtr anchor_frame = getLast()->getFrame();
Eigen::Vector2s point2D;
point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
@@ -292,8 +292,8 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
Scalar distance = params_tracker_landmark_image_->distance; // arbitrary value
Eigen::Vector4s vec_homogeneous;
- point2D = pinhole::depixellizePoint(getSensorPtr()->getIntrinsicPtr()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensorPtr()))->getCorrectionVector(),point2D);
+ point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
+ point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
Eigen::Vector3s point3D;
point3D.head<2>() = point2D;
@@ -303,17 +303,17 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
- LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensorPtr(), feat_point_image_ptr->getDescriptor());
+ LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensor(), feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_ahp_ptr->id());
return lmk_ahp_ptr;
}
-ConstraintBasePtr ProcessorTrackerLandmarkImage::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFramePtr())
+ if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame())
{
- return ConstraintBasePtr();
+ return FactorBasePtr();
}
else
{
@@ -322,9 +322,9 @@ ConstraintBasePtr ProcessorTrackerLandmarkImage::createConstraint(FeatureBasePtr
LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
- ConstraintAHPPtr constraint_ptr = ConstraintAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
+ FactorAHPPtr factor_ptr = FactorAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
- return constraint_ptr;
+ return factor_ptr;
}
}
@@ -365,8 +365,8 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
Transform<Scalar,3,Eigen::Affine> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
// world to anchor robot frame
- Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getPPtr()->getState()); // sadly we cannot put a Map over a translation
- const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getOPtr()->getState()));
+ Translation<Scalar,3> t_w_r0(_landmark->getAnchorFrame()->getP()->getState()); // sadly we cannot put a Map over a translation
+ const Quaternions q_w_r0(Eigen::Vector4s(_landmark->getAnchorFrame()->getO()->getState()));
T_W_R0 = t_w_r0 * q_w_r0;
// world to current robot frame
@@ -375,17 +375,17 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
T_W_R1 = t_w_r1 * q_w_r1;
// anchor robot to anchor camera
- Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getPPtr()->getState());
- const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getOPtr()->getState()));
+ Translation<Scalar,3> t_r0_c0(_landmark->getAnchorSensor()->getP()->getState());
+ const Quaternions q_r0_c0(Eigen::Vector4s(_landmark->getAnchorSensor()->getO()->getState()));
T_R0_C0 = t_r0_c0 * q_r0_c0;
// current robot to current camera
- Translation<Scalar,3> t_r1_c1(this->getSensorPtr()->getPPtr()->getState());
- const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensorPtr()->getOPtr()->getState()));
+ Translation<Scalar,3> t_r1_c1(this->getSensor()->getP()->getState());
+ const Quaternions q_r1_c1(Eigen::Vector4s(this->getSensor()->getO()->getState()));
T_R1_C1 = t_r1_c1 * q_r1_c1;
// Transform lmk from c0 to c1 and exit
- Vector4s landmark_hmg_c0 = _landmark->getPPtr()->getState(); // lmk in anchor frame
+ Vector4s landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
_point3D_hmg = T_R1_C1.inverse(Eigen::Affine) * T_W_R1.inverse(Eigen::Affine) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
}
@@ -447,17 +447,17 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
{
unsigned int num_lmks_in_img = 0;
- Eigen::VectorXs current_state = last_ptr_->getFramePtr()->getState();
- SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensorPtr());
+ Eigen::VectorXs current_state = last_ptr_->getFrame()->getState();
+ SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
- for (auto landmark_base_ptr : getProblem()->getMapPtr()->getLandmarkList())
+ for (auto landmark_base_ptr : getProblem()->getMap()->getLandmarkList())
{
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_base_ptr);
Eigen::Vector4s point3D_hmg;
landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
- Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsicPtr()->getState(), // k
+ Eigen::Vector2s point2D = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
camera->getDistortionVector(), // d
point3D_hmg.head(3)); // v
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
index 70efbcde6d4ecee513151c81ebadb3b7b144dc03..c3f91c0278eda1a7a73657f4d18d40710c59f999 100644
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ b/src/processor/processor_tracker_landmark_polyline.cpp
@@ -25,11 +25,11 @@ void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _
Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensorPtr()->extrinsicsInCaptures() || !getSensorPtr()->getPPtr()->isFixed()
- || !getSensorPtr()->getOPtr()->isFixed() || !extrinsics_transformation_computed_)
+ if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
+ || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
{
- t_robot_sensor_ = getSensorPtr()->getPPtr()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensorPtr()->getOPtr()->getState()(0)).matrix();
+ t_robot_sensor_ = getSensor()->getP()->getState();
+ R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
extrinsics_transformation_computed_ = true;
}
@@ -51,8 +51,8 @@ void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _
}
-unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBaseList& _landmarks_searched,
- FeatureBaseList& _features_found,
+unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
+ FeatureBasePtrList& _features_found,
LandmarkMatchMap& _feature_landmark_correspondences)
{
//std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl;
@@ -304,7 +304,7 @@ bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
// option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
for (auto new_ftr : new_features_last_)
{
- if (last_ptr_->getFramePtr()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapturePtr()->getFramePtr()->id() > params_->loop_frames_th)
+ if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
{
std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
return true;
@@ -314,13 +314,13 @@ bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
}
void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBaseList& _polyline_list)
+ FeatureBasePtrList& _polyline_list)
{
//std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl;
// TODO: sort corners by bearing
std::list<laserscanutils::Polyline> polylines;
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines);
+ line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
for (auto&& pl : polylines)
{
@@ -368,8 +368,8 @@ void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark
////////// landmark with origin
else
{
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getOPtr()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = polyline_landmark->getPPtr()->getState();
+ Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
+ const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
{
@@ -535,11 +535,11 @@ ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()
}
}
-void ProcessorTrackerLandmarkPolyline::establishConstraints()
+void ProcessorTrackerLandmarkPolyline::establishFactors()
{
//TODO: update with new index in landmarks
- //std::cout << "ProcessorTrackerLandmarkPolyline::establishConstraints" << std::endl;
+ //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl;
LandmarkPolylineMatchPtr polyline_match;
FeaturePolyline2DPtr polyline_feature;
LandmarkPolyline2DPtr polyline_landmark;
@@ -792,7 +792,7 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
//std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
//std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
- // Constraints for all inner and defined feature points
+ // Factors for all inner and defined feature points
int lmk_point_id = polyline_match->landmark_match_from_id_;
for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++)
@@ -807,48 +807,48 @@ void ProcessorTrackerLandmarkPolyline::establishConstraints()
// First not defined point
if (ftr_point_id == 0 && !polyline_feature->isFirstDefined())
- // first point to line constraint
+ // first point to line factor
{
int lmk_next_point_id = lmk_point_id+1;
if (lmk_next_point_id > polyline_landmark->getLastId())
lmk_next_point_id -= polyline_landmark->getNPoints();
//std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
- last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
+ last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
ftr_point_id, lmk_point_id, lmk_next_point_id));
- //std::cout << "constraint added" << std::endl;
+ //std::cout << "factor added" << std::endl;
}
// Last not defined point
else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined())
- // last point to line constraint
+ // last point to line factor
{
int lmk_prev_point_id = lmk_point_id-1;
if (lmk_prev_point_id < polyline_landmark->getFirstId())
lmk_prev_point_id += polyline_landmark->getNPoints();
//std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- last_feature->addConstraint(std::make_shared<ConstraintPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
+ last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
ftr_point_id, lmk_point_id, lmk_prev_point_id));
- //std::cout << "constraint added" << std::endl;
+ //std::cout << "factor added" << std::endl;
}
// Defined point
else
- // point to point constraint
+ // point to point factor
{
//std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
//std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
//std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
//std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
- last_feature->addConstraint(std::make_shared<ConstraintPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
+ last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
ftr_point_id, lmk_point_id));
- //std::cout << "constraint added" << std::endl;
+ //std::cout << "factor added" << std::endl;
}
}
- //std::cout << "Constraints established" << std::endl;
+ //std::cout << "Factors established" << std::endl;
}
}
-void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_list)
+void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list)
{
//std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl;
std::vector<Scalar> object_L({12, 5.9, 1.2});
@@ -951,39 +951,39 @@ void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBaseList& _lmk_
polyline_ptr->classify(object_class[classification]);
// Unfix origin
- polyline_ptr->getPPtr()->unfix();
- polyline_ptr->getOPtr()->unfix();
+ polyline_ptr->getP()->unfix();
+ polyline_ptr->getO()->unfix();
// Move origin to B
- polyline_ptr->getPPtr()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
+ polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
- polyline_ptr->getOPtr()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
+ polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
//std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
//std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
//std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
//std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << polyline_ptr->getPPtr()->getVector().transpose() << std::endl;
- //std::cout << "frame orientation: " << polyline_ptr->getOPtr()->getVector() << std::endl;
+ //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl;
+ //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
// Fix polyline points to its respective relative positions
if (configuration)
{
- polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
+ polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
+ polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, 0));
+ polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
+ polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
}
else
{
- polyline_ptr->getPointStateBlockPtr(A_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlockPtr(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- polyline_ptr->getPointStateBlockPtr(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
+ polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(0, 0));
+ polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
+ polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
+ polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
}
for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
{
- polyline_ptr->getPointStateBlockPtr(id)->fix();
+ polyline_ptr->getPointStateBlock(id)->fix();
}
}
}
@@ -994,10 +994,10 @@ void ProcessorTrackerLandmarkPolyline::postProcess()
//std::cout << "postProcess: " << std::endl;
//std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
//std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
- classifyPolilines(getProblem()->getMapPtr()->getLandmarkList());
+ classifyPolilines(getProblem()->getMap()->getLandmarkList());
}
-ConstraintBasePtr ProcessorTrackerLandmarkPolyline::createConstraint(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
// not used
return nullptr;
diff --git a/src/sensor/sensor_GPS.cpp b/src/sensor/sensor_GPS.cpp
index 7745e65b1124834e266916846109e3cf96ad621a..323616be956041905389ad5b083c85c11af3707e 100644
--- a/src/sensor/sensor_GPS.cpp
+++ b/src/sensor/sensor_GPS.cpp
@@ -24,12 +24,12 @@ SensorGPS::~SensorGPS()
//
}
-StateBlockPtr SensorGPS::getMapPPtr() const
+StateBlockPtr SensorGPS::getMapP() const
{
return getStateBlockPtrStatic(3);
}
-StateBlockPtr SensorGPS::getMapOPtr() const
+StateBlockPtr SensorGPS::getMapO() const
{
return getStateBlockPtrStatic(4);
}
diff --git a/src/sensor/sensor_GPS_fix.cpp b/src/sensor/sensor_GPS_fix.cpp
index 427dc45182c9c5efa9319a0e56ca9bebf21922eb..c69f99b6a2254b277dc250fed85b3f2105159db4 100644
--- a/src/sensor/sensor_GPS_fix.cpp
+++ b/src/sensor/sensor_GPS_fix.cpp
@@ -29,7 +29,7 @@ SensorBasePtr SensorGPSFix::create(const std::string& _unique_name, const Eigen:
assert((_extrinsics.size() == 2 || _extrinsics.size() == 3)
&& "Bad extrinsic vector size. Should be 2 for 2D, 3 for 3D.");
SensorGPSFixPtr sen = std::make_shared<SensorGPSFix>(_extrinsics, std::static_pointer_cast<IntrinsicsGPSFix>(_intrinsics));
- sen->getPPtr()->fix();
+ sen->getP()->fix();
sen->setName(_unique_name);
return sen;
}
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 92616d53373d40c081638ddf0433a83692d575ae..a30f777fc109da531c0d56237a650ccad0090bf5 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -1,8 +1,8 @@
#include "base/sensor/sensor_base.h"
#include "base/state_block.h"
#include "base/state_quaternion.h"
-#include "base/constraint/constraint_block_absolute.h"
-#include "base/constraint/constraint_quaternion_absolute.h"
+#include "base/factor/factor_block_absolute.h"
+#include "base/factor/factor_quaternion_absolute.h"
namespace wolf {
@@ -176,11 +176,11 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
// set feature problem
ftr_prior->setProblem(getProblem());
- // create & add constraint absolute
+ // create & add factor absolute
if (is_quaternion)
- ftr_prior->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(_sb));
+ ftr_prior->addFactor(std::make_shared<FactorQuaternionAbsolute>(_sb));
else
- ftr_prior->addConstraint(std::make_shared<ConstraintBlockAbsolute>(_sb, _start_idx, _size));
+ ftr_prior->addFactor(std::make_shared<FactorBlockAbsolute>(_sb, _start_idx, _size));
// store feature in params_prior_map_
params_prior_map_[_i] = ftr_prior;
@@ -222,7 +222,7 @@ CaptureBasePtr SensorBase::lastKeyCapture(void)
{
// we search for the most recent Capture of this sensor which belongs to a KeyFrame
CaptureBasePtr capture = nullptr;
- FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
FrameBaseRevIter frame_rev_it = frame_list.rbegin();
while (frame_rev_it != frame_list.rend())
{
@@ -242,7 +242,7 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
{
// we search for the most recent Capture of this sensor before _ts
CaptureBasePtr capture = nullptr;
- FrameBaseList frame_list = getProblem()->getTrajectoryPtr()->getFrameList();
+ FrameBasePtrList frame_list = getProblem()->getTrajectory()->getFrameList();
// We iterate in reverse since we're likely to find it close to the rbegin() place.
FrameBaseRevIter frame_rev_it = frame_list.rbegin();
@@ -260,34 +260,34 @@ CaptureBasePtr SensorBase::lastCapture(const TimeStamp& _ts)
return capture;
}
-StateBlockPtr SensorBase::getPPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getP(const TimeStamp _ts)
{
- return getStateBlockPtr(0, _ts);
+ return getStateBlock(0, _ts);
}
-StateBlockPtr SensorBase::getOPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getO(const TimeStamp _ts)
{
- return getStateBlockPtr(1, _ts);
+ return getStateBlock(1, _ts);
}
-StateBlockPtr SensorBase::getIntrinsicPtr(const TimeStamp _ts)
+StateBlockPtr SensorBase::getIntrinsic(const TimeStamp _ts)
{
- return getStateBlockPtr(2, _ts);
+ return getStateBlock(2, _ts);
}
-StateBlockPtr SensorBase::getPPtr()
+StateBlockPtr SensorBase::getP()
{
- return getStateBlockPtr(0);
+ return getStateBlock(0);
}
-StateBlockPtr SensorBase::getOPtr()
+StateBlockPtr SensorBase::getO()
{
- return getStateBlockPtr(1);
+ return getStateBlock(1);
}
-StateBlockPtr SensorBase::getIntrinsicPtr()
+StateBlockPtr SensorBase::getIntrinsic()
{
- return getStateBlockPtr(2);
+ return getStateBlock(2);
}
SizeEigen SensorBase::computeCalibSize() const
@@ -321,7 +321,7 @@ Eigen::VectorXs SensorBase::getCalibration() const
bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
- capture_ptr->setSensorPtr(shared_from_this());
+ capture_ptr->setSensor(shared_from_this());
for (const auto processor : processor_list_)
{
@@ -334,27 +334,27 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
ProcessorBasePtr SensorBase::addProcessor(ProcessorBasePtr _proc_ptr)
{
processor_list_.push_back(_proc_ptr);
- _proc_ptr->setSensorPtr(shared_from_this());
+ _proc_ptr->setSensor(shared_from_this());
_proc_ptr->setProblem(getProblem());
return _proc_ptr;
}
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i)
{
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, cap))
- return cap->getStateBlockPtr(_i);
+ return cap->getStateBlock(_i);
return getStateBlockPtrStatic(_i);
}
-StateBlockPtr SensorBase::getStateBlockPtr(unsigned int _i, const TimeStamp& _ts)
+StateBlockPtr SensorBase::getStateBlock(unsigned int _i, const TimeStamp& _ts)
{
CaptureBasePtr cap;
if (isStateBlockDynamic(_i, _ts, cap))
- return cap->getStateBlockPtr(_i);
+ return cap->getStateBlock(_i);
return getStateBlockPtrStatic(_i);
}
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 6e20ff960177e9be75bef2dfc4342cdb96b23bc8..54c08c69f3a263b6d71a1b461baa393e62db0d10 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -19,7 +19,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, const IntrinsicsC
{
assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
useRawImages();
- pinhole::computeCorrectionModel(getIntrinsicPtr()->getState(), distortion_, correction_);
+ pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_);
}
SensorCamera::SensorCamera(const Eigen::VectorXs& _extrinsics, IntrinsicsCameraPtr _intrinsics_ptr) :
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index d710d9e77a0794674b873366233e8841a68f7b76..d5fdb9e5ee702a8789307eb875e8d026a9d608b3 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -62,7 +62,7 @@ SensorBasePtr SensorDiffDrive::create(const std::string& _unique_name,
// return intrinsics_;
//}
-//void SensorDiffDrive::initIntrisicsPtr()
+//void SensorDiffDrive::initIntrisics()
//{
// assert(intrinsic_ptr_ == nullptr &&
// "SensorDiffDrive::initIntrisicsPtr should only be called once at construction.");
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index f8541646827009f9209e2e4dc2ef4feadbccdead..17f6e562bb242ba315369effd722391f52f7cf4c 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -14,16 +14,16 @@ SolverManager::SolverManager(const ProblemPtr& _wolf_problem) :
void SolverManager::update()
{
// REMOVE CONSTRAINTS
- auto ctr_notification_it = wolf_problem_->getConstraintNotificationMap().begin();
- while ( ctr_notification_it != wolf_problem_->getConstraintNotificationMap().end() )
+ auto fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
+ while ( fac_notification_it != wolf_problem_->getFactorNotificationMap().end() )
{
- if (ctr_notification_it->second == REMOVE)
+ if (fac_notification_it->second == REMOVE)
{
- removeConstraint(ctr_notification_it->first);
- ctr_notification_it = wolf_problem_->getConstraintNotificationMap().erase(ctr_notification_it);
+ removeFactor(fac_notification_it->first);
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
}
else
- ctr_notification_it++;
+ fac_notification_it++;
}
// ADD/REMOVE STATE BLOCS
@@ -67,17 +67,17 @@ void SolverManager::update()
}
// ADD CONSTRAINTS
- ctr_notification_it = wolf_problem_->getConstraintNotificationMap().begin();
- while (ctr_notification_it != wolf_problem_->getConstraintNotificationMap().end())
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().begin();
+ while (fac_notification_it != wolf_problem_->getFactorNotificationMap().end())
{
- assert(wolf_problem_->getConstraintNotificationMap().begin()->second == ADD && "unexpected constraint notification value after all REMOVE have been processed, this should be ADD");
+ assert(wolf_problem_->getFactorNotificationMap().begin()->second == ADD && "unexpected factor notification value after all REMOVE have been processed, this should be ADD");
- addConstraint(wolf_problem_->getConstraintNotificationMap().begin()->first);
- ctr_notification_it = wolf_problem_->getConstraintNotificationMap().erase(ctr_notification_it);
+ addFactor(wolf_problem_->getFactorNotificationMap().begin()->first);
+ fac_notification_it = wolf_problem_->getFactorNotificationMap().erase(fac_notification_it);
}
// UPDATE STATE BLOCKS (state, fix or local parameterization)
- for (auto state_ptr : wolf_problem_->getStateBlockList())
+ for (auto state_ptr : wolf_problem_->getStateBlockPtrList())
{
assert(state_blocks_.find(state_ptr)!=state_blocks_.end() && "Updating the state of an unregistered StateBlock !");
@@ -106,11 +106,11 @@ void SolverManager::update()
}
}
- assert(wolf_problem_->getConstraintNotificationMap().empty() && "wolf problem's constraints notification map not empty after update");
+ assert(wolf_problem_->getFactorNotificationMap().empty() && "wolf problem's factors notification map not empty after update");
assert(wolf_problem_->getStateBlockNotificationMap().empty() && "wolf problem's state_blocks notification map not empty after update");
}
-wolf::ProblemPtr SolverManager::getProblemPtr()
+wolf::ProblemPtr SolverManager::getProblem()
{
return wolf_problem_;
}
@@ -124,7 +124,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
// update StateBlocks with optimized state value.
/// @todo whatif someone has changed the state notification during opti ??
- /// JV: I do not see a problem here, the solver provides the optimal state given the constraints, if someone changed the state during optimization, it will be overwritten by the optimal one.
+ /// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
it_end = state_blocks_.end();
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index a25c72beffa514c822c5275ff61ed8bf294ec4dc..2ba7af7081a64abec0149777e790c6fc355b6c49 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -48,28 +48,28 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
//std::cout << "state units removed!" << std::endl;
// ADD CONSTRAINTS
- ConstraintBaseList ctr_list;
- _problem_ptr->getTrajectoryPtr()->getConstraintList(ctr_list);
- //std::cout << "ctr_list.size() = " << ctr_list.size() << std::endl;
- for(auto ctr_it = ctr_list.begin(); ctr_it!=ctr_list.end(); ctr_it++)
- if ((*ctr_it)->getPendingStatus() == ADD_PENDING)
- addConstraint(*ctr_it);
-
- //std::cout << "constraints updated!" << std::endl;
+ FactorBasePtrList fac_list;
+ _problem_ptr->getTrajectory()->getFactorList(fac_list);
+ //std::cout << "fac_list.size() = " << fac_list.size() << std::endl;
+ for(auto fac_it = fac_list.begin(); fac_it!=fac_list.end(); fac_it++)
+ if ((*fac_it)->getPendingStatus() == ADD_PENDING)
+ addFactor(*fac_it);
+
+ //std::cout << "factors updated!" << std::endl;
}
}
-void SolverManager::addConstraint(ConstraintBasePtr _corr_ptr)
+void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
//TODO MatrixXs J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
- // solverQR->addConstraint(_corr_ptr, J, e);
+ // solverQR->addFactor(_corr_ptr, J, e);
-// constraint_map_[_corr_ptr->id()] = blockIdx;
+// factor_map_[_corr_ptr->id()] = blockIdx;
_corr_ptr->setPendingStatus(NOT_PENDING);
}
-void SolverManager::removeConstraint(const unsigned int& _corr_idx)
+void SolverManager::removeFactor(const unsigned int& _corr_idx)
{
// TODO
}
@@ -85,31 +85,31 @@ void SolverManager::addStateUnit(StateBlockPtr _st_ptr)
{
// TODO
//std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl;
- //ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
+ //ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization);
break;
}
case ST_THETA:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_1D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_VECTOR:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
case ST_POINT_3D:
{
//std::cout << "No Local Parametrization to be added" << std::endl;
- ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
+ ceres_problem_->AddParameterBlock(_st_ptr->get(), ((StateBlockPtr)_st_ptr)->BLOCK_SIZE, nullptr);
break;
}
default:
@@ -136,26 +136,26 @@ void SolverManager::updateStateUnitStatus(StateBlockPtr _st_ptr)
// TODO
// if (!_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockVariable(_st_ptr->getPtr());
+// ceres_problem_->SetParameterBlockVariable(_st_ptr->get());
// else if (_st_ptr->isFixed())
-// ceres_problem_->SetParameterBlockConstant(_st_ptr->getPtr());
+// ceres_problem_->SetParameterBlockConstant(_st_ptr->get());
// else
// printf("\nERROR: Update state unit status with unknown status");
//
// _st_ptr->setPendingStatus(NOT_PENDING);
}
-ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPtr)
+ceres::CostFunction* SolverManager::createCostFunction(FactorBasePtr _corrPtr)
{
//std::cout << "adding ctr " << _corrPtr->id() << std::endl;
//_corrPtr->print();
- switch (_corrPtr->getConstraintType())
+ switch (_corrPtr->getFactorType())
{
- case CTR_GPS_FIX_2D:
+ case FAC_GPS_FIX_2D:
{
- ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintGPS2D,
+ FactorGPS2D* specific_ptr = (FactorGPS2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorGPS2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -169,10 +169,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPt
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D_COMPLEX_ANGLE:
+ case FAC_ODOM_2D_COMPLEX_ANGLE:
{
- ConstraintOdom2DComplexAngle* specific_ptr = (ConstraintOdom2DComplexAngle*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2DComplexAngle,
+ FactorOdom2DComplexAngle* specific_ptr = (FactorOdom2DComplexAngle*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2DComplexAngle,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -186,10 +186,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPt
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_ODOM_2D:
+ case FAC_ODOM_2D:
{
- ConstraintOdom2D* specific_ptr = (ConstraintOdom2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintOdom2D,
+ FactorOdom2D* specific_ptr = (FactorOdom2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorOdom2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -203,10 +203,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPt
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_CORNER_2D:
+ case FAC_CORNER_2D:
{
- ConstraintCorner2D* specific_ptr = (ConstraintCorner2D*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintCorner2D,
+ FactorCorner2D* specific_ptr = (FactorCorner2D*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorCorner2D,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -220,10 +220,10 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPt
specific_ptr->block9Size>(specific_ptr);
break;
}
- case CTR_IMU:
+ case FAC_IMU:
{
- ConstraintIMU* specific_ptr = (ConstraintIMU*)(_corrPtr);
- return new ceres::AutoDiffCostFunction<ConstraintIMU,
+ FactorIMU* specific_ptr = (FactorIMU*)(_corrPtr);
+ return new ceres::AutoDiffCostFunction<FactorIMU,
specific_ptr->residualSize,
specific_ptr->block0Size,
specific_ptr->block1Size,
@@ -238,7 +238,7 @@ ceres::CostFunction* SolverManager::createCostFunction(ConstraintBasePtr _corrPt
break;
}
default:
- std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
+ std::cout << "Unknown factor type! Please add it in the CeresWrapper::createCostFunction()" << std::endl;
return nullptr;
}
diff --git a/src/state_block.cpp b/src/state_block.cpp
index 46411e7c30aefb15f921bb9f9fa9c7a114a6154b..b4427c2aae025a794ae433bcb723011f1110f499 100644
--- a/src/state_block.cpp
+++ b/src/state_block.cpp
@@ -33,7 +33,7 @@ void StateBlock::setFixed(bool _fixed)
//
//void StateBlock::removeFromProblem(ProblemPtr _problem_ptr)
//{
-// _problem_ptr->removeStateBlockPtr(shared_from_this());
+// _problem_ptr->removeStateBlock(shared_from_this());
//}
}
diff --git a/src/track_matrix.cpp b/src/track_matrix.cpp
index 8bb0e0abe4213f51f3a6ff4de7a5da0a4cd7602f..50ab808912128f28279b9d0ad39b8580f251faca 100644
--- a/src/track_matrix.cpp
+++ b/src/track_matrix.cpp
@@ -49,8 +49,8 @@ void TrackMatrix::add(size_t _track_id, CaptureBasePtr _cap, FeatureBasePtr _ftr
assert( (_track_id > 0) && (_track_id <= track_id_count_) && "Provided track ID does not exist. Use newTrack() instead.");
_ftr->setTrackId(_track_id);
- if (_cap != _ftr->getCapturePtr())
- _ftr->setCapturePtr(_cap);
+ if (_cap != _ftr->getCapture())
+ _ftr->setCapture(_cap);
tracks_[_track_id].emplace(_cap->getTimeStamp(), _ftr);
snapshots_[_cap->id()].emplace(_track_id, _ftr); // will create new snapshot if _cap_id is not present
}
@@ -62,7 +62,7 @@ void TrackMatrix::remove(size_t _track_id)
{
for (auto const& pair_time_ftr : tracks_.at(_track_id))
{
- SizeStd cap_id = pair_time_ftr.second->getCapturePtr()->id();
+ SizeStd cap_id = pair_time_ftr.second->getCapture()->id();
snapshots_.at(cap_id).erase(_track_id);
if (snapshots_.at(cap_id).empty())
snapshots_.erase(cap_id);
@@ -94,10 +94,10 @@ void TrackMatrix::remove(CaptureBasePtr _cap)
void TrackMatrix::remove(FeatureBasePtr _ftr)
{
- // assumes _ftr->getCapturePtr() and _ftr->trackId() are valid
+ // assumes _ftr->getCapture() and _ftr->trackId() are valid
if (_ftr)
{
- if (auto cap = _ftr->getCapturePtr())
+ if (auto cap = _ftr->getCapture())
{
tracks_ .at(_ftr->trackId()).erase(cap->getTimeStamp());
if (tracks_.at(_ftr->trackId()).empty())
@@ -191,7 +191,7 @@ FeatureBasePtr TrackMatrix::feature(size_t _track_id, CaptureBasePtr _cap)
CaptureBasePtr TrackMatrix::firstCapture(size_t _track_id)
{
- return firstFeature(_track_id)->getCapturePtr();
+ return firstFeature(_track_id)->getCapture();
}
}
diff --git a/src/trajectory_base.cpp b/src/trajectory_base.cpp
index 557d072fd1fe1cbba36f69ed58c87c67b9185da5..5820b99ba5df29f281ee2e4e61c82368a93d3561 100644
--- a/src/trajectory_base.cpp
+++ b/src/trajectory_base.cpp
@@ -19,7 +19,7 @@ TrajectoryBase::~TrajectoryBase()
FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
{
// link up
- _frame_ptr->setTrajectoryPtr(shared_from_this());
+ _frame_ptr->setTrajectory(shared_from_this());
_frame_ptr->setProblem(getProblem());
if (_frame_ptr->isKey())
@@ -38,16 +38,16 @@ FrameBasePtr TrajectoryBase::addFrame(FrameBasePtr _frame_ptr)
return _frame_ptr;
}
-void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
+void TrajectoryBase::getFactorList(FactorBasePtrList & _fac_list)
{
for(auto fr_ptr : getFrameList())
- fr_ptr->getConstraintList(_ctr_list);
+ fr_ptr->getFactorList(_fac_list);
}
void TrajectoryBase::sortFrame(FrameBasePtr _frame_ptr)
{
moveFrame(_frame_ptr, computeFrameOrder(_frame_ptr));
- // last_key_frame_ptr_ = findLastKeyFramePtr(); // done in moveFrame() just above
+ // last_key_frame_ptr_ = findLastKeyFrame(); // done in moveFrame() just above
}
void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
@@ -56,7 +56,7 @@ void TrajectoryBase::moveFrame(FrameBasePtr _frm_ptr, FrameBaseIter _place)
{
frame_list_.remove(_frm_ptr);
frame_list_.insert(_place, _frm_ptr);
- last_key_frame_ptr_ = findLastKeyFramePtr();
+ last_key_frame_ptr_ = findLastKeyFrame();
}
}
@@ -68,7 +68,7 @@ FrameBaseIter TrajectoryBase::computeFrameOrder(FrameBasePtr _frame_ptr)
return getFrameList().begin();
}
-FrameBasePtr TrajectoryBase::findLastKeyFramePtr()
+FrameBasePtr TrajectoryBase::findLastKeyFrame()
{
// NOTE: Assumes keyframes are sorted by timestamp
for (auto frm_rit = getFrameList().rbegin(); frm_rit != getFrameList().rend(); ++frm_rit)
diff --git a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
index 2337df07c51f3dd1027d5e1b6004ca426221b722..a3f9362b5a91d32d793a6459e409c2849f151bc4 100644
--- a/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
+++ b/src/yaml/processor_tracker_feature_trifocal_yaml.cpp
@@ -52,7 +52,7 @@ static ProcessorParamsBasePtr createProcessorParamsTrackerFeatureTrifocal(const
params->n_cells_v = algorithm["grid vert cells"] .as<int>();
params->min_response_new_feature = algorithm["min response new features"] .as<int>();
params->max_euclidean_distance = algorithm["max euclidean distance"] .as<Scalar>();
- params->min_track_length_for_constraint = algorithm["min track length for constraint"].as<int>();
+ params->min_track_length_for_factor = algorithm["min track length for factor"].as<int>();
YAML::Node noise = config["noise"];
params->pixel_noise_std = noise ["pixel noise std"].as<Scalar>();
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 47e53807a4afa37b9fe04e6cb80fb702ad007986..9c46f4d94400f5e8651eb73b53f4e5261cac8564 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -45,12 +45,12 @@ wolf_add_gtest(gtest_capture_base gtest_capture_base.cpp)
target_link_libraries(gtest_capture_base ${PROJECT_NAME})
# CaptureBase class test
-#wolf_add_gtest(gtest_constraint_sparse gtest_constraint_sparse.cpp)
-#target_link_libraries(gtest_constraint_sparse ${PROJECT_NAME})
+#wolf_add_gtest(gtest_factor_sparse gtest_factor_sparse.cpp)
+#target_link_libraries(gtest_factor_sparse ${PROJECT_NAME})
-# ConstraintAutodiff class test
-wolf_add_gtest(gtest_constraint_autodiff gtest_constraint_autodiff.cpp)
-target_link_libraries(gtest_constraint_autodiff ${PROJECT_NAME})
+# FactorAutodiff class test
+wolf_add_gtest(gtest_factor_autodiff gtest_factor_autodiff.cpp)
+target_link_libraries(gtest_factor_autodiff ${PROJECT_NAME})
# FeatureBase classes test
wolf_add_gtest(gtest_eigen_predicates gtest_eigen_predicates.cpp)
@@ -130,31 +130,31 @@ wolf_add_gtest(gtest_ceres_manager gtest_ceres_manager.cpp)
target_link_libraries(gtest_ceres_manager ${PROJECT_NAME})
ENDIF(Ceres_FOUND)
-# ConstraintAbs(P/O/V) classes test
-wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp)
-target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME})
+# FactorAbs(P/O/V) classes test
+wolf_add_gtest(gtest_factor_absolute gtest_factor_absolute.cpp)
+target_link_libraries(gtest_factor_absolute ${PROJECT_NAME})
-# ConstraintAutodiffDistance3D test
-wolf_add_gtest(gtest_constraint_autodiff_distance_3D gtest_constraint_autodiff_distance_3D.cpp)
-target_link_libraries(gtest_constraint_autodiff_distance_3D ${PROJECT_NAME})
+# FactorAutodiffDistance3D test
+wolf_add_gtest(gtest_factor_autodiff_distance_3D gtest_factor_autodiff_distance_3D.cpp)
+target_link_libraries(gtest_factor_autodiff_distance_3D ${PROJECT_NAME})
IF(vision_utils_FOUND)
-# ConstraintAutodiffTrifocal test
-wolf_add_gtest(gtest_constraint_autodiff_trifocal gtest_constraint_autodiff_trifocal.cpp)
-target_link_libraries(gtest_constraint_autodiff_trifocal ${PROJECT_NAME})
+# FactorAutodiffTrifocal test
+wolf_add_gtest(gtest_factor_autodiff_trifocal gtest_factor_autodiff_trifocal.cpp)
+target_link_libraries(gtest_factor_autodiff_trifocal ${PROJECT_NAME})
ENDIF(vision_utils_FOUND)
-# ConstraintOdom3D class test
-wolf_add_gtest(gtest_constraint_odom_3D gtest_constraint_odom_3D.cpp)
-target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
+# FactorOdom3D class test
+wolf_add_gtest(gtest_factor_odom_3D gtest_factor_odom_3D.cpp)
+target_link_libraries(gtest_factor_odom_3D ${PROJECT_NAME})
-# ConstraintPose2D class test
-wolf_add_gtest(gtest_constraint_pose_2D gtest_constraint_pose_2D.cpp)
-target_link_libraries(gtest_constraint_pose_2D ${PROJECT_NAME})
+# FactorPose2D class test
+wolf_add_gtest(gtest_factor_pose_2D gtest_factor_pose_2D.cpp)
+target_link_libraries(gtest_factor_pose_2D ${PROJECT_NAME})
-# ConstraintPose3D class test
-wolf_add_gtest(gtest_constraint_pose_3D gtest_constraint_pose_3D.cpp)
-target_link_libraries(gtest_constraint_pose_3D ${PROJECT_NAME})
+# FactorPose3D class test
+wolf_add_gtest(gtest_factor_pose_3D gtest_factor_pose_3D.cpp)
+target_link_libraries(gtest_factor_pose_3D ${PROJECT_NAME})
# FeatureIMU test
wolf_add_gtest(gtest_feature_IMU gtest_feature_IMU.cpp)
diff --git a/test/gtest_IMU.cpp b/test/gtest_IMU.cpp
index e9261f75b76930b75cdac6b0f02eb0d11a052413..7487eb2fe49ada12c03120ce83810abf3d0d8943 100644
--- a/test/gtest_IMU.cpp
+++ b/test/gtest_IMU.cpp
@@ -24,7 +24,7 @@ using std::make_shared;
using std::static_pointer_cast;
using std::string;
-class Process_Constraint_IMU : public testing::Test
+class Process_Factor_IMU : public testing::Test
{
public:
// Wolf objects
@@ -63,9 +63,9 @@ class Process_Constraint_IMU : public testing::Test
VectorXs D_corrected_imu, x1_corrected_imu; // corrected with imu_tools
VectorXs D_preint, x1_preint; // preintegrated with processor_imu
VectorXs D_corrected, x1_corrected; // corrected with processor_imu
- VectorXs D_optim, x1_optim; // optimized using constraint_imu
+ VectorXs D_optim, x1_optim; // optimized using factor_imu
VectorXs D_optim_imu, x1_optim_imu; // corrected with imu_tools using optimized bias
- VectorXs x0_optim; // optimized using constraint_imu
+ VectorXs x0_optim; // optimized using factor_imu
// Trajectory buffer of correction Jacobians
std::vector<MatrixXs> Buf_Jac_preint_prc;
@@ -276,8 +276,8 @@ class Process_Constraint_IMU : public testing::Test
sensor_imu->process(capture_imu);
- D_preint = processor_imu->getLastPtr()->getDeltaPreint();
- D_corrected = processor_imu->getLastPtr()->getDeltaCorrected(bias_real);
+ D_preint = processor_imu->getLast()->getDeltaPreint();
+ D_corrected = processor_imu->getLast()->getDeltaCorrected(bias_real);
}
return processor_imu->getBuffer();
}
@@ -311,9 +311,9 @@ class Process_Constraint_IMU : public testing::Test
// wolf objects
KF_0 = problem->setPrior(x0, P0, t0, dt/2);
- C_0 = processor_imu->getOriginPtr();
+ C_0 = processor_imu->getOrigin();
- processor_imu->getLastPtr()->setCalibrationPreint(bias_preint);
+ processor_imu->getLast()->setCalibrationPreint(bias_preint);
return true;
}
@@ -441,12 +441,12 @@ class Process_Constraint_IMU : public testing::Test
void setFixedBlocks()
{
// this sets each state block status fixed / unfixed
- KF_0->getPPtr()->setFixed(p0_fixed);
- KF_0->getOPtr()->setFixed(q0_fixed);
- KF_0->getVPtr()->setFixed(v0_fixed);
- KF_1->getPPtr()->setFixed(p1_fixed);
- KF_1->getOPtr()->setFixed(q1_fixed);
- KF_1->getVPtr()->setFixed(v1_fixed);
+ KF_0->getP()->setFixed(p0_fixed);
+ KF_0->getO()->setFixed(q0_fixed);
+ KF_0->getV()->setFixed(v0_fixed);
+ KF_1->getP()->setFixed(p1_fixed);
+ KF_1->getO()->setFixed(q1_fixed);
+ KF_1->getV()->setFixed(v1_fixed);
}
void setKfStates()
@@ -490,7 +490,7 @@ class Process_Constraint_IMU : public testing::Test
capture_imu = make_shared<CaptureIMU>(t+dt, sensor_imu, data, sensor_imu->getNoiseCov());
processor_imu->process(capture_imu);
- KF_1 = problem->getLastKeyFramePtr();
+ KF_1 = problem->getLastKeyFrame();
C_1 = KF_1->getCaptureList().front(); // front is IMU
CM_1 = static_pointer_cast<CaptureMotion>(C_1);
@@ -583,7 +583,7 @@ class Process_Constraint_IMU : public testing::Test
};
-class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
+class Process_Factor_IMU_ODO : public Process_Factor_IMU
{
public:
// Wolf objects
@@ -595,7 +595,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
{
// ===================================== IMU
- Process_Constraint_IMU::SetUp();
+ Process_Factor_IMU::SetUp();
// ===================================== ODO
string wolf_root = _WOLF_ROOT_DIR;
@@ -618,7 +618,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
virtual void integrateAll() override
{
// ===================================== IMU
- Process_Constraint_IMU::integrateAll();
+ Process_Factor_IMU::integrateAll();
// ===================================== ODO
Vector6s data;
@@ -636,7 +636,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
virtual void integrateAllTrajectories() override
{
// ===================================== IMU
- Process_Constraint_IMU::integrateAllTrajectories();
+ Process_Factor_IMU::integrateAllTrajectories();
// ===================================== ODO
Vector6s data;
@@ -654,7 +654,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
virtual void buildProblem() override
{
// ===================================== IMU
- Process_Constraint_IMU::buildProblem();
+ Process_Factor_IMU::buildProblem();
// ===================================== ODO
// Process ODO for the callback to take effect
@@ -666,7 +666,7 @@ class Process_Constraint_IMU_ODO : public Process_Constraint_IMU
};
-TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -710,7 +710,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_PQV_b__PQV_b) // F_ixed___e_stimat
}
-TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, test_capture) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -747,7 +747,7 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
// ===================================== RUN ALL
Eigen::Vector6s initial_bias((Eigen::Vector6s()<< .002, .0007, -.001, .003, -.002, .001).finished());
- sensor_imu->getIntrinsicPtr()->setState(initial_bias);
+ sensor_imu->getIntrinsic()->setState(initial_bias);
configureAll();
integrateAll();
buildProblem();
@@ -756,7 +756,7 @@ TEST_F(Process_Constraint_IMU, test_capture) // F_ixed___e_stimated
ASSERT_MATRIX_APPROX(KF_0->getCaptureOf(sensor_imu)->getCalibration(), KF_1->getCaptureOf(sensor_imu)->getCalibration(), 1e-8 );
}
-TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -800,7 +800,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqv_b__PQV_b) // F_ixed___e_stimat
}
-TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -844,7 +844,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_pqV_b__PQv_b) // F_ixed___e_stimat
}
-TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -888,7 +888,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_PQV_b__PQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -932,7 +932,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__PQv_b) // F_ixed___e_stimated
}
-TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -976,7 +976,7 @@ TEST_F(Process_Constraint_IMU, MotionRandom_pqV_b__pQV_b) // F_ixed___e_stimated
}
-TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1020,7 +1020,7 @@ TEST_F(Process_Constraint_IMU, MotionConstant_NonNullState_PQV_b__PQV_b) // F_ix
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1064,7 +1064,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQV_b) // F_ixe
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1108,7 +1108,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__PQv_b) // F_ixe
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1152,7 +1152,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__Pqv_b) // F_ixe
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1196,7 +1196,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pQv_b) // F_ixe
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1240,7 +1240,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstantRotation_PQV_b__pqv_b) // F_ixe
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1284,7 +1284,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqv_b__pqV_b) // F_ixed___e_st
}
-TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1328,7 +1328,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionConstant_pqV_b__pqv_b) // F_ixed___e_st
}
-TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1372,7 +1372,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PQV_b__pqv_b) // F_ixed___e_stim
}
-TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1416,7 +1416,7 @@ TEST_F(Process_Constraint_IMU_ODO, MotionRandom_PqV_b__pqV_b) // F_ixed___e_stim
}
-TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
+TEST_F(Process_Factor_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b) // F_ixed___e_stimated
{
// ================================================================================================================ //
@@ -1504,28 +1504,28 @@ TEST_F(Process_Constraint_IMU_ODO, RecoverTrajectory_MotionRandom_PqV_b__pqV_b)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.*";
- // ::testing::GTEST_FLAG(filter) = "Process_Constraint_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.*";
+ // ::testing::GTEST_FLAG(filter) = "Process_Factor_IMU_ODO.RecoverTrajectory_MotionRandom_PqV_b__pqV_b";
return RUN_ALL_TESTS();
}
/* Some notes :
*
- * - Process_Constraint_IMU_ODO.MotionConstant_PQv_b__PQv_b :
+ * - Process_Factor_IMU_ODO.MotionConstant_PQv_b__PQv_b :
* this test will not work + jacobian is rank deficient because of estimating both initial
* and final velocities.
* IMU data integration is done with correct biases (so this is the case of a calibrated IMU). Before solving the problem, we perturbate the initial bias.
- * We solve the problem by fixing all states excepted V1 and V2. while creating the constraints, both velocities are corrected using the difference between the actual
+ * We solve the problem by fixing all states excepted V1 and V2. while creating the factors, both velocities are corrected using the difference between the actual
* bias and the bias used during preintegration. One way to solve this in the solver side would be to make the actual bias match the preintegraion bias so that the
* difference is 0 and does not affect the states of the KF. Another possibility is to have both velocities modified without changing the biases. it is likely that this
- * solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other constraints here.)
+ * solution is chosen in this case (bias changes is penalized between 2 KeyFrames, but velocities have no other factors here.)
*
* - Bias evaluation with a precision of 1e-4 :
* The bias introduced in the data for the preintegration steps is different of the real bias. This is simulating the case of a non calibrated IMU
* Because of cross relations between acc and gyro biases (respectively a_b and w_b) it is difficult to expect a better estimation.
* A small change in a_b can be cancelled by a small variation in w_b. in other words : there are local minima.
- * In addition, for Process_Constraint_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
+ * In addition, for Process_Factor_IMU tests, P and V are tested against 1e-5 precision while 1e-8 is used for Q.
* Errors tend to be distributed in different estimated variable when we get into a local minima (to minimize residuals in a least square sense).
*/
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index d1ec1884cc79fb858f4b97f8078ba8281c508753..71217c8e6df3dc2cd0a9ce5bc2e73c97ec19c72a 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -18,9 +18,9 @@ TEST(CaptureBase, ConstructorNoSensor)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
ASSERT_EQ(C->getTimeStamp(), 1.2);
- ASSERT_FALSE(C->getFramePtr());
+ ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
- ASSERT_FALSE(C->getSensorPtr());
+ ASSERT_FALSE(C->getSensor());
}
TEST(CaptureBase, ConstructorWithSensor)
@@ -28,11 +28,11 @@ TEST(CaptureBase, ConstructorWithSensor)
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
ASSERT_EQ(C->getTimeStamp(), 1.5);
- ASSERT_FALSE(C->getFramePtr());
+ ASSERT_FALSE(C->getFrame());
ASSERT_FALSE(C->getProblem());
- ASSERT_TRUE(C->getSensorPtr());
- ASSERT_FALSE(C->getSensorPPtr());
- ASSERT_FALSE(C->getSensorOPtr());
+ ASSERT_TRUE(C->getSensor());
+ ASSERT_FALSE(C->getSensorP());
+ ASSERT_FALSE(C->getSensorO());
}
TEST(CaptureBase, Static_sensor_params)
@@ -44,14 +44,14 @@ TEST(CaptureBase, Static_sensor_params)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S)); // timestamp = 1.5
// query sensor blocks
- ASSERT_EQ(S->getPPtr(), p);
- ASSERT_EQ(S->getOPtr(), o);
- ASSERT_EQ(S->getIntrinsicPtr(), i);
+ ASSERT_EQ(S->getP(), p);
+ ASSERT_EQ(S->getO(), o);
+ ASSERT_EQ(S->getIntrinsic(), i);
// query capture blocks
- ASSERT_EQ(C->getSensorPPtr(), p);
- ASSERT_EQ(C->getSensorOPtr(), o);
- ASSERT_EQ(C->getSensorIntrinsicPtr(), i);
+ ASSERT_EQ(C->getSensorP(), p);
+ ASSERT_EQ(C->getSensorO(), o);
+ ASSERT_EQ(C->getSensorIntrinsic(), i);
}
TEST(CaptureBase, Dynamic_sensor_params)
@@ -63,14 +63,14 @@ TEST(CaptureBase, Dynamic_sensor_params)
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, S, p, o, i)); // timestamp = 1.5
// query sensor blocks -- need KFs to find some Capture with the params
- // ASSERT_EQ(S->getPPtr(), p);
- // ASSERT_EQ(S->getOPtr(), o);
- // ASSERT_EQ(S->getIntrinsicPtr(), i);
+ // ASSERT_EQ(S->getP(), p);
+ // ASSERT_EQ(S->getO(), o);
+ // ASSERT_EQ(S->getIntrinsic(), i);
// query capture blocks
- ASSERT_EQ(C->getSensorPPtr(), p);
- ASSERT_EQ(C->getSensorOPtr(), o);
- ASSERT_EQ(C->getSensorIntrinsicPtr(), i);
+ ASSERT_EQ(C->getSensorP(), p);
+ ASSERT_EQ(C->getSensorO(), o);
+ ASSERT_EQ(C->getSensorIntrinsic(), i);
}
TEST(CaptureBase, addFeature)
@@ -107,7 +107,7 @@ TEST(CaptureBase, process)
SensorBasePtr S(std::make_shared<SensorBase>("DUMMY", nullptr, nullptr, nullptr, 2));
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.5, nullptr));
ASSERT_DEATH({C->process();},""); // No sensor in the capture should fail
- C->setSensorPtr(S);
+ C->setSensor(S);
ASSERT_TRUE(C->process()); // This should not fail (although it does nothing)
}
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index 097969799576f40f960c04e8a736532eb9989ec1..cbe91a6fdf1c276f55bf50e9c32451f0618feb5b 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -12,8 +12,8 @@
#include "base/sensor/sensor_base.h"
#include "base/state_block.h"
#include "base/capture/capture_void.h"
-#include "base/constraint/constraint_pose_2D.h"
-#include "base/constraint/constraint_quaternion_absolute.h"
+#include "base/factor/factor_pose_2D.h"
+#include "base/factor/factor_quaternion_absolute.h"
#include "base/solver/solver_manager.h"
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/local_parametrization_angle.h"
@@ -55,20 +55,20 @@ class CeresManagerWrapper : public CeresManager
return ceres_problem_->NumParameterBlocks();
};
- int numConstraints()
+ int numFactors()
{
return ceres_problem_->NumResidualBlocks();
};
- bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return ctr_2_residual_idx_.find(ctr_ptr) != ctr_2_residual_idx_.end() && ctr_2_costfunction_.find(ctr_ptr) != ctr_2_costfunction_.end();
+ return fac_2_residual_idx_.find(fac_ptr) != fac_2_residual_idx_.end() && fac_2_costfunction_.find(fac_ptr) != fac_2_costfunction_.end();
};
bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param)
{
return state_blocks_local_param_.find(st) != state_blocks_local_param_.end() &&
- state_blocks_local_param_.at(st)->getLocalParametrizationPtr() == local_param &&
+ state_blocks_local_param_.at(st)->getLocalParametrization() == local_param &&
ceres_problem_->GetParameterization(getAssociatedMemBlockPtr(st)) == state_blocks_local_param_.at(st).get();
};
@@ -85,7 +85,7 @@ TEST(CeresManager, Create)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// check double ointers to branches
- ASSERT_EQ(P, ceres_manager_ptr->getProblemPtr());
+ ASSERT_EQ(P, ceres_manager_ptr->getProblem());
// run ceres manager check
ceres_manager_ptr->check();
@@ -312,140 +312,140 @@ TEST(CeresManager, DoubleRemoveStateBlock)
ceres_manager_ptr->check();
}
-TEST(CeresManager, AddConstraint)
+TEST(CeresManager, AddFactor)
{
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
ceres_manager_ptr->update();
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 1);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
ceres_manager_ptr->check();
}
-TEST(CeresManager, DoubleAddConstraint)
+TEST(CeresManager, DoubleAddFactor)
{
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
- // add constraint again
- P->addConstraint(c);
+ // add factor again
+ P->addFactor(c);
// update solver
ceres_manager_ptr->update();
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 1);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 1);
// run ceres manager check
ceres_manager_ptr->check();
}
-TEST(CeresManager, RemoveConstraint)
+TEST(CeresManager, RemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
ceres_manager_ptr->update();
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
// update solver
ceres_manager_ptr->update();
- // check constraint
- ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+ // check factor
+ ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
ceres_manager_ptr->check();
}
-TEST(CeresManager, AddRemoveConstraint)
+TEST(CeresManager, AddRemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
- ASSERT_TRUE(P->getConstraintNotificationMap().begin()->first == c);
+ ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
- ASSERT_TRUE(P->getConstraintNotificationMap().empty());
+ ASSERT_TRUE(P->getFactorNotificationMap().empty());
// update solver
ceres_manager_ptr->update();
- // check constraint
- ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+ // check factor
+ ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
ceres_manager_ptr->check();
}
-TEST(CeresManager, DoubleRemoveConstraint)
+TEST(CeresManager, DoubleRemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
ceres_manager_ptr->update();
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
// update solver
ceres_manager_ptr->update();
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
ASSERT_DEATH({
// update solver
ceres_manager_ptr->update();},"");
- // check constraint
- ASSERT_FALSE(ceres_manager_ptr->isConstraintRegistered(c));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 0);
+ // check factor
+ ASSERT_FALSE(ceres_manager_ptr->isFactorRegistered(c));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 0);
// run ceres manager check
ceres_manager_ptr->check();
@@ -462,7 +462,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
P->addStateBlock(sb_ptr);
@@ -489,7 +489,7 @@ TEST(CeresManager, RemoveLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// add stateblock
P->addStateBlock(sb_ptr);
@@ -530,7 +530,7 @@ TEST(CeresManager, AddLocalParam)
// Local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_param_ptr);
+ sb_ptr->setLocalParametrization(local_param_ptr);
// update solver
ceres_manager_ptr->update();
@@ -543,87 +543,87 @@ TEST(CeresManager, AddLocalParam)
ceres_manager_ptr->check();
}
-TEST(CeresManager, ConstraintsRemoveLocalParam)
+TEST(CeresManager, FactorsRemoveLocalParam)
{
ProblemPtr P = Problem::create("PO 3D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) 2 constraints quaternion
+ // Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintQuaternionAbsolutePtr c1 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
- ConstraintQuaternionAbsolutePtr c2 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+ FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+ FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// remove local param
- F->getOPtr()->removeLocalParametrization();
+ F->getO()->removeLocalParametrization();
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
+ ASSERT_FALSE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
ceres_manager_ptr->check();
}
-TEST(CeresManager, ConstraintsUpdateLocalParam)
+TEST(CeresManager, FactorsUpdateLocalParam)
{
ProblemPtr P = Problem::create("PO 3D");
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
- // Create (and add) 2 constraints quaternion
+ // Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintQuaternionAbsolutePtr c1 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
- ConstraintQuaternionAbsolutePtr c2 = std::static_pointer_cast<ConstraintQuaternionAbsolute>(f->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(F->getOPtr())));
+ FactorQuaternionAbsolutePtr c1 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
+ FactorQuaternionAbsolutePtr c2 = std::static_pointer_cast<FactorQuaternionAbsolute>(f->addFactor(std::make_shared<FactorQuaternionAbsolute>(F->getO())));
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),F->getOPtr()->getLocalParametrizationPtr()));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),F->getO()->getLocalParametrization()));
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// remove local param
LocalParametrizationBasePtr local_param_ptr = std::make_shared<LocalParametrizationQuaternionGlobal>();
- F->getOPtr()->setLocalParametrizationPtr(local_param_ptr);
+ F->getO()->setLocalParametrization(local_param_ptr);
// update solver
ceres_manager_ptr->update();
// check local param
- ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getOPtr()));
- ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getOPtr(),local_param_ptr));
+ ASSERT_TRUE(ceres_manager_ptr->hasLocalParametrization(F->getO()));
+ ASSERT_TRUE(ceres_manager_ptr->hasThisLocalParametrization(F->getO(),local_param_ptr));
- // check constraint
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c1));
- ASSERT_TRUE(ceres_manager_ptr->isConstraintRegistered(c2));
- ASSERT_EQ(ceres_manager_ptr->numConstraints(), 2);
+ // check factor
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c1));
+ ASSERT_TRUE(ceres_manager_ptr->isFactorRegistered(c2));
+ ASSERT_EQ(ceres_manager_ptr->numFactors(), 2);
// run ceres manager check
ceres_manager_ptr->check();
diff --git a/test/gtest_constraint_sparse.cpp b/test/gtest_constraint_sparse.cpp
deleted file mode 100644
index 900f39d5b57c773a83acfadb6285a3cf88fecb05..0000000000000000000000000000000000000000
--- a/test/gtest_constraint_sparse.cpp
+++ /dev/null
@@ -1,52 +0,0 @@
-/**
- * \file gtest_constraint_sparse.cpp
- *
- * Created on: Apr 25, 2017
- * \author: jsola
- */
-
-#include "utils_gtest.h"
-
-#include "constraint_sparse.h"
-
-using namespace wolf;
-
-// Dummy class for avoiding the pure virtual compilation error
-template <JacobianMethod J>
-class ConstraintSparseObject : public ConstraintSparse<1, 2, 1>
-{
- public:
- ConstraintSparseObject(ConstraintType _tp, bool _apply_loss_function, ConstraintStatus _status, StateBlockPtr _xy, StateBlockPtr _theta) :
- ConstraintSparse<1, 2, 1>(_tp, _apply_loss_function, _status, _xy, _theta)
- {
- //
- }
- virtual ~ConstraintSparseObject(){}
-
- virtual JacobianMethod getJacobianMethod() const
- {
- return J;
- }
-};
-
-TEST(ConstraintSparseAnalytic, Constructor)
-{
- ConstraintSparseObject<JAC_ANALYTIC> ctr_analytic(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_analytic.getJacobianMethod(), JAC_ANALYTIC);
- ASSERT_EQ(ctr_analytic.getApplyLossFunction(), false);
- ASSERT_EQ(ctr_analytic.getStatus(), CTR_ACTIVE);
- ASSERT_EQ(ctr_analytic.getSize(), 1);
-
- ConstraintSparseObject<JAC_AUTO> ctr_auto(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_auto.getJacobianMethod(), JAC_AUTO);
-
- ConstraintSparseObject<JAC_NUMERIC> ctr_numeric(CTR_AHP, false, CTR_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
- ASSERT_EQ(ctr_numeric.getJacobianMethod(), JAC_NUMERIC);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
-
diff --git a/test/gtest_constraint_IMU.cpp b/test/gtest_factor_IMU.cpp
similarity index 85%
rename from test/gtest_constraint_IMU.cpp
rename to test/gtest_factor_IMU.cpp
index 7ef93e9a37102913f4d7543ce7deb48a6b086669..6c8990c291af5bb43dc9e042f9ca60dc84854e75 100644
--- a/test/gtest_constraint_IMU.cpp
+++ b/test/gtest_factor_IMU.cpp
@@ -1,5 +1,5 @@
/**
- * \file gtest_constraint_imu.cpp
+ * \file gtest_factor_imu.cpp
*
* Created on: Jan 01, 2017
* \author: Dinesh Atchuthan
@@ -7,7 +7,7 @@
//Wolf
#include "base/capture/capture_IMU.h"
-#include "base/constraint/constraint_pose_3D.h"
+#include "base/factor/factor_pose_3D.h"
#include "base/processor/processor_IMU.h"
#include "base/sensor/sensor_IMU.h"
#include "base/processor/processor_odom_3D.h"
@@ -33,7 +33,7 @@ using namespace wolf;
* and finally the last stateafter integration and the last timestamp, Then it should contain all IMU data and related timestamps
*/
-class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
+class FactorIMU_biasTest_Static_NullBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -108,7 +108,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
}
- KF1 = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -119,7 +119,7 @@ class ConstraintIMU_biasTest_Static_NullBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
+class FactorIMU_biasTest_Static_NonNullAccBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -192,7 +192,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
sen_imu->process(imu_ptr);
}
- KF1 = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ KF1 = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
KF1->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -203,7 +203,7 @@ class ConstraintIMU_biasTest_Static_NonNullAccBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
+class FactorIMU_biasTest_Static_NonNullGyroBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -276,7 +276,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -287,7 +287,7 @@ class ConstraintIMU_biasTest_Static_NonNullGyroBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
+class FactorIMU_biasTest_Static_NonNullBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -362,7 +362,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state); //We expect to find this solution, this can be perturbated in following tests
//===================================================== END{PROCESS DATA}
@@ -373,7 +373,7 @@ class ConstraintIMU_biasTest_Static_NonNullBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
+class FactorIMU_biasTest_Move_NullBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -449,7 +449,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -460,7 +460,7 @@ class ConstraintIMU_biasTest_Move_NullBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
+class FactorIMU_biasTest_Move_NonNullBias : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -532,7 +532,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -543,7 +543,7 @@ class ConstraintIMU_biasTest_Move_NonNullBias : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
+class FactorIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -625,7 +625,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -636,7 +636,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotCst : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
+class FactorIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -724,7 +724,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
sen_imu->process(imu_ptr);
}
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(t);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -737,7 +737,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst : public testing::Test
// var(b1,b2), inv(p1,q1,v1,p2,q2,v2); fac1: imu(p,q,v)+(b1=b2)
-class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorIMU_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -823,7 +823,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
}
expected_final_state << 0,0,0, quat_current.x(), quat_current.y(), quat_current.z(), quat_current.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -834,7 +834,7 @@ class ConstraintIMU_biasTest_Move_NonNullBiasRot : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
+class FactorIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -930,8 +930,8 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
Eigen::Quaternions quat_odo(Eigen::Quaternions::Identity());
Eigen::Quaternions quat_imu(Eigen::Quaternions::Identity());
- WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+ WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
for(unsigned int i = 1; i<=1000; i++)
{
@@ -955,16 +955,16 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
capture_imu->setData(data_imu);
sensor_imu->process(capture_imu);
- WOLF_TRACE("last delta preint: ", processor_imu->getLastPtr()->getDeltaPreint().transpose());
- WOLF_TRACE("last jacoob bias : ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
+ WOLF_TRACE("last delta preint: ", processor_imu->getLast()->getDeltaPreint().transpose());
+ WOLF_TRACE("last jacoob bias : ", processor_imu->getLast()->getJacobianCalib().row(0));
//when we find a IMU timestamp corresponding with this odometry timestamp then we process odometry measurement
if(t_imu.get() >= t_odo.get())
{
WOLF_TRACE("====== create ODOM KF ========");
-// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
// PROCESS ODOM 3D DATA
data_odo.head(3) << 0,0,0;
@@ -972,15 +972,15 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
capture_odo->setTimeStamp(t_odo);
capture_odo->setData(data_odo);
-// WOLF_TRACE("Jac calib: ", processor_imu->getLastPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("last calib: ", processor_imu->getLastPtr()->getCalibration().transpose());
-// WOLF_TRACE("last calib preint: ", processor_imu->getLastPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getLast()->getJacobianCalib().row(0));
+// WOLF_TRACE("last calib: ", processor_imu->getLast()->getCalibration().transpose());
+// WOLF_TRACE("last calib preint: ", processor_imu->getLast()->getCalibrationPreint().transpose());
sensor_odo->process(capture_odo);
-// WOLF_TRACE("Jac calib: ", processor_imu->getOriginPtr()->getJacobianCalib().row(0));
-// WOLF_TRACE("orig calib: ", processor_imu->getOriginPtr()->getCalibration().transpose());
-// WOLF_TRACE("orig calib preint: ", processor_imu->getOriginPtr()->getCalibrationPreint().transpose());
+// WOLF_TRACE("Jac calib: ", processor_imu->getOrigin()->getJacobianCalib().row(0));
+// WOLF_TRACE("orig calib: ", processor_imu->getOrigin()->getCalibration().transpose());
+// WOLF_TRACE("orig calib preint: ", processor_imu->getOrigin()->getCalibrationPreint().transpose());
//prepare next odometry measurement
quat_odo = Eigen::Quaternions::Identity(); //set to identity to have next odom relative to this last KF
@@ -992,7 +992,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
expected_final_state.resize(10);
expected_final_state << 0,0,0, quat_imu.x(), quat_imu.y(), quat_imu.z(), quat_imu.w(), 0,0,0;
- last_KF = problem->getTrajectoryPtr()->closestKeyFrameToTimeStamp(t_imu);
+ last_KF = problem->getTrajectory()->closestKeyFrameToTimeStamp(t_imu);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -1018,7 +1018,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
+class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -1125,7 +1125,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
imu_ptr->setData(data_imu);
sen_imu->process(imu_ptr);
- D_cor = processor_ptr_imu->getLastPtr()->getDeltaCorrected(origin_bias);
+ D_cor = processor_ptr_imu->getLast()->getDeltaCorrected(origin_bias);
if(ts.get() >= t_odom.get())
{
@@ -1145,7 +1145,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
}
expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
WOLF_TRACE("X_correct(t) : ", imu::composeOverState(x_origin, D_cor, ts - t).transpose());
@@ -1159,7 +1159,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY : public testing::Test
virtual void TearDown(){}
};
-class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
+class FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
{
public:
wolf::TimeStamp t;
@@ -1321,7 +1321,7 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
}
expected_final_state << 0,0,0, current_quatState.x(), current_quatState.y(), current_quatState.z(), current_quatState.w(), 0,0,0;
- last_KF = wolf_problem_ptr_->getTrajectoryPtr()->closestKeyFrameToTimeStamp(ts);
+ last_KF = wolf_problem_ptr_->getTrajectory()->closestKeyFrameToTimeStamp(ts);
last_KF->setState(expected_final_state);
//===================================================== END{PROCESS DATA}
@@ -1335,21 +1335,21 @@ class ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY : public testing::Test
// tests with following conditions :
// var(b1,b2), invar(p1,q1,v1,p2,q2,v2), factor : imu(p,q,v)
-TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
KF0->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<1,2,3,1,2,3).finished());
- KF1->getPPtr()->setState(expected_final_state.head(3));
- KF1->getOPtr()->setState(expected_final_state.segment(3,4));
- KF1->getVPtr()->setState(expected_final_state.segment(7,3));
+ KF1->getP()->setState(expected_final_state.head(3));
+ KF1->getO()->setState(expected_final_state.segment(3,4));
+ KF1->getV()->setState(expected_final_state.segment(7,3));
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
KF1->getCaptureOf(sen_imu)->setCalibration((Vector6s()<<-1,-2,-3,-1,-2,-3).finished());
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1362,15 +1362,15 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
}
-TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1453,20 +1453,20 @@ TEST_F(ConstraintIMU_biasTest_Static_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
- KF1->getPPtr()->setState(expected_final_state.head(3));
- KF1->getOPtr()->setState(expected_final_state.segment(3,4));
- KF1->getVPtr()->setState(expected_final_state.segment(7,3));
+ KF1->getP()->setState(expected_final_state.head(3));
+ KF1->getO()->setState(expected_final_state.segment(3,4));
+ KF1->getV()->setState(expected_final_state.segment(7,3));
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1478,15 +1478,15 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_in
ASSERT_MATRIX_APPROX(KF1->getCaptureOf(sen_imu)->getCalibration().tail(3), origin_bias.tail(3), wolf::Constants::EPS)
}
-TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- KF0->getPPtr()->fix();
- KF0->getOPtr()->fix();
- KF0->getVPtr()->fix();
- KF1->getPPtr()->fix();
- KF1->getOPtr()->fix();
- KF1->getVPtr()->fix();
+ KF0->getP()->fix();
+ KF0->getO()->fix();
+ KF0->getV()->fix();
+ KF1->getP()->fix();
+ KF1->getO()->fix();
+ KF1->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1569,20 +1569,20 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullAccBias,VarB1B2_InvarP1Q1V1P2Q2V2_Er
}
}
-TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
- last_KF->getPPtr()->setState(expected_final_state.head(3));
- last_KF->getOPtr()->setState(expected_final_state.segment(3,4));
- last_KF->getVPtr()->setState(expected_final_state.segment(7,3));
+ last_KF->getP()->setState(expected_final_state.head(3));
+ last_KF->getO()->setState(expected_final_state.segment(3,4));
+ last_KF->getV()->setState(expected_final_state.segment(7,3));
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport;
@@ -1591,15 +1591,15 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_i
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
}
-TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1671,15 +1671,15 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullGyroBias,VarB1B2_InvarP1Q1V1P2Q2V2_E
}
}
-TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1752,18 +1752,18 @@ TEST_F(ConstraintIMU_biasTest_Static_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBi
}
}
-TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
@@ -1773,15 +1773,15 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
}
-TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1851,18 +1851,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
}
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1); // 0: nothing, 1: BriefReport, 2: FullReport
@@ -1872,15 +1872,15 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -1951,18 +1951,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBias,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias
}
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -1972,15 +1972,15 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_i
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -2051,18 +2051,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotCst,VarB1B2_InvarP1Q1V1P2Q2V2_E
}
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
@@ -2072,15 +2072,15 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
+TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2V2_ErrBias)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
wolf::Scalar epsilon_bias = 0.0000001;
Eigen::VectorXs perturbed_bias(origin_bias);
@@ -2151,16 +2151,16 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst,VarB1B2_InvarP1Q1V1P2Q2
}
}
-TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
last_KF->setState(expected_final_state);
@@ -2172,16 +2172,16 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q
}
-//TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+//TEST_F(FactorIMU_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
//{
// //prepare problem for solving
-// origin_KF->getPPtr()->fix();
-// origin_KF->getOPtr()->fix();
-// origin_KF->getVPtr()->unfix();
+// origin_KF->getP()->fix();
+// origin_KF->getO()->fix();
+// origin_KF->getV()->unfix();
//
-// last_KF->getPPtr()->unfix();
-// last_KF->getOPtr()->fix();
-// last_KF->getVPtr()->unfix();
+// last_KF->getP()->unfix();
+// last_KF->getO()->fix();
+// last_KF->getV()->unfix();
//
// last_KF->setState(expected_final_state);
//
@@ -2193,18 +2193,18 @@ TEST_F(ConstraintIMU_biasTest_Move_NonNullBiasRotAndVCst, VarB1B2V1P2V2_InvarP1Q
//
//}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2238,18 +2238,18 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2_InvarP1Q1V1P2Q2
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2262,7 +2262,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
@@ -2285,18 +2285,18 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V2_InvarP1Q1V1P2
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2308,24 +2308,24 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1V2_InvarP1Q1P2
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2336,26 +2336,26 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1Q2V2_InvarP1Q1
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2364,26 +2364,26 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2V2_InvarP1Q1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
//jacobian matrix rank deficient here - estimating both initial and final velocity
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
@@ -2392,27 +2392,27 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2V1P2Q2V2_InvarP1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2422,13 +2422,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2451,26 +2451,26 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarB1B2P2Q2V2_InvarP1Q1
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
- //Add fix constraint on yaw to make the problem observable
+ //Add fix factor on yaw to make the problem observable
Eigen::MatrixXs featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXs::Identity(6,6);
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->unfix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->unfix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2480,13 +2480,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2509,26 +2509,26 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V1V2_initOK)
{
- //Add fix constraint on yaw to make the problem observable
+ //Add fix factor on yaw to make the problem observable
Eigen::MatrixXs featureFix_cov(6,6);
featureFix_cov = Eigen::MatrixXs::Identity(6,6);
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
+ FactorFix3DPtr fac_fix = std::static_pointer_cast<FactorPose3D>(ffix->addFactor(std::make_shared<FactorPose3D>(ffix)));
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->unfix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->unfix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2538,13 +2538,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
std::string report = ceres_manager_wolf_diff->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager_wolf_diff->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sen_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2567,18 +2567,18 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->fix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->fix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2618,18 +2618,18 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2_InvarP1Q1V1P2Q2V
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.001);
@@ -2642,7 +2642,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
Eigen::MatrixXs covX(10,10);
@@ -2664,18 +2664,18 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V2_InvarP1Q1V1P2Q
WOLF_WARN("Big 2*stdev on one or more biases! Max coeff :", cov_stdev.tail(6).maxCoeff())
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2684,25 +2684,25 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1V2_InvarP1Q1P2Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->fix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->fix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2711,27 +2711,27 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1Q2V2_InvarP1Q1P
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q2_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->fix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->fix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2740,25 +2740,25 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2V2_InvarP1Q1Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->unfix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->unfix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.0001);
@@ -2767,27 +2767,27 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2V1P2Q2V2_InvarP1Q
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*10000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*10000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
}
-TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
+TEST_F(FactorIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V1_initOK)
{
//prepare problem for solving
- origin_KF->getPPtr()->fix();
- origin_KF->getOPtr()->fix();
- origin_KF->getVPtr()->fix();
+ origin_KF->getP()->fix();
+ origin_KF->getO()->fix();
+ origin_KF->getV()->fix();
last_KF->setState(expected_final_state);
- last_KF->getPPtr()->unfix();
- last_KF->getOPtr()->unfix();
- last_KF->getVPtr()->unfix();
+ last_KF->getP()->unfix();
+ last_KF->getO()->unfix();
+ last_KF->getV()->unfix();
//perturbation of origin bias
Eigen::Vector6s random_err(Eigen::Vector6s::Random() * 0.00001);
@@ -2797,13 +2797,13 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
std::string report = ceres_manager->solve(1);// 0: nothing, 1: BriefReport, 2: FullReport
ceres_manager->computeCovariances(ALL);
- ASSERT_MATRIX_APPROX(origin_KF->getVPtr()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
+ ASSERT_MATRIX_APPROX(origin_KF->getV()->getState(), x_origin.segment(7,3), wolf::Constants::EPS*1000)
ASSERT_MATRIX_APPROX(origin_KF->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
ASSERT_MATRIX_APPROX(last_KF ->getCaptureOf(sensor_imu)->getCalibration(), origin_bias, 1e-5)
- ASSERT_MATRIX_APPROX(last_KF->getPPtr()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
- ASSERT_MATRIX_APPROX(last_KF->getVPtr()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
- Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getOPtr()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
+ ASSERT_MATRIX_APPROX(last_KF->getP()->getState(), expected_final_state.head(3), wolf::Constants::EPS*100)
+ ASSERT_MATRIX_APPROX(last_KF->getV()->getState(), expected_final_state.segment(7,3), wolf::Constants::EPS*1000)
+ Eigen::Map<const Eigen::Quaternions> estimatedLastQuat(last_KF->getO()->getState().data()), expectedLastQuat(expected_final_state.segment(3,4).data());
ASSERT_QUATERNION_APPROX(estimatedLastQuat, expectedLastQuat, wolf::Constants::EPS*100)
Eigen::Matrix<wolf::Scalar, 10, 1> cov_stdev, actual_state(last_KF->getState());
@@ -2831,10 +2831,10 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot, VarB1B2P2Q2V2_InvarP1Q1V
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.*:ConstraintIMU_biasTest_Static_NullBias.*:ConstraintIMU_biasTest_Static_NonNullAccBias.*:ConstraintIMU_biasTest_Static_NonNullGyroBias.*";
-// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
-// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
-// ::testing::GTEST_FLAG(filter) = "ConstraintIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
+ ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.*:FactorIMU_biasTest_Static_NullBias.*:FactorIMU_biasTest_Static_NonNullAccBias.*:FactorIMU_biasTest_Static_NonNullGyroBias.*";
+// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRotY.VarB1B2V1V2_InvarP1Q1P2Q2_initOK";
+// ::testing::GTEST_FLAG(filter) = "FactorIMU_ODOM_biasTest_Move_NonNullBiasRot.VarB1B2_InvarP1Q1V1P2Q2V2_initOK";
+// ::testing::GTEST_FLAG(filter) = "FactorIMU_biasTest_Move_NonNullBiasRot.VarB1B2V1P2V2_InvarP1Q1Q2_initOK";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_constraint_absolute.cpp b/test/gtest_factor_absolute.cpp
similarity index 67%
rename from test/gtest_constraint_absolute.cpp
rename to test/gtest_factor_absolute.cpp
index f5f03b27dc0d5e1a11b5e8301c5178f6d1929088..204c6ce633a36380d46eacbc65d66abe1a7747fe 100644
--- a/test/gtest_constraint_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -1,13 +1,13 @@
/**
- * \file gtest_constraint_absolute.cpp
+ * \file gtest_factor_absolute.cpp
*
* Created on: Dec 9, 2017
* \author: datchuth
*/
#include "utils_gtest.h"
-#include "base/constraint/constraint_block_absolute.h"
-#include "base/constraint/constraint_quaternion_absolute.h"
+#include "base/factor/factor_block_absolute.h"
+#include "base/factor/factor_quaternion_absolute.h"
#include "base/capture/capture_motion.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -37,19 +37,19 @@ CeresManager ceres_mgr(problem_ptr);
// Two frames
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
-// Capture, feature and constraint
+// Capture, feature and factor
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("IMU ABS", 0, nullptr, pose10, 10, 9, nullptr));
////////////////////////////////////////////////////////
-/* In the tests below, we check that ConstraintBlockAbsolute and ConstraintQuaternionAbsolute are working fine
- * These are absolute constraints related to a specific part of the frame's state
- * Both features and constraints are created in the TEST(). Hence, tests will not interfere each others.
+/* In the tests below, we check that FactorBlockAbsolute and FactorQuaternionAbsolute are working fine
+ * These are absolute factors related to a specific part of the frame's state
+ * Both features and factors are created in the TEST(). Hence, tests will not interfere each others.
*/
-TEST(ConstraintBlockAbs, ctr_block_abs_p)
+TEST(FactorBlockAbs, fac_block_abs_p)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -61,13 +61,13 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState(), pose10.head<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getP()->getState(), pose10.head<3>(), 1e-6);
}
-TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2)
+TEST(FactorBlockAbs, fac_block_abs_p_tail2)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSITION TAIL 2", pose10.segment<2>(1), data_cov.bottomRightCorner<2,2>()));
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr(),1,2));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getP(),1,2));
// Unfix frame 0, perturb frm0
frm0->unfix();
@@ -77,13 +77,13 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_tail2)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only orientation is constrained
- ASSERT_MATRIX_APPROX(frm0->getPPtr()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getP()->getState().tail<2>(), pose10.segment<2>(1), 1e-6);
}
-TEST(ConstraintBlockAbs, ctr_block_abs_v)
+TEST(FactorBlockAbs, fac_block_abs_v)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("VELOCITY", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
- fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()));
+ fea0->addFactor(std::make_shared<FactorBlockAbsolute>(fea0->getFrame()->getV()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -95,13 +95,13 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getVPtr()->getState(), pose10.tail<3>(), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getV()->getState(), pose10.tail<3>(), 1e-6);
}
-TEST(ConstraintQuatAbs, ctr_block_abs_o)
+TEST(FactorQuatAbs, fac_block_abs_o)
{
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("QUATERNION", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
- fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()));
+ fea0->addFactor(std::make_shared<FactorQuaternionAbsolute>(fea0->getFrame()->getO()));
ASSERT_TRUE(problem_ptr->check(0));
@@ -113,7 +113,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o)
std::string brief_report = ceres_mgr.solve(wolf::SolverManager::ReportVerbosity::BRIEF);
//only velocity is constrained
- ASSERT_MATRIX_APPROX(frm0->getOPtr()->getState(), pose10.segment<4>(3), 1e-6);
+ ASSERT_MATRIX_APPROX(frm0->getO()->getState(), pose10.segment<4>(3), 1e-6);
}
int main(int argc, char **argv)
diff --git a/test/gtest_constraint_autodiff.cpp b/test/gtest_factor_autodiff.cpp
similarity index 72%
rename from test/gtest_constraint_autodiff.cpp
rename to test/gtest_factor_autodiff.cpp
index 9305f71ea64703cc29ecb741c38f017f4afdd85d..a053b3f82438b8ac9c358fc45b4b656dd9df65ff 100644
--- a/test/gtest_constraint_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -1,5 +1,5 @@
/*
- * gtest_constraint_autodiff.cpp
+ * gtest_factor_autodiff.cpp
*
* Created on: May 24 2017
* Author: jvallve
@@ -10,9 +10,9 @@
#include "base/sensor/sensor_odom_2D.h"
#include "base/capture/capture_void.h"
#include "base/feature/feature_odom_2D.h"
-#include "base/constraint/constraint_odom_2D.h"
-#include "base/constraint/constraint_odom_2D_analytic.h"
-#include "base/constraint/constraint_autodiff.h"
+#include "base/factor/factor_odom_2D.h"
+#include "base/factor/factor_odom_2D_analytic.h"
+#include "base/factor/factor_autodiff.h"
using namespace wolf;
using namespace Eigen;
@@ -37,15 +37,15 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINT
- ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addConstraint(constraint_ptr);
- fr1_ptr->addConstrainedBy(constraint_ptr);
-
- ASSERT_TRUE(constraint_ptr->getFeaturePtr());
- ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr());
- ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr()->getFramePtr());
- ASSERT_TRUE(constraint_ptr->getFeaturePtr()->getCapturePtr()->getSensorPtr());
- ASSERT_TRUE(constraint_ptr->getFrameOtherPtr());
+ FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(factor_ptr);
+ fr1_ptr->addConstrainedBy(factor_ptr);
+
+ ASSERT_TRUE(factor_ptr->getFeature());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getFrame());
+ ASSERT_TRUE(factor_ptr->getFeature()->getCapture()->getSensor());
+ ASSERT_TRUE(factor_ptr->getFrameOther());
}
TEST(CaptureAutodiff, ResidualOdom2d)
@@ -74,22 +74,22 @@ TEST(CaptureAutodiff, ResidualOdom2d)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINT
- ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addConstraint(constraint_ptr);
- fr1_ptr->addConstrainedBy(constraint_ptr);
+ FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(factor_ptr);
+ fr1_ptr->addConstrainedBy(factor_ptr);
// EVALUATE
- Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
double const* const* parameters = states_ptr.data();
- Eigen::VectorXs residuals(constraint_ptr->getSize());
- constraint_ptr->evaluate(parameters, residuals.data(), nullptr);
+ Eigen::VectorXs residuals(factor_ptr->getSize());
+ factor_ptr->evaluate(parameters, residuals.data(), nullptr);
ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
ASSERT_TRUE(residuals.minCoeff() > -1e-9);
@@ -121,22 +121,22 @@ TEST(CaptureAutodiff, JacobianOdom2d)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINT
- ConstraintOdom2DPtr constraint_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addConstraint(constraint_ptr);
- fr1_ptr->addConstrainedBy(constraint_ptr);
+ FactorOdom2DPtr factor_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(factor_ptr);
+ fr1_ptr->addConstrainedBy(factor_ptr);
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
std::vector<Eigen::MatrixXs> Jauto;
- Eigen::VectorXs residuals(constraint_ptr->getSize());
- constraint_ptr->evaluate(states_ptr, residuals, Jauto);
+ Eigen::VectorXs residuals(factor_ptr->getSize());
+ factor_ptr->evaluate(states_ptr, residuals, Jauto);
std::cout << Jauto.size() << std::endl;
@@ -203,30 +203,30 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
capture_ptr->addFeature(feature_ptr);
// CONSTRAINTS
- ConstraintOdom2DPtr ctr_autodiff_ptr = std::make_shared<ConstraintOdom2D>(feature_ptr, fr1_ptr);
- feature_ptr->addConstraint(ctr_autodiff_ptr);
- fr1_ptr->addConstrainedBy(ctr_autodiff_ptr);
- ConstraintOdom2DAnalyticPtr ctr_analytic_ptr = std::make_shared<ConstraintOdom2DAnalytic>(feature_ptr, fr1_ptr);
- feature_ptr->addConstraint(ctr_analytic_ptr);
- fr1_ptr->addConstrainedBy(ctr_analytic_ptr);
+ FactorOdom2DPtr fac_autodiff_ptr = std::make_shared<FactorOdom2D>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(fac_autodiff_ptr);
+ fr1_ptr->addConstrainedBy(fac_autodiff_ptr);
+ FactorOdom2DAnalyticPtr fac_analytic_ptr = std::make_shared<FactorOdom2DAnalytic>(feature_ptr, fr1_ptr);
+ feature_ptr->addFactor(fac_analytic_ptr);
+ fr1_ptr->addConstrainedBy(fac_analytic_ptr);
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getOPtr()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getPPtr()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getOPtr()->getState();
+ const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const Scalar*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
- Eigen::VectorXs residuals(ctr_autodiff_ptr->getSize());
+ Eigen::VectorXs residuals(fac_autodiff_ptr->getSize());
clock_t t = clock();
- ctr_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
+ fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
t = clock();
- //TODO ConstraintAnalytic::evaluate
-// ctr_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
+ //TODO FactorAnalytic::evaluate
+// fac_analytic_ptr->evaluate(states_ptr, residuals, Janalytic);
// std::cout << "analytic evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
//
// for (auto i = 0; i < Jautodiff.size(); i++)
diff --git a/test/gtest_constraint_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
similarity index 79%
rename from test/gtest_constraint_autodiff_distance_3D.cpp
rename to test/gtest_factor_autodiff_distance_3D.cpp
index ae3313afe1f6d926221330e428282a6a06fa902e..7559bfa7582aa1449232391806e498fb294d03be 100644
--- a/test/gtest_constraint_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_constraint_autodiff_distance_3D.cpp
+ * \file gtest_factor_autodiff_distance_3D.cpp
*
* Created on: Apr 10, 2018
* \author: jsola
*/
-#include "base/constraint/constraint_autodiff_distance_3D.h"
+#include "base/factor/factor_autodiff_distance_3D.h"
#include "base/problem.h"
#include "base/logging.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -16,7 +16,7 @@
using namespace wolf;
using namespace Eigen;
-class ConstraintAutodiffDistance3D_Test : public testing::Test
+class FactorAutodiffDistance3D_Test : public testing::Test
{
public:
Vector3s pos1, pos2;
@@ -28,7 +28,7 @@ class ConstraintAutodiffDistance3D_Test : public testing::Test
FrameBasePtr F1, F2;
CaptureBasePtr C2;
FeatureBasePtr f2;
- ConstraintAutodiffDistance3DPtr c2;
+ FactorAutodiffDistance3DPtr c2;
Vector1s dist;
Matrix1s dist_cov;
@@ -62,15 +62,15 @@ class ConstraintAutodiffDistance3D_Test : public testing::Test
F2->addCapture(C2);
f2 = std::make_shared<FeatureBase>("Dist", dist, dist_cov);
C2->addFeature(f2);
- c2 = std::make_shared<ConstraintAutodiffDistance3D>(f2, F1, nullptr, false, CTR_ACTIVE);
- f2->addConstraint(c2);
+ c2 = std::make_shared<FactorAutodiffDistance3D>(f2, F1, nullptr, false, FAC_ACTIVE);
+ f2->addFactor(c2);
F1->addConstrainedBy(c2);
}
};
-TEST_F(ConstraintAutodiffDistance3D_Test, ground_truth)
+TEST_F(FactorAutodiffDistance3D_Test, ground_truth)
{
Scalar res;
@@ -79,7 +79,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, ground_truth)
ASSERT_NEAR(res, 0.0, 1e-8);
}
-TEST_F(ConstraintAutodiffDistance3D_Test, expected_residual)
+TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
{
Scalar measurement = 1.400;
@@ -93,7 +93,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, expected_residual)
ASSERT_NEAR(res, res_expected, 1e-8);
}
-TEST_F(ConstraintAutodiffDistance3D_Test, solve)
+TEST_F(FactorAutodiffDistance3D_Test, solve)
{
Scalar measurement = 1.400;
f2->setMeasurement(Vector1s(measurement));
@@ -101,7 +101,7 @@ TEST_F(ConstraintAutodiffDistance3D_Test, solve)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
// Check distance between F1 and F2 positions -- must match the measurement
- ASSERT_NEAR( (F1->getPPtr()->getState() - F2->getPPtr()->getState()).norm(), measurement, 1e-10);
+ ASSERT_NEAR( (F1->getP()->getState() - F2->getP()->getState()).norm(), measurement, 1e-10);
}
int main(int argc, char **argv)
diff --git a/test/gtest_constraint_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
similarity index 69%
rename from test/gtest_constraint_autodiff_trifocal.cpp
rename to test/gtest_factor_autodiff_trifocal.cpp
index 6447dcfe4f69e434f02a9219942e46048417a697..71401381fe35c768d047b8324e0b2e2f74df35c4 100644
--- a/test/gtest_constraint_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -5,12 +5,12 @@
#include "base/ceres_wrapper/ceres_manager.h"
#include "base/processor/processor_tracker_feature_trifocal.h"
#include "base/capture/capture_image.h"
-#include "base/constraint/constraint_autodiff_trifocal.h"
+#include "base/factor/factor_autodiff_trifocal.h"
using namespace Eigen;
using namespace wolf;
-class ConstraintAutodiffTrifocalTest : public testing::Test{
+class FactorAutodiffTrifocalTest : public testing::Test{
public:
Vector3s pos1, pos2, pos3, pos_cam, point;
Vector3s euler1, euler2, euler3, euler_cam;
@@ -28,7 +28,7 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
FrameBasePtr F1, F2, F3;
CaptureImagePtr I1, I2, I3;
FeatureBasePtr f1, f2, f3;
- ConstraintAutodiffTrifocalPtr c123;
+ FactorAutodiffTrifocalPtr c123;
virtual void SetUp() override
{
@@ -75,11 +75,11 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
* f3: p3 = (0,0)
*
* We form a Wolf tree with three frames, three captures,
- * three features, and one trifocal constraint:
+ * three features, and one trifocal factor:
*
* Frame F1, Capture C1, feature f1
* Frame F2, Capture C2, feature f2
- * Frame F3, Capture C3, feature f3, constraint c123
+ * Frame F3, Capture C3, feature f3, factor c123
*
* The three frame poses F1, F2, F3 and the camera pose S
* in the robot frame are variables subject to optimization
@@ -151,15 +151,15 @@ class ConstraintAutodiffTrifocalTest : public testing::Test{
f3 = std::make_shared<FeatureBase>("PIXEL", pix, pix_cov); // pixel at origin
I3-> addFeature(f3);
- // trifocal constraint
- c123 = std::make_shared<ConstraintAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, CTR_ACTIVE);
- f3 ->addConstraint (c123);
+ // trifocal factor
+ c123 = std::make_shared<FactorAutodiffTrifocal>(f1, f2, f3, proc_trifocal, false, FAC_ACTIVE);
+ f3 ->addFactor (c123);
f1 ->addConstrainedBy(c123);
f2 ->addConstrainedBy(c123);
}
};
-TEST_F(ConstraintAutodiffTrifocalTest, expectation)
+TEST_F(FactorAutodiffTrifocalTest, expectation)
{
// Homogeneous transform C2 wrt C1
Matrix4s _c1Hc2; _c1Hc2 <<
@@ -228,7 +228,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, expectation)
ASSERT_MATRIX_APPROX(c2Ec1/c2Ec1(0,1), _c2Ec1/_c2Ec1(0,1), 1e-8);
}
-TEST_F(ConstraintAutodiffTrifocalTest, residual)
+TEST_F(FactorAutodiffTrifocalTest, residual)
{
vision_utils::TrifocalTensor tensor;
Matrix3s c2Ec1;
@@ -241,7 +241,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, residual)
ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-8);
}
-TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
+TEST_F(FactorAutodiffTrifocalTest, error_jacobians)
{
vision_utils::TrifocalTensor tensor;
Matrix3s c2Ec1;
@@ -312,11 +312,11 @@ TEST_F(ConstraintAutodiffTrifocalTest, error_jacobians)
}
-TEST_F(ConstraintAutodiffTrifocalTest, operator_parenthesis)
+TEST_F(FactorAutodiffTrifocalTest, operator_parenthesis)
{
Vector3s res;
- // Constraint with exact states should give zero residual
+ // Factor with exact states should give zero residual
c123->operator ()(pos1.data(), vquat1.data(),
pos2.data(), vquat2.data(),
pos3.data(), vquat3.data(),
@@ -326,25 +326,25 @@ TEST_F(ConstraintAutodiffTrifocalTest, operator_parenthesis)
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
}
-TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
+TEST_F(FactorAutodiffTrifocalTest, solve_F1)
{
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -362,8 +362,8 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
pose_perturbated.segment(3,4).normalize();
F1->setState(pose_perturbated);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -383,14 +383,14 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -407,26 +407,26 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F1)
}
-TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
+TEST_F(FactorAutodiffTrifocalTest, solve_F2)
{
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -444,8 +444,8 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
pose_perturbated.segment(3,4).normalize();
F2->setState(pose_perturbated);
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -465,14 +465,14 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -489,26 +489,26 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F2)
}
-TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
+TEST_F(FactorAutodiffTrifocalTest, solve_F3)
{
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -526,8 +526,8 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
pose_perturbated.segment(3,4).normalize();
F3->setState(pose_perturbated);
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -548,14 +548,14 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -572,26 +572,26 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_F3)
}
-TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
+TEST_F(FactorAutodiffTrifocalTest, solve_S)
{
F1->setState(pose1);
F2->setState(pose2);
F3->setState(pose3);
- S ->getPPtr()->setState(pos_cam);
- S ->getOPtr()->setState(vquat_cam);
+ S ->getP()->setState(pos_cam);
+ S ->getO()->setState(vquat_cam);
// Residual with prior
Vector3s res;
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -599,7 +599,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
S_p. data(), S_o. data(),
res.data());
- WOLF_DEBUG("Initial state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
+ WOLF_DEBUG("Initial state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
WOLF_DEBUG("residual before perturbing: ", res.transpose());
ASSERT_MATRIX_APPROX(res, Vector3s::Zero(), 1e-8);
@@ -609,11 +609,11 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
Vector4s ori_perturbated = vquat_cam + 0.1 * Vector4s::Random();
ori_perturbated.normalize();
Vector7s pose_perturbated; pose_perturbated << pos_perturbated, ori_perturbated;
- S->getPPtr()->setState(pos_perturbated);
- S->getOPtr()->setState(ori_perturbated);
+ S->getP()->setState(pos_perturbated);
+ S->getO()->setState(ori_perturbated);
- S_p = S->getPPtr()->getState();
- S_o = S->getOPtr()->getState();
+ S_p = S->getP()->getState();
+ S_o = S->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -633,14 +633,14 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
- F1_p = F1->getPPtr()->getState();
- F1_o = F1->getOPtr()->getState();
- F2_p = F2->getPPtr()->getState();
- F2_o = F2->getOPtr()->getState();
- F3_p = F3->getPPtr()->getState();
- F3_o = F3->getOPtr()->getState();
- S_p = S ->getPPtr()->getState();
- S_o = S ->getOPtr()->getState();
+ F1_p = F1->getP()->getState();
+ F1_o = F1->getO()->getState();
+ F2_p = F2->getP()->getState();
+ F2_o = F2->getO()->getState();
+ F3_p = F3->getP()->getState();
+ F3_o = F3->getO()->getState();
+ S_p = S ->getP()->getState();
+ S_o = S ->getO()->getState();
c123->operator ()(F1_p.data(), F1_o.data(),
F2_p.data(), F2_o.data(),
@@ -648,7 +648,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
S_p. data(), S_o. data(),
res.data());
- WOLF_DEBUG("solved state: ", S->getPPtr()->getState().transpose(), " ", S->getOPtr()->getState().transpose());
+ WOLF_DEBUG("solved state: ", S->getP()->getState().transpose(), " ", S->getO()->getState().transpose());
WOLF_DEBUG("residual after solve: ", res.transpose());
WOLF_DEBUG(report, " AND UNION");
@@ -657,7 +657,7 @@ TEST_F(ConstraintAutodiffTrifocalTest, solve_S)
}
-class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifocalTest
+class FactorAutodiffTrifocalMultiPointTest : public FactorAutodiffTrifocalTest
{
/*
* In this test class we add 8 more points and perform optimization on the camera frames.
@@ -671,11 +671,11 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
public:
std::vector<FeatureBasePtr> fv1, fv2, fv3;
- std::vector<ConstraintAutodiffTrifocalPtr> cv123;
+ std::vector<FactorAutodiffTrifocalPtr> cv123;
virtual void SetUp() override
{
- ConstraintAutodiffTrifocalTest::SetUp();
+ FactorAutodiffTrifocalTest::SetUp();
Matrix<Scalar, 2, 9> c1p_can;
c1p_can <<
@@ -710,8 +710,8 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
fv3.push_back(std::make_shared<FeatureBase>("PIXEL", c3p_can.col(i), pix_cov));
I3->addFeature(fv3.at(i));
- cv123.push_back(std::make_shared<ConstraintAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, CTR_ACTIVE));
- fv3.at(i)->addConstraint(cv123.at(i));
+ cv123.push_back(std::make_shared<FactorAutodiffTrifocal>(fv1.at(i), fv2.at(i), fv3.at(i), proc_trifocal, false, FAC_ACTIVE));
+ fv3.at(i)->addFactor(cv123.at(i));
fv1.at(i)->addConstrainedBy(cv123.at(i));
fv2.at(i)->addConstrainedBy(cv123.at(i));
}
@@ -720,7 +720,7 @@ class ConstraintAutodiffTrifocalMultiPointTest : public ConstraintAutodiffTrifoc
};
-TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
+TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -732,22 +732,22 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->fix(); // This fixes the scale
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->fix(); // This fixes the scale
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, keep scale
- F1->getPPtr()->setState( pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( pos2 ); // this fixes the scale
- F2->getOPtr()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
+ F1->getP()->setState( pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( pos2 ); // this fixes the scale
+ F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -756,19 +756,19 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), pos2 , 1e-10);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-10);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), pos3 , 1e-10);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-10);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-10);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-10);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-10);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-10);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
@@ -785,7 +785,7 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point)
}
-TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
+TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -797,22 +797,22 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->fix(); // This fixes the scale
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->fix(); // This fixes the scale
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
- F1->getPPtr()->setState( 2 * pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( 2 * pos2 );
- F2->getOPtr()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( 2 * pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
+ F1->getP()->setState( 2 * pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( 2 * pos2 );
+ F2->getO()->setState(( vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( 2 * pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState(( vquat3 + 0.2*Vector4s::Random()).normalized());
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -821,19 +821,19 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), 2 * pos2, 1e-8);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), 2 * pos3, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), 2 * pos2, 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), 2 * pos3, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
@@ -849,9 +849,9 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
}
}
-#include "base/constraint/constraint_autodiff_distance_3D.h"
+#include "base/factor/factor_autodiff_distance_3D.h"
-TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
+TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
{
/*
* In this test we add 8 more points and perform optimization on the camera frames.
@@ -859,28 +859,28 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
* C1 is not optimized as it acts as the reference
* S is not optimized either as observability is compromised
* C2 and C3 are optimized
- * The scale is observed through a distance constraint
+ * The scale is observed through a distance factor
*
*/
- S ->getPPtr()->fix(); // do not calibrate sensor pos
- S ->getOPtr()->fix(); // do not calibrate sensor ori
- F1->getPPtr()->fix(); // Cam 1 acts as reference
- F1->getOPtr()->fix(); // Cam 1 acts as reference
- F2->getPPtr()->unfix(); // Estimate Cam 2 pos
- F2->getOPtr()->unfix(); // Estimate Cam 2 ori
- F3->getPPtr()->unfix(); // Estimate Cam 3 pos
- F3->getOPtr()->unfix(); // Estimate Cam 3 ori
+ S ->getP()->fix(); // do not calibrate sensor pos
+ S ->getO()->fix(); // do not calibrate sensor ori
+ F1->getP()->fix(); // Cam 1 acts as reference
+ F1->getO()->fix(); // Cam 1 acts as reference
+ F2->getP()->unfix(); // Estimate Cam 2 pos
+ F2->getO()->unfix(); // Estimate Cam 2 ori
+ F3->getP()->unfix(); // Estimate Cam 3 pos
+ F3->getO()->unfix(); // Estimate Cam 3 ori
// Perturbate states, change scale
- F1->getPPtr()->setState( pos1 );
- F1->getOPtr()->setState( vquat1 );
- F2->getPPtr()->setState( pos2 + 0.2*Vector3s::Random() );
- F2->getOPtr()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
- F3->getPPtr()->setState( pos3 + 0.2*Vector3s::Random());
- F3->getOPtr()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
-
- // Add a distance constraint to fix the scale
+ F1->getP()->setState( pos1 );
+ F1->getO()->setState( vquat1 );
+ F2->getP()->setState( pos2 + 0.2*Vector3s::Random() );
+ F2->getO()->setState((vquat2 + 0.2*Vector4s::Random()).normalized());
+ F3->getP()->setState( pos3 + 0.2*Vector3s::Random());
+ F3->getO()->setState((vquat3 + 0.2*Vector4s::Random()).normalized());
+
+ // Add a distance factor to fix the scale
Scalar distance = sqrt(2.0);
Scalar distance_std = 0.1;
@@ -888,17 +888,17 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
F3->addCapture(Cd);
FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1s(distance), Matrix1s(distance_std * distance_std));
Cd->addFeature(fd);
- ConstraintAutodiffDistance3DPtr cd = std::make_shared<ConstraintAutodiffDistance3D>(fd, F1, nullptr, false, CTR_ACTIVE);
- fd->addConstraint(cd);
+ FactorAutodiffDistance3DPtr cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
+ fd->addFactor(cd);
F1->addConstrainedBy(cd);
- cd->setStatus(CTR_INACTIVE);
+ cd->setStatus(FAC_INACTIVE);
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
WOLF_DEBUG("DISTANCE CONSTRAINT INACTIVE: \n", report);
problem->print(1,0,1,0);
- cd->setStatus(CTR_ACTIVE);
+ cd->setStatus(FAC_ACTIVE);
report = ceres_manager->solve(SolverManager::ReportVerbosity::BRIEF);
// Print results
@@ -906,19 +906,19 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
problem->print(1,0,1,0);
// Evaluate final states
- ASSERT_MATRIX_APPROX(F2->getPPtr()->getState(), pos2 , 1e-8);
- ASSERT_MATRIX_APPROX(F2->getOPtr()->getState(), vquat2, 1e-8);
- ASSERT_MATRIX_APPROX(F3->getPPtr()->getState(), pos3 , 1e-8);
- ASSERT_MATRIX_APPROX(F3->getOPtr()->getState(), vquat3, 1e-8);
-
- Eigen::VectorXs F1_p = F1->getPPtr()->getState();
- Eigen::VectorXs F1_o = F1->getOPtr()->getState();
- Eigen::VectorXs F2_p = F2->getPPtr()->getState();
- Eigen::VectorXs F2_o = F2->getOPtr()->getState();
- Eigen::VectorXs F3_p = F3->getPPtr()->getState();
- Eigen::VectorXs F3_o = F3->getOPtr()->getState();
- Eigen::VectorXs S_p = S ->getPPtr()->getState();
- Eigen::VectorXs S_o = S ->getOPtr()->getState();
+ ASSERT_MATRIX_APPROX(F2->getP()->getState(), pos2 , 1e-8);
+ ASSERT_MATRIX_APPROX(F2->getO()->getState(), vquat2, 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getP()->getState(), pos3 , 1e-8);
+ ASSERT_MATRIX_APPROX(F3->getO()->getState(), vquat3, 1e-8);
+
+ Eigen::VectorXs F1_p = F1->getP()->getState();
+ Eigen::VectorXs F1_o = F1->getO()->getState();
+ Eigen::VectorXs F2_p = F2->getP()->getState();
+ Eigen::VectorXs F2_o = F2->getO()->getState();
+ Eigen::VectorXs F3_p = F3->getP()->getState();
+ Eigen::VectorXs F3_o = F3->getO()->getState();
+ Eigen::VectorXs S_p = S ->getP()->getState();
+ Eigen::VectorXs S_o = S ->getO()->getState();
// evaluate residuals
Vector3s res;
@@ -937,12 +937,12 @@ TEST_F(ConstraintAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalTest.solve_F1";
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalTest.solve_F2";
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalTest.solve_F3";
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalTest.solve_S";
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalTest.solve_multi_point";
- // ::testing::GTEST_FLAG(filter) = "ConstraintAutodiffTrifocalMultiPointTest.solve_multi_point_distance";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F1";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F2";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_F3";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_S";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalTest.solve_multi_point";
+ // ::testing::GTEST_FLAG(filter) = "FactorAutodiffTrifocalMultiPointTest.solve_multi_point_distance";
return RUN_ALL_TESTS();
}
diff --git a/test/gtest_constraint_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
similarity index 80%
rename from test/gtest_constraint_odom_3D.cpp
rename to test/gtest_factor_odom_3D.cpp
index 945d49fbf7aeb42564f64cc06c45cd4408ffda7e..c3f767d56ea04682d2e33651c4e4e1a89f865585 100644
--- a/test/gtest_constraint_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -1,5 +1,5 @@
/**
- * \file gtest_constraint_odom_3D.cpp
+ * \file gtest_factor_odom_3D.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
@@ -7,7 +7,7 @@
#include "utils_gtest.h"
-#include "base/constraint/constraint_odom_3D.h"
+#include "base/factor/factor_odom_3D.h"
#include "base/capture/capture_motion.h"
#include "base/ceres_wrapper/ceres_manager.h"
@@ -40,25 +40,25 @@ CeresManager ceres_mgr(problem_ptr);
FrameBasePtr frm0 = problem_ptr->emplaceFrame(KEY_FRAME, problem_ptr->zeroState(), TimeStamp(0));
FrameBasePtr frm1 = problem_ptr->emplaceFrame(KEY_FRAME, delta, TimeStamp(1));
-// Capture, feature and constraint from frm1 to frm0
+// Capture, feature and factor from frm1 to frm0
CaptureBasePtr cap1 = frm1->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 1, nullptr, delta, 7, 6, nullptr));
FeatureBasePtr fea1 = cap1->addFeature(std::make_shared<FeatureBase>("ODOM 3D", delta, data_cov));
-ConstraintOdom3DPtr ctr1 = std::static_pointer_cast<ConstraintOdom3D>(fea1->addConstraint(std::make_shared<ConstraintOdom3D>(fea1, frm0, nullptr))); // create and add
-ConstraintBasePtr dummy = frm0->addConstrainedBy(ctr1);
+FactorOdom3DPtr ctr1 = std::static_pointer_cast<FactorOdom3D>(fea1->addFactor(std::make_shared<FactorOdom3D>(fea1, frm0, nullptr))); // create and add
+FactorBasePtr dummy = frm0->addConstrainedBy(ctr1);
////////////////////////////////////////////////////////
-TEST(ConstraintOdom3D, check_tree)
+TEST(FactorOdom3D, check_tree)
{
ASSERT_TRUE(problem_ptr->check(0));
}
-TEST(ConstraintOdom3D, expectation)
+TEST(FactorOdom3D, expectation)
{
ASSERT_MATRIX_APPROX(ctr1->expectation() , delta, 1e-6);
}
-TEST(ConstraintOdom3D, fix_0_solve)
+TEST(FactorOdom3D, fix_0_solve)
{
// Fix frame 0, perturb frm1
@@ -73,7 +73,7 @@ TEST(ConstraintOdom3D, fix_0_solve)
}
-TEST(ConstraintOdom3D, fix_1_solve)
+TEST(FactorOdom3D, fix_1_solve)
{
// Fix frame 1, perturb frm0
frm0->unfix();
diff --git a/test/gtest_constraint_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
similarity index 79%
rename from test/gtest_constraint_pose_2D.cpp
rename to test/gtest_factor_pose_2D.cpp
index 125893e67c05a659327f7ac7d81391219845d50b..d7bce0c84f734613f1a2f143bebf2690897149f2 100644
--- a/test/gtest_constraint_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_constraint_pose_2D.cpp
+ * \file gtest_factor_pose_2D.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
-#include "base/constraint/constraint_pose_2D.h"
+#include "base/factor/factor_pose_2D.h"
#include "utils_gtest.h"
#include "base/capture/capture_motion.h"
@@ -32,24 +32,24 @@ CeresManager ceres_mgr(problem);
// Two frames
FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
-// Capture, feature and constraint from frm1 to frm0
+// Capture, feature and factor from frm1 to frm0
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 2D", 0, nullptr, pose, 3, 3, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-ConstraintPose2DPtr ctr0 = std::static_pointer_cast<ConstraintPose2D>(fea0->addConstraint(std::make_shared<ConstraintPose2D>(fea0)));
+FactorPose2DPtr ctr0 = std::static_pointer_cast<FactorPose2D>(fea0->addFactor(std::make_shared<FactorPose2D>(fea0)));
////////////////////////////////////////////////////////
-TEST(ConstraintPose2D, check_tree)
+TEST(FactorPose2D, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
-//TEST(ConstraintFix, expectation)
+//TEST(FactorFix, expectation)
//{
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(ConstraintPose2D, solve)
+TEST(FactorPose2D, solve)
{
// Fix frame 0, perturb frm1
diff --git a/test/gtest_constraint_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
similarity index 82%
rename from test/gtest_constraint_pose_3D.cpp
rename to test/gtest_factor_pose_3D.cpp
index 05de4bb61427285fa846fd25a7b7e4d379bca30e..0af4c3c406f02c9c87cabb35b5e43fd7137fe28b 100644
--- a/test/gtest_constraint_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -1,11 +1,11 @@
/**
- * \file gtest_constraint_fix_3D.cpp
+ * \file gtest_factor_fix_3D.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
-#include "base/constraint/constraint_pose_3D.h"
+#include "base/factor/factor_pose_3D.h"
#include "utils_gtest.h"
#include "base/capture/capture_motion.h"
@@ -38,24 +38,24 @@ CeresManager ceres_mgr(problem);
// Two frames
FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeStamp(0));
-// Capture, feature and constraint
+// Capture, feature and factor
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>("ODOM 3D", 0, nullptr, pose7, 7, 6, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
+FactorPose3DPtr ctr0 = std::static_pointer_cast<FactorPose3D>(fea0->addFactor(std::make_shared<FactorPose3D>(fea0)));
////////////////////////////////////////////////////////
-TEST(ConstraintPose3D, check_tree)
+TEST(FactorPose3D, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
-//TEST(ConstraintFix3D, expectation)
+//TEST(FactorFix3D, expectation)
//{
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(ConstraintPose3D, solve)
+TEST(FactorPose3D, solve)
{
// Fix frame 0, perturb frm1
diff --git a/test/gtest_factor_sparse.cpp b/test/gtest_factor_sparse.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6c8f212be2030c5d04707eb5b5798848f36d8710
--- /dev/null
+++ b/test/gtest_factor_sparse.cpp
@@ -0,0 +1,52 @@
+/**
+ * \file gtest_factor_sparse.cpp
+ *
+ * Created on: Apr 25, 2017
+ * \author: jsola
+ */
+
+#include "utils_gtest.h"
+
+#include "factor_sparse.h"
+
+using namespace wolf;
+
+// Dummy class for avoiding the pure virtual compilation error
+template <JacobianMethod J>
+class FactorSparseObject : public FactorSparse<1, 2, 1>
+{
+ public:
+ FactorSparseObject(FactorType _tp, bool _apply_loss_function, FactorStatus _status, StateBlockPtr _xy, StateBlockPtr _theta) :
+ FactorSparse<1, 2, 1>(_tp, _apply_loss_function, _status, _xy, _theta)
+ {
+ //
+ }
+ virtual ~FactorSparseObject(){}
+
+ virtual JacobianMethod getJacobianMethod() const
+ {
+ return J;
+ }
+};
+
+TEST(FactorSparseAnalytic, Constructor)
+{
+ FactorSparseObject<JAC_ANALYTIC> fac_analytic(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_analytic.getJacobianMethod(), JAC_ANALYTIC);
+ ASSERT_EQ(fac_analytic.getApplyLossFunction(), false);
+ ASSERT_EQ(fac_analytic.getStatus(), FAC_ACTIVE);
+ ASSERT_EQ(fac_analytic.getSize(), 1);
+
+ FactorSparseObject<JAC_AUTO> fac_auto(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_auto.getJacobianMethod(), JAC_AUTO);
+
+ FactorSparseObject<JAC_NUMERIC> fac_numeric(FAC_AHP, false, FAC_ACTIVE, std::make_shared<StateBlock>(2), std::make_shared<StateBlock>(1));
+ ASSERT_EQ(fac_numeric.getJacobianMethod(), JAC_NUMERIC);
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
+
diff --git a/test/gtest_feature_IMU.cpp b/test/gtest_feature_IMU.cpp
index 5ce136d6625931f585621b1e664430defc70969c..82828c6c274a3ed2e72d53f15e8f625583644697 100644
--- a/test/gtest_feature_IMU.cpp
+++ b/test/gtest_feature_IMU.cpp
@@ -61,7 +61,7 @@ class FeatureIMU_test : public testing::Test
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
// give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
imu_ptr = std::make_shared<CaptureIMU>(t, sensor_ptr, data_, Eigen::Matrix6s::Identity(), Eigen::Vector6s::Zero());
- imu_ptr->setFramePtr(origin_frame); //to get ptr to Frm in processorIMU and then get biases
+ imu_ptr->setFrame(origin_frame); //to get ptr to Frm in processorIMU and then get biases
//process data
data_ << 2, 0, 9.8, 0, 0, 0;
@@ -75,22 +75,22 @@ class FeatureIMU_test : public testing::Test
sensor_ptr->process(imu_ptr);
//create Frame
- ts = problem->getProcessorMotionPtr()->getBuffer().get().back().ts_;
- state_vec = problem->getProcessorMotionPtr()->getCurrentState();
+ ts = problem->getProcessorMotion()->getBuffer().get().back().ts_;
+ state_vec = problem->getProcessorMotion()->getCurrentState();
last_frame = std::make_shared<FrameBase>(KEY_FRAME, ts, std::make_shared<StateBlock>(state_vec.head(3)), std::make_shared<StateBlock>(state_vec.segment(3,4)), std::make_shared<StateBlock>(state_vec.head(3)));
- problem->getTrajectoryPtr()->addFrame(last_frame);
+ problem->getTrajectory()->addFrame(last_frame);
//create a feature
- delta_preint = problem->getProcessorMotionPtr()->getMotion().delta_integr_;
- delta_preint_cov = problem->getProcessorMotionPtr()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
- VectorXs calib_preint = problem->getProcessorMotionPtr()->getBuffer().getCalibrationPreint();
- dD_db_jacobians = problem->getProcessorMotionPtr()->getMotion().jacobian_calib_;
+ delta_preint = problem->getProcessorMotion()->getMotion().delta_integr_;
+ delta_preint_cov = problem->getProcessorMotion()->getMotion().delta_integr_cov_ + MatrixXs::Identity(9,9)*1e-08;
+ VectorXs calib_preint = problem->getProcessorMotion()->getBuffer().getCalibrationPreint();
+ dD_db_jacobians = problem->getProcessorMotion()->getMotion().jacobian_calib_;
feat_imu = std::make_shared<FeatureIMU>(delta_preint,
delta_preint_cov,
calib_preint,
dD_db_jacobians,
imu_ptr);
- feat_imu->setCapturePtr(imu_ptr); //associate the feature to a capture
+ feat_imu->setCapture(imu_ptr); //associate the feature to a capture
}
@@ -113,7 +113,7 @@ TEST_F(FeatureIMU_test, check_frame)
// set variables
using namespace wolf;
- FrameBasePtr left_frame = feat_imu->getFramePtr();
+ FrameBasePtr left_frame = feat_imu->getFrame();
wolf::TimeStamp t;
left_frame->getTimeStamp(t);
origin_frame->getTimeStamp(ts);
@@ -124,12 +124,12 @@ TEST_F(FeatureIMU_test, check_frame)
ASSERT_TRUE(left_frame->isKey());
wolf::StateBlockPtr origin_pptr, origin_optr, origin_vptr, left_pptr, left_optr, left_vptr;
- origin_pptr = origin_frame->getPPtr();
- origin_optr = origin_frame->getOPtr();
- origin_vptr = origin_frame->getVPtr();
- left_pptr = left_frame->getPPtr();
- left_optr = left_frame->getOPtr();
- left_vptr = left_frame->getVPtr();
+ origin_pptr = origin_frame->getP();
+ origin_optr = origin_frame->getO();
+ origin_vptr = origin_frame->getV();
+ left_pptr = left_frame->getP();
+ left_optr = left_frame->getO();
+ left_vptr = left_frame->getV();
ASSERT_MATRIX_APPROX(origin_pptr->getState(), left_pptr->getState(), wolf::Constants::EPS_SMALL);
Eigen::Map<const Eigen::Quaternions> origin_Quat(origin_optr->getState().data()), left_Quat(left_optr->getState().data());
@@ -149,15 +149,15 @@ TEST_F(FeatureIMU_test, access_members)
ASSERT_MATRIX_APPROX(feat_imu->getMeasurement().transpose(), delta.transpose(), wolf::Constants::EPS);
}
-//TEST_F(FeatureIMU_test, addConstraint)
+//TEST_F(FeatureIMU_test, addFactor)
//{
// using namespace wolf;
//
// FrameBasePtr frm_imu = std::static_pointer_cast<FrameBase>(last_frame);
// auto cap_imu = last_frame->getCaptureList().back();
-// ConstraintIMUPtr constraint_imu = std::make_shared<ConstraintIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
-// feat_imu->addConstraint(constraint_imu);
-// origin_frame->addConstrainedBy(constraint_imu);
+// FactorIMUPtr factor_imu = std::make_shared<FactorIMU>(feat_imu, std::static_pointer_cast<CaptureIMU>(cap_imu), processor_ptr_);
+// feat_imu->addFactor(factor_imu);
+// origin_frame->addConstrainedBy(factor_imu);
//}
int main(int argc, char **argv)
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 98e032cb3ea7b0ba8f5bb975c1ed7a7adc43f4e5..06436282a934f4b61bcfe04e50c77b1f3f27eea8 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -11,7 +11,7 @@
#include "base/frame_base.h"
#include "base/sensor/sensor_odom_2D.h"
#include "base/processor/processor_odom_2D.h"
-#include "base/constraint/constraint_odom_2D.h"
+#include "base/factor/factor_odom_2D.h"
#include "base/capture/capture_motion.h"
#include <iostream>
@@ -39,16 +39,16 @@ TEST(FrameBase, StateBlocks)
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
ASSERT_EQ(F->getStateBlockVec().size(), (unsigned int) 3);
- ASSERT_EQ(F->getPPtr()->getState().size(),(unsigned int) 2);
- ASSERT_EQ(F->getOPtr()->getState().size(),(unsigned int) 1);
- ASSERT_EQ(F->getVPtr(), nullptr);
+ ASSERT_EQ(F->getP()->getState().size(),(unsigned int) 2);
+ ASSERT_EQ(F->getO()->getState().size(),(unsigned int) 1);
+ ASSERT_EQ(F->getV(), nullptr);
}
TEST(FrameBase, LinksBasic)
{
FrameBasePtr F = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- ASSERT_FALSE(F->getTrajectoryPtr());
+ ASSERT_FALSE(F->getTrajectory());
ASSERT_FALSE(F->getProblem());
// ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
// ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
@@ -61,14 +61,14 @@ TEST(FrameBase, LinksBasic)
TEST(FrameBase, LinksToTree)
{
- // Problem with 2 frames and one motion constraint between them
+ // Problem with 2 frames and one motion factor between them
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
- P->getHardwarePtr()->addSensor(S);
+ P->getHardware()->addSensor(S);
FrameBasePtr F1 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
T->addFrame(F1);
FrameBasePtr F2 = make_shared<FrameBase>(1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
@@ -79,8 +79,8 @@ TEST(FrameBase, LinksToTree)
ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
FeatureBasePtr f = make_shared<FeatureBase>("f", Vector1s(1), Matrix<Scalar,1,1>::Identity()*.01);
C->addFeature(f);
- ConstraintOdom2DPtr c = make_shared<ConstraintOdom2D>(f, F2, p);
- f->addConstraint(c);
+ FactorOdom2DPtr c = make_shared<FactorOdom2D>(f, F2, p);
+ f->addFactor(c);
// c-by link F2 -> c not yet established
ASSERT_TRUE(F2->getConstrainedByList().empty());
@@ -94,12 +94,12 @@ TEST(FrameBase, LinksToTree)
// tree is now consistent
ASSERT_TRUE(P->check(0));
- // F1 has one capture and no constraints-by
+ // F1 has one capture and no factors-by
ASSERT_FALSE(F1->getCaptureList().empty());
ASSERT_TRUE(F1->getConstrainedByList().empty());
ASSERT_EQ(F1->getHits() , (unsigned int) 0);
- // F2 has no capture and one constraint-by
+ // F2 has no capture and one factor-by
ASSERT_TRUE(F2->getCaptureList().empty());
ASSERT_FALSE(F2->getConstrainedByList().empty());
ASSERT_EQ(F2->getHits() , (unsigned int) 1);
@@ -111,13 +111,13 @@ TEST(FrameBase, LinksToTree)
ASSERT_FALSE(F1->isFixed());
F1->fix();
ASSERT_TRUE(F1->isFixed());
- F1->getPPtr()->unfix();
+ F1->getP()->unfix();
ASSERT_FALSE(F1->isFixed());
F1->unfix();
ASSERT_FALSE(F1->isFixed());
- F1->getPPtr()->fix();
+ F1->getP()->fix();
ASSERT_FALSE(F1->isFixed());
- F1->getOPtr()->fix();
+ F1->getO()->fix();
ASSERT_TRUE(F1->isFixed());
// set key
@@ -151,9 +151,9 @@ TEST(FrameBase, GetSetState)
// Set the state, check that state blocks hold the current states
F.setState(x);
- ASSERT_TRUE((p - F.getPPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((q - F.getOPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- ASSERT_TRUE((v - F.getVPtr()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((p - F.getP()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((q - F.getO()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
+ ASSERT_TRUE((v - F.getV()->getState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
// Get the state, form 1 by reference
F.getState(x1);
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index fcf60798e88d353bf18371d2a5c3f455b98f597e..5f479b19ec09a04c604aaca010c98cfd6bd37c6f 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -9,7 +9,7 @@
// Classes under test
#include "base/processor/processor_odom_2D.h"
-#include "base/constraint/constraint_odom_2D.h"
+#include "base/factor/factor_odom_2D.h"
// Wolf includes
#include "base/sensor/sensor_odom_2D.h"
@@ -73,7 +73,7 @@ void show(const ProblemPtr& problem)
using std::endl;
cout << std::setprecision(4);
- for (FrameBasePtr F : problem->getTrajectoryPtr()->getFrameList())
+ for (FrameBasePtr F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
{
@@ -97,7 +97,7 @@ void show(const ProblemPtr& problem)
}
}
-TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
+TEST(Odom2D, FactorFix_and_FactorOdom2D)
{
using std::cout;
using std::endl;
@@ -109,8 +109,8 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
// a
// |
// GND
- // `absolute` is made with ConstraintFix
- // `motion` is made with ConstraintOdom2D
+ // `absolute` is made with FactorFix
+ // `motion` is made with FactorOdom2D
std::cout << std::setprecision(4);
@@ -132,7 +132,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
FrameBasePtr F1 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
CaptureBasePtr C1 = F1->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f1 = C1->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
- ConstraintBasePtr c1 = f1->addConstraint(std::make_shared<ConstraintOdom2D>(f1, F0, nullptr));
+ FactorBasePtr c1 = f1->addFactor(std::make_shared<FactorOdom2D>(f1, F0, nullptr));
F0->addConstrainedBy(c1);
// KF2 and motion from KF1
@@ -140,7 +140,7 @@ TEST(Odom2D, ConstraintFix_and_ConstraintOdom2D)
FrameBasePtr F2 = Pr->emplaceFrame(KEY_FRAME, Vector3s::Zero(), t);
CaptureBasePtr C2 = F2->addCapture(std::make_shared<CaptureBase>("ODOM 2D", t));
FeatureBasePtr f2 = C2->addFeature(std::make_shared<FeatureBase>("ODOM 2D", delta, delta_cov));
- ConstraintBasePtr c2 = f2->addConstraint(std::make_shared<ConstraintOdom2D>(f2, F1, nullptr));
+ FactorBasePtr c2 = f2->addFactor(std::make_shared<FactorOdom2D>(f2, F1, nullptr));
F1->addConstrainedBy(c2);
ASSERT_TRUE(Pr->check(0));
@@ -412,7 +412,7 @@ TEST(Odom2D, KF_callback)
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
- ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getLastKeyFrameCovariance() , integrated_cov_vector [n_split], 1e-6);
////////////////////////////////////////////////////////////////
@@ -453,7 +453,7 @@ TEST(Odom2D, KF_callback)
ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_1) , integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
- ASSERT_POSE2D_APPROX(problem->getLastKeyFramePtr()->getState() , integrated_pose_vector[n_split], 1e-6);
+ ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(problem->getFrameCovariance(keyframe_2) , integrated_cov_vector [n_split], 1e-6);
// Check full trajectory
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 6bb1eccbc1cac0070a412b63e7ebcf3a62d238b1..e39c870881bf6e2abb57e771a2b79a3ed3706ce2 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -27,10 +27,10 @@ SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(proble
TEST(ParameterPrior, initial_extrinsics)
{
ASSERT_TRUE(problem_ptr->check(0));
- ASSERT_TRUE(odom_sensor_ptr_->getPPtr());
- ASSERT_TRUE(odom_sensor_ptr_->getOPtr());
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),initial_extrinsics.head(3),1e-9);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),initial_extrinsics.tail(4),1e-9);
+ ASSERT_TRUE(odom_sensor_ptr_->getP());
+ ASSERT_TRUE(odom_sensor_ptr_->getO());
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),initial_extrinsics.head(3),1e-9);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),initial_extrinsics.tail(4),1e-9);
}
TEST(ParameterPrior, prior_p)
@@ -39,7 +39,7 @@ TEST(ParameterPrior, prior_p)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_o)
@@ -48,7 +48,7 @@ TEST(ParameterPrior, prior_o)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getOPtr()->getState(),prior_extrinsics.tail(4),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getO()->getState(),prior_extrinsics.tail(4),1e-6);
}
TEST(ParameterPrior, prior_p_overwrite)
@@ -57,7 +57,7 @@ TEST(ParameterPrior, prior_p_overwrite)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState(),prior2_extrinsics.head(3),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState(),prior2_extrinsics.head(3),1e-6);
}
TEST(ParameterPrior, prior_p_segment)
@@ -66,7 +66,7 @@ TEST(ParameterPrior, prior_p_segment)
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
- ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getPPtr()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
+ ASSERT_MATRIX_APPROX(odom_sensor_ptr_->getP()->getState().segment(1,2),prior_extrinsics.segment(1,2),1e-6);
}
TEST(ParameterPrior, prior_o_segment)
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index 1cf49acaa2bf803f3aa7108ae243bfe7cf4495c8..8ee7623dc68b7f637677fd24ceb6f90b083a4b39 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -24,9 +24,9 @@ TEST(Problem, create)
ProblemPtr P = Problem::create("POV 3D");
// check double ointers to branches
- ASSERT_EQ(P, P->getHardwarePtr()->getProblem());
- ASSERT_EQ(P, P->getTrajectoryPtr()->getProblem());
- ASSERT_EQ(P, P->getMapPtr()->getProblem());
+ ASSERT_EQ(P, P->getHardware()->getProblem());
+ ASSERT_EQ(P, P->getTrajectory()->getProblem());
+ ASSERT_EQ(P, P->getMap()->getProblem());
// check frame structure through the state size
ASSERT_EQ(P->getFrameStructureSize(), 10);
@@ -42,7 +42,7 @@ TEST(Problem, Sensors)
// check pointers
ASSERT_EQ(P, S->getProblem());
- ASSERT_EQ(P->getHardwarePtr(), S->getHardwarePtr());
+ ASSERT_EQ(P->getHardware(), S->getHardware());
}
@@ -51,7 +51,7 @@ TEST(Problem, Processor)
ProblemPtr P = Problem::create("PO 3D");
// check motion processor is null
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// add a motion sensor and processor
SensorBasePtr Sm = std::make_shared<SensorOdom3D>((Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D()); // with dummy intrinsics
@@ -62,12 +62,12 @@ TEST(Problem, Processor)
Sm->addProcessor(Pm);
// check motion processor IS NOT set by addSensor <-- using InstallProcessor it should, see test Installers
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// set processor motion
P->setProcessorMotion(Pm);
// re-check motion processor IS set by addSensor
- ASSERT_EQ(P->getProcessorMotionPtr(), Pm);
+ ASSERT_EQ(P->getProcessorMotion(), Pm);
}
TEST(Problem, Installers)
@@ -87,16 +87,16 @@ TEST(Problem, Installers)
S->addProcessor(pt);
// check motion processor IS NOT set
- ASSERT_FALSE(P->getProcessorMotionPtr());
+ ASSERT_FALSE(P->getProcessorMotion());
// install processor motion
ProcessorBasePtr pm = P->installProcessor("ODOM 3D", "odom integrator", "odometer", wolf_root + "/src/examples/processor_odom_3D.yaml");
// check motion processor is set
- ASSERT_TRUE(P->getProcessorMotionPtr());
+ ASSERT_TRUE(P->getProcessorMotion());
// check motion processor is correct
- ASSERT_EQ(P->getProcessorMotionPtr(), pm);
+ ASSERT_EQ(P->getProcessorMotion(), pm);
}
TEST(Problem, SetOrigin_PO_2D)
@@ -109,25 +109,25 @@ TEST(Problem, SetOrigin_PO_2D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- // check that we have one frame, one capture, one feature, one constraint
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ // check that we have one frame, one capture, one feature, one factor
+ TrajectoryBasePtr T = P->getTrajectory();
ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFramePtr();
+ FrameBasePtr F = P->getLastFrame();
ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
CaptureBasePtr C = F->getCaptureList().front();
ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getConstraintList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
- // check that the constraint is absolute (no pointers to other F, f, or L)
- ConstraintBasePtr c = f->getConstraintList().front();
- ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getLandmarkOtherPtr());
+ // check that the factor is absolute (no pointers to other F, f, or L)
+ FactorBasePtr c = f->getFactorList().front();
+ ASSERT_FALSE(c->getFrameOther());
+ ASSERT_FALSE(c->getLandmarkOther());
// check that the Feature measurement and covariance are the ones provided
ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
@@ -146,25 +146,25 @@ TEST(Problem, SetOrigin_PO_3D)
P->setPrior(x0, P0, t0, 1.0);
// check that no sensor has been added
- ASSERT_EQ(P->getHardwarePtr()->getSensorList().size(), (unsigned int) 0);
+ ASSERT_EQ(P->getHardware()->getSensorList().size(), (unsigned int) 0);
// check that the state is correct
ASSERT_TRUE((x0 - P->getCurrentState()).isMuchSmallerThan(1, Constants::EPS_SMALL));
- // check that we have one frame, one capture, one feature, one constraint
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ // check that we have one frame, one capture, one feature, one factor
+ TrajectoryBasePtr T = P->getTrajectory();
ASSERT_EQ(T->getFrameList().size(), (unsigned int) 1);
- FrameBasePtr F = P->getLastFramePtr();
+ FrameBasePtr F = P->getLastFrame();
ASSERT_EQ(F->getCaptureList().size(), (unsigned int) 1);
CaptureBasePtr C = F->getCaptureList().front();
ASSERT_EQ(C->getFeatureList().size(), (unsigned int) 1);
FeatureBasePtr f = C->getFeatureList().front();
- ASSERT_EQ(f->getConstraintList().size(), (unsigned int) 1);
+ ASSERT_EQ(f->getFactorList().size(), (unsigned int) 1);
- // check that the constraint is absolute (no pointers to other F, f, or L)
- ConstraintBasePtr c = f->getConstraintList().front();
- ASSERT_FALSE(c->getFrameOtherPtr());
- ASSERT_FALSE(c->getLandmarkOtherPtr());
+ // check that the factor is absolute (no pointers to other F, f, or L)
+ FactorBasePtr c = f->getFactorList().front();
+ ASSERT_FALSE(c->getFrameOther());
+ ASSERT_FALSE(c->getLandmarkOther());
// check that the Feature measurement and covariance are the ones provided
ASSERT_TRUE((x0 - f->getMeasurement()).isMuchSmallerThan(1, Constants::EPS_SMALL));
@@ -190,7 +190,7 @@ TEST(Problem, emplaceFrame_factory)
ASSERT_EQ(f2->getType().compare("POV 3D"), 0);
// check that all frames are effectively in the trajectory
- ASSERT_EQ(P->getTrajectoryPtr()->getFrameList().size(), (unsigned int) 3);
+ ASSERT_EQ(P->getTrajectory()->getFrameList().size(), (unsigned int) 3);
// check that all frames are linked to Problem
ASSERT_EQ(f0->getProblem(), P);
@@ -207,12 +207,12 @@ TEST(Problem, StateBlocks)
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs, wolf_root + "/src/examples/sensor_odom_3D.yaml");
- ASSERT_EQ(P->getStateBlockList().size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) 2);
// 3 state blocks, fixed
SensorBasePtr St = P->installSensor ("CAMERA", "camera", xs, wolf_root + "/src/examples/camera_params_ueye_sim.yaml");
- ASSERT_EQ(P->getStateBlockList().size(), (unsigned int) (2 + 3));
+ ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int) (2 + 3));
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int) (2 + 3));
ProcessorParamsTrackerFeaturePtr params = std::make_shared<ProcessorParamsTrackerFeature>();
@@ -226,14 +226,14 @@ TEST(Problem, StateBlocks)
// 2 state blocks, estimated
P->emplaceFrame("PO 3D", KEY_FRAME, xs, 0);
- ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
+ ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
ASSERT_EQ(P->getStateBlockNotificationMap().size(), (unsigned int)(2 + 3 + 2));
// P->print(4,1,1,1);
// change some SB properties
St->unfixExtrinsics();
- ASSERT_EQ(P->getStateBlockList().size(), (unsigned int)(2 + 3 + 2));
+ ASSERT_EQ(P->getStateBlockPtrList().size(), (unsigned int)(2 + 3 + 2));
ASSERT_EQ(P->getStateBlockNotificationMap().size(),(unsigned int)(2 + 3 + 2 /*+ 2*/)); // XXX: 2 more notifications on the same SB!
// P->print(4,1,1,1);
diff --git a/test/gtest_processor_IMU.cpp b/test/gtest_processor_IMU.cpp
index a6ee8c8ac4df0bc05094d2fdc0b72a653cb2bab7..8a966f77b39390e80ad2ad5e68655bbb764ebf85 100644
--- a/test/gtest_processor_IMU.cpp
+++ b/test/gtest_processor_IMU.cpp
@@ -172,7 +172,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
- 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met
- 1 frame that would be used by future data
*/
- ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3);
+ ASSERT_EQ(problem->getTrajectory()->getFrameList().size(),(unsigned int) 3);
/* if max_time_span == 2, Wolf tree should be
@@ -195,7 +195,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
Estim, ts=2.1, x = ( . . .)
C4 -> S1
*/
- //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above
+ //TODO : ASSERT TESTS to make sure the factors are correctly set + check the tree above
}
@@ -215,20 +215,20 @@ TEST_F(ProcessorIMUt, interpolate)
// make one step to depart from origin
cap_imu_ptr->setTimeStamp(0.05);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_ref = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_ref = problem->getProcessorMotion()->getMotion();
// make two steps, then pretend it's just one
cap_imu_ptr->setTimeStamp(0.10);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_int_gt = problem->getProcessorMotion()->getMotion();
cap_imu_ptr->setTimeStamp(0.15);
sensor_ptr->process(cap_imu_ptr);
- Motion mot_final = problem->getProcessorMotionPtr()->getMotion();
+ Motion mot_final = problem->getProcessorMotion()->getMotion();
mot_final.delta_ = mot_final.delta_integr_;
Motion mot_sec = mot_final;
-// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0);
+// problem->getProcessorMotion()->getBuffer().print(0,1,1,0);
class P : public wolf::ProcessorIMU
{
@@ -267,8 +267,8 @@ TEST_F(ProcessorIMUt, acc_x)
Vector6s b; b << 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
}
TEST_F(ProcessorIMUt, acc_y)
@@ -293,8 +293,8 @@ TEST_F(ProcessorIMUt, acc_y)
Vector6s b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 1000; //how many ms
@@ -307,8 +307,8 @@ TEST_F(ProcessorIMUt, acc_y)
// advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20
x << 0,10,0, 0,0,0,1, 0,20,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, acc_z)
@@ -330,8 +330,8 @@ TEST_F(ProcessorIMUt, acc_z)
Vector6s b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS_SMALL);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL);
//do so for 5s
const unsigned int iter = 50; //how 0.1s
@@ -344,8 +344,8 @@ TEST_F(ProcessorIMUt, acc_z)
// advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10
x << 0,0,25, 0,0,0,1, 0,0,10;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS);
- ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibration() , b, wolf::Constants::EPS);
+ ASSERT_MATRIX_APPROX(problem->getProcessorMotion()->getLast()->getCalibrationPreint() , b, wolf::Constants::EPS);
}
TEST_F(ProcessorIMUt, check_Covariance)
@@ -361,8 +361,8 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr);
- VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ VectorXs delta_preint(problem->getProcessorMotion()->getMotion().delta_integr_);
+// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotion()->getCurrentDeltaPreintCov();
ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
@@ -428,8 +428,8 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// init things
problem->setPrior(x0, P0, t, 0.01);
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+ problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
+ problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
@@ -484,8 +484,8 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
Vector6s bias; bias << abx,aby,0, 0,0,0;
Vector3s acc_bias = bias.head(3);
- problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
- problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
+ problem->getProcessorMotion()->getOrigin()->setCalibration(bias);
+ problem->getProcessorMotion()->getLast()->setCalibrationPreint(bias);
wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index aba23263605f27b70cd04886374bb82013abf6e5..7479ca984eda6dff7504595c66a959ae1d3aebfa 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -89,7 +89,7 @@ TEST(ProcessorBase, KeyFrameCallback)
// problem->print(4,1,1,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 2D")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 2D")==0 );
}
}
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
index 4e2b427fb500e01122994abd8ded959b8c460bff..425f6a96b9a9af4dcdcf5a491a8fb2ac75f98cf3 100644
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
@@ -99,10 +99,10 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
F4->addCapture(capture_dummy);
// Add first three states to trajectory
- problem->getTrajectoryPtr()->addFrame(F1);
- problem->getTrajectoryPtr()->addFrame(F2);
- problem->getTrajectoryPtr()->addFrame(F3);
- problem->getTrajectoryPtr()->addFrame(F4);
+ problem->getTrajectory()->addFrame(F1);
+ problem->getTrajectory()->addFrame(F2);
+ problem->getTrajectory()->addFrame(F3);
+ problem->getTrajectory()->addFrame(F4);
// Add same covariances for all states
Eigen::Matrix2s position_covariance_matrix;
@@ -148,7 +148,7 @@ TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
1.2,
sensor_ptr);
// Make 3rd frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFrame(F3);
// trigger search for loopclosure
processor_ptr_point2d->process(incomming_dummy);
@@ -171,17 +171,17 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
position_covariance_matrix << 5.0, 0.0,
0.0, 9.0;
- problem->addCovarianceBlock( F1->getPPtr(), F1->getPPtr(),
+ problem->addCovarianceBlock( F1->getP(), F1->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F2->getPPtr(), F2->getPPtr(),
+ problem->addCovarianceBlock( F2->getP(), F2->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F3->getPPtr(), F3->getPPtr(),
+ problem->addCovarianceBlock( F3->getP(), F3->getP(),
position_covariance_matrix);
- problem->addCovarianceBlock( F4->getPPtr(), F4->getPPtr(),
+ problem->addCovarianceBlock( F4->getP(), F4->getP(),
position_covariance_matrix);
// Make 3rd frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F3);
+ problem->getTrajectory()->setLastKeyFrame(F3);
// trigger search for loopclosure
processor_ptr_ellipse2d->process(incomming_dummy);
@@ -193,7 +193,7 @@ TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
// Make 4th frame last Keyframe
- problem->getTrajectoryPtr()->setLastKeyFramePtr(F4);
+ problem->getTrajectory()->setLastKeyFrame(F4);
// trigger search for loopclosure again
processor_ptr_ellipse2d->process(incomming_dummy);
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index d94815f9210090c887bebde5d416ce8fbeaf0e4f..6819cba18e0ec605a6272e8ac6e05e44cf58a6d3 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -52,9 +52,9 @@ using namespace wolf;
// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal detectNewFeatures is empty." << std::endl;
//}
//
-//TEST(ProcessorTrackerFeatureTrifocal, createConstraint)
+//TEST(ProcessorTrackerFeatureTrifocal, createFactor)
//{
-// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal createConstraint is empty." << std::endl;
+// std::cout << "033[1;33m [WARN]:033[0m gtest for ProcessorTrackerFeatureTrifocal createFactor is empty." << std::endl;
//}
TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
@@ -137,11 +137,11 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
capt_trk = make_shared<CaptureImage>(t, sens_trk, image);
proc_trk->process(capt_trk);
- CaptureBasePtr prev = proc_trk->getPrevOriginPtr();
+ CaptureBasePtr prev = proc_trk->getPrevOrigin();
problem->print(2,0,0,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFramePtr()->getType().compare("PO 3D")==0 );
+ ASSERT_TRUE( problem->getLastKeyFrame()->getType().compare("PO 3D")==0 );
}
}
diff --git a/test/gtest_sensor_camera.cpp b/test/gtest_sensor_camera.cpp
index 14b24deb1a0e23b39dc9bb55d1cf24355bd1a357..9ca4d657144b0ed1ae1568a4495f674e3c130dc7 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -48,17 +48,17 @@ TEST(SensorCamera, Intrinsics_Raw_Rectified)
// default is raw
ASSERT_TRUE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
cam->useRectifiedImages();
ASSERT_FALSE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_rectified, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_rectified, cam->getIntrinsic()->getState(), 1e-8);
cam->useRawImages();
ASSERT_TRUE(cam->isUsingRawImages());
ASSERT_MATRIX_APPROX(K_raw, cam->getIntrinsicMatrix(), 1e-8);
- ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsicPtr()->getState(), 1e-8);
+ ASSERT_MATRIX_APPROX(k_raw, cam->getIntrinsic()->getState(), 1e-8);
}
TEST(SensorCamera, Distortion)
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 9d5fe610b2ef612d2f21d8be00bfafd55e2fbca9..04c7e6d7a06b2d76224c9b4c1df6e5da3a6f7f03 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -12,7 +12,7 @@
#include "base/sensor/sensor_base.h"
#include "base/state_block.h"
#include "base/capture/capture_void.h"
-#include "base/constraint/constraint_pose_2D.h"
+#include "base/factor/factor_pose_2D.h"
#include "base/solver/solver_manager.h"
#include "base/local_parametrization_base.h"
#include "base/local_parametrization_angle.h"
@@ -26,7 +26,7 @@ WOLF_PTR_TYPEDEFS(SolverManagerWrapper);
class SolverManagerWrapper : public SolverManager
{
public:
- std::list<ConstraintBasePtr> constraints_;
+ std::list<FactorBasePtr> factors_;
std::map<StateBlockPtr,bool> state_block_fixed_;
std::map<StateBlockPtr,LocalParametrizationBasePtr> state_block_local_param_;
@@ -45,9 +45,9 @@ class SolverManagerWrapper : public SolverManager
return state_block_fixed_.at(st);
};
- bool isConstraintRegistered(const ConstraintBasePtr& ctr_ptr) const
+ bool isFactorRegistered(const FactorBasePtr& fac_ptr) const
{
- return std::find(constraints_.begin(), constraints_.end(), ctr_ptr) != constraints_.end();
+ return std::find(factors_.begin(), factors_.end(), fac_ptr) != factors_.end();
};
bool hasThisLocalParametrization(const StateBlockPtr& st, const LocalParametrizationBasePtr& local_param) const
@@ -61,7 +61,7 @@ class SolverManagerWrapper : public SolverManager
};
virtual void computeCovariances(const CovarianceBlocksToBeComputed blocks){};
- virtual void computeCovariances(const StateBlockList& st_list){};
+ virtual void computeCovariances(const StateBlockPtrList& st_list){};
// The following are dummy implementations
bool hasConverged() { return true; }
@@ -74,18 +74,18 @@ class SolverManagerWrapper : public SolverManager
protected:
virtual std::string solveImpl(const ReportVerbosity report_level){ return std::string("");};
- virtual void addConstraint(const ConstraintBasePtr& ctr_ptr)
+ virtual void addFactor(const FactorBasePtr& fac_ptr)
{
- constraints_.push_back(ctr_ptr);
+ factors_.push_back(fac_ptr);
};
- virtual void removeConstraint(const ConstraintBasePtr& ctr_ptr)
+ virtual void removeFactor(const FactorBasePtr& fac_ptr)
{
- constraints_.remove(ctr_ptr);
+ factors_.remove(fac_ptr);
};
virtual void addStateBlock(const StateBlockPtr& state_ptr)
{
state_block_fixed_[state_ptr] = state_ptr->isFixed();
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
};
virtual void removeStateBlock(const StateBlockPtr& state_ptr)
{
@@ -98,10 +98,10 @@ class SolverManagerWrapper : public SolverManager
};
virtual void updateStateBlockLocalParametrization(const StateBlockPtr& state_ptr)
{
- if (state_ptr->getLocalParametrizationPtr() == nullptr)
+ if (state_ptr->getLocalParametrization() == nullptr)
state_block_local_param_.erase(state_ptr);
else
- state_block_local_param_[state_ptr] = state_ptr->getLocalParametrizationPtr();
+ state_block_local_param_[state_ptr] = state_ptr->getLocalParametrization();
};
};
@@ -111,7 +111,7 @@ TEST(SolverManager, Create)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// check double pointers to branches
- ASSERT_EQ(P, solver_manager_ptr->getProblemPtr());
+ ASSERT_EQ(P, solver_manager_ptr->getProblem());
}
TEST(SolverManager, AddStateBlock)
@@ -263,7 +263,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
// Local param
LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_ptr);
+ sb_ptr->setLocalParametrization(local_ptr);
// Fix state block
sb_ptr->fix();
@@ -298,7 +298,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
// Local param
LocalParametrizationBasePtr local_ptr = std::make_shared<LocalParametrizationAngle>();
- sb_ptr->setLocalParametrizationPtr(local_ptr);
+ sb_ptr->setLocalParametrization(local_ptr);
// check flags
ASSERT_FALSE(sb_ptr->stateUpdated());
@@ -495,7 +495,7 @@ TEST(SolverManager, AddUpdatedStateBlock)
ASSERT_EQ(P->getStateBlockNotificationMap().begin()->second, REMOVE);
}
-TEST(SolverManager, AddConstraint)
+TEST(SolverManager, AddFactor)
{
ProblemPtr P = Problem::create("PO 2D");
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
@@ -504,20 +504,20 @@ TEST(SolverManager, AddConstraint)
Vector2s state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
solver_manager_ptr->update();
- // check constraint
- ASSERT_TRUE(solver_manager_ptr->isConstraintRegistered(c));
+ // check factor
+ ASSERT_TRUE(solver_manager_ptr->isFactorRegistered(c));
}
-TEST(SolverManager, RemoveConstraint)
+TEST(SolverManager, RemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
@@ -526,26 +526,26 @@ TEST(SolverManager, RemoveConstraint)
Vector2s state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
solver_manager_ptr->update();
- // add constraint
- P->removeConstraint(c);
+ // add factor
+ P->removeFactor(c);
// update solver
solver_manager_ptr->update();
- // check constraint
- ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+ // check factor
+ ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
-TEST(SolverManager, AddRemoveConstraint)
+TEST(SolverManager, AddRemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
@@ -554,27 +554,27 @@ TEST(SolverManager, AddRemoveConstraint)
Vector2s state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
- ASSERT_TRUE(P->getConstraintNotificationMap().begin()->first == c);
+ ASSERT_TRUE(P->getFactorNotificationMap().begin()->first == c);
- // add constraint
- P->removeConstraint(c);
+ // add factor
+ P->removeFactor(c);
- ASSERT_TRUE(P->getConstraintNotificationMap().empty());
+ ASSERT_TRUE(P->getFactorNotificationMap().empty());
// update solver
solver_manager_ptr->update();
- // check constraint
- ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+ // check factor
+ ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
-TEST(SolverManager, DoubleRemoveConstraint)
+TEST(SolverManager, DoubleRemoveFactor)
{
ProblemPtr P = Problem::create("PO 2D");
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
@@ -583,29 +583,29 @@ TEST(SolverManager, DoubleRemoveConstraint)
Vector2s state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
- // Create (and add) constraint point 2d
+ // Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY_FRAME, P->zeroState(), TimeStamp(0));
CaptureBasePtr C = F->addCapture(std::make_shared<CaptureVoid>(0, nullptr));
FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("ODOM 2D", Vector3s::Zero(), Matrix3s::Identity()));
- ConstraintPose2DPtr c = std::static_pointer_cast<ConstraintPose2D>(f->addConstraint(std::make_shared<ConstraintPose2D>(f)));
+ FactorPose2DPtr c = std::static_pointer_cast<FactorPose2D>(f->addFactor(std::make_shared<FactorPose2D>(f)));
// update solver
solver_manager_ptr->update();
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
// update solver
solver_manager_ptr->update();
- // remove constraint
- P->removeConstraint(c);
+ // remove factor
+ P->removeFactor(c);
// update solver
solver_manager_ptr->update();
- // check constraint
- ASSERT_FALSE(solver_manager_ptr->isConstraintRegistered(c));
+ // check factor
+ ASSERT_FALSE(solver_manager_ptr->isFactorRegistered(c));
}
int main(int argc, char **argv)
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 9d65d8c0fef5d11aca4cd94792f031feecbe122f..e3d533a5fb31f4f524cde8630446ba0e5dda259b 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -288,7 +288,7 @@ TEST_F(TrackMatrixTest, snapshotAsList)
* f2 trk 1
*/
- std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapturePtr()); // get track 0 as vector
+ std::list<FeatureBasePtr> lt0 = track_matrix.snapshotAsList(f0->getCapture()); // get track 0 as vector
ASSERT_EQ(lt0.size() , (unsigned int) 2);
ASSERT_EQ(lt0.front(), f0);
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index acbd4f242566d8fe97e71043b62ce96cc5d1c41b..d77defcbc697d87cf827a465d1ef99e9221293a6 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -62,7 +62,7 @@ TEST(TrajectoryBase, ClosestKeyFrame)
{
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
// kf1 kf2 f3 frames
@@ -99,7 +99,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
using std::make_shared;
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
DummyNotificationProcessor N(P);
@@ -118,26 +118,26 @@ TEST(TrajectoryBase, Add_Remove_Frame)
T->addFrame(f1); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
T->addFrame(f2); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 4);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
T->addFrame(f3); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 3);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 4);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 4);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
std::cout << __LINE__ << std::endl;
N.update();
@@ -147,27 +147,27 @@ TEST(TrajectoryBase, Add_Remove_Frame)
f2->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 2);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastFramePtr()->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
+ ASSERT_EQ(T->getLastFrame()->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
std::cout << __LINE__ << std::endl;
f3->remove(); // F
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 1);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 2);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 2);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 2);
std::cout << __LINE__ << std::endl;
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f1->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f1->id());
f1->remove(); // KF
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (unsigned int) 0);
- ASSERT_EQ(P->getStateBlockList(). size(), (unsigned int) 0);
+ ASSERT_EQ(P->getStateBlockPtrList(). size(), (unsigned int) 0);
ASSERT_EQ(P->getStateBlockNotificationMap(). size(), (unsigned int) 4);
std::cout << __LINE__ << std::endl;
@@ -182,7 +182,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
using std::make_shared;
ProblemPtr P = Problem::create("PO 2D");
- TrajectoryBasePtr T = P->getTrajectoryPtr();
+ TrajectoryBasePtr T = P->getTrajectory();
// Trajectory status:
// kf1 kf2 f3 frames
@@ -196,23 +196,23 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// add frames and keyframes in random order --> keyframes must be sorted after that
T->addFrame(f2); // KF2
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
T->addFrame(f3); // F3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
T->addFrame(f1); // KF1
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
f3->setKey(); // set KF3
if (debug) P->print(2,0,0,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
FrameBasePtr f4 = std::make_shared<FrameBase>(NON_KEY_FRAME, 1.5, make_shared<StateBlock>(2), make_shared<StateBlock>(1)); // non-key-frame
T->addFrame(f4);
@@ -221,8 +221,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 2 3 1.5 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f4->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f4->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
f4->setKey();
// Trajectory status:
@@ -230,8 +230,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 1.5 2 3 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f3->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f3->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f3->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f3->id());
f2->setTimeStamp(4);
// Trajectory status:
@@ -239,8 +239,8 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// 1 1.5 3 4 time stamps
// --+-----+-----+------+---> time
if (debug) P->print(2,0,1,0);
- ASSERT_EQ(T->getLastFramePtr() ->id(), f2->id());
- ASSERT_EQ(T->getLastKeyFramePtr()->id(), f2->id());
+ ASSERT_EQ(T->getLastFrame() ->id(), f2->id());
+ ASSERT_EQ(T->getLastKeyFrame()->id(), f2->id());
f4->setTimeStamp(0.5);
// Trajectory status:
diff --git a/wolf_scripts/substitution.sh b/wolf_scripts/substitution.sh
new file mode 100755
index 0000000000000000000000000000000000000000..04ebdd6be9a348b77d7d94fc75af1725eb336cc1
--- /dev/null
+++ b/wolf_scripts/substitution.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+# folder=$1
+# for file in $(ag '(SensorBase|ProcessorBase|FrameBase|CaptureBase|FeatureBase|FactorBase|LandmarkBase|StateBlock)List' . -o); do
+for file in $(ag 'Ptr\(' . -o); do
+ # file=$(echo $file | sed "s/ //g")
+ target=$(echo $file | cut -d':' -f 1)
+ line=$(echo $file | cut -d':' -f 2)
+ subs=$(echo $file | cut -d':' -f 3)
+ # echo "$target@$line@$subs"
+ # subs_line=${line}s/${subs}/${subs%List}PtrList/gp
+ # echo $subs_line
+ # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
+ # sed -n -e $line's/Ptr\( \)*(\( \)*)/\1(\2)/gp' $target
+ sed -i -e $line's/(/(/g' $target
+done
+
+# for file in $(ag -l -g constraint $folder); do
+# new_file=$(echo $file | sed -e "s/constraint/factor/g")
+# mv $file $new_file
+# done
\ No newline at end of file