From 06a047051a8e149ca566b98e5650bc34178cf645 Mon Sep 17 00:00:00 2001
From: jcasals <jcasals@iri.upc.edu>
Date: Mon, 30 Mar 2020 15:21:56 +0200
Subject: [PATCH] Rename
---
...laser_2D.cpp => demo_capture_laser_2d.cpp} | 0
...or_odom_3D.cpp => demo_factor_odom_3d.cpp} | 0
...odom_3D.cpp => demo_processor_odom_3d.cpp} | 0
.../vision/factor/factor_autodiff_trifocal.h | 4 +-
include/vision/factor/factor_epipolar.h | 2 +-
include/vision/landmark/landmark_AHP.h | 2 +-
include/vision/landmark/landmark_HP.h | 2 +-
...andmark_point_3D.h => landmark_point_3d.h} | 20 ++++----
include/vision/math/pinhole_tools.h | 46 +++++++++---------
.../processor_tracker_landmark_image.h | 2 +-
src/landmark/landmark_AHP.cpp | 8 ++--
src/landmark/landmark_HP.cpp | 8 ++--
src/landmark/landmark_point_3D.cpp | 19 --------
src/landmark/landmark_point_3d.cpp | 19 ++++++++
src/processor/processor_bundle_adjustment.cpp | 42 ++++++++--------
.../processor_tracker_landmark_image.cpp | 48 +++++++++----------
src/sensor/sensor_camera.cpp | 4 +-
test/gtest_factor_autodiff_trifocal.cpp | 8 ++--
test/gtest_factor_pixelHP.cpp | 6 +--
...est_processor_tracker_feature_trifocal.cpp | 14 +++---
test/gtest_sensor_camera.cpp | 2 +-
21 files changed, 128 insertions(+), 128 deletions(-)
rename demos/{demo_capture_laser_2D.cpp => demo_capture_laser_2d.cpp} (100%)
rename demos/{demo_factor_odom_3D.cpp => demo_factor_odom_3d.cpp} (100%)
rename demos/{demo_processor_odom_3D.cpp => demo_processor_odom_3d.cpp} (100%)
rename include/vision/landmark/{landmark_point_3D.h => landmark_point_3d.h} (54%)
delete mode 100644 src/landmark/landmark_point_3D.cpp
create mode 100644 src/landmark/landmark_point_3d.cpp
diff --git a/demos/demo_capture_laser_2D.cpp b/demos/demo_capture_laser_2d.cpp
similarity index 100%
rename from demos/demo_capture_laser_2D.cpp
rename to demos/demo_capture_laser_2d.cpp
diff --git a/demos/demo_factor_odom_3D.cpp b/demos/demo_factor_odom_3d.cpp
similarity index 100%
rename from demos/demo_factor_odom_3D.cpp
rename to demos/demo_factor_odom_3d.cpp
diff --git a/demos/demo_processor_odom_3D.cpp b/demos/demo_processor_odom_3d.cpp
similarity index 100%
rename from demos/demo_processor_odom_3D.cpp
rename to demos/demo_processor_odom_3d.cpp
diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_autodiff_trifocal.h
index 0ec752c5e..d243f644f 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_autodiff_trifocal.h
@@ -218,7 +218,7 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_
// Re-write info matrix (for debug only)
// double pix_noise = 1.0;
- // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3d::Identity(); // one PLP (2D) and one epipolar (1D) factor
+ // sqrt_information_upper = pow(1.0/pix_noise, 2) * Matrix3d::Identity(); // one PLP (2d) and one epipolar (1d) factor
}
// Destructor
@@ -301,7 +301,7 @@ inline void FactorAutodiffTrifocal::expectation(const MatrixBase<D1>& _wtr1,
* C2 is specified by R and T wrt C1 so that
* T is the position of C2 wrt C1
* R is the orientation of C2 wrt C1
- * There is a 3D point P, noted P1 when expressed in C1 and P2 when expressed in C2:
+ * There is a 3d point P, noted P1 when expressed in C1 and P2 when expressed in C2:
* P1 = T + R * P2
*
* Coplanarity condition: a' * (b x c) = 0 with {a,b,c} three coplanar vectors.
diff --git a/include/vision/factor/factor_epipolar.h b/include/vision/factor/factor_epipolar.h
index d380de518..b127c201b 100644
--- a/include/vision/factor/factor_epipolar.h
+++ b/include/vision/factor/factor_epipolar.h
@@ -100,7 +100,7 @@ inline bool FactorEpipolar::operator ()(const T* const _frame_own_p,
* C2 is specified by R and t wrt C1 so that
* t is the position of C2 wrt C1
* R is the orientation of C2 wrt C1
- * There is a 3D point P, noted P1 when expressed in C1 and P2 when expressed in C2:
+ * There is a 3d point P, noted P1 when expressed in C1 and P2 when expressed in C2:
* P1 = t + R * P2
*
* Co-planarity condition: a' * (b x c) = 0 with {a,b,c} three co-planar vectors.
diff --git a/include/vision/landmark/landmark_AHP.h b/include/vision/landmark/landmark_AHP.h
index 2402f81ad..0dffb6dc8 100644
--- a/include/vision/landmark/landmark_AHP.h
+++ b/include/vision/landmark/landmark_AHP.h
@@ -23,7 +23,7 @@ class LandmarkAHP : public LandmarkBase
SensorBasePtr anchor_sensor_;
public:
- LandmarkAHP(Eigen::Vector4d _position_homogeneous, FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor, cv::Mat _2D_descriptor);
+ LandmarkAHP(Eigen::Vector4d _position_homogeneous, FrameBasePtr _anchor_frame, SensorBasePtr _anchor_sensor, cv::Mat _2d_descriptor);
virtual ~LandmarkAHP();
diff --git a/include/vision/landmark/landmark_HP.h b/include/vision/landmark/landmark_HP.h
index 00d660268..ee9815df2 100644
--- a/include/vision/landmark/landmark_HP.h
+++ b/include/vision/landmark/landmark_HP.h
@@ -22,7 +22,7 @@ class LandmarkHP : public LandmarkBase
public:
- LandmarkHP(Eigen::Vector4d _position_homogeneous, SensorBasePtr _sensor_, cv::Mat _2D_descriptor);
+ LandmarkHP(Eigen::Vector4d _position_homogeneous, SensorBasePtr _sensor_, cv::Mat _2d_descriptor);
virtual ~LandmarkHP();
diff --git a/include/vision/landmark/landmark_point_3D.h b/include/vision/landmark/landmark_point_3d.h
similarity index 54%
rename from include/vision/landmark/landmark_point_3D.h
rename to include/vision/landmark/landmark_point_3d.h
index 2c74c9ee9..1a5d5d1a6 100644
--- a/include/vision/landmark/landmark_point_3D.h
+++ b/include/vision/landmark/landmark_point_3d.h
@@ -1,5 +1,5 @@
-#ifndef LANDMARK_POINT_3D_H
-#define LANDMARK_POINT_3D_H
+#ifndef LANDMARK_POINT_3d_H
+#define LANDMARK_POINT_3d_H
//Wolf includes
#include "core/landmark/landmark_base.h"
@@ -9,18 +9,18 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(LandmarkPoint3D);
+WOLF_PTR_TYPEDEFS(LandmarkPoint3d);
//class
-class LandmarkPoint3D : public LandmarkBase
+class LandmarkPoint3d : public LandmarkBase
{
protected:
cv::Mat descriptor_;
Eigen::Vector3d position_;
public:
- LandmarkPoint3D(Eigen::Vector3d _position, cv::Mat _2D_descriptor);
+ LandmarkPoint3d(Eigen::Vector3d _position, cv::Mat _2d_descriptor);
- virtual ~LandmarkPoint3D();
+ virtual ~LandmarkPoint3d();
const Eigen::Vector3d point() const;
@@ -28,21 +28,21 @@ class LandmarkPoint3D : public LandmarkBase
void setDescriptor(const cv::Mat& _descriptor);
};
-inline const Eigen::Vector3d LandmarkPoint3D::point() const
+inline const Eigen::Vector3d LandmarkPoint3d::point() const
{
return getP()->getState();
}
-inline const cv::Mat& LandmarkPoint3D::getDescriptor() const
+inline const cv::Mat& LandmarkPoint3d::getDescriptor() const
{
return descriptor_;
}
-inline void LandmarkPoint3D::setDescriptor(const cv::Mat& _descriptor)
+inline void LandmarkPoint3d::setDescriptor(const cv::Mat& _descriptor)
{
descriptor_ = _descriptor;
}
} // namespace wolf
-#endif // LANDMARK_POINT_3D_H
+#endif // LANDMARK_POINT_3d_H
diff --git a/include/vision/math/pinhole_tools.h b/include/vision/math/pinhole_tools.h
index d978719aa..f4e9814d1 100644
--- a/include/vision/math/pinhole_tools.h
+++ b/include/vision/math/pinhole_tools.h
@@ -29,8 +29,8 @@ using Eigen::Matrix;
/**
* Pin-hole canonical projection.
- * \param _v a 3D point to project
- * \param _up the projected point in the normalized 2D plane
+ * \param _v a 3d point to project
+ * \param _up the projected point in the normalized 2d plane
*/
template<typename Derived1, typename Derived2>
inline void
@@ -46,8 +46,8 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
/**
* Pin-hole canonical projection.
- * \return the projected point in the normalized 2D plane
- * \param _v a 3D point
+ * \return the projected point in the normalized 2d plane
+ * \param _v a 3d point
*/
template<typename Derived>
inline Matrix<typename Derived::Scalar, 2, 1>
@@ -65,8 +65,8 @@ projectPointToNormalizedPlane(const MatrixBase<Derived>& _v)
/**
* Pin-hole canonical projection, return also distance (not depth!).
- * \param _v a 3D point
- * \param _up the projected point in the normalized 2D plane
+ * \param _v a 3d point
+ * \param _up the projected point in the normalized 2d plane
* \param _dist the distance from the camera to the point
*/
template<typename Derived1, typename Derived2>
@@ -84,8 +84,8 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
/**
* Pin-hole canonical projection, with jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
+ * \param _v the 3d point to project
+ * \param _up the projected 2d point
* \param _UP_v the Jacibian of \a u wrt \a v
*/
template<typename Derived1, typename Derived2, typename Derived3>
@@ -110,8 +110,8 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
/**
* Pin-hole canonical projection, with distance (not depth!) and jacobian
- * \param _v the 3D point to project
- * \param _up the projected 2D point
+ * \param _v the 3d point to project
+ * \param _up the projected 2d point
* \param _UP_v the Jacibian of \a u wrt \a v
*/
template<typename Derived1, typename Derived2, typename Derived3>
@@ -132,9 +132,9 @@ projectPointToNormalizedPlane(const MatrixBase<Derived1>& _v,
/**
* Canonical back-projection.
- * \param _u the 2D point in the image plane
+ * \param _u the 2d point in the image plane
* \param _depth point's depth orthogonal to image plane. Defaults to 1.0
- * \return the back-projected 3D point at the given depth
+ * \return the back-projected 3d point at the given depth
*/
template<typename Derived>
Matrix<typename Derived::Scalar, 3, 1>
@@ -153,9 +153,9 @@ backprojectPointFromNormalizedPlane(const MatrixBase<Derived> & _u,
/**
* Canonical back-projection.
- * \param u the 2D point in the image plane.
+ * \param u the 2d point in the image plane.
* \param depth point's depth orthogonal to image plane.
- * \param p the 3D point.
+ * \param p the 3d point.
* \param P_u Jacobian of p wrt u.
* \param P_depth Jacobian of p wrt depth.
*/
@@ -514,7 +514,7 @@ void depixellizePoint(const MatrixBase<Derived1>& k,
* Project a point into a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
+ * \param v the 3d point to project, or the 3d director vector
* \return the projected and distorted point
*/
template<typename Derived1, typename Derived2, typename Derived3>
@@ -533,7 +533,7 @@ Matrix<typename Derived3::Scalar, 2, 1> projectPoint(const MatrixBase<Derived1>&
* Project a point into a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
+ * \param v the 3d point to project, or the 3d director vector
* \param u the projected and distorted point
* \param U_v the Jacobian of \a u wrt \a v
*/
@@ -564,9 +564,9 @@ void projectPoint(const MatrixBase<Derived1>& k,
* Project a point into a pin-hole camera with radial distortion.
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
+ * \param v the 3d point to project, or the 3d director vector
* \param u the projected and distorted point
- * \param dist distance from the optical center to the 3D point
+ * \param dist distance from the optical center to the 3d point
*/
template<typename Derived1, typename Derived2, typename Derived3, typename Derived4>//, typename T>
void projectPoint(const MatrixBase<Derived1>& k,
@@ -589,7 +589,7 @@ void projectPoint(const MatrixBase<Derived1>& k,
* Project a point into a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param d the radial distortion parameters vector
- * \param v the 3D point to project, or the 3D director vector
+ * \param v the 3d point to project, or the 3d director vector
* \param u the projected and distorted point
* \param dist the distance from the camera to the point
* \param U_v the Jacobian of \a u wrt \a v
@@ -623,9 +623,9 @@ void projectPoint(const MatrixBase<Derived1>& k,
* Back-Project a point from a pin-hole camera with radial distortion
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param c the radial undistortion parameters vector
- * \param u the 2D pixel
+ * \param u the 2d pixel
* \param depth the depth prior
- * \return the back-projected 3D point
+ * \return the back-projected 3d point
*/
template<typename Derived1, typename Derived2, typename Derived3>
Matrix<typename Derived3::Scalar, 3, 1> backprojectPoint(const MatrixBase<Derived1>& k,
@@ -644,9 +644,9 @@ Matrix<typename Derived3::Scalar, 3, 1> backprojectPoint(const MatrixBase<Derive
* Back-Project a point from a pin-hole camera with radial distortion; give Jacobians
* \param k the vector of intrinsic parameters, k = [u0, v0, au, av]
* \param c the radial undistortion parameters vector
- * \param u the 2D pixel
+ * \param u the 2d pixel
* \param depth the depth prior
- * \param p the back-projected 3D point
+ * \param p the back-projected 3d point
* \param P_u Jacobian of p wrt u
* \param P_depth Jacobian of p wrt depth
*/
diff --git a/include/vision/processor/processor_tracker_landmark_image.h b/include/vision/processor/processor_tracker_landmark_image.h
index 27d4e77ec..00e3545fb 100644
--- a/include/vision/processor/processor_tracker_landmark_image.h
+++ b/include/vision/processor/processor_tracker_landmark_image.h
@@ -188,7 +188,7 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
void landmarkInCurrentCamera(const Eigen::VectorXd& _frame_state,
const LandmarkAHPPtr _landmark,
- Eigen::Vector4d& _point3D_hmg);
+ Eigen::Vector4d& _point3d_hmg);
// These only to debug, will disappear one day soon
public:
diff --git a/src/landmark/landmark_AHP.cpp b/src/landmark/landmark_AHP.cpp
index bf54f6738..90e12c088 100644
--- a/src/landmark/landmark_AHP.cpp
+++ b/src/landmark/landmark_AHP.cpp
@@ -1,6 +1,6 @@
#include "vision/landmark/landmark_AHP.h"
-#include "core/state_block/state_homogeneous_3D.h"
+#include "core/state_block/state_homogeneous_3d.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -10,9 +10,9 @@ namespace wolf {
LandmarkAHP::LandmarkAHP(Eigen::Vector4d _position_homogeneous,
FrameBasePtr _anchor_frame,
SensorBasePtr _anchor_sensor,
- cv::Mat _2D_descriptor) :
- LandmarkBase("AHP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
- cv_descriptor_(_2D_descriptor.clone()),
+ cv::Mat _2d_descriptor) :
+ LandmarkBase("AHP", std::make_shared<StateHomogeneous3d>(_position_homogeneous)),
+ cv_descriptor_(_2d_descriptor.clone()),
anchor_frame_(_anchor_frame),
anchor_sensor_(_anchor_sensor)
{
diff --git a/src/landmark/landmark_HP.cpp b/src/landmark/landmark_HP.cpp
index d844591a4..869e88f97 100644
--- a/src/landmark/landmark_HP.cpp
+++ b/src/landmark/landmark_HP.cpp
@@ -1,6 +1,6 @@
#include "vision/landmark/landmark_HP.h"
-#include "core/state_block/state_homogeneous_3D.h"
+#include "core/state_block/state_homogeneous_3d.h"
#include "core/common/factory.h"
#include "core/yaml/yaml_conversion.h"
@@ -9,9 +9,9 @@ namespace wolf {
/* Landmark - Homogeneous Point*/
LandmarkHP::LandmarkHP(Eigen::Vector4d _position_homogeneous,
SensorBasePtr _sensor,
- cv::Mat _2D_descriptor) :
- LandmarkBase("HP", std::make_shared<StateHomogeneous3D>(_position_homogeneous)),
- cv_descriptor_(_2D_descriptor.clone())
+ cv::Mat _2d_descriptor) :
+ LandmarkBase("HP", std::make_shared<StateHomogeneous3d>(_position_homogeneous)),
+ cv_descriptor_(_2d_descriptor.clone())
{
}
diff --git a/src/landmark/landmark_point_3D.cpp b/src/landmark/landmark_point_3D.cpp
deleted file mode 100644
index de6a47f71..000000000
--- a/src/landmark/landmark_point_3D.cpp
+++ /dev/null
@@ -1,19 +0,0 @@
-#include "vision/landmark/landmark_point_3D.h"
-
-namespace wolf {
-
-LandmarkPoint3D::LandmarkPoint3D(Eigen::Vector3d _position, cv::Mat _2D_descriptor) :
- LandmarkBase("POINT 3D", std::make_shared<StateBlock>(_position, false)),
- descriptor_(_2D_descriptor)
-{
- //LandmarkPoint3D* landmark_ptr = (LandmarkPoint3D*)_p_ptr;
-// position_ =
-// descriptor_ = _2D_descriptor;
-}
-
-LandmarkPoint3D::~LandmarkPoint3D()
-{
- //
-}
-
-}
diff --git a/src/landmark/landmark_point_3d.cpp b/src/landmark/landmark_point_3d.cpp
new file mode 100644
index 000000000..3c566e495
--- /dev/null
+++ b/src/landmark/landmark_point_3d.cpp
@@ -0,0 +1,19 @@
+#include "vision/landmark/landmark_point_3d.h"
+
+namespace wolf {
+
+LandmarkPoint3d::LandmarkPoint3d(Eigen::Vector3d _position, cv::Mat _2d_descriptor) :
+ LandmarkBase("POINT 3d", std::make_shared<StateBlock>(_position, false)),
+ descriptor_(_2d_descriptor)
+{
+ //LandmarkPoint3d* landmark_ptr = (LandmarkPoint3d*)_p_ptr;
+// position_ =
+// descriptor_ = _2d_descriptor;
+}
+
+LandmarkPoint3d::~LandmarkPoint3d()
+{
+ //
+}
+
+}
diff --git a/src/processor/processor_bundle_adjustment.cpp b/src/processor/processor_bundle_adjustment.cpp
index d70e7e7dc..290f5bb4a 100644
--- a/src/processor/processor_bundle_adjustment.cpp
+++ b/src/processor/processor_bundle_adjustment.cpp
@@ -122,7 +122,7 @@ void ProcessorBundleAdjustment::preProcess()
}
-// //get points 2D from inlier matches
+// //get points 2d from inlier matches
// PointVector pts1, pts2;
// for (auto match : capture_image_incoming_->matches_from_precedent_)
// {
@@ -244,11 +244,11 @@ void ProcessorBundleAdjustment::postProcess()
//// if (fact->getLandmarkOther()->getConstrainedByList().size() >= 2)
//// {
//// //3d points
-//// auto point3D = std::static_pointer_cast<LandmarkHP>(fact->getLandmarkOther())->point();
-//// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+//// auto point3d = std::static_pointer_cast<LandmarkHP>(fact->getLandmarkOther())->point();
+//// landmark_points.push_back(cv::Point3f(point3d(0),point3d(1),point3d(2)));
//// //2d points
-//// auto point2D = feat->getMeasurement();
-//// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+//// auto point2d = feat->getMeasurement();
+//// image_points.push_back(cv::Point2f(point2d(0), point2d(1)));
//// }
//// }
//// }
@@ -263,11 +263,11 @@ void ProcessorBundleAdjustment::postProcess()
//// if (lmk_base->getConstrainedByList().size() >= 2)
// {
// //3d points
-// auto point3D = std::static_pointer_cast<LandmarkHP>(lmk_base)->point();
-// landmark_points.push_back(cv::Point3f(point3D(0),point3D(1),point3D(2)));
+// auto point3d = std::static_pointer_cast<LandmarkHP>(lmk_base)->point();
+// landmark_points.push_back(cv::Point3f(point3d(0),point3d(1),point3d(2)));
// //2d points
-// auto point2D = feat->getMeasurement();
-// image_points.push_back(cv::Point2f(point2D(0), point2D(1)));
+// auto point2d = feat->getMeasurement();
+// image_points.push_back(cv::Point2f(point2d(0), point2d(1)));
// }
// }
// }
@@ -404,7 +404,7 @@ void ProcessorBundleAdjustment::postProcess()
unsigned int track_id = feat->trackId();
if (lmk_track_map_.count(track_id))
{
- Vector3d point3D = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
+ Vector3d point3d = std::static_pointer_cast<LandmarkHP>(lmk_track_map_[track_id])->point();
Transform<double,3,Isometry> T_w_r
= Translation<double,3>(feat_base->getFrame()->getP()->getState())
@@ -413,14 +413,14 @@ void ProcessorBundleAdjustment::postProcess()
= Translation<double,3>(feat_base->getCapture()->getSensorP()->getState())
* Quaterniond(feat_base->getCapture()->getSensorO()->getState().data());
- Eigen::Matrix<double, 3, 1> point3D_c = T_r_c.inverse()
+ Eigen::Matrix<double, 3, 1> point3d_c = T_r_c.inverse()
* T_w_r.inverse()
- * point3D;
+ * point3d;
// SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(getSensor());
- Vector2d point2D = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3D_c);
- cv::Point point = cv::Point(point2D(0), point2D(1));
+ Vector2d point2d = pinhole::projectPoint(getSensor()->getIntrinsic()->getState(), camera->getDistortionVector(), point3d_c);
+ cv::Point point = cv::Point(point2d(0), point2d(1));
cv::circle(image_debug_, point, 3, cv::Scalar(0,0,255) , 1 , 8);
std::string id = std::to_string(lmk_track_map_[track_id]->id());
@@ -655,20 +655,20 @@ FactorBasePtr ProcessorBundleAdjustment::emplaceFactor(FeatureBasePtr _feature_p
LandmarkBasePtr ProcessorBundleAdjustment::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
- Eigen::Vector2d point2D = _feature_ptr->getMeasurement();
+ Eigen::Vector2d point2d = _feature_ptr->getMeasurement();
- Eigen::Vector3d point3D;
- point3D = pinhole::backprojectPoint(
+ Eigen::Vector3d point3d;
+ point3d = pinhole::backprojectPoint(
getSensor()->getIntrinsic()->getState(),
(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
- point2D);
+ point2d);
//double distance = params_bundle_adjustment_->distance; // arbitrary value
double distance = 1;
Eigen::Vector4d vec_homogeneous_c;
- vec_homogeneous_c = {point3D(0),point3D(1),point3D(2),point3D.norm()/distance};
+ vec_homogeneous_c = {point3d(0),point3d(1),point3d(2),point3d.norm()/distance};
vec_homogeneous_c.normalize();
//TODO: lmk from camera to world coordinate frame.
@@ -683,8 +683,8 @@ LandmarkBasePtr ProcessorBundleAdjustment::emplaceLandmark(FeatureBasePtr _featu
* vec_homogeneous_c;
-// std::cout << "Point2D: " << point2D.transpose() << std::endl;
-// std::cout << "Point3D: " << point3D.transpose() << std::endl;
+// std::cout << "Point2d: " << point2d.transpose() << std::endl;
+// std::cout << "Point3d: " << point3d.transpose() << std::endl;
// std::cout << "Hmg_c: " << vec_homogeneous.transpose() << std::endl;
// std::cout << "Hmg_w: " << vec_homogeneous_w.transpose() << std::endl;
//vec_homogeneous_w = vec_homogeneous;
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index a0024e3ef..f4c171c9b 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -107,14 +107,14 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrL
// project landmark into incoming capture
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_in_ptr);
SensorCameraPtr camera = std::static_pointer_cast<SensorCamera>(this->getSensor());
- Eigen::Vector4d point3D_hmg;
+ Eigen::Vector4d point3d_hmg;
Eigen::Vector2d pixel;
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
+ landmarkInCurrentCamera(current_state, landmark_ptr, point3d_hmg);
pixel = pinhole::projectPoint(camera->getIntrinsic()->getState(),
camera->getDistortionVector(),
- point3D_hmg.head<3>());
+ point3d_hmg.head<3>());
if(pinhole::isInImage(pixel, image_.width_, image_.height_))
{
@@ -236,23 +236,23 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::emplaceLandmark(FeatureBasePtr _f
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
FrameBasePtr anchor_frame = getLast()->getFrame();
- Eigen::Vector2d point2D;
- point2D[0] = feat_point_image_ptr->getKeypoint().pt.x;
- point2D[1] = feat_point_image_ptr->getKeypoint().pt.y;
+ Eigen::Vector2d point2d;
+ point2d[0] = feat_point_image_ptr->getKeypoint().pt.x;
+ point2d[1] = feat_point_image_ptr->getKeypoint().pt.y;
double distance = params_tracker_landmark_image_->distance; // arbitrary value
Eigen::Vector4d vec_homogeneous;
- point2D = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2D);
- point2D = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2D);
+ point2d = pinhole::depixellizePoint(getSensor()->getIntrinsic()->getState(),point2d);
+ point2d = pinhole::undistortPoint((std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),point2d);
- Eigen::Vector3d point3D;
- point3D.head<2>() = point2D;
- point3D(2) = 1;
+ Eigen::Vector3d point3d;
+ point3d.head<2>() = point2d;
+ point3d(2) = 1;
- point3D.normalize();
+ point3d.normalize();
- vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
+ vec_homogeneous = {point3d(0),point3d(1),point3d(2),1/distance};
auto lmk_ahp_ptr = LandmarkBase::emplace<LandmarkAHP>(getProblem()->getMap(),
vec_homogeneous,
@@ -285,7 +285,7 @@ FactorBasePtr ProcessorTrackerLandmarkImage::emplaceFactor(FeatureBasePtr _featu
void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorXd& _current_state,
const LandmarkAHPPtr _landmark,
- Eigen::Vector4d& _point3D_hmg)
+ Eigen::Vector4d& _point3d_hmg)
{
using namespace Eigen;
@@ -311,8 +311,8 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
* We use Eigen::Transform which is like using homogeneous transform matrices with a simpler API
*/
- // Assert frame is 3D with at least PQ
- assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3D pose or 16 for IMU.");
+ // Assert frame is 3d with at least PQ
+ assert((_current_state.size() == 7 || _current_state.size() == 16) && "Wrong state size! Should be 7 for 3d pose or 16 for Imu.");
// ALL TRANSFORMS
Transform<double,3,Eigen::Isometry> T_W_R0, T_W_R1, T_R0_C0, T_R1_C1;
@@ -339,7 +339,7 @@ void ProcessorTrackerLandmarkImage::landmarkInCurrentCamera(const Eigen::VectorX
// Transform lmk from c0 to c1 and exit
Vector4d landmark_hmg_c0 = _landmark->getP()->getState(); // lmk in anchor frame
- _point3D_hmg = T_R1_C1.inverse(Eigen::Isometry) * T_W_R1.inverse(Eigen::Isometry) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
+ _point3d_hmg = T_R1_C1.inverse(Eigen::Isometry) * T_W_R1.inverse(Eigen::Isometry) * T_W_R0 * T_R0_C0 * landmark_hmg_c0;
}
double ProcessorTrackerLandmarkImage::match(const cv::Mat _target_descriptor, const cv::Mat _candidate_descriptors, DMatchVector& _cv_matches)
@@ -407,20 +407,20 @@ void ProcessorTrackerLandmarkImage::drawLandmarks(cv::Mat _image)
{
LandmarkAHPPtr landmark_ptr = std::static_pointer_cast<LandmarkAHP>(landmark_base_ptr);
- Eigen::Vector4d point3D_hmg;
- landmarkInCurrentCamera(current_state, landmark_ptr, point3D_hmg);
+ Eigen::Vector4d point3d_hmg;
+ landmarkInCurrentCamera(current_state, landmark_ptr, point3d_hmg);
- Eigen::Vector2d point2D = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
+ Eigen::Vector2d point2d = pinhole::projectPoint(camera->getIntrinsic()->getState(), // k
camera->getDistortionVector(), // d
- point3D_hmg.head(3)); // v
+ point3d_hmg.head(3)); // v
- if(pinhole::isInImage(point2D,image_.width_,image_.height_))
+ if(pinhole::isInImage(point2d,image_.width_,image_.height_))
{
num_lmks_in_img++;
cv::Point2f point;
- point.x = point2D[0];
- point.y = point2D[1];
+ point.x = point2d[0];
+ point.y = point2d[1];
cv::circle(_image, point, 4, cv::Scalar(51.0, 51.0, 255.0), 1, 3, 0);
cv::putText(_image, std::to_string(landmark_ptr->id()), point, cv:: FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(100.0, 100.0, 255.0) );
diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp
index 317159e96..0c7b89f8e 100644
--- a/src/sensor/sensor_camera.cpp
+++ b/src/sensor/sensor_camera.cpp
@@ -18,7 +18,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXd& _extrinsics, const ParamsSenso
pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
using_raw_(true)
{
- assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3D");
+ assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3d");
useRawImages();
pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_);
}
@@ -52,7 +52,7 @@ SensorBasePtr SensorCamera::create(const std::string& _unique_name, //
const Eigen::VectorXd& _extrinsics_pq, //
const ParamsSensorBasePtr _intrinsics)
{
- assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
+ assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3d.");
std::shared_ptr<ParamsSensorCamera> intrinsics_ptr = std::static_pointer_cast<ParamsSensorCamera>(_intrinsics);
SensorCameraPtr sen_ptr = std::make_shared<SensorCamera>(_extrinsics_pq, intrinsics_ptr);
diff --git a/test/gtest_factor_autodiff_trifocal.cpp b/test/gtest_factor_autodiff_trifocal.cpp
index 2deaea98d..08d3e4b2b 100644
--- a/test/gtest_factor_autodiff_trifocal.cpp
+++ b/test/gtest_factor_autodiff_trifocal.cpp
@@ -168,7 +168,7 @@ TEST_F(FactorAutodiffTrifocalTest, InfoMatrix)
/** Ground truth covariance. Rationale:
* Due to the orthogonal configuration (see line 40 and onwards), we have:
* Let s = pixel_noise_std.
- * Let d = 1 the distance from the cameras to the 3D point
+ * Let d = 1 the distance from the cameras to the 3d point
* Let k be a proportionality constant related to the projection and pixellization process
* Let S = k*d*s
* The pixel on camera 1 retroprojects a conic PDF with width S = k*s*d
@@ -884,7 +884,7 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_scale)
}
}
-#include "core/factor/factor_autodiff_distance_3D.h"
+#include "core/factor/factor_autodiff_distance_3d.h"
TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
{
@@ -925,8 +925,8 @@ TEST_F(FactorAutodiffTrifocalMultiPointTest, solve_multi_point_distance)
auto fd = FeatureBase::emplace<FeatureBase>(Cd, "DISTANCE", Vector1d(distance), Matrix1d(distance_std * distance_std));
// FeatureBasePtr fd = std::make_shared<FeatureBase>("DISTANCE", Vector1d(distance), Matrix1d(distance_std * distance_std));
// Cd->addFeature(fd);
- auto cd = FactorBase::emplace<FactorAutodiffDistance3D>(fd, fd, F1, nullptr, false, FAC_ACTIVE);
- // FACTORAUTODIFFDISTANCE3DPTR cd = std::make_shared<FactorAutodiffDistance3D>(fd, F1, nullptr, false, FAC_ACTIVE);
+ auto cd = FactorBase::emplace<FactorAutodiffDistance3d>(fd, fd, F1, nullptr, false, FAC_ACTIVE);
+ // FACTORAUTODIFFDISTANCE3dPTR cd = std::make_shared<FactorAutodiffDistance3d>(fd, F1, nullptr, false, FAC_ACTIVE);
// fd->addFactor(cd);
// F1->addConstrainedBy(cd);
diff --git a/test/gtest_factor_pixelHP.cpp b/test/gtest_factor_pixelHP.cpp
index 853cb9c90..37f1fd438 100644
--- a/test/gtest_factor_pixelHP.cpp
+++ b/test/gtest_factor_pixelHP.cpp
@@ -324,7 +324,7 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition2ObservableDoF)
std::cout << orig.transpose() << std::endl;
std::cout << F1->getP()->getState().transpose() << std::endl;
- // This test is no good because it checks 3 DoF and only 2DoF are observable.
+ // This test is no good because it checks 3 DoF and only 2doF are observable.
//ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
// We use the following alternative:
// Frame must be in the X axis, so Y=0 and Z=0
@@ -416,7 +416,7 @@ TEST_F(FactorPixelHPTest, testSolveFramePosition)
std::cout << orig.transpose() << std::endl;
std::cout << F1->getP()->getState().transpose() << std::endl;
- // This test checks 3 DoF (3DoF are observable).
+ // This test checks 3 DoF (3doF are observable).
ASSERT_MATRIX_APPROX(F1->getP()->getState(), orig, 1e-6);
Eigen::VectorXd expect = c11->expectation();
@@ -619,7 +619,7 @@ TEST_F(FactorPixelHPTest, testSolveBundleAdjustment)
}
- // This test checks 3 DoF (3DoF are observable).
+ // This test checks 3 DoF (3doF are observable).
ASSERT_MATRIX_APPROX(F1->getP()->getState(), p1, 1e-6);
ASSERT_MATRIX_APPROX(F2->getP()->getState(), p2, 1e-6);
ASSERT_MATRIX_APPROX(F3->getP()->getState(), p3, 1e-6);
diff --git a/test/gtest_processor_tracker_feature_trifocal.cpp b/test/gtest_processor_tracker_feature_trifocal.cpp
index 314b315bf..4cab2cbc0 100644
--- a/test/gtest_processor_tracker_feature_trifocal.cpp
+++ b/test/gtest_processor_tracker_feature_trifocal.cpp
@@ -7,7 +7,7 @@
#include <core/utils/utils_gtest.h>
#include "core/common/wolf.h"
#include "core/utils/logging.h"
-#include "core/processor/processor_odom_3D.h"
+#include "core/processor/processor_odom_3d.h"
#include "vision_utils/vision_utils.h"
@@ -104,15 +104,15 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
// sens_trk->addProcessor(proc_trk);
// Install odometer (sensor and processor)
- ParamsSensorOdom3DPtr params = std::make_shared<ParamsSensorOdom3D>();
+ ParamsSensorOdom3dPtr params = std::make_shared<ParamsSensorOdom3d>();
params->min_disp_var = 0.000001;
params->min_rot_var = 0.000001;
- SensorBasePtr sens_odo = problem->installSensor("SensorOdom3D", "odometer", (Vector7d() << 0,0,0, 0,0,0,1).finished(), params);
- ProcessorParamsOdom3DPtr proc_odo_params = make_shared<ProcessorParamsOdom3D>();
+ SensorBasePtr sens_odo = problem->installSensor("SensorOdom3d", "odometer", (Vector7d() << 0,0,0, 0,0,0,1).finished(), params);
+ ProcessorParamsOdom3dPtr proc_odo_params = make_shared<ProcessorParamsOdom3d>();
proc_odo_params->voting_active = true;
proc_odo_params->time_tolerance = dt/2;
proc_odo_params->max_buff_length = 3;
- ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom3D", "odometer", sens_odo, proc_odo_params);
+ ProcessorBasePtr proc_odo = problem->installProcessor("ProcessorOdom3d", "odometer", sens_odo, proc_odo_params);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
@@ -124,7 +124,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
Matrix6d P = Matrix6d::Identity() * 0.000001;
problem->setPrior(x, P, t, dt/2); // KF1
- CaptureOdom3DPtr capt_odo = make_shared<CaptureOdom3D>(t, sens_odo, Vector6d::Zero(), P);
+ CaptureOdom3dPtr capt_odo = make_shared<CaptureOdom3d>(t, sens_odo, Vector6d::Zero(), P);
// Track
cv::Mat image(intr->height, intr->width, CV_8UC3); // OpenCV cv::Mat(rows, cols)
@@ -151,7 +151,7 @@ TEST(ProcessorTrackerFeatureTrifocal, KeyFrameCallback)
problem->print(2,0,1,0);
// Only odom creating KFs
- ASSERT_TRUE( problem->getLastKeyFrame()->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 e3890984d..ac73083b2 100644
--- a/test/gtest_sensor_camera.cpp
+++ b/test/gtest_sensor_camera.cpp
@@ -126,7 +126,7 @@ TEST(SensorCamera, install)
params->distortion = Eigen::Vector3d( 0, 0, 0 );
// Wolf problem
- ProblemPtr problem = Problem::create("PO 3D",3);
+ ProblemPtr problem = Problem::create("PO 3d",3);
// Install camera
auto sen = problem->installSensor("CAMERA", "camera", extrinsics, params);
--
GitLab