From e312252006a9e398cd969b7de84658cae6620126 Mon Sep 17 00:00:00 2001 From: Manohar Paluri Date: Tue, 17 Nov 2009 00:51:27 +0000 Subject: [PATCH] Changed camera matrix to a shared pointer --- cpp/VSLAMFactor.cpp | 8 ++++---- cpp/VSLAMFactor.h | 5 +++-- cpp/VSLAMGraph.cpp | 3 ++- cpp/testVSLAMFactor.cpp | 6 +++--- 4 files changed, 12 insertions(+), 10 deletions(-) diff --git a/cpp/VSLAMFactor.cpp b/cpp/VSLAMFactor.cpp index f40687120..636566dfb 100644 --- a/cpp/VSLAMFactor.cpp +++ b/cpp/VSLAMFactor.cpp @@ -19,10 +19,10 @@ VSLAMFactor::VSLAMFactor() { landmarkNumber_ = 222; cameraFrameName_ = symbol('x',cameraFrameNumber_); landmarkName_ = symbol('l',landmarkNumber_); - K_ = Cal3_S2(444,555,666,777,888); + K_ = shared_ptrK(new Cal3_S2(444,555,666,777,888)); } /* ************************************************************************* */ -VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const Cal3_S2 &K) +VSLAMFactor::VSLAMFactor(const Point2& z, double sigma, int cn, int ln, const shared_ptrK &K) : NonlinearFactor(z.vector(), sigma) { cameraFrameNumber_ = cn; @@ -50,7 +50,7 @@ bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const { Vector VSLAMFactor::predict(const VSLAMConfig& c) const { Pose3 pose = c.cameraPose(cameraFrameNumber_); Point3 landmark = c.landmarkPoint(landmarkNumber_); - return project(SimpleCamera(K_,pose), landmark).vector(); + return project(SimpleCamera(*K_,pose), landmark).vector(); } /* ************************************************************************* */ @@ -67,7 +67,7 @@ GaussianFactor::shared_ptr VSLAMFactor::linearize(const VSLAMConfig& c) const Pose3 pose = c.cameraPose(cameraFrameNumber_); Point3 landmark = c.landmarkPoint(landmarkNumber_); - SimpleCamera camera(K_,pose); + SimpleCamera camera(*K_,pose); // Jacobians Matrix Dh1 = Dproject_pose(camera, landmark); diff --git a/cpp/VSLAMFactor.h b/cpp/VSLAMFactor.h index e7c72bde8..604159ffc 100644 --- a/cpp/VSLAMFactor.h +++ b/cpp/VSLAMFactor.h @@ -25,12 +25,13 @@ private: int cameraFrameNumber_, landmarkNumber_; std::string cameraFrameName_, landmarkName_; - Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. + boost::shared_ptr K_; // Calibration stored in each factor. FD: need to think about this. typedef NonlinearFactor ConvenientFactor; public: typedef boost::shared_ptr shared_ptr; // shorthand for a smart pointer to a factor + typedef boost::shared_ptr shared_ptrK; /** * Default constructor @@ -45,7 +46,7 @@ public: * @param landmarkNumber is the index of the landmark * @param K the constant calibration */ - VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const Cal3_S2& K); + VSLAMFactor(const Point2& z, double sigma, int cameraFrameNumber, int landmarkNumber, const shared_ptrK & K); /** diff --git a/cpp/VSLAMGraph.cpp b/cpp/VSLAMGraph.cpp index 45d9ac5f7..830650c22 100644 --- a/cpp/VSLAMGraph.cpp +++ b/cpp/VSLAMGraph.cpp @@ -60,7 +60,8 @@ VSLAMGraph::VSLAMGraph(const std::string& path) //VSLAMFactor::shared_ptr testing(new VSLAMFactor()); //factors_.push_back(testing); - VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z.vector(), sigma, i+1, j, K)); + VSLAMFactor::shared_ptr f1(new VSLAMFactor::VSLAMFactor(z.vector(), sigma, i+1, j, + VSLAMFactor::shared_ptrK(new Cal3_S2(K)))); factors_.push_back(f1); //cout << "Added factor " << i+1 << endl; // just to keep a record of all the feature id's that have been encountered diff --git a/cpp/testVSLAMFactor.cpp b/cpp/testVSLAMFactor.cpp index 941595a7a..c8ae05e5a 100644 --- a/cpp/testVSLAMFactor.cpp +++ b/cpp/testVSLAMFactor.cpp @@ -32,7 +32,7 @@ TEST( VSLAMFactor, error ) double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K)); + factor(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, VSLAMFactor::shared_ptrK(new Cal3_S2(K)))); // For the following configuration, the factor predicts 320,240 VSLAMConfig config; @@ -77,10 +77,10 @@ TEST( VSLAMFactor, equals ) double sigma=1.0; int cameraFrameNumber=1, landmarkNumber=1; boost::shared_ptr - factor1(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K)); + factor1(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, VSLAMFactor::shared_ptrK(new Cal3_S2(K)))); boost::shared_ptr - factor2(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, K)); + factor2(new VSLAMFactor(z, sigma, cameraFrameNumber, landmarkNumber, VSLAMFactor::shared_ptrK(new Cal3_S2(K)))); CHECK(assert_equal(*factor1, *factor2)); }