jerome/360toPerspective
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Working Camera class in C++.

Browse source
This commit is contained in:
Jerome 2023-04-30 11:47:40 +02:00
parent f1a2c4860f
commit 14dd949f10
2 changed files with 39 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
cpp/src/Camera.cpp
View file

@ -81,7 +81,7 @@ Eigen::Vector3d Camera::ComputeRayDirInInertialFrame(Eigen::Vector2d const& p_no
Eigen::Vector3d Camera::ProjectPointToSensorCameraFrame(Eigen::Vector3d const& p_camera) const {
Eigen::Vector3d p_sensor = p_camera;
if(p_camera[2] != 0.0)
p_sensor *= tanFOV2/std::abs(p_camera[2]);
p_sensor /= tanFOV2*std::abs(p_camera[2]);
return p_sensor;
}

50
cpp/src/tests.cpp
View file

@ -49,8 +49,20 @@ static size_t num_tests_ko = 0;
#define EXPECT_VEC_EQ(actual, expected) EXPECT_VEC_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_VEC_EQ_IMPL(actual, expected, file, line) \
do { \
auto&& d = (actual) - (expected); \
EXPECT_DOUBLE_EQ_IMPL(d.array().abs().maxCoeff(), 0.0, file, line); \
for(unsigned int i = 0 ; i < (expected).size() ; i++) { \
EXPECT_DOUBLE_EQ_IMPL((actual)[i], (expected)[i], file, line); \
} \
} while (false)
// EXPECT_MAT_EQ macro
#define EXPECT_MAT_EQ(actual, expected) EXPECT_MAT_EQ_IMPL(actual, expected, __FILE__, __LINE__)
#define EXPECT_MAT_EQ_IMPL(actual, expected, file, line) \
do { \
for(unsigned int i = 0 ; i < (expected).rows() ; i++) { \
for(unsigned int j = 0 ; j < (expected).rows() ; j++) { \
EXPECT_DOUBLE_EQ_IMPL((actual)(i,j), (expected)(i,j), file, line); \
} \
} \
} while (false)
#define PRINT_TESTS_SUMMARY() \
@ -78,8 +90,8 @@ int main() {
EXPECT_EQ(camera.GetForward(), Eigen::Vector3d(1, 0, 0));
EXPECT_EQ(camera.GetUp(), Eigen::Vector3d(0, 0, 1));
EXPECT_DOUBLE_EQ(camera.GetFOV(), 90.0*M_PI/180.0);
EXPECT_EQ(camera.GetR(), default_R);
EXPECT_EQ(camera.GetR_T(), default_R.transpose());
EXPECT_MAT_EQ(camera.GetR(), default_R);
EXPECT_MAT_EQ(camera.GetR_T(), default_R.transpose());
// Check target computation
Eigen::Vector3d target(1, 1, 1);
@ -91,7 +103,7 @@ int main() {
-0. , 0.8164965809277261, -0.5773502691896258;
EXPECT_VEC_EQ(camera.GetForward(), Eigen::Vector3d(0.57735027, 0.57735027, 0.57735027));
EXPECT_VEC_EQ(camera.GetR(), target_R);
EXPECT_MAT_EQ(camera.GetR(), target_R);
// Set some values using the setters
Eigen::Vector3d fwd(1, 0, 0);
@ -115,9 +127,8 @@ int main() {
// Test point projections
camera.SetFOV(20*DEG2RAD);
camera.SetUp(Eigen::Vector3d(0, 0, 1));
camera.SetTarget(Eigen::Vector3d(1.,.5,.3));
PRINT_VAR(camera.GetR());
PRINT_VAR(camera.GetR_T());
EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 0.0, 0.0)), Eigen::Vector3d(0.0, 0.0, -1.0));
EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0, 0.0)), Eigen::Vector3d(0.1736481776669304, 0, -0.9848077530122081));
@ -125,11 +136,26 @@ int main() {
EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d( 1.0, -1.0)), Eigen::Vector3d(0.1710878697460355, -0.1710878697460355, -0.970287525247814));
EXPECT_VEC_EQ(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(-0.1710878697460355, -0.1710878697460355, -0.970287525247814));
std::cout << (camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(0.0, 0.0)).transpose()) << std::endl;
std::cout << (camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 0.0)).transpose()) << std::endl;
std::cout << (camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 1.0)).transpose()) << std::endl;
std::cout << (camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, -1.0)).transpose()) << std::endl;
std::cout << (camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(-1.0, -1.0)).transpose()) << std::endl;
EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(0.0, 0.0)), Eigen::Vector3d(0.86386842558136, 0.43193421279068, 0.259160527674408));
EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 0.0)), Eigen::Vector3d(0.9284021489814165, 0.2700565097745997, 0.2552232969284919));
EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, 1.0)), Eigen::Vector3d(0.8750553718495241, 0.2462456324858795, 0.416702753385336));
EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(1.0, -1.0)), Eigen::Vector3d(0.9543717844957083, 0.2859038388089716, 0.08621770069290194));
EXPECT_VEC_EQ(camera.ComputeRayDirInInertialFrame(Eigen::Vector2d(-1.0, -1.0)), Eigen::Vector3d(0.8013461417446023, 0.5919551243111837, 0.08621770069290194));
EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(0.0, 0.0))), Eigen::Vector3d(0.0, 0.0, -1.0/tan(camera.GetFOV()/2.0)));
EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(1.0, 1.0))), Eigen::Vector3d(1.0, 1.0, -1.0/tan(camera.GetFOV()/2.0)));
EXPECT_VEC_EQ(camera.ProjectPointToSensorCameraFrame(camera.ComputeRayDirInCameraFrame(Eigen::Vector2d(-1.0, 0.5))), Eigen::Vector3d(-1.0, 0.5, -1.0/tan(camera.GetFOV()/2.0)));
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.0, 0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0, 0.0, -1.0/tan(camera.GetFOV()/2.0))), true);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0, 1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.0, -1.0, -1.0/tan(camera.GetFOV()/2.0))), true);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d(-1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 1.5, -0.5, -1.0/tan(camera.GetFOV()/2.0))), false);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, 1.5, -1.0/tan(camera.GetFOV()/2.0))), false);
EXPECT_EQ(camera.IsPointVisibleCameraFrame(Eigen::Vector3d( 0.5, -0.5, +1.0/tan(camera.GetFOV()/2.0))), false);
// Check static methods
unsigned int width = 640, height = 480;