Skip to content

Commit

Permalink
fix colored_icp demo and refactor continuoustime traj
Browse files Browse the repository at this point in the history
  • Loading branch information
@koide3
koide3 committed Jun 30, 2024
1 parent e31faae commit 3e0d7a1
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 143 deletions.
16 changes: 8 additions & 8 deletions include/gtsam_points/util/continuous_trajectory.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,50 +27,50 @@ class ContinuousTrajectory {
* @param end_time End time of the trajectory
* @param knot_interval Time interval of spline control knots
*/
ContinuousTrajectory(const char symbol, const double start_time, const double end_time, const double knot_interval);
ContinuousTrajectory(char symbol, double start_time, double end_time, double knot_interval);

/**
* @brief Time of a spline knot
*/
const double knot_stamp(const int i) const;
double knot_stamp(int i) const;

/**
* @brief Key knot ID for a given time
*/
const int knot_id(const double t) const;
int knot_id(double t) const;

/**
* @brief Number of spline knots
*/
const int knot_max_id() const;
int knot_max_id() const;

/**
* @brief Get an expression of the interpolated time at t
* @param t Time
* @param t_ Expression of t
*/
const gtsam::Pose3_ pose(const double t, const gtsam::Double_& t_);
gtsam::Pose3_ pose(double t, const gtsam::Double_& t_);

/**
* @brief Calculate the interpolated time at t
* @param values Values including knot poses
* @param t Time
*/
const gtsam::Pose3 pose(const gtsam::Values& values, const double t);
gtsam::Pose3 pose(const gtsam::Values& values, double t);

/**
* @brief Get an expression of the linear acceleration and angular velocity at t
* @param t Time
* @param t_ Expression of t
*/
const gtsam::Vector6_ imu(const double t, const gtsam::Double_& t_, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));
gtsam::Vector6_ imu(double t, const gtsam::Double_& t_, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));

/**
* @brief Calculate the linear acceleration and angular velocity at t
* @param values Values including knot poses
* @param t Time
*/
const gtsam::Vector6 imu(const gtsam::Values& values, const double t, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));
gtsam::Vector6 imu(const gtsam::Values& values, double t, const Eigen::Vector3d& g = Eigen::Vector3d(0.0, 0.0, 9.80665));

