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

progress on tests

release/4.3a0
lcarlone 2021-08-25 21:39:57 -04:00
parent df82ca1b0c
commit 492c2b8561
2 changed files with 106 additions and 91 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
gtsam/slam/SmartProjectionFactorP.h
View File

@ -46,15 +46,15 @@ namespace gtsam {
* @addtogroup SLAM
*/
template<class CAMERA>
class SmartProjectionFactorP: public SmartProjectionFactor<CAMERA> {
class SmartProjectionFactorP : public SmartProjectionFactor<CAMERA> {
private:
private:
typedef SmartProjectionFactor<CAMERA> Base;
typedef SmartProjectionFactorP<CAMERA> This;
typedef CAMERA Camera;
typedef typename CAMERA::CalibrationType CALIBRATION;
protected:
protected:
/// shared pointer to calibration object (one for each observation)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_;
@ -62,22 +62,23 @@ protected:
/// Pose of the camera in the body frame (one for each observation)
std::vector<Pose3> body_P_sensors_;
public:
public:
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor, only for serialization
SmartProjectionFactorP() {}
SmartProjectionFactorP() {
}
/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @param params parameters for the smart projection factors
*/
SmartProjectionFactorP(
const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams())
SmartProjectionFactorP(const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params =
SmartProjectionParams())
: Base(sharedNoiseModel, params) {
}
@ -95,7 +96,8 @@ public:
* @param body_P_sensor (fixed) camera extrinsic calibration
*/
void add(const Point2& measured, const Key& poseKey,
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor = Pose3::identity()) {
const boost::shared_ptr<CALIBRATION>& K, const Pose3 body_P_sensor =
Pose3::identity()) {
// store measurement and key
this->measured_.push_back(measured);
this->keys_.push_back(poseKey);
@ -113,15 +115,17 @@ public:
* @param Ks vector of (fixed) intrinsic calibration objects
* @param body_P_sensors vector of (fixed) extrinsic calibration objects
*/
void add(const Point2Vector& measurements,
const std::vector<Key>& poseKeys,
void add(const Point2Vector& measurements, const std::vector<Key>& poseKeys,
const std::vector<boost::shared_ptr<CALIBRATION>>& Ks,
const std::vector<Pose3> body_P_sensors) {
const std::vector<Pose3> body_P_sensors = std::vector<Pose3>()) {
assert(poseKeys.size() == measurements.size());
assert(poseKeys.size() == Ks.size());
assert(poseKeys.size() == body_P_sensors.size());
for (size_t i = 0; i < measurements.size(); i++) {
add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]);
if (poseKeys.size() == body_P_sensors.size()) {
add(measurements[i], poseKeys[i], Ks[i], body_P_sensors[i]);
} else {
add(measurements[i], poseKeys[i], Ks[i]); // use default extrinsics
}
}
}
@ -141,7 +145,7 @@ public:
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const override {
DefaultKeyFormatter) const override {
std::cout << s << "SmartProjectionFactorP: \n ";
for (size_t i = 0; i < K_all_.size(); i++) {
std::cout << "-- Measurement nr " << i << std::endl;
@ -155,13 +159,16 @@ public:
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This *e = dynamic_cast<const This*>(&p);
double extrinsicCalibrationEqual = true;
if(this->body_P_sensors_.size() == e->body_P_sensors().size()){
for(size_t i=0; i< this->body_P_sensors_.size(); i++){
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])){
extrinsicCalibrationEqual = false; break;
if (this->body_P_sensors_.size() == e->body_P_sensors().size()) {
for (size_t i = 0; i < this->body_P_sensors_.size(); i++) {
if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) {
extrinsicCalibrationEqual = false;
break;
}
}
}else{ extrinsicCalibrationEqual = false; }
} else {
extrinsicCalibrationEqual = false;
}
return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& extrinsicCalibrationEqual;
@ -173,7 +180,7 @@ public:
double error(const Values& values) const override {
if (this->active(values)) {
return this->totalReprojectionError(cameras(values));
} else { // else of active flag
} else { // else of active flag
return 0.0;
}
}
@ -215,4 +222,4 @@ struct traits<SmartProjectionFactorP<CAMERA> > : public Testable<
SmartProjectionFactorP<CAMERA> > {
};
} // \ namespace gtsam
} // \ namespace gtsam

146
gtsam/slam/tests/testSmartProjectionFactorP.cpp
View File

@ -114,77 +114,85 @@ TEST( SmartProjectionFactorP, noiseless ) {
double expectedError = 0.0;
EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7);
// SmartFactorP::Cameras cameras = factor.cameras(values);
// double actualError2 = factor.totalReprojectionError(cameras);
// EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
//
// // Calculate expected derivative for point (easiest to check)
// std::function<Vector(Point3)> f = //
// std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
//
// // Calculate using computeEP
// Matrix actualE;
// factor.triangulateAndComputeE(actualE, values);
//
// // get point
// boost::optional<Point3> point = factor.point();
// CHECK(point);
//
// // calculate numerical derivative with triangulated point
// Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
// EXPECT(assert_equal(expectedE, actualE, 1e-7));
//
// // Calculate using reprojectionError
// SmartFactorP::Cameras::FBlocks F;
// Matrix E;
// Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
// EXPECT(assert_equal(expectedE, E, 1e-7));
//
// EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
//
// // Calculate using computeJacobians
// Vector b;
// SmartFactorP::FBlocks Fs;
// factor.computeJacobians(Fs, E, b, cameras, *point);
// double actualError3 = b.squaredNorm();
// EXPECT(assert_equal(expectedE, E, 1e-7));
// EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
SmartFactorP::Cameras cameras = factor.cameras(values);
double actualError2 = factor.totalReprojectionError(cameras);
EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7);
// Calculate expected derivative for point (easiest to check)
std::function<Vector(Point3)> f = //
std::bind(&SmartFactorP::whitenedError<Point3>, factor, cameras, std::placeholders::_1);
// Calculate using computeEP
Matrix actualE;
factor.triangulateAndComputeE(actualE, values);
// get point
boost::optional<Point3> point = factor.point();
CHECK(point);
// calculate numerical derivative with triangulated point
Matrix expectedE = sigma * numericalDerivative11<Vector, Point3>(f, *point);
EXPECT(assert_equal(expectedE, actualE, 1e-7));
// Calculate using reprojectionError
SmartFactorP::Cameras::FBlocks F;
Matrix E;
Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E);
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT(assert_equal(Z_4x1, actualErrors, 1e-7));
// Calculate using computeJacobians
Vector b;
SmartFactorP::FBlocks Fs;
factor.computeJacobians(Fs, E, b, cameras, *point);
double actualError3 = b.squaredNorm();
EXPECT(assert_equal(expectedE, E, 1e-7));
EXPECT_DOUBLES_EQUAL(expectedError, actualError3, 1e-6);
}
/* *************************************************************************/
TEST( SmartProjectionFactorP, noisy ) {
using namespace vanillaPose;
// Project two landmarks into two cameras
Point2 pixelError(0.2, 0.2);
Point2 level_uv = level_camera.project(landmark1) + pixelError;
Point2 level_uv_right = level_camera_right.project(landmark1);
Values values;
values.insert(x1, cam1.pose());
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
values.insert(x2, pose_right.compose(noise_pose));
SmartFactorP::shared_ptr factor(new SmartFactorP(model));
factor->add(level_uv, x1, sharedK);
factor->add(level_uv_right, x2, sharedK);
double actualError1 = factor->error(values);
SmartFactorP::shared_ptr factor2(new SmartFactorP(model));
Point2Vector measurements;
measurements.push_back(level_uv);
measurements.push_back(level_uv_right);
std::vector<boost::shared_ptr<Cal3_S2>> sharedKs;
sharedKs.push_back(sharedK);
sharedKs.push_back(sharedK);
std::vector<Pose3> body_P_sensors;
body_P_sensors.push_back(Pose3::identity());
body_P_sensors.push_back(Pose3::identity());
KeyVector views {x1, x2};
factor2->add(measurements, views, sharedKs, body_P_sensors);
double actualError2 = factor2->error(values);
DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
}
///* *************************************************************************/
//TEST( SmartProjectionFactorP, noisy ) {
//
// using namespace vanillaPose;
//
// // Project two landmarks into two cameras
// Point2 pixelError(0.2, 0.2);
// Point2 level_uv = level_camera.project(landmark1) + pixelError;
// Point2 level_uv_right = level_camera_right.project(landmark1);
//
// Values values;
// values.insert(x1, cam1.pose());
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
// Point3(0.5, 0.1, 0.3));
// values.insert(x2, pose_right.compose(noise_pose));
//
// SmartFactorP::shared_ptr factor(new SmartFactorP(model, sharedK));
// factor->add(level_uv, x1);
// factor->add(level_uv_right, x2);
//
// double actualError1 = factor->error(values);
//
// SmartFactorP::shared_ptr factor2(new SmartFactorP(model, sharedK));
// Point2Vector measurements;
// measurements.push_back(level_uv);
// measurements.push_back(level_uv_right);
//
// KeyVector views {x1, x2};
//
// factor2->add(measurements, views);
// double actualError2 = factor2->error(values);
// DOUBLES_EQUAL(actualError1, actualError2, 1e-7);
//}
//
///* *************************************************************************/
//TEST(SmartProjectionFactorP, smartFactorWithSensorBodyTransform) {
// using namespace vanillaPose;