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

compiles, testing now

release/4.3a0
lcarlone 2021-08-25 21:05:56 -04:00
parent 432921e362
commit 37dfb0c3da
3 changed files with 1329 additions and 1328 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
gtsam/slam/SmartProjectionFactorP.h
View File

@ -98,7 +98,7 @@ public:
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 // store measurement and key
this->measured_.push_back(measured); this->measured_.push_back(measured);
this->keys_.push_back(key); this->keys_.push_back(poseKey);
// store fixed intrinsic calibration // store fixed intrinsic calibration
K_all_.push_back(K); K_all_.push_back(K);
// store fixed extrinsics of the camera // store fixed extrinsics of the camera
@ -186,9 +186,9 @@ public:
*/ */
typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras(const Values& values) const override {
typename Base::Cameras cameras; typename Base::Cameras cameras;
for (const Key& k : this->keys_) { for (const Key& i : this->keys_) {
const Pose3& body_P_cam = body_P_sensors_[i]; const Pose3& body_P_cam = body_P_sensors_[i];
const Pose3 world_P_sensor_k = values.at<Pose3>(k) * body_P_cam; const Pose3 world_P_sensor_k = values.at<Pose3>(i) * body_P_cam;
cameras.emplace_back(world_P_sensor_k, K_all_[i]); cameras.emplace_back(world_P_sensor_k, K_all_[i]);
} }
return cameras; return cameras;
@ -201,7 +201,8 @@ public:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_all_);
ar & BOOST_SERIALIZATION_NVP(body_P_sensors_);
} }
}; };

1
gtsam/slam/tests/smartFactorScenarios.h
View File

@ -18,6 +18,7 @@
#pragma once #pragma once
#include <gtsam/slam/SmartProjectionPoseFactor.h> #include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <gtsam/slam/SmartProjectionFactorP.h>
#include <gtsam/slam/SmartProjectionFactor.h> #include <gtsam/slam/SmartProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>

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

File diff suppressed because it is too large Load Diff