How to integrate PosedImage to HashedWaveletOctree? #53
-
I try to integrate depth image as Are these 3 classes enough ( Code: #include <Eigen/Dense>
#include <iostream>
#include "wavemap/utils/print/eigen.h"
#include "wavemap/integrator/measurement_model/continuous_ray.h"
#include "wavemap/integrator/projection_model/pinhole_camera_projector.h"
#include "wavemap/integrator/projective/coarse_to_fine/hashed_wavelet_integrator.h"
using namespace wavemap;
class DepthIntegrator {
public:
DepthIntegrator(std::shared_ptr<HashedWaveletOctree> map_ptr)
: integrator_config_(0.5, 20.0),
pinhole_config_(320.0, 320.0, 320.0, 240.0, 640, 480),
ray_config_(0.01, 0.5, 0.5),
const_image_ptr_(std::make_shared<Image<float>>(pinhole_config_.width, pinhole_config_.height)),
const_projector_ptr_(std::make_shared<const PinholeCameraProjector>(pinhole_config_)),
const_measurement_model_ptr_(std::make_shared<const ContinuousRay>(ray_config_, const_projector_ptr_, const_image_ptr_)),
posed_image_ptr_(std::make_shared<PosedImage<>>(const_projector_ptr_->getDimensions())),
offset_image_ptr_(std::make_shared<Image<Vector2D>>(const_projector_ptr_->getDimensions())),
integrator_(integrator_config_, const_projector_ptr_, posed_image_ptr_, offset_image_ptr_, const_measurement_model_ptr_, map_ptr) {
}
virtual ~DepthIntegrator() = default;
void integrateDepth(Eigen::MatrixXf& depth, Eigen::Matrix4f& pose) {
PosedImage<float> posed_image(depth);
Transformation3D tf3d(pose);
posed_image.setPose(tf3d);
integrator_.integrateRangeImage(posed_image);
}
private:
ProjectiveIntegratorConfig integrator_config_;
PinholeCameraProjectorConfig pinhole_config_;
ContinuousRayConfig ray_config_;
std::shared_ptr<const PinholeCameraProjector> const_projector_ptr_;
std::shared_ptr<const Image<float>> const_image_ptr_;
std::shared_ptr<const ContinuousRay> const_measurement_model_ptr_;
std::shared_ptr<PosedImage<float>> posed_image_ptr_;
std::shared_ptr<Image<Vector2D>> offset_image_ptr_;
HashedWaveletIntegrator integrator_;
};
int main() {
wavemap::HashedWaveletOctreeConfig config;
auto map_ptr = std::make_shared<wavemap::HashedWaveletOctree>(config);
std::cout << "Min: " << print::eigen::oneLine(map_ptr->getMinIndex()) << "\n";
std::cout << "Max: " << print::eigen::oneLine(map_ptr->getMaxIndex()) << "\n";
std::cout << "Mem: " << map_ptr->getMemoryUsage() << "\n";
auto integrator = DepthIntegrator(map_ptr);
Eigen::MatrixXf depth_mat(640, 480);
depth_mat.setRandom();
Eigen::Matrix4f pose;
pose.setIdentity();
integrator.integrateDepth(depth_mat, pose);
std::cout << "Min: " << print::eigen::oneLine(map_ptr->getMinIndex()) << "\n";
std::cout << "Max: " << print::eigen::oneLine(map_ptr->getMaxIndex()) << "\n";
std::cout << "Mem: " << map_ptr->getMemoryUsage() << "\n";
return 0;
} Stdout:
Then I used |
Beta Was this translation helpful? Give feedback.
Replies: 2 comments 1 reply
-
The main issue is that you need to share the same image between the measurement model and the integrator. In your case, the measurement model computes its updates using an uninitialized image. Here's your code with minimal changes to make it work:
|
Beta Was this translation helpful? Give feedback.
-
@victorreijgwart thank you so much! I just chaecked and it works exactly as I wanted. Moreover, your |
Beta Was this translation helpful? Give feedback.
The main issue is that you need to share the same image between the measurement model and the integrator. In your case, the measurement model computes its updates using an uninitialized image.
Also, be careful to initialize the class members in the right order and avoid passing uninitialized members as a constructor argument to other members.
Here's your code with minimal changes to make it work: