12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed reproj error jacobians and added solid tests

release/4.3a0
lcarlone 2021-08-28 13:42:31 -04:00
parent 09853bfa13
commit 6467e3043d
3 changed files with 51 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
gtsam/geometry/SphericalCamera.cpp
View File

@ -92,12 +92,24 @@ Unit3 SphericalCamera::project(const Point3& point,
}
/* ************************************************************************* */
Vector2 SphericalCamera::reprojectionError(const Point3& point, const Unit3& measured,
OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
// on-manifold version of: camera.project(point) - zi
std::cout << "SphericalCam:reprojectionError fix jacobians " << std::endl;
return measured.localCoordinates( project2(point, Dpose, Dpoint) );
Vector2 SphericalCamera::reprojectionError(
const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
// project point
if (Dpose || Dpoint) {
Matrix26 H_project_pose;
Matrix23 H_project_point;
Matrix22 H_error;
Unit3 projected = project2(point, H_project_pose, H_project_point);
Vector2 error = measured.errorVector(projected, boost::none, H_error);
if (Dpose)
*Dpose = H_error * H_project_pose;
if (Dpoint)
*Dpoint = H_error * H_project_point;
return error;
} else {
return measured.errorVector(project2(point, Dpose, Dpoint));
}
}
/* ************************************************************************* */

11
gtsam/geometry/SphericalCamera.h
View File

@ -39,17 +39,6 @@ class GTSAM_EXPORT EmptyCal {
}
};
//
//class GTSAM_EXPORT EmptyCal {
// public:
// EmptyCal(){}
// virtual ~EmptyCal() = default;
// using shared_ptr = boost::shared_ptr<EmptyCal>;
// void print(const std::string& s) const {
// std::cout << "empty calibration: " << s << std::endl;
// }
//};
/**
* A spherical camera class that has a Pose3 and measures bearing vectors
* @addtogroup geometry

33
gtsam/geometry/tests/testSphericalCamera.cpp
View File

@ -109,6 +109,39 @@ TEST( SphericalCamera, Dproject)
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Vector2 reprojectionError2(const Pose3& pose, const Point3& point, const Unit3& measured) {
return Camera(pose).reprojectionError(point,measured);
}
/* ************************************************************************* */
TEST( SphericalCamera, reprojectionError) {
Matrix Dpose, Dpoint;
Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose,
point1, bearing1);
Matrix numerical_point = numericalDerivative32(reprojectionError2, pose,
point1, bearing1);
EXPECT(assert_equal(Vector2(0.0, 0.0), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
TEST( SphericalCamera, reprojectionError_noisy) {
Matrix Dpose, Dpoint;
Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
Vector2 result = camera.reprojectionError(point1, bearing_noisy, Dpose,
Dpoint);
Matrix numerical_pose = numericalDerivative31(reprojectionError2, pose,
point1, bearing_noisy);
Matrix numerical_point = numericalDerivative32(reprojectionError2, pose,
point1, bearing_noisy);
EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST( SphericalCamera, Dproject2)