Merge branch 'origin/release/2.4.0'
Frank Dellaert
commit
3b71fe47bc
|
|
@ -17,28 +17,29 @@
|
|||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
|
||||
using namespace boost::assign;
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
void create5PointExample1() {
|
||||
/* ************************************************************************* */
|
||||
|
||||
void createExampleBALFile(const string& filename, const vector<Point3>& P,
|
||||
const Pose3& pose1, const Pose3& pose2, const Cal3Bundler& K =
|
||||
Cal3Bundler()) {
|
||||
|
||||
// Class that will gather all data
|
||||
SfM_data data;
|
||||
|
||||
// Create two cameras and corresponding essential matrix E
|
||||
// Create two cameras
|
||||
Rot3 aRb = Rot3::yaw(M_PI_2);
|
||||
Point3 aTb(0.1, 0, 0);
|
||||
Pose3 identity, aPb(aRb, aTb);
|
||||
data.cameras.push_back(SfM_Camera(identity));
|
||||
data.cameras.push_back(SfM_Camera(aPb));
|
||||
|
||||
// Create test data, we need at least 5 points
|
||||
vector<Point3> P;
|
||||
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
|
||||
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
|
||||
data.cameras.push_back(SfM_Camera(pose1, K));
|
||||
data.cameras.push_back(SfM_Camera(pose2, K));
|
||||
|
||||
BOOST_FOREACH(const Point3& p, P) {
|
||||
|
||||
|
|
@ -51,19 +52,59 @@ void create5PointExample1() {
|
|||
|
||||
// Project points in both cameras
|
||||
for (size_t i = 0; i < 2; i++)
|
||||
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
|
||||
track.measurements.push_back(make_pair(i, data.cameras[i].project(p)));
|
||||
|
||||
// Add track to data
|
||||
data.tracks.push_back(track);
|
||||
}
|
||||
|
||||
// Assumes example is run in ${GTSAM_TOP}/build/examples
|
||||
const string filename = "../../examples/data/5pointExample1.txt";
|
||||
writeBAL(filename, data);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
void create5PointExample1() {
|
||||
|
||||
// Create two cameras poses
|
||||
Rot3 aRb = Rot3::yaw(M_PI_2);
|
||||
Point3 aTb(0.1, 0, 0);
|
||||
Pose3 pose1, pose2(aRb, aTb);
|
||||
|
||||
// Create test data, we need at least 5 points
|
||||
vector<Point3> P;
|
||||
P += Point3(0, 0, 1), Point3(-0.1, 0, 1), Point3(0.1, 0, 1), //
|
||||
Point3(0, 0.5, 0.5), Point3(0, -0.5, 0.5);
|
||||
|
||||
// Assumes example is run in ${GTSAM_TOP}/build/examples
|
||||
const string filename = "../../examples/data/5pointExample1.txt";
|
||||
createExampleBALFile(filename, P, pose1, pose2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
void create5PointExample2() {
|
||||
|
||||
// Create two cameras poses
|
||||
Rot3 aRb = Rot3::yaw(M_PI_2);
|
||||
Point3 aTb(10, 0, 0);
|
||||
Pose3 pose1, pose2(aRb, aTb);
|
||||
|
||||
// Create test data, we need at least 5 points
|
||||
vector<Point3> P;
|
||||
P += Point3(0, 0, 100), Point3(-10, 0, 100), Point3(10, 0, 100), //
|
||||
Point3(0, 50, 50), Point3(0, -50, 50), Point3(-20, 0, 80), Point3(20, -50, 80);
|
||||
|
||||
// Assumes example is run in ${GTSAM_TOP}/build/examples
|
||||
const string filename = "../../examples/data/5pointExample2.txt";
|
||||
Cal3Bundler K(500, 0, 0);
|
||||
createExampleBALFile(filename, P, pose1, pose2,K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
create5PointExample1();
|
||||
create5PointExample2();
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,65 @@
|
|||
2 7 14
|
||||
|
||||
0 0 0 -0
|
||||
1 0 -3.0616169978683830426e-15 -50
|
||||
0 1 -50 -0
|
||||
1 1 -6.1232339957367660852e-15 -100
|
||||
0 2 50 -0
|
||||
1 2 0 -0
|
||||
0 3 0 -500
|
||||
1 3 500 -100.00000000000002842
|
||||
0 4 0 500
|
||||
1 4 -500 -99.999999999999957367
|
||||
0 5 -125 -0
|
||||
1 5 -1.1481063742006437494e-14 -187.5
|
||||
0 6 125 312.5
|
||||
1 6 -312.5 62.500000000000028422
|
||||
|
||||
3.141592653589793116
|
||||
0
|
||||
0
|
||||
-0
|
||||
0
|
||||
0
|
||||
500
|
||||
0
|
||||
0
|
||||
|
||||
2.2214414690791830509
|
||||
2.2214414690791826068
|
||||
0
|
||||
-6.1232339957367662824e-16
|
||||
-10
|
||||
0
|
||||
500
|
||||
0
|
||||
0
|
||||
|
||||
0
|
||||
0
|
||||
100
|
||||
|
||||
-10
|
||||
0
|
||||
100
|
||||
|
||||
10
|
||||
0
|
||||
100
|
||||
|
||||
0
|
||||
50
|
||||
50
|
||||
|
||||
0
|
||||
-50
|
||||
50
|
||||
|
||||
-20
|
||||
0
|
||||
80
|
||||
|
||||
20
|
||||
-50
|
||||
80
|
||||
|
||||
|
|
@ -66,7 +66,8 @@ public:
|
|||
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix,
|
||||
LieScalar> {
|
||||
|
||||
Point2 pA_, pB_; ///< Measurements in image A and B
|
||||
Point3 dP1_; ///< 3D point corresponding to measurement in image 1
|
||||
Point2 p1_, p2_; ///< Measurements in image 1 and image 2
|
||||
Cal3_S2 K_; ///< Calibration
|
||||
|
||||
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base;
|
||||
|
|
@ -77,7 +78,9 @@ public:
|
|||
/// Constructor
|
||||
EssentialMatrixFactor2(Key key1, Key key2, const Point2& pA, const Point2& pB,
|
||||
const Cal3_S2& K, const SharedNoiseModel& model) :
|
||||
Base(model, key1, key2), pA_(pA), pB_(pB), K_(K) {
|
||||
Base(model, key1, key2), p1_(pA), p2_(pB), K_(K) {
|
||||
Point2 xy = K_.calibrate(p1_);
|
||||
dP1_ = Point3(xy.x(), xy.y(), 1);
|
||||
}
|
||||
|
||||
/// @return a deep copy of this factor
|
||||
|
|
@ -91,7 +94,7 @@ public:
|
|||
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
|
||||
Base::print(s);
|
||||
std::cout << " EssentialMatrixFactor2 with measurements\n ("
|
||||
<< pA_.vector().transpose() << ")' and (" << pB_.vector().transpose()
|
||||
<< p1_.vector().transpose() << ")' and (" << p2_.vector().transpose()
|
||||
<< ")'" << std::endl;
|
||||
}
|
||||
|
||||
|
|
@ -102,11 +105,11 @@ public:
|
|||
|
||||
// We have point x,y in image 1
|
||||
// Given a depth Z, the corresponding 3D point P1 = Z*(x,y,1) = (x,y,1)/d
|
||||
// We then convert to first camera by 2P = 1R2Õ*(P1-1T2)
|
||||
// We then convert to first camera by 2P = 1R2<EFBFBD>*(P1-1T2)
|
||||
// The homogeneous coordinates of can be written as
|
||||
// 2R1*(P1-1T2) == 2R1*d*(P1-1T2) == 2R1*((x,y,1)-d*1T2)
|
||||
// Note that this is just a homography for d==0
|
||||
Point3 dP1(pA_.x(), pA_.y(), 1);
|
||||
// The point d*P1 = (x,y,1) is computed in constructor as dP1_
|
||||
|
||||
// Project to normalized image coordinates, then uncalibrate
|
||||
Point2 pi;
|
||||
|
|
@ -114,7 +117,7 @@ public:
|
|||
|
||||
Point3 _1T2 = E.direction().point3();
|
||||
Point3 d1T2 = d * _1T2;
|
||||
Point3 dP2 = E.rotation().unrotate(dP1 - d1T2);
|
||||
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2);
|
||||
Point2 pn = SimpleCamera::project_to_camera(dP2);
|
||||
pi = K_.uncalibrate(pn);
|
||||
|
||||
|
|
@ -126,7 +129,7 @@ public:
|
|||
|
||||
Point3 _1T2 = E.direction().point3(D_1T2_dir);
|
||||
Point3 d1T2 = d * _1T2;
|
||||
Point3 dP2 = E.rotation().unrotate(dP1 - d1T2, DdP2_rot, DP2_point);
|
||||
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
|
||||
Point2 pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
|
||||
pi = K_.uncalibrate(pn, boost::none, Dpi_pn);
|
||||
|
||||
|
|
@ -141,7 +144,7 @@ public:
|
|||
*Dd = -(Dpi_pn * (Dpn_dP2 * (DP2_point * _1T2.vector())));
|
||||
|
||||
}
|
||||
Point2 reprojectionError = pi - pB_;
|
||||
Point2 reprojectionError = pi - p2_;
|
||||
return reprojectionError.vector();
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,6 +20,8 @@ using namespace gtsam;
|
|||
|
||||
typedef noiseModel::Isotropic::shared_ptr Model;
|
||||
|
||||
namespace example1 {
|
||||
|
||||
const string filename = findExampleDataFile("5pointExample1.txt");
|
||||
SfM_data data;
|
||||
bool readOK = readBAL(filename, data);
|
||||
|
|
@ -207,12 +209,74 @@ TEST (EssentialMatrixFactor2, minimization) {
|
|||
EssentialMatrix actual = result.at<EssentialMatrix>(100);
|
||||
EXPECT(assert_equal(trueE, actual,1e-1));
|
||||
for (size_t i = 0; i < 5; i++)
|
||||
EXPECT(assert_equal(result.at<LieScalar>(i), truth.at<LieScalar>(i),1e-1));
|
||||
EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
}
|
||||
|
||||
} // namespace example1
|
||||
|
||||
//*************************************************************************
|
||||
|
||||
namespace example2 {
|
||||
|
||||
const string filename = findExampleDataFile("5pointExample2.txt");
|
||||
SfM_data data;
|
||||
bool readOK = readBAL(filename, data);
|
||||
Rot3 aRb = data.cameras[1].pose().rotation();
|
||||
Point3 aTb = data.cameras[1].pose().translation();
|
||||
double baseline = 10; // actual baseline of the camera
|
||||
|
||||
Point2 pA(size_t i) {
|
||||
return data.tracks[i].measurements[0].second;
|
||||
}
|
||||
Point2 pB(size_t i) {
|
||||
return data.tracks[i].measurements[1].second;
|
||||
}
|
||||
|
||||
// Matches Cal3Bundler K(500, 0, 0);
|
||||
Cal3_S2 K(500, 500, 0, 0, 0);
|
||||
|
||||
//*************************************************************************
|
||||
TEST (EssentialMatrixFactor2, extraTest) {
|
||||
// Additional test with camera moving in positive X direction
|
||||
|
||||
// We start with a factor graph and add constraints to it
|
||||
// Noise sigma is 1, assuming pixel measurements
|
||||
NonlinearFactorGraph graph;
|
||||
Model model = noiseModel::Isotropic::Sigma(2, 1);
|
||||
for (size_t i = 0; i < data.number_tracks(); i++)
|
||||
graph.add(EssentialMatrixFactor2(100, i, pA(i), pB(i), K, model));
|
||||
|
||||
// Check error at ground truth
|
||||
Values truth;
|
||||
EssentialMatrix trueE(aRb, aTb);
|
||||
truth.insert(100, trueE);
|
||||
for (size_t i = 0; i < data.number_tracks(); i++) {
|
||||
Point3 P1 = data.tracks[i].p;
|
||||
truth.insert(i, LieScalar(baseline / P1.z()));
|
||||
}
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtParams parameters;
|
||||
// parameters.setVerbosity("ERROR");
|
||||
LevenbergMarquardtOptimizer optimizer(graph, truth, parameters);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
// Check result
|
||||
EssentialMatrix actual = result.at<EssentialMatrix>(100);
|
||||
EXPECT(assert_equal(trueE, actual,1e-1));
|
||||
for (size_t i = 0; i < data.number_tracks(); i++)
|
||||
EXPECT(assert_equal(truth.at<LieScalar>(i),result.at<LieScalar>(i),1e-1));
|
||||
|
||||
// Check error at result
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
Loading…
Reference in New Issue