/**
* @brief Optimize spline knots to fit the interpolated trajectory to a set of poses
Expand Down
140 changes: 13 additions & 127 deletions src/demo/demo_colored_registration.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,5 @@
#include <random>
#include <thread>
#include <iostream>

#include <gtest/gtest.h>
Expand All @@ -18,127 +19,6 @@
#include <glk/pointcloud_buffer.hpp>
#include <guik/viewer/light_viewer.hpp>

/*
struct ColoredGICPTestBase : public testing::Test {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void SetUp() override {
std::mt19937 mt;
std::normal_distribution<> ndist(0.0, 1e-2);
delta = Eigen::Isometry3d::Identity();
delta.translation() += Eigen::Vector3d(0.15, 0.15, 0.25);
delta.linear() = Eigen::AngleAxisd(0.05, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix();
for (double x = -4.0; x <= 4.0; x += 0.02) {
for (double y = -4.0; y < 4.0; y += 0.02) {
const Eigen::Vector4d pt(x, y, 2.0, 1.0);
target_points.push_back(pt + Eigen::Vector4d(ndist(mt), ndist(mt), ndist(mt), 0.0));
const int dx = round(x);
const int dy = round(y);
const double d = Eigen::Vector2d(x - dx, y - dy).norm();
const double intensity = d < 0.1 ? 1.0 : 0.0;
target_intensities.push_back(intensity + ndist(mt));
}
}
for (double x = -2.0; x <= 2.0; x += 0.04) {
for (double y = -2.0; y < 2.0; y += 0.04) {
const Eigen::Vector4d pt(x, y, 2.0, 1.0);
source_points.push_back(delta * pt + Eigen::Vector4d(ndist(mt), ndist(mt), ndist(mt), 0.0));
const int dx = round(x);
const int dy = round(y);
const double d = Eigen::Vector2d(x - dx, y - dy).norm();
const double intensity = d < 0.1 ? 1.0 : 0.0;
source_intensities.push_back(intensity + ndist(mt));
}
}
}
void test_factor(const gtsam::NonlinearFactor::shared_ptr& factor, const std::string& tag) {
const double error_angle_tol = 0.5 * M_PI / 180.0;
const double error_trans_tol = 0.02;
gtsam::Values values;
values.insert(0, gtsam::Pose3::Identity());
values.insert(1, gtsam::Pose3::Identity());
// Forward test (fix the first)
gtsam::NonlinearFactorGraph graph;
graph.add(factor);
graph.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(0, gtsam::Pose3::Identity(), gtsam::noiseModel::Isotropic::Precision(6, 1e6));
gtsam_points::LevenbergMarquardtExtParams lm_params;
lm_params.setMaxIterations(30);
values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
Eigen::Isometry3d estimated((values.at<gtsam::Pose3>(0).inverse() * values.at<gtsam::Pose3>(1)).matrix());
Eigen::Isometry3d error = estimated * delta;
double error_angle = Eigen::AngleAxisd(error.linear()).angle();
double error_trans = error.translation().norm();
EXPECT_LE(error_angle, error_angle_tol) << "[FORWARD] Too large rotation error " << tag;
EXPECT_LE(error_trans, error_trans_tol) << "[FORWARD] Too large translation error" << tag;
// Backward test (fix the second)
values.update(0, gtsam::Pose3::Identity());
values.update(1, gtsam::Pose3::Identity());
graph.erase(graph.begin() + 1);
graph.emplace_shared<gtsam::PriorFactor<gtsam::Pose3>>(1, gtsam::Pose3::Identity(), gtsam::noiseModel::Isotropic::Precision(6, 1e6));
values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
estimated = Eigen::Isometry3d((values.at<gtsam::Pose3>(0).inverse() * values.at<gtsam::Pose3>(1)).matrix());
error = estimated * delta;
error_angle = Eigen::AngleAxisd(error.linear()).angle();
error_trans = error.translation().norm();
EXPECT_LE(error_angle, error_angle_tol) << "[BACKWARD] Too large rotation error" << tag;
EXPECT_LE(error_trans, error_trans_tol) << "[BACKWARD] Too large translation error" << tag;
}
Eigen::Isometry3d delta;
std::vector<double> target_intensities;
std::vector<double> source_intensities;
std::vector<Eigen::Vector4d> target_points;
std::vector<Eigen::Vector4d> source_points;
};
TEST_F(ColoredGICPTestBase, Check) {
gtsam_points::PointCloudCPU::Ptr target(new gtsam_points::PointCloudCPU(target_points));
target->add_intensities(target_intensities);
auto target_gradients = gtsam_points::IntensityGradients::estimate(target, 10, 50, 1);
EXPECT_NE(target->normals, nullptr);
EXPECT_NE(target->covs, nullptr);
gtsam_points::PointCloud::Ptr target_ = target;
auto target_gradients2 = gtsam_points::IntensityGradients::estimate(target_, 50, 1);
gtsam_points::PointCloudCPU::Ptr source(new gtsam_points::PointCloudCPU(source_points));
source->add_intensities(source_intensities);
source->add_covs(gtsam_points::estimate_covariances(source->points, source->size()));
std::shared_ptr<gtsam_points::KdTree> target_tree(new gtsam_points::KdTree(target->points, target->size()));
std::shared_ptr<gtsam_points::IntensityKdTree> target_intensity_tree(
new gtsam_points::IntensityKdTree(target->points, target->intensities, target->size()));
test_factor(gtsam::make_shared<gtsam_points::IntegratedColoredGICPFactor>(0, 1, target, source, target_tree, target_gradients), "DEFAULT");
test_factor(
gtsam::make_shared<gtsam_points::IntegratedColoredGICPFactor>(0, 1, target, source, target_intensity_tree, target_gradients),
"ESTIMATE_PHOTO_AND_GEOM");
test_factor(
gtsam::make_shared<gtsam_points::IntegratedColoredGICPFactor>(0, 1, target, source, target_intensity_tree, target_gradients2),
"ESTIMATE_PHOTO_ONLY");
}
*/

