-
Notifications
You must be signed in to change notification settings - Fork 44
Description
#include <clipper/clipper.h>
#include <clipper/utils.h>
#include <clipper/invariants/euclidean_distance.h>
#include <ros/ros.h>
#include <ros/package.h>
void runClipperMatching(
Eigen::Matrix3Xd model,
Eigen::Matrix3Xd data) {
clipper::invariants::EuclideanDistance::Params iparams;
clipper::invariants::EuclideanDistancePtr invariant =
std::make_sharedclipper::invariants::EuclideanDistance(iparams);
clipper::Params params;
clipper::CLIPPER clipper(invariant, params);
clipper.scorePairwiseConsistency(model, data);
clipper.solve();
// check that the select clique was correct
clipper::Association Ainliers = clipper.getSelectedAssociations();
// Output associations
clipper::Association inliers = clipper.getSelectedAssociations();
for (int i = 0; i < inliers.rows(); ++i) {
int model_idx = inliers(i, 0);
int data_idx = inliers(i, 1);
ROS_INFO_STREAM(" Model[" << model_idx << "] (" << model.col(model_idx).transpose()
<< ") <-> Data[" << data_idx << "] (" << data.col(data_idx).transpose() << ")");
}
// // return matches;
}
int main(int argc, char **argv) {
// Initialize the ROS system and specify the name of the node
ros::init(argc, argv, "april_slam_cpp");
ROS_INFO("[CLIPPER TEST] Running object graph matching test...");
// create a target/model point cloud of data
// Eigen::Matrix3Xd model(3, 4);
// model.col(0) << 1.0, 2.0, 0.0;
// model.col(1) << 3.0, 1.5, 0.0;
// model.col(2) << 2.0, 4.0, 0.0;
// model.col(3) << 0.0, 0.0, 0.0;
// Eigen::Matrix3Xd data(3, 4);
// data.col(0) << 0.1, -0.1, 0.0;
// data.col(1) << 1.05, 2.1, 0.0;
// data.col(2) << 2.95, 1.6, 0.0;
// data.col(3) << 2.1, 3.95, 0.0;
// create a target/model point cloud of data
Eigen::Matrix3Xd model(3, 4);
model.col(0) << 0, 0, 0;
model.col(1) << 2, 0, 0;
model.col(2) << 0, 3, 0;
model.col(3) << 2, 2, 0;
// transform of data w.r.t model
Eigen::Affine3d T_MD;
T_MD = Eigen::AngleAxisd(M_PI/8, Eigen::Vector3d::UnitZ());
T_MD.translation() << 5, 3, 0;
Eigen::Matrix3Xd data = T_MD.inverse() * model;
runClipperMatching(model, data);
// // an empty association set will be assumed to be all-to-all
// clipper.scorePairwiseConsistency(model, data);
// clipper::invariants::EuclideanDistance::Params iparams;
// clipper::invariants::EuclideanDistancePtr invariant =
// std::make_shared<clipper::invariants::EuclideanDistance>(iparams);
// clipper::Params params;
// clipper::CLIPPER clipper(invariant, params);
// clipper.solve();
// // check that the select clique was correct
// clipper::Association Ainliers = clipper.getSelectedAssociations();
// // Output associations
// clipper::Association inliers = clipper.getSelectedAssociations();
// ROS_INFO_STREAM("[CLIPPER TEST] Inlier Matches:");
// for (int i = 0; i < inliers.rows(); ++i) {
// int model_idx = inliers(i, 0);
// int data_idx = inliers(i, 1);
// ROS_INFO_STREAM(" Model[" << model_idx << "] (" << model.col(model_idx).transpose()
// << ") <-> Data[" << data_idx << "] (" << data.col(data_idx).transpose() << ")");
// }
ROS_INFO("[CLIPPER TEST] Matching test complete.");
// Create a handle to this process' node
ros::NodeHandle nh;
// ROS enters a loop, pumping callbacks. Internally, it will call all the callbacks waiting to be called at that point in time.
ros::spin();
return 0;
}
Dear developers of clipper,
I am trying to implement clipper into my ros1 cpp project (see the simple test code i posted). I copied it directly from your unit test script and this error keeps showing up:
process[dagnn-2]: started with pid [231643]
[INFO] [1755533940.195183810]: [CLIPPER TEST] Running object graph matching test...
[INFO] [1755533940.195794904]: Model[1] (2 0 0) <-> Data[1] (-3.91969 -1.62359 0)
[INFO] [1755533940.195878481]: Model[3] (2 2 0) <-> Data[3] (-3.15432 0.224171 0)
[INFO] [1755533940.195937045]: Model[2] (0 3 0) <-> Data[2] (-4.6194 1.91342 0)
[INFO] [1755533940.195994181]: Model[0] (0 0 0) <-> Data[0] ( -5.76745 -0.858221 0)
double free or corruption (out)
[dagnn-2] process has died [pid 231643, exit code -6, cmd /home/shuoyuan/catkin_oldslam_ws/devel/lib/dagnn/dagnn __name:=dagnn __log:=/home/shuoyuan/.ros/log/0c06eda6-7c4f-11f0-8c6e-2752ba58e0c7/dagnn-2.log].
log file: /home/shuoyuan/.ros/log/0c06eda6-7c4f-11f0-8c6e-2752ba58e0c7/dagnn-2*.log
^C[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done
The eigen version i used to compile clipper is 3.3.7 and system ubuntu 20. I really appreciate your kind reply. Please let me know if anything else i can provide from my side.
Regards
Shuoyuan