diff --git a/cpp/src/Camera.cpp b/cpp/src/Camera.cpp
index 1685c86..6b2ef32 100644
--- a/cpp/src/Camera.cpp
+++ b/cpp/src/Camera.cpp
@@ -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;
 }
 
diff --git a/cpp/src/tests.cpp b/cpp/src/tests.cpp
index f1b82dc..6d2d729 100644
--- a/cpp/src/tests.cpp
+++ b/cpp/src/tests.cpp
@@ -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;