int main(int argc, char** argv) {
std::mt19937 mt;

Expand All @@ -153,6 +33,8 @@ int main(int argc, char** argv) {
int method = 0;
float intensity_scale = 100.0f;

std::thread optimization_thread = std::thread([] {});

auto viewer = guik::viewer();
viewer->disable_xy_grid();

Expand All @@ -163,8 +45,8 @@ int main(int argc, char** argv) {
std::normal_distribution<> ndist(0.0, 1e-2);
std::uniform_real_distribution<> udist;

delta.translation() += Eigen::Vector3d(0.25 * udist(mt), 0.25 * udist(mt), 0.5 * udist(mt));
delta.linear() = Eigen::AngleAxisd(0.2 * udist(mt), Eigen::Vector3d(udist(mt), udist(mt), udist(mt)).normalized()).toRotationMatrix();
delta.translation() = Eigen::Vector3d(0.25 * udist(mt), 0.25 * udist(mt), 0.5 * udist(mt));
delta.linear() = Eigen::AngleAxisd(0.3 * udist(mt), Eigen::Vector3d(udist(mt), udist(mt), udist(mt)).normalized()).toRotationMatrix();

std::vector<double> target_intensities;
std::vector<double> source_intensities;
Expand Down Expand Up @@ -251,16 +133,20 @@ int main(int argc, char** argv) {
lm_params.setMaxIterations(100);
lm_params.status_msg_callback = [&](const std::string& msg) { viewer->append_text(msg); };
lm_params.callback = [&](const gtsam_points::LevenbergMarquardtOptimizationStatus& status, const gtsam::Values& values) {
viewer->find_drawable("source").first->set_model_matrix(values.at<gtsam::Pose3>(1).matrix());
viewer->find_drawable("source_coord").first->set_model_matrix(delta.matrix() * values.at<gtsam::Pose3>(1).matrix());
viewer->spin_once();
const Eigen::Isometry3d transform(values.at<gtsam::Pose3>(1).matrix());
viewer->invoke([=] {
viewer->find_drawable("source").first->set_model_matrix(transform);
viewer->find_drawable("source_coord").first->set_model_matrix(delta * transform);
});
};

values = gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize();
optimization_thread.join();
optimization_thread = std::thread([=] { gtsam_points::LevenbergMarquardtOptimizerExt(graph, values, lm_params).optimize(); });
}
});

viewer->spin();
optimization_thread.join();

return 0;
}
16 changes: 8 additions & 8 deletions src/gtsam_points/util/continuous_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,46 +16,46 @@

namespace gtsam_points {

ContinuousTrajectory::ContinuousTrajectory(const char symbol, const double start_time, const double end_time, const double knot_interval)
ContinuousTrajectory::ContinuousTrajectory(char symbol, double start_time, double end_time, double knot_interval)
: symbol(symbol),
start_time(start_time - knot_interval / 2),
end_time(end_time + knot_interval / 2),
knot_interval(knot_interval) {}

const double ContinuousTrajectory::knot_stamp(const int i) const {
double ContinuousTrajectory::knot_stamp(int i) const {
return start_time + (i - 1) * knot_interval;
}

const int ContinuousTrajectory::knot_id(const double t) const {
int ContinuousTrajectory::knot_id(double t) const {
return static_cast<int>(std::floor((t - start_time) / knot_interval)) + 1;
}

const int ContinuousTrajectory::knot_max_id() const {
int ContinuousTrajectory::knot_max_id() const {
return knot_id(end_time) + 2;
}

const gtsam::Pose3_ ContinuousTrajectory::pose(const double t, const gtsam::Double_& t_) {
gtsam::Pose3_ ContinuousTrajectory::pose(double t, const gtsam::Double_& t_) {
const int knot_i = knot_id(t);
const double knot_t = knot_stamp(knot_i);
const gtsam::Double_ p_ = (1.0 / knot_interval) * (t_ - gtsam::Double_(knot_t));

return gtsam_points::bspline(gtsam::Symbol(symbol, knot_i), p_);
}

const gtsam::Pose3 ContinuousTrajectory::pose(const gtsam::Values& values, const double t) {
gtsam::Pose3 ContinuousTrajectory::pose(const gtsam::Values& values, double t) {
const auto pose_ = pose(t, gtsam::Double_(t));
return pose_.value(values);
}

const gtsam::Vector6_ ContinuousTrajectory::imu(const double t, const gtsam::Double_& t_, const Eigen::Vector3d& g) {
gtsam::Vector6_ ContinuousTrajectory::imu(double t, const gtsam::Double_& t_, const Eigen::Vector3d& g) {
const int knot_i = knot_id(t);
const double knot_t = knot_stamp(knot_i);
const gtsam::Double_ p_ = (1.0 / knot_interval) * (t_ - gtsam::Double_(knot_t));

return gtsam_points::bspline_imu(gtsam::Symbol(symbol, knot_i), p_, knot_interval, g);
}

const gtsam::Vector6 ContinuousTrajectory::imu(const gtsam::Values& values, const double t, const Eigen::Vector3d& g) {
gtsam::Vector6 ContinuousTrajectory::imu(const gtsam::Values& values, double t, const Eigen::Vector3d& g) {
const auto imu_ = imu(t, gtsam::Double_(t), g);
return imu_.value(values);
}
Expand Down

0 comments on commit 3e0d7a1

Please sign in to comment.