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

started serious testing: all tests pass for now

release/4.3a0
lcarlone 2021-07-21 13:58:47 -04:00
parent 02d2d97a8e
commit 30f304e733
4 changed files with 84 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/base/Lie.h
View File

@ -17,7 +17,7 @@
* @author Frank Dellaert
* @author Mike Bosse
* @author Duy Nguyen Ta
* @author shteren1
* @author Yotam Stern
*/

2
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY;
/* ************************************************************************* */
TEST( SmartProjectionPoseFactor, Constructor) {

85
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -167,12 +167,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
return K_all_;
}
/// return (for each observation) the key of the pair of poses from which we interpolate
const std::vector<std::pair<Key, Key>> world_P_body_key_pairs() const {
return world_P_body_key_pairs_;
}
/// return the interpolation factors gammas
const std::vector<double> getGammas() const {
return gammas_;
}
/// return the interpolation factors gammas
/// return the extrinsic camera calibration body_P_sensors
const std::vector<Pose3> body_P_sensors() const {
return body_P_sensors_;
}
@ -192,7 +197,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
std::cout << " pose2 key: "
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " gamma: " << gammas_[i] << std::endl;
K_all_[i]->print("calibration = ");
body_P_sensors_[i].print("extrinsic calibration:\n");
K_all_[i]->print("intrinsic calibration = ");
}
Base::print("", keyFormatter);
}
@ -202,8 +208,30 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
const SmartProjectionPoseFactorRollingShutter<CALIBRATION>* e =
dynamic_cast<const SmartProjectionPoseFactorRollingShutter<CALIBRATION>*>(&p);
double keyPairsEqual = true;
if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){
for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){
const Key key1own = world_P_body_key_pairs_[k].first;
const Key key1e = e->world_P_body_key_pairs()[k].first;
const Key key2own = world_P_body_key_pairs_[k].second;
const Key key2e = e->world_P_body_key_pairs()[k].second;
if ( !(key1own == key1e) || !(key2own == key2e) ){
keyPairsEqual = false; break;
}
}
}else{ keyPairsEqual = false; }
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;
}
}
}else{ extrinsicCalibrationEqual = false; }
return e && Base::equals(p, tol) && K_all_ == e->calibration()
&& gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors();
&& gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual;
}
/**
@ -249,8 +277,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
* and the error vector b. Note that the jacobians are computed for a given point.
*/
void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b,
const Values& values) const
override {
const Values& values) const {
if (!this->result_) {
throw("computeJacobiansWithTriangulatedPoint");
} else { // valid result: compute jacobians
@ -296,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
/// linearize and return a Hessianfactor that is an approximation of error(p)
boost::shared_ptr<RegularHessianFactor<DimBlock> > createHessianFactor(
const Values& values, const double lambda = 0.0, bool diagonalDamping =
false) const override {
false) const {
// we may have multiple cameras sharing the same extrinsic cals, hence the number
// of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we
@ -313,29 +340,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor<
throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: "
"measured_.size() inconsistent with input");
// triangulate 3D point at given linearization point
triangulateSafe(cameras(values));
if (!this->result_) { // failed: return "empty/zero" Hessian
for (Matrix& m : Gs)
m = Matrix::Zero(DimPose, DimPose);
for (Vector& v : gs)
v = Vector::Zero(DimPose);
return boost::make_shared < RegularHessianFactor<DimPose>
> ( this->keys_, Gs, gs, 0.0);
}
// compute Jacobian given triangulated 3D Point
FBlocks Fs;
Matrix F, E;
Vector b;
computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
// Whiten using noise model
this->noiseModel_->WhitenSystem(E, b);
for (size_t i = 0; i < Fs.size(); i++)
Fs[i] = this->noiseModel_->Whiten(Fs[i]);
// // triangulate 3D point at given linearization point
// triangulateSafe(cameras(values));
//
// if (!this->result_) { // failed: return "empty/zero" Hessian
// for (Matrix& m : Gs)
// m = Matrix::Zero(DimPose, DimPose);
// for (Vector& v : gs)
// v = Vector::Zero(DimPose);
// return boost::make_shared < RegularHessianFactor<DimPose>
// > ( this->keys_, Gs, gs, 0.0);
// }
//
// // compute Jacobian given triangulated 3D Point
// FBlocks Fs;
// Matrix F, E;
// Vector b;
// computeJacobiansWithTriangulatedPoint(Fs, E, b, values);
//
// // Whiten using noise model
// this->noiseModel_->WhitenSystem(E, b);
// for (size_t i = 0; i < Fs.size(); i++)
// Fs[i] = this->noiseModel_->Whiten(Fs[i]);
//
// // build augmented Hessian (with last row/column being the information vector)
// Matrix3 P;
// This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping);

53
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -10,13 +10,13 @@
* -------------------------------------------------------------------------- */
/**
* @file testSmartProjectionPoseFactorRollingShutter.cpp
* @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class
* @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp
* @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class
* @author Luca Carlone
* @date July 2021
*/
#include "gtsam/slam/tests/smartFactorScenarios.h"
//#include "gtsam/slam/tests/smartFactorScenarios.h"
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -27,6 +27,7 @@
#include <boost/assign/std/map.hpp>
#include <iostream>
using namespace gtsam;
using namespace boost::assign;
using namespace std::placeholders;
@ -47,17 +48,15 @@ static Symbol x3('X', 3);
static Point2 measurement1(323.0, 240.0);
LevenbergMarquardtParams lmParams;
// Make more verbose like so (in tests):
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
typedef SmartProjectionPoseFactorRollingShutter<Cal3_S2> SmartFactorRS;
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
/* ************************************************************************* */
TEST( SmartProjectionPoseFactorRollingShutter, Constructor) {
SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model));
}
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor2) {
TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
@ -65,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
}
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor3) {
TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
}
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Constructor4) {
TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
@ -81,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
}
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, params) {
TEST( SmartProjectionPoseFactorRollingShutter, params) {
using namespace vanillaPose;
SmartProjectionParams params;
double rt = params.getRetriangulationThreshold();
@ -92,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) {
}
/* ************************************************************************* *
TEST( SmartProjectionPoseFactor, Equals ) {
TEST( SmartProjectionPoseFactorRollingShutter, Equals ) {
using namespace vanillaPose;
SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK));
factor1->add(measurement1, x1);
@ -104,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, noiseless ) {
TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) {
using namespace vanillaPose;
@ -162,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, noisy ) {
TEST( SmartProjectionPoseFactorRollingShutter, noisy ) {
using namespace vanillaPose;
@ -196,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) {
}
/* *************************************************************************
TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) {
using namespace vanillaPose;
// create arbitrary body_T_sensor (transforms from sensor to body)
@ -271,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) {
using namespace vanillaPose2;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
@ -332,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, Factors ) {
TEST( SmartProjectionPoseFactorRollingShutter, Factors ) {
using namespace vanillaPose;
@ -496,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) {
using namespace vanillaPose;
@ -550,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, jacobianSVD ) {
TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) {
using namespace vanillaPose;
@ -606,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, landmarkDistance ) {
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
using namespace vanillaPose;
@ -665,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
using namespace vanillaPose;
@ -731,7 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) {
using namespace vanillaPose2;
@ -778,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, CheckHessian) {
TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) {
KeyVector views {x1, x2, x3};
@ -786,7 +785,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
// Two slightly different cameras
Pose3 pose2 = level_pose
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
Camera cam2(pose2, sharedK);
Camera cam3(pose3, sharedK);
@ -860,7 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
}
/* *************************************************************************
TEST( SmartProjectionPoseFactor, Hessian ) {
TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) {
using namespace vanillaPose2;