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

Changed camera matrix to a shared pointer

release/4.3a0
Manohar Paluri 2009-11-17 00:51:27 +00:00
parent 256c094039
commit e312252006
4 changed files with 12 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
cpp/VSLAMFactor.cpp
View File

@ -19,10 +19,10 @@ VSLAMFactor::VSLAMFactor() {
landmarkNumber_ = 222; landmarkNumber_ = 222;
cameraFrameName_ = symbol('x',cameraFrameNumber_); cameraFrameName_ = symbol('x',cameraFrameNumber_);
landmarkName_ = symbol('l',landmarkNumber_); 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<VSLAMConfig>(z.vector(), sigma) : NonlinearFactor<VSLAMConfig>(z.vector(), sigma)
{ {
cameraFrameNumber_ = cn; cameraFrameNumber_ = cn;
@ -50,7 +50,7 @@ bool VSLAMFactor::equals(const VSLAMFactor& p, double tol) const {
Vector VSLAMFactor::predict(const VSLAMConfig& c) const { Vector VSLAMFactor::predict(const VSLAMConfig& c) const {
Pose3 pose = c.cameraPose(cameraFrameNumber_); Pose3 pose = c.cameraPose(cameraFrameNumber_);
Point3 landmark = c.landmarkPoint(landmarkNumber_); 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_); Pose3 pose = c.cameraPose(cameraFrameNumber_);
Point3 landmark = c.landmarkPoint(landmarkNumber_); Point3 landmark = c.landmarkPoint(landmarkNumber_);
SimpleCamera camera(K_,pose); SimpleCamera camera(*K_,pose);
// Jacobians // Jacobians
Matrix Dh1 = Dproject_pose(camera, landmark); Matrix Dh1 = Dproject_pose(camera, landmark);

5
cpp/VSLAMFactor.h
View File

@ -25,12 +25,13 @@ private:
int cameraFrameNumber_, landmarkNumber_; int cameraFrameNumber_, landmarkNumber_;
std::string cameraFrameName_, landmarkName_; std::string cameraFrameName_, landmarkName_;
Cal3_S2 K_; // Calibration stored in each factor. FD: need to think about this. boost::shared_ptr<Cal3_S2> K_; // Calibration stored in each factor. FD: need to think about this.
typedef NonlinearFactor<VSLAMConfig> ConvenientFactor; typedef NonlinearFactor<VSLAMConfig> ConvenientFactor;
public: public:
typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor typedef boost::shared_ptr<VSLAMFactor> shared_ptr; // shorthand for a smart pointer to a factor
typedef boost::shared_ptr<Cal3_S2> shared_ptrK;
/** /**
* Default constructor * Default constructor
@ -45,7 +46,7 @@ public:
* @param landmarkNumber is the index of the landmark * @param landmarkNumber is the index of the landmark
* @param K the constant calibration * @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);
/** /**

3
cpp/VSLAMGraph.cpp
View File

@ -60,7 +60,8 @@ VSLAMGraph::VSLAMGraph(const std::string& path)
//VSLAMFactor::shared_ptr testing(new VSLAMFactor()); //VSLAMFactor::shared_ptr testing(new VSLAMFactor());
//factors_.push_back(testing); //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); factors_.push_back(f1);
//cout << "Added factor " << i+1 << endl; //cout << "Added factor " << i+1 << endl;
// just to keep a record of all the feature id's that have been encountered // just to keep a record of all the feature id's that have been encountered

6
cpp/testVSLAMFactor.cpp
View File

@ -32,7 +32,7 @@ TEST( VSLAMFactor, error )
double sigma=1.0; double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<VSLAMFactor> boost::shared_ptr<VSLAMFactor>
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 // For the following configuration, the factor predicts 320,240
VSLAMConfig config; VSLAMConfig config;
@ -77,10 +77,10 @@ TEST( VSLAMFactor, equals )
double sigma=1.0; double sigma=1.0;
int cameraFrameNumber=1, landmarkNumber=1; int cameraFrameNumber=1, landmarkNumber=1;
boost::shared_ptr<VSLAMFactor> boost::shared_ptr<VSLAMFactor>
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<VSLAMFactor> boost::shared_ptr<VSLAMFactor>
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)); CHECK(assert_equal(*factor1, *factor2));
} }