fix local transform bug
fix another bug
second energy term should be right. Connection between point cloud and graph is a